Extended Kalman Filter

Motivation

Linear systems do not exist in reality. We have to deal with nonlinear discrete-time systems

xk⏟current state=fkβˆ’1(xkβˆ’1⏟previous state,ukβˆ’1⏟inputs,wkβˆ’1⏟process noise)yk⏟measurement=hk(xk,vk⏟measurement noise) \begin{aligned} \underbrace{\mathbf{x}_{k}}_{\text{current state}}&=\mathbf{f}_{k-1}(\underbrace{\mathbf{x}_{k-1}}_{\text{previous state}}, \underbrace{\mathbf{u}_{k-1}}_{\text{inputs}}, \underbrace{\mathbf{w}_{k-1}}_{\text{process noise}}) \\\\ \underbrace{\mathbf{y}_{k}}_{\text{measurement}}&=\mathbf{h}_{k}(\mathbf{x}_{k}, \underbrace{\mathbf{v}_{k}}_{\text{measurement noise}}) \end{aligned}

How can we adapt Kalman Filter to nonlinear discrete-time systems?

πŸ’‘ Idea: Linearizing a Nonlinear System

Choose an operating point aa and approxiamte the nonlinear function by a tangent line at that point.

ζˆͺ屏2022-07-20 17.02.10

We compute this linear approximation using a first-order Taylor expansion

f(x)β‰ˆf(a)+βˆ‚f(x)βˆ‚x∣x=a(xβˆ’a) f(x) \approx f(a)+\left.\frac{\partial f(x)}{\partial x}\right|_{x=a}(x-a)

Extended Kalman Filter

For EKF, we choose the operationg point to be our most recent state estimate, our known input, and zero noise.

  • Linearized motion model

    xk=fkβˆ’1(xkβˆ’1,ukβˆ’1,wkβˆ’1)β‰ˆfkβˆ’1(x^kβˆ’1,ukβˆ’1,0)+βˆ‚fkβˆ’1βˆ‚xkβˆ’1∣x^kβˆ’1,ukβˆ’1,0⏟Fkβˆ’1(xkβˆ’1βˆ’x^kβˆ’1)+βˆ‚fkβˆ’1βˆ‚wkβˆ’1∣x^kβˆ’1,ukβˆ’1,0⏟Lkβˆ’1wkβˆ’1 \mathbf{x}_{k}=\mathbf{f}_{k-1}\left(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}, \mathbf{w}_{k-1}\right) \approx \mathbf{f}_{k-1}\left(\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k-1}, \mathbf{0}\right) + \underbrace{\left.\frac{\partial \mathbf{f}_{k-1}}{\partial \mathbf{x}_{k-1}}\right|_{\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k-1}, \mathbf{0}}}_{\mathbf{F}_{k-1}}\left(\mathbf{x}_{k-1}-\hat{\mathbf{x}}_{k-1}\right)+\underbrace{\left.\frac{\partial \mathbf{f}_{k-1}}{\partial \mathbf{w}_{k-1}}\right|_{\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k-1}, \mathbf{0}}}_{\mathbf{L}_{k-1}} \mathbf{w}_{k-1}
  • Linearized measurement model

    yk=hk(xk,vk)β‰ˆhk(xΛ‡k,0)+βˆ‚hkβˆ‚xk∣xΛ‡k,0⏟Hk(xkβˆ’xΛ‡k)+βˆ‚hkβˆ‚vk∣xΛ‡k,0⏟Mkvk \mathbf{y}_{k}=\mathbf{h}_{k}\left(\mathbf{x}_{k}, \mathbf{v}_{k}\right) \approx \mathbf{h}_{k}\left(\check{\mathbf{x}}_{k}, \mathbf{0}\right)+\underbrace{\left.\frac{\partial \mathbf{h}_{k}}{\partial \mathbf{x}_{k}}\right|_{\check{\mathbf{x}}_{k}, \mathbf{0}}}_{\mathbf{H}_{k}}\left(\mathbf{x}_{k}-\check{\mathbf{x}}_{k}\right)+\underbrace{\left.\frac{\partial \mathbf{h}_{k}}{\partial \mathbf{v}_{k}}\right|_{\check{\mathbf{x}}_{k}, \mathbf{0}}}_{\mathbf{M}_{k}} \mathbf{v}_{k}

