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.