Extended Kalman Filters Steve Rogers In estimation theory, the extended Kalman filter (EKF) is the nonlinear version of the Kalman filter which linearizes about an estimate of the current mean and covariance. Generally, 1st order Taylor series approximations are used for linearization. In the case of well-defined transition models, the EKF has been considered the de facto standard in the theory of nonlinear state estimation, navigation systems and GPS. In the extended Kalman filter, the discrete state transition and observation equations must be differentiable functions. State transition:
x k = f ( x k−1 , uk −1) + w k z k =h ( x k ) + v k
Observation Model:
The following discrete predict and update equations are based on the state transition and observation equations. Predict Predicted state estimate:
̂x k ∨k−1= f ( ̂x k −1∨k−1 , u k−1 )
P k∨ k−1=F k−1 P k∨k −1 F Tk−1 +Q k−1
Predicted covariance estimate: Update
Innovation or measurement residual:
̃y k =z k −h ( ̂x k∨k −1 )
T Innovation covariance: S k =H k P k ∨k−1 H k + Rk
Kalman gain:
K k = P k∨k−1 H Tk S −1 k
Updated state estimate:
̂x k ∨k = ̂x k∨k −1+ K k ỹ k
Updated covariance estimate:
P k∨ k =( I −K k H k ) P k∨k−1
The Jacobians of the state transition and observation equations are: F k−1=
∂f ∂x
]
̂xk −1∨k−1 , uk −1
,
H k=
∂h ∂x
]
̂x k∨k−1
Usually, the Jacobian matrices are evaluated at each time step. The Jacobian matrices may then be used in the Kalman filter equations. An example 1 of a quarter car wheel slip control model may help explain the application of an EKF. Longitudinal friction coefficient:
μ x =a ( 1−e
−bγ
−cγ )
Quarter car dynamics: J w= ́ R F x −sign ( w ) T b m v́ =−F x
γ=
v−Rw Rw =1− v v
F x= F z μ x F z =mg
The actuator model includes a time constant and a time delay.
−τs
T b=e
a T s +a ref ,
where τ is .005 s and ‘a’ is 70 rad/s. The other parameters are listed next. % Vehicle parameters Wr = 0.32; % wheel radius R J = 1; % wheel moment v0 = 30; % initial velocity m = 450; % 1/4 car mass % Road surface parameters (a b c) % Pacejka models for dry asphalt roadCoeffs = [1.28 23.99 0.52]; % dry asphalt roadCoeffs = [0.86 33.82 0.35]; % wet asphalt roadCoeffs = [1.37 6.46 0.67]; % Cobblestone roadCoeffs = [0.19 94.13 0.06]; % Snow
For the above tire model we choose the dry asphalt values. We may define the states as [w, v, γ], that is, [tire rotational velocity, car forward velocity, slip]. Additional states were defined in the downloaded version 7. Since there are five equations of motion with three states the Jacobian state transition matrix will be 3x3. Since the EKF is a discrete set of equations, we 1st set up the equations of motion in a discrete form as follows. ‘Ts’ below is the sampling rate. w= ́
R F x sign ( w ) T b R F x sign ( w ) T b − → w k =wk −1+Ts − J J J J
(
)
v́ =
( )
−F x −F x → v k =v k−1 +Ts m m
γ =1−
Rw Rw → γ k =γ k−1 +Ts 1− v v
(
State transition matrix:
Observation model:
[
F=
) 1
0
0 R v k−1 0
−Ts
Ts
1 R w k−1 v 2k−1 0
Ts Rgm J 0 −Tsg
0
0 0 μx 0
]
H =[ 1 0 0 0 ]
The Simulink model as downloaded from the link 1 follows with the plotted results. The downloaded version is a 5 state version, the 1 st four of which are as above. The 5th state is the mass.
The actual signals are in the next plot.
The slip appears somewhat noisy. The EKF may be tuned to improve the noise level by manipulating the Q and R matrices. 1. http://www.goddardconsulting.ca/simulink-extended-kalman-filter-quarter-car.html