Fkβˆ’1,Lkβˆ’1,Hk,Mk\mathbf{F}_{k-1}, \mathbf{L}_{k-1}, \mathbf{H}_{k}, \mathbf{M}_{k} are Jacobian matrices.

Intuitively, the Jacobian matrix tells you how fast each output of the function is changing along each input dimension.

With our linearized models and Jacobians, we can now use the Kalman Filter equations.

ζˆͺ屏2022-07-20 17.21.13
We still use the nonlinear model to propagate the mean of the state estimate in prediction step and compute the measurement residual innovation in correction step.

Example

Similar to the self-driving car localisation example in Linear Kalman Fitler, but this time we use an onboard sensor, a camera, to measure the altitude of distant landmarks relative to the horizon.

ζˆͺ屏2022-07-20 17.30.16

(SS and DD are known in advance)

Sate:

x=[ppΛ™] \mathbf{x}=\left[\begin{array}{l} p \\ \dot{p} \end{array}\right]

Input:

u=p¨ \mathbf{u}=\ddot{p}

Motion/Process model

xk=f(xkβˆ’1,ukβˆ’1,wkβˆ’1)=[1Ξ”t01]xkβˆ’1+[0Ξ”t]ukβˆ’1+wkβˆ’1 \begin{aligned} \mathbf{x}_{k} &=\mathbf{f}\left(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}, \mathbf{w}_{k-1}\right) \\\\ &=\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} \end{aligned}

Landmark measurement model (nonlinear!)

yk=Ο•k=h(pk,vk)=tanβ‘βˆ’1(SDβˆ’pk)+vk \begin{aligned} y_{k} &=\phi_{k}=h\left(p_{k}, v_{k}\right) \\\\ &=\tan ^{-1}\left(\frac{S}{D-p_{k}}\right)+v_{k} \end{aligned}

Noise densities

vk∼N(0,0.01)wk∼N(0,(0.1)12Γ—2) v_{k} \sim \mathcal{N}(0,0.01) \quad \mathbf{w}_{k} \sim \mathcal{N}\left(\mathbf{0},(0.1) \mathbf{1}_{2 \times 2}\right)

The Jacobian matrices in this example are:

  • Motion model Jacobians

    Fkβˆ’1=βˆ‚fβˆ‚xkβˆ’1∣x^kβˆ’1,ukβˆ’1,0=[1Ξ”t01]Lkβˆ’1=βˆ‚fβˆ‚wkβˆ’1∣x^kβˆ’1,ukβˆ’1,0=12Γ—2 \begin{array}{l} \mathbf{F}_{k-1}=\left.\frac{\partial \mathbf{f}}{\partial \mathbf{x}_{k-1}}\right|_{\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k-1}, \mathbf{0}}=\left[\begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array}\right] \\\\ \mathbf{L}_{k-1}=\left.\frac{\partial \mathbf{f}}{\partial \mathbf{w}_{k-1}}\right|_{\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k-1}, \mathbf{0}}=\mathbf{1}_{2 \times 2} \end{array}
  • Measurement model Jacobians

    Hk=βˆ‚hβˆ‚xk∣xΛ‡k,0=[S(Dβˆ’p˘k)2+S20]Mk=βˆ‚hβˆ‚vk∣xΛ‡k,0=1 \begin{array}{l} \mathbf{H}_{k}=\left.\frac{\partial h}{\partial \mathbf{x}_{k}}\right|_{\check{\mathbf{x}}_{k}, \mathbf{0}}=\left[\begin{array}{ll} \frac{S}{\left(D-\breve{p}_{k}\right)^{2}+S^{2}} & 0 \end{array}\right] \\\\ M_{k}=\left.\frac{\partial h}{\partial v_{k}}\right|_{\check{\mathbf{x}}_{k}, \mathbf{0}}=1 \end{array}

Given

x^0∼N([05],[0.01001])Ξ”t=0.5 su0=βˆ’2[ m/s2]y1=Ο€/6[rad]S=20[m]D=40[m] \begin{array}{l} \hat{\mathbf{x}}_{0} \sim \mathcal{N}\left(\left[\begin{array}{l} 0 \\ 5 \end{array}\right], \quad\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}=\pi / 6[\mathrm{rad}]\\ S=20[m] \quad D=40[m] \end{array}

