Linear Kalman Filter Intuition Example
Estimation of the 1D position of the vehicle.
Starting from an initial probabilistic estimate at time k β 1 k-1 k β 1
Note: The initial estimate, the predicted state, and the final corrected state are all random variabless that we will specify their means and covariances.
Use a motion model to predict our new state Use observation model (e.g. derived from GPS) to correct that prediction of vehicle position at time k k k In this way, we can think of the Kalman filter as a technique to fuse information from different sensors to produce a final estimate of some unknown state, taking the uncertainty in motion and measurements into account.
The Linear Dynamical System Motion model:
x k = F k β 1 x k β 1 + G k β 1 u k β 1 + w k β 1
\mathbf{x}_{k}=\mathbf{F}_{k-1} \mathbf{x}_{k-1}+\mathbf{G}_{k-1} \mathbf{u}_{k-1}+\mathbf{w}_{k-1}
x k β = F k β 1 β x k β 1 β + G k β 1 β u k β 1 β + w k β 1 β where
u k \mathbf{u}_k u k β w k \mathbf{w}_k w k β w k βΌ N ( 0 , Q k ) \mathbf{w}_{k} \sim \mathcal{N}\left(\mathbf{0}, \mathbf{Q}_{k}\right) w k β βΌ N ( 0 , Q k β ) Measurement model
y k = H k x k + v k
\mathbf{y}_{k}=\mathbf{H}_{k} \mathbf{x}_{k}+\mathbf{v}_{k}
y k β = H k β x k β + v k β where
v k \mathbf{v}_{k} v k β v k βΌ N ( 0 , R k ) \mathbf{v}_{k} \sim \mathcal{N}\left(\mathbf{0}, \mathbf{R}_{k}\right) v k β βΌ N ( 0 , R k β ) Kalman Filter Steps Prediction We use the process model to predict how our state evolves since the last time step, and will propagate our uncertainty.
x Λ k = F k β 1 x k β 1 + G k β 1 u k β 1 P Λ k = F k β 1 P ^ k β 1 F k β 1 T + Q k β 1
\begin{array}{l}
\check{\mathbf{x}}_{k}=\mathbf{F}_{k-1} \mathbf{x}_{k-1}+\mathbf{G}_{k-1} \mathbf{u}_{k-1} \\
\check{\mathbf{P}}_{k}=\mathbf{F}_{k-1} \hat{\mathbf{P}}_{k-1} \mathbf{F}_{k-1}^{T}+\mathbf{Q}_{k-1}
\end{array}
x Λ k β = F k β 1 β x k β 1 β + G k β 1 β u k β 1 β P Λ k β = F k β 1 β P ^ k β 1 β F k β 1 T β + Q k β 1 β β Correction We use measurement to correct that prediction
Notation:
Optimal Gain
K k = P Λ k H k T ( H k P Λ k H k T + R k ) β 1
\mathbf{K}_{k}=\check{\mathbf{P}}_{k} \mathbf{H}_{k}^{T}\left(\mathbf{H}_{k} \check{\mathbf{P}}_{k} \mathbf{H}_{k}^{T}+\mathbf{R}_{k}\right)^{-1}
K k β = P Λ k β H k T β ( H k β P Λ k β H k T β + R k β ) β 1 Correction
x ^ k = x Λ k + K k ( y k β H k x Λ k ) β innovation P ^ k = ( I β K k H k ) P Λ k
\begin{aligned}
\hat{\mathbf{x}}_{k} &=\check{\mathbf{x}}_{k}+\mathbf{K}_{k}\underbrace{\left(\mathbf{y}_{k}-\mathbf{H}_{k} \check{\mathbf{x}}_{k}\right)}_{\text{innovation}} \\
\hat{\mathbf{P}}_{k}&=\left(\mathbf{I}-\mathbf{K}_{k} \mathbf{H}_{k}\right) \check{\mathbf{P}}_{k}
\end{aligned}
x ^ k β P ^ k β β = x Λ k β + K k β innovation ( y k β β H k β x Λ k β ) β β = ( I β K k β H k β ) P Λ k β β Summary
Example Consider a self-driving vehicle estimating its own position.
The state vector includes the position and its first derivative, velocity.
x = [ p d p d t = p Λ ]
\mathbf{x}=\left[\begin{array}{c}
p \\
\frac{d p}{d t}=\dot{p}
\end{array}\right]
x = [ p d t d p β = p Λ β β ] Input is the scalar acceleration
u = a = d 2 p d t 2
\mathbf{u}=a=\frac{d^{2} p}{d t^{2}}
u = a = d t 2 d 2 p β THe linear dynamical system is
Motion/Process model
x k = [ 1 Ξ t 0 1 ] x k β 1 + [ 0 Ξ t ] u k β 1 + w k β 1
\mathbf{x}_{k}=\left[\begin{array}{cc}
1 & \Delta t \\
0 & 1
\end{array}\right] \mathbf{x}_{k-1}+\left[\begin{array}{c}
0 \\
\Delta t
\end{array}\right] \mathbf{u}_{k-1}+\mathbf{w}_{k-1}
x k β = [ 1 0 β Ξ t 1 β ] x k β 1 β + [ 0 Ξ t β ] u k β 1 β + w k β 1 β Position observation
y k = [ 1 0 ] x k + v k
y_{k}=\left[\begin{array}{ll}
1 & 0
\end{array}\right] \mathbf{x}_{k}+v_{k}
y k β = [ 1 β 0 β ] x k β + v k β Nose densities
v k βΌ N ( 0 , 0.05 ) w k βΌ N ( 0 , ( 0.1 ) 1 2 Γ 2 )
v_{k} \sim \mathcal{N}(0,0.05) \quad \mathbf{w}_{k} \sim \mathcal{N}\left(\mathbf{0},(0.1) \mathbf{1}_{2 \times 2}\right)
v k β βΌ N ( 0 , 0.05 ) w k β βΌ N ( 0 , ( 0.1 ) 1 2 Γ 2 β ) Given the data at time step k = 0 k=0 k = 0
x ^ 0 βΌ N ( [ 0 5 ] , [ 0.01 0 0 1 ] ) Ξ t = 0.5 s u 0 = β 2 [ m / s 2 ] y 1 = 2.2 [ m ]
\begin{array}{l}
\hat{\mathbf{x}}_{0} \sim \mathcal{N}\left(\left[\begin{array}{l}
0 \\
5
\end{array}\right],\left[\begin{array}{cc}
0.01 & 0 \\
0 & 1
\end{array}\right]\right) \\
\Delta t=0.5 \mathrm{~s} \\
u_{0}=-2\left[\mathrm{~m} / \mathrm{s}^{2}\right] \quad y_{1}=2.2[\mathrm{~m}]
\end{array}
x ^ 0 β βΌ N ( [ 0 5 β ] , [ 0.01 0 β 0 1 β ] ) Ξ t = 0.5 s u 0 β = β 2 [ m / s 2 ] y 1 β = 2.2 [ m ] β We want to estimate the state at time step k = 1 k=1 k = 1
Prediction step
x Λ k = F k β 1 x k β 1 + G k β 1 u k β 1 [ p Λ 1 p Λ 1 ] = [ 1 0.5 0 1 ] [ 0 5 ] + [ 0 0.5 ] ( β 2 ) = [ 2.5 4 ]
\begin{aligned}
\check{\mathbf{x}}_{k} &=\mathbf{F}_{k-1} \mathbf{x}_{k-1}+\mathbf{G}_{k-1} \mathbf{u}_{k-1} \\\\
{\left[\begin{array}{c}
\check{p}_{1} \\
\check{p}_{1}
\end{array}\right] } &=\left[\begin{array}{cc}
1 & 0.5 \\
0 & 1
\end{array}\right]\left[\begin{array}{l}
0 \\
5
\end{array}\right]+\left[\begin{array}{c}
0 \\
0.5
\end{array}\right](-2)=\left[\begin{array}{c}
2.5 \\
4
\end{array}\right]
\end{aligned}
x Λ k β [ p Λ β 1 β p Λ β 1 β β ] β = F k β 1 β x k β 1 β + G k β 1 β u k β 1 β = [ 1 0 β 0.5 1 β ] [ 0 5 β ] + [ 0 0.5 β ] ( β 2 ) = [ 2.5 4 β ] β P Λ k = F k β 1 P ^ k β 1 F k β 1 T + Q k β 1 P Λ 1 = [ 1 0.5 0 1 ] [ 0.01 0 0 1 ] [ 1 0.5 0 1 ] T + [ 0.1 0 0 0.1 ] = [ 0.36 0.5 0.5 1.1 ]
\begin{aligned}
\check{\mathbf{P}}_{k} &=\mathbf{F}_{k-1} \hat{\mathbf{P}}_{k-1} \mathbf{F}_{k-1}^{T}+\mathbf{Q}_{k-1} \\\\
\check{\mathbf{P}}_{1} &=\left[\begin{array}{cc}
1 & 0.5 \\
0 & 1
\end{array}\right]\left[\begin{array}{cc}
0.01 & 0 \\
0 & 1
\end{array}\right]\left[\begin{array}{cc}
1 & 0.5 \\
0 & 1
\end{array}\right]^{T}+\left[\begin{array}{cc}
0.1 & 0 \\
0 & 0.1
\end{array}\right]=\left[\begin{array}{cc}
0.36 & 0.5 \\
0.5 & 1.1
\end{array}\right]
\end{aligned}
P Λ k β P Λ 1 β β = F k β 1 β P ^ k β 1 β F k β 1 T β + Q k β 1 β = [ 1 0 β 0.5 1 β ] [ 0.01 0 β 0 1 β ] [ 1 0 β 0.5 1 β ] T + [ 0.1 0 β 0 0.1 β ] = [ 0.36 0.5 β 0.5 1.1 β ] β Correction step
Best Linear Unbiased Estimator (BLUE) If we have white, uncorrelated zero-mean noise, the Kalman Fitler is the best (i.e. , lowest variance) unbiased estimator that uses only a linear combination of measurements.
Bias We repeat the above Kalman filter for K K K
The bias
An estimator of filter is unbiased k k k
E [ e ^ k ] = E [ p ^ k β p k ] = E [ p ^ k ] β p k = 0 β k β N
E\left[\hat{e}_{k}\right]=E\left[\hat{p}_{k}-p_{k}\right]=E\left[\hat{p}_{k}\right]-p_{k}=0 \qquad \forall k \in \mathbb{N}
E [ e ^ k β ] = E [ p ^ β k β β p k β ] = E [ p ^ β k β ] β p k β = 0 β k β N Bias in Kalman filter state estimation Consider the error dynamics
Predicted state error
e Λ k = x Λ k β x k
\check{\mathbf{e}}_{k}=\check{\mathbf{x}}_{k}-\mathbf{x}_{k}
e Λ k β = x Λ k β β x k β Corrected state error
e ^ k = x ^ k β x k
\hat{\mathbf{e}}_{k}=\hat{\mathbf{x}}_{k}-\mathbf{x}_{k}
e ^ k β = x ^ k β β x k β Using the Kalman Fitler equations, we can derive
e Λ k = F k β 1 e Λ k β 1 β w k e ^ k = ( 1 β K k H k ) e Λ k + K k v k
\begin{array}{l}
\check{\mathbf{e}}_{k}=\mathbf{F}_{k-1} \check{\mathbf{e}}_{k-1}-\mathbf{w}_{k} \\
\hat{\mathbf{e}}_{k}=\left(\mathbf{1}-\mathbf{K}_{k} \mathbf{H}_{k}\right) \check{\mathbf{e}}_{k}+\mathbf{K}_{k} \mathbf{v}_{k}
\end{array}
e Λ k β = F k β 1 β e Λ k β 1 β β w k β e ^ k β = ( 1 β K k β H k β ) e Λ k β + K k β v k β β So long as
The initial state estimate is unbiased (E [ e ^ 0 ] = 0 E\left[\hat{\mathbf{e}}_{0}\right]=\mathbf{0} E [ e ^ 0 β ] = 0 The noise is white, uncorrelated and zero mean (E [ v ] = 0 , E [ w ] = 0 E[\mathbf{v}]=\mathbf{0}, E[\mathbf{w}]=\mathbf{0} E [ v ] = 0 , E [ w ] = 0 Then the state estiamte is unbiased
E [ e Λ k ] = E [ F k β 1 e Λ k β 1 β w k ] = F k β 1 E [ e Λ k β 1 ] β E [ w k ] = 0
\begin{aligned}
E\left[\check{\mathbf{e}}_{k}\right] &=E\left[\mathbf{F}_{k-1} \check{\mathbf{e}}_{k-1}-\mathbf{w}_{k}\right] \\
&=\mathbf{F}_{k-1} E\left[\check{\mathbf{e}}_{k-1}\right]-E\left[\mathbf{w}_{k}\right] \\
&=\mathbf{0}
\end{aligned}
E [ e Λ k β ] β = E [ F k β 1 β e Λ k β 1 β β w k β ] = F k β 1 β E [ e Λ k β 1 β ] β E [ w k β ] = 0 β E [ e ^ k ] = E [ ( 1 β K k H k ) e Λ k + K k v k ] = ( 1 β K k H k ) E [ e Λ k ] + K k E [ v k ] = 0
\begin{aligned}
E\left[\hat{\mathbf{e}}_{k}\right] &=E\left[\left(\mathbf{1}-\mathbf{K}_{k} \mathbf{H}_{k}\right) \check{\mathbf{e}}_{k}+\mathbf{K}_{k} \mathbf{v}_{k}\right] \\
&=\left(\mathbf{1}-\mathbf{K}_{k} \mathbf{H}_{k}\right) E\left[\check{\mathbf{e}}_{k}\right]+\mathbf{K}_{k} E\left[\mathbf{v}_{k}\right] \\
&=\mathbf{0}
\end{aligned}
E [ e ^ k β ] β = E [ ( 1 β K k β H k β ) e Λ k β + K k β v k β ] = ( 1 β K k β H k β ) E [ e Λ k β ] + K k β E [ v k β ] = 0 β Consistency A filter is consistent k k k
E [ e ^ k 2 ] = E [ ( p ^ k β p k ) 2 ] = P ^ k
E\left[\hat{e}_{k}^{2}\right]=E\left[\left(\hat{p}_{k}-p_{k}\right)^{2}\right]=\hat{P}_{k}
E [ e ^ k 2 β ] = E [ ( p ^ β k β β p k β ) 2 ] = P ^ k β This means that the filter is neither overconfident nor underconfident in the estimate it has produced.
The Kalman Fitler is consistent in state estimate.
E [ e Λ k e Λ k T ] = P Λ k E [ e ^ k e ^ k T ] = P ^ k
E\left[\check{\mathbf{e}}_{k} \check{\mathbf{e}}_{k}^{T}\right]=\check{\mathbf{P}}_{k} \qquad E\left[\hat{\mathbf{e}}_{k} \hat{\mathbf{e}}_{k}^{T}\right]=\hat{\mathbf{P}}_{k}
E [ e Λ k β e Λ k T β ] = P Λ k β E [ e ^ k β e ^ k T β ] = P ^ k β so long as
the initial state estimate is consistent (E [ e ^ 0 e ^ 0 T ] = P Λ 0 E\left[\hat{\mathbf{e}}_{0} \hat{\mathbf{e}}_{0}^{T}\right]=\check{\mathbf{P}}_{0} E [ e ^ 0 β e ^ 0 T β ] = P Λ 0 β the noise is white and zero-mean (E [ v ] = 0 , E [ w ] = 0 E[\mathbf{v}]=\mathbf{0}, E[\mathbf{w}]=\mathbf{0} E [ v ] = 0 , E [ w ] = 0 Reference 
