an introduction to the kalman filter by, santhosh kumar
TRANSCRIPT
An Introduction To The Kalman Filter
By,Santhosh
Kumar
The Problem
System state cannot be measured directlyNeed to estimate “optimally” from measurements
Measuring Devices Estimato
r
MeasurementError Sources
System State (desired but not known)
External Controls
Observed Measurement
s
Optimal Estimate of
System State
SystemError Sources
System
Black Box
What is a Kalman Filter?
The Kalman Filter is essentially a set of mathematical equations that implement a predictor – corrector type estimator that is OPTIMAL – when some presumed conditions are met.
Optimal? For linear system and white Gaussian
errors, Kalman filter is “best” estimate based on all previous measurements
For non-linear system optimality is ‘qualified’
What’s so great about Kalman Filter?
noise smoothing (improve noisy measurements)
state estimation (for state feedback)recursive (computes next estimate using
only most recent measurement)
Discrete Kalman Filter
11 kkkk wBuAxx
kkk vHxz
Estimates the state x of a discrete-time controlled process that is governed by the linear stochastic difference equation
with a measurement
Components of a Kalman Filter
1kw
Matrix (nxn) that relates the state at the previous time step k-1 to k without controls or noise.
A
Matrix (nxl) that describes how the control u changes the state from k-1 to k.B
Matrix (mxn) that describes how to map the state xk to a measurement zk.
H
1kv
Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance R and Q respectively.
Estimates and Errors
is the priori state estimate at step k.
is the posteriori state estimate at step k given measurement
Errors:
Error covariance matrices:
Kalman Filter’s task is to find
ˆ x k n
ˆ x k n
ek x k ˆ x k
ek x k ˆ x k
Pk E[ek
ek T
]
Pk E[ek ekT ]
kx̂
kz
Residual and Kalman Gain
Expected value
◦ innovation is
The optimal Kalman gain Kk is
kx̂
ˆ x k ˆ x k Kk(zk Hˆ x k
)
zk Hˆ x k
K k Pk HT (HPk
HT R) 1
Pk
HT
HPk HT R
Discrete Kalman Filter Algorithm
Prediction (Time Update)
(1) Project the state ahead
(2) Project the error covariance ahead
Correction (Measurement Update)
(1) Compute the Kalman Gain
(2) Update estimate with measurement zk
(3) Update Error Covariance
kkk BuA
1ˆˆ xx
QAAPP Tkk
1
1)( RHHPHPK Tk
Tkk
)ˆ(ˆˆ kkkkk HzK xxx
kkk PHKP )1(
Extended Kalman FilterSuppose the state-estimation and
measurement equations are non-linear:
◦ process noise w is drawn from N(0,Q), with covariance matrix Q.
◦ measurement noise v is drawn from N(0,R), with covariance matrix R.
),,( 11 kkkk f wuxx
),( kkk h vxz
Jacobian Matrix Recap
For a scalar function y=f(x),
For a vector function y=f(x),
y f (x)x
y Jx y1
yn
f1
x1
(x) f1
xn
(x)
fn
x1
(x) fn
xn
(x)
x1
xn
Linearize the Non-LinearThe equations that linearize a kalman estimate are
Where, and are actual state and measurement
vectors. and are approx. state and measurement
vectors. and are process and measurement noise.
(Cont.)
11
~
1
~
)( kkkkk WwxxAxx
kkkkk VvxxHzz )(~~
kx kz
kx~
kz~
kw kv
Linearize the Non-Linear(Cont.)
Let A be the Jacobian of f with respect to x.
Let W be the Jacobian of h with respect to w.
Let H be the Jacobian of h with respect to x.
Let V be the Jacobian of h with respect to v.
)0,,( 1
~
kk
j
iij x
fuxA
)0,(~
k
j
iij x
hxH
)0,(~
k
j
iij x
hV x
)0,,( 1
~
kk
j
iij w
fW ux
Extended Kalman Filter Algorithm
Prediction (Time Update)
(1) Project the state ahead
(2) Project the error covariance ahead
Correction (Measurement Update)
(1) Compute the Kalman Gain
(2) Update estimate with measurement zk
(3) Update Error Covariance
Tkkk
Tkkkk WQWAPAP 11
1)( Tkkk
Tkkk
Tkkk VRVHPHHPK
))0,ˆ((ˆˆ kkkkk hzK xxx
kkk PHKP )1(
)0,,( 1
^^
kkk fx ux
Quick Example – Constant Model
Measuring Devices Estimato
r
MeasurementError Sources
System State
External Controls
Observed Measurement
s
Optimal Estimate of
System State
SystemError Sources
System
Black Box
Quick Example – Constant Model
Time Update Equation
Measurement Update Equation
^
1
^
kk xx
QPP kk
1
1)( RPPK kkk
)(~^~^^
kkkkk xzKxx
kkk PKP )1(
Quick Example – Constant Model
Quick Example – Constant Model
Quick Example – Constant Model
QUERIES?????