What is the position estimate p^1\hat{p}_1?

Prediction:

xΛ‡1=f0(x^0,u0,0)[pΛ‡1pΛ™1]=[10.501][05]+[00.5](βˆ’2)=[2.54]PΛ‡1=F0P^0F0T+L0Q0L0TPΛ‡1=[10.501][0.01001][100.51]+[1001][0.1000.1][1001]=[0.360.50.51.1] \begin{array}{c} \check{\mathbf{x}}_{1}=\mathbf{f}_{0}\left(\hat{\mathbf{x}}_{0}, \mathbf{u}_{0}, \mathbf{0}\right) \\ {\left[\begin{array}{c} \check{p}_{1} \\ \dot{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]} \\\\ \check{\mathbf{P}}_{1}=\mathbf{F}_{0} \hat{\mathbf{P}}_{0} \mathbf{F}_{0}^{T}+\mathbf{L}_{0} \mathbf{Q}_{0} \mathbf{L}_{0}^{T} \\ \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 \\ 0.5 & 1 \end{array}\right]+\left[\begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} 0.1 & 0 \\ 0 & 0.1 \end{array}\right]\left[\begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array}\right]=\left[\begin{array}{cc} 0.36 & 0.5 \\ 0.5 & 1.1 \end{array}\right] \end{array}

Correction:

K1=PΛ‡1H1T(H1PΛ‡1H1T+M1R1M1T)βˆ’1=[0.360.50.51.1][0.0110]([0.0110][0.360.50.51.1][0.0110]+1(0.01)(1))βˆ’1=[0.400.55] \begin{aligned} \mathbf{K}_{1} &=\check{\mathbf{P}}_{1} \mathbf{H}_{1}^{T}\left(\mathbf{H}_{1} \check{\mathbf{P}}_{1} \mathbf{H}_{1}^{T}+\mathbf{M}_{1} \mathbf{R}_{1} \mathbf{M}_{1}^{T}\right)^{-1} \\ &=\left[\begin{array}{cc} 0.36 & 0.5 \\ 0.5 & 1.1 \end{array}\right]\left[\begin{array}{c} 0.011 \\ 0 \end{array}\right]\left(\left[\begin{array}{ll} 0.011 & 0 \end{array}\right]\left[\begin{array}{cc} 0.36 & 0.5 \\ 0.5 & 1.1 \end{array}\right]\left[\begin{array}{c} 0.011 \\ 0 \end{array}\right]+1(0.01)(1)\right)^{-1} \\ &=\left[\begin{array}{c} 0.40 \\ 0.55 \end{array}\right] \end{aligned} x^1=xΛ‡1+K1(y1βˆ’h1(xΛ‡1,0))[p^1pΛ™^1]=[2.54]+[0.400.55](0.52βˆ’0.49)=[2.514.02] \begin{aligned} \hat{\mathbf{x}}_{1} &=\check{\mathbf{x}}_{1}+\mathbf{K}_{1}\left(\mathbf{y}_{1}-\mathbf{h}_{1}\left(\check{\mathbf{x}}_{1}, \mathbf{0}\right)\right) \\ {\left[\begin{array}{c} \hat{p}_{1} \\ \hat{\dot{p}}_{1} \end{array}\right]}&={\left[\begin{array}{c} 2.5 \\ 4 \end{array}\right]+\left[\begin{array}{c} 0.40 \\ 0.55 \end{array}\right](0.52-0.49)=\left[\begin{array}{c} 2.51 \\ 4.02 \end{array}\right] } \end{aligned} P^1=(1βˆ’K1H1)PΛ‡1=[0.360.500.501.1] \begin{aligned} \hat{\mathbf{P}}_{1} &=\left(\mathbf{1}-\mathbf{K}_{1} \mathbf{H}_{1}\right) \check{\mathbf{P}}_{1} \\ &=\left[\begin{array}{cc} 0.36 & 0.50 \\ 0.50 & 1.1 \end{array}\right] \end{aligned}

Summary

  • The EKF uses linearization to adapt the Kalman Filter to nonlinear systems

  • Linearization works by computing a local linear apporximation to a nonlinear function using the first-order Taylor expansion on a chosen operating point (in this case, the last state estimate)

Reference