ance, the UD-factorized version of the square-root-filter
is used [Bierman 75]. Details may be found in [Wuensche
88; Mysliwetz 90; Bierman 77; Maybeck 79]. By exploit-
ing the sparseness of the transition matrix in the dynamical
models a speedup can be achieved. Special care has to be
taken inthe initialization phase when good object hypothe-
ses are in demand. From feature aggregations which may
have been collected in a systematic search covering ex-
tended regions of the image, the existence of objects has
to be hypothesized.
The general form of a dynamical model for a system of
order n (number of state components necessary to uniqu-
ely specify the system state x(1) with r control inputs u(7))
is, in vector notation
«f Cx, u, 2) * v (0, (1)
where v (7) is process noise with covariance matrix Q.
The measured variables y, an m-vector, are related to
the state vector x through the nonlinear relation (including
perspective mapping with parameters p)
y=h(x,p)+w, (2)
where w (7) is a measurement noise term with covari-
ance matrix R .
Let ® ( xo, uo, #;) be the linearized transition matrix
from xo, uo at ?; to x" at 1-1 for the deterministic part of
(1) and C. the Jacobian matrix of the deterministic part of
(2)
= Sh(x)
X ye
C(x")
T (3)
P is the covariance matrix of the state variables (n x n)
and KF is the Kalman filter gain matrix (n x /) , then the
EKPF procedure may be summarized as follows.
1. During initialization (t = 0) a hypothesis of the
complete state vector and the covariance matrices is given.
Q, R, P'(0), x (0) (4)
KF(0)= P'(0).C" [C-P"(0).CT R |
2. The position of each feature in the actual image can be
predicted by forward application ofthe laws of perspective
projection exploiting a model of the camera used for
measurement.
y (i)sh( (n), p) (5)
3. The difference between the actually measured value and
the predicted value multiplied by the Kalman filter gain
matrix KF is used to update the state prediction vector x"
. ^
to form the new estimate vector x. In case of no
measurement data the second additive term is O.
Xl) = x" (4) + KF) ly t) - y" (6) (6)
4. The control input will be determined.
u(t)--k xu) (7)
5. A state prediction x" (1141 ) for the next measurement
p
can be made, where D is the state transition matrix over
one sampling period and G is the control effectiveness
matrix for the components of the control vector u.
X (ta1)9 0-X(65)4- G-u(i) (8)
6. The covariance matrix of the actual prediction error is
calculated.
P (n)-P' (n) -KF()-C-P' (1) (9)
7. Estimation of error covariances P'
P' (i4). P(u). ol «9 (10)
8. With the error covariances P" and the Jacobian matrix
C the Kalman filter gain matrix KF is computed.
KFüi) 2 PG) CPG) cT «R[! — (1)
9. Set i+1 to i, wait for the new measurement and go to
step 2.
i=i+1 (12)
The current best estimates are the roofed ones (^) which
originate fromthe expected ones ( * )(eqs. (7,8)) by adding
the measurement innovations with the gain matrix KF.
This matrix is influenced by the covariance matrices Q and
R which give room for filter tuning in order to adjust
convergence behavior (see [Maybeck 79, Wuensche 88,
Mysliwetz 90] for details).
With the information, that is on the one hand gained
from perception of the environment and on the other hand
brought into the system by a priori knowledge, it is made
possible with the help of the 4D-approach to determine the
robot's state. The next substantial step towards a success-
ful autonomous navigation is a sophisticated guidance and
control system. In the following section our solution to this
problem will be discussed.
7. Vehicle guidance and control
In vehicle guidance and control, the top down com-
ponent of mission realization according to some plan
developed on the basis of a more or less accurate world
model has to meet with the bottom up component of actual
vehicle performance and environment encountered.
Without knowledge about the road network, reaching the
desired goal by controlling the vehicle by chance is un-
likely; however, without watching the sensory input and
comparing it to some apriori knowledge about safe tra-
jectory steering in a local environment, reaching the
desired goal is equally unlikely. The latter case is even
dangerous, while the former one may be an enjoyable ride
when local control is adequate, but with respect to the
intended mission it will be in vain.