Professional Documents
Culture Documents
the computational cost and therefore its suitability for real- W r related to observation noise.
T
time applications.
T The extended Kalman filter predicts the future state of
The process noise v v v v is assumed to be applied
system xˆ k1 based on the available system model f (.)
to the control input, v v to velocity input, and v to the
and projects ahead the state error covariance matrix Pk 1
steer angle input. The vehicle is assumed to be equipped
with a sensor (range-laser finder) that provides a using the time update equations:
measurement of range ri and bearing i to an observed xˆ k 1 f (x k , u k ) (7)
feature i relative to the vehicle as following: P k 1 f k Pk f k
T
GuQ k G T
u (8)
Where
( x x i ) 2 ( y y i ) 2 r
i
r
h ( X ) tan 1 y y i (2)
i
x xi
where ( x i , y i ) is the position landmark in map and
IJCSI International Journal of Computer Science Issues, Vol. 7, Issue 2, No 2, March 2010 17
www.IJCSI.org
f 3 f 3 (17)
u1 u 2 And the discrete time observation model is:
(x (k ) x (k )) 2 ( y (k ) y (k )) 2 (k )
2) Measurement updates equations ri (k ) i i r
(k ) 1 y ( k ) y i ( k )
Once measurements z k become available the Kalman i tan ( k ) ( k )
x ( k ) x i ( k )
gain matrix K k is computed and used to incorporate the (18)
measurement into the state estimate xˆ k . The state error Fig.2 briefly describes the cyclic localization procedure
covariance for the updated state estimate Pk is also during the localization procedure using EKF.
computed using the following measurement update
equations:
K k Pk H kT (H k Pk H kT R k ) 1 (11)
xˆ k xˆ K k (z k h (xˆ ))
k k (12)
Pk (I K k H k )P k (13)
Where I is an identity matrix and H k is following:
h
Hk (14)
X
In the above equations, xˆ k is an estimation of the system
sate vector x k and Pk is the covariance matrix
Fig.2 Localization algorithm based on EKF
corresponding to the state estimation error defined by
Pk E {(x k xˆ k )(x k xˆ k )T ) (15)
The algorithm consists of such steps as position
The difference between the prediction and observed
prediction, observation, measurement prediction, matching
measurements is called the measurement innovation, or
and estimation. The detailed discussion of these steps will
residual, generally denoted as rk : be followed.
rk z k h (xˆ k ) (16) Step1) Initialization
Initialize the state vector xˆ 0 and covariance matrix P0 of
The innovation represents the additional information
variable to the filter in consequence to the observation the mobile robot
z k . For an optimal filter the innovation sequence is a Step 2) Robot Position Prediction
The robot position at time step k+1 is predicted based on
sequence of independent Gaussian random variables. its old localization (at time step k) and its movement due
the control input u k :
IJCSI International Journal of Computer Science Issues, Vol. 7, Issue 2, No 2, March 2010 18
www.IJCSI.org
h
x y
basic idea behind this technique is to make the actual
h k (25) value of the covariance of the residual to be consistent
X h2 h2 h2 with its theoretical value [12]. The innovation sequence
x y rk has a theoretical covariance S k that is obtained from
Step 4) Matching EKF algorithm. The actual residual covariance Cˆ k can be
The goal of the matching procedure is to produce an approximated by its sample covariance, through averaging
assignment from measurements to the landmarks (stored in inside a moving window of size N as the following:
the map). If the measurement result satisfies the following 1 k
Fig.10 The figure shows the RMS error in localization Based on AFEKF
40
and EKF (the results obtain over 50 Monte Carlo runs). In this
experiment, measurement noise is r 0.1 , 1.0 and control
20
1.5 1
Finally, the consistency of both localizations Based on
AFEKF and EKF are compared. In this regard, we have
1 considered the situation where the sensor statistics are set
X(m)
Y(m)
wrongly as: r 2.0 and 0.5 and the noise
0.5
0.5
0 0
covariance Q k is completely known. To verify the
0 50 100 150 0 50 100 150
time(s) time(s) consistency of both algorithms, average Normalized
0.4 Estimation Error Squared (NEES) is used as a measure
RMS-EKF
0.3 RMS-FAEKF factor. For an available ground truth x k and an estimated
Teta(deg)
0.15 0.15
X(m)
Y(m)
0.1 0.1
0.05 0.05
0 0
Fig.13 Consistency of localization based on EKF
0 50 100 150 0 50 100 150
time(s) time(s)
0.4
RMS-EKF
0.3 RMS-FAEKF
Teta(deg)
0.2
0.1
0
0 50 100 150
time(s)
Fig.12 The figure shows the RMS error in localization Based on AFEKF
and EKF (the results obtain over 50 Monte Carlo runs). In this
experiment, measurement noise is truly considered as r 2 ,
0.1 and control noise is wrongly considered as v 0.03 m/s , Fig.14 Consistency of localization based on AFEKF
0.5 o