Extended Kalman Filter

proprioception stateEstimation

see Kalman Filter for basic explanation of the Kalman Filter.

Setup

The Basic Kalman Filter only functions on linear systems. It assumes that your state can be modeled as:

$${\mathbf{x}}_k={\mathbf{F}}_k{\mathbf{x}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k+{\mathbf{W}}_k$$ $${\mathbf{z}}_k={\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{v}}_k$$

So as a linear system (in matrix form).

The Extended Kalman Filter expands on the capabilities of the basic Kalman Filter to handle non-linear systems modeled by

$${\mathbf{x}}_k=f({\mathbf{x}}_{k-1},{\mathbf{u}}_k)+{\mathbf{w}}_k$$ $${\mathbf{z}}_k=h({\mathbf{x}}_k)+{\mathbf{v}}_k$$

Relationship with Bayes Filter

Coming from Bayes Filter. We make alot of assumptions to make the math easier to handle.

$$\underset{\text{posterior belief}}{\underbrace{p({\mathbf{x}}_k|{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k})}}=\eta \underset{\begin{array}{c}\text{observation}\\ \text{correction}\\ \text{using}\mathbf{g}(\cdot )\end{array}}{\underbrace{p({\mathbf{y}}_k|{\mathbf{x}}_k)}}\int \underset{\begin{array}{c}\text{motion prediction}\\ \text{using }\mathbf{f}(\cdot )\end{array}}{\underbrace{p({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{v}}_k)}}\underset{\text{prior belief}}{\underbrace{p({\mathbf{x}}_{k-1}|{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k-1},{\mathbf{y}}_{0:k-1})}}d{\mathbf{x}}_{k-1}$$

We assume that our posterior belief is Gaussian, and our noise variables are Gaussian and outside our non-linear state transition and observation models.

• Why Gaussians? Two reasons
  • Its a way to represent a continuous PDF in a finite way
  • The normal product of two gaussians is also a gaussian
  • A large portion of randomness in our world is Gaussian in nature (its a safe-enough approximation)
  • Note: Passing our Gaussians through non-linear functions cause the output PDF to be non-Gaussian.

You can draw connections better if you take a look at the Generalized Gaussian Filter

Linearization

The way an EKF gets around this is by linearizing the non-linear system! This is done via a Taylor Series (only using a first-order approximation).

$${\mathbf{F}}_k=\frac{\partial f}{\partial \mathbf{x}}{|}_{{\hat{\mathbf{x}}}_{k-1|k-1},{\mathbf{u}}_k}{\mathbf{H}}_k=\frac{\partial h}{\partial \mathbf{x}}{|}_{{\hat{\mathbf{x}}}_{k|k-1}}$$

These both become Jacobians where…

$$[{\mathbf{F}}_k{]}_{ij}=\frac{\partial f_i}{\partial x_j},[{\mathbf{H}}_k{]}_{ij}=\frac{\partial h_i}{\partial x_j}$$

Predict

First compute the state estimate prior with the non-linear state transition function

$${\hat{\mathbf{x}}}_{k|k-1}=f({\hat{\mathbf{x}}}_{k-1|k-1},{\mathbf{u}}_k)$$

We derive the first order approximation of the non-linear state about the paste state estimate and control input.

$${\mathbf{F}}_k=\frac{\partial f}{\partial \mathbf{x}}{|}_{{\hat{\mathbf{x}}}_{k-1|k-1},{\mathbf{u}}_k}$$

We then use that to predict our state covariance prior.

$${\mathbf{P}}_{k|k-1}=\underset{\text{We use jacobian here}}{\underbrace{{\mathbf{F}}_k{\mathbf{P}}_{k-1|k-1}{\mathbf{F}}_k^T}}+{\mathbf{Q}}_k$$

Update

Compute the first order approximation of the non-linear measurement function using our predicted state prior.

$${\mathbf{H}}_k=\frac{\partial h}{\partial \mathbf{x}}{|}_{{\hat{\mathbf{x}}}_{k|k-1}}$$

Compute the Kalman Gain

$${\mathbf{K}}_k=\frac{{\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^T}{{\mathbf{H}}_k{\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^T+{\mathbf{R}}_k}$$

Update the priors with the measurement using the Kalman Gain (this becomes the posterior)

$${\hat{\mathbf{x}}}_{k|k}={\hat{\mathbf{x}}}_{k|k-1}+{\mathbf{K}}_k({\mathbf{z}}_k-\underset{\begin{array}{c}\text{We use}\\ \text{non-linear}\\ \text{here}\end{array}}{\underbrace{h({\hat{\mathbf{x}}}_{k|k-1})}})$$ $${\mathbf{P}}_{k|k}=(\mathbf{I}-{\mathbf{H}}_k{\mathbf{K}}_k){\mathbf{P}}_{k|k-1}$$

Another way to Represent this

There’s just some notation mess because I learned these two concepts from two different sources. The Extended Kalman Filter can also be defined as the following:

$$\text{predictor:}{\check{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^T+{\mathbf{Q}}_k^{\prime },\text{\hspace{1em}(4.32a)}$$ $${\check{\mathbf{x}}}_k=\mathbf{f}({\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0),\text{\hspace{1em}(4.32b)}$$ $$\text{Kalman gain:}{\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{G}}_k^T{\left({\mathbf{G}}_k{\check{\mathbf{P}}}_k{\mathbf{G}}_k^T+{\mathbf{R}}_k^{\prime }\right)}^{-1},\text{\hspace{1em}(4.32c)}$$ $$\text{corrector:}{\hat{\mathbf{P}}}_k=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{G}}_k){\check{\mathbf{P}}}_k,\text{\hspace{1em}(4.32d)}$$ $${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\underset{\text{innovation}}{\underbrace{\left({\mathbf{y}}_k-\mathbf{g}({\check{\mathbf{x}}}_k,0)\right)}}.\text{\hspace{1em}(4.32e)}$$

[![error] Vee is the predicted prior. hat is the corrected posterior

When do we have a non-linear system?

Its pretty often.

IE. an Ackermann Model is an example of a non-linear state transition model IE. landmark detection would be an example of a non-linear measurement model

---

EXAMPLE (2D ROBOT LOCALIZATION)

Say a robot moves in 2D with state

$${\mathbf{x}}_k=\left[\begin{array}{c}{x}_k\\ y_k\\ {\theta }_k\end{array}\right]$$

The Motion Model of the robot is differential drive and is given by

$$f({\mathbf{x}}_{k-1},{\mathbf{u}}_k)=\left[\begin{array}{c}{x}_{k-1}+v_k\cos ({\theta }_{k-1})\Delta t\\ y_{k-1}+v_k\sin ({\theta }_{k-1})\Delta t\\ {\theta }_{k-1}+{\omega }_k\Delta t\end{array}\right]{\mathbf{u}}_k=\left[\begin{array}{c}{v}_k\\ {\omega }_k\end{array}\right]$$

Similarly, its measurement model is given by landmark detection (the landmark being in the same coordinate frame as the robot)

$$h({\mathbf{x}}_k){|}_{p=(m_x,m_y)}=\left[\begin{array}{c}\sqrt{(m_x-x_k{)}^2+(m_y-y_k{)}^2}\\ \underset{\text{a func}}{\underbrace{\text{atan2}}}(m_y-y_k,m_x-x_k)\end{array}\right]{\mathbf{z}}_k=\left[\begin{array}{c}{D}_k\\ {\gamma }_k\end{array}\right]$$

During Prediction

$${\hat{\mathbf{x}}}_{k|k-1}=f({\hat{\mathbf{x}}}_{k-1|k-1},{\mathbf{u}}_k)$$ $${\mathbf{F}}_k=\left[\begin{array}{ccc}1& 0& -v_k\sin ({\theta }_{k-1})\Delta t\\ 0& 1& v_k\cos ({\theta }_{k-1})\Delta t\\ 0& 0& 1\end{array}\right]so{\mathbf{F}}_k{|}_{{\hat{x}}_{k-1|k-1},{\mathbf{u}}_k}=\left[\begin{array}{ccc}1& 0& -v_k\sin ({\hat{\theta }}_{k-1|k-1})\Delta t\\ 0& 1& v_k\cos ({\hat{\theta }}_{k-1|k-1})\Delta t\\ 0& 0& 1\end{array}\right]$$ $${\mathbf{P}}_{k|k-1}={\mathbf{F}}_k{\mathbf{P}}_{k-1|k-1}{\mathbf{F}}_k^T+{\mathbf{Q}}_k$$

During Update Given that $q=(m_x-{\hat{x}}_{k|k-1}{)}^2+(m_x-{\hat{y}}_{k|k-1}{)}^2$

$${\mathbf{H}}_k=\left[\begin{array}{ccc}\frac{-(m_x-x_k)}{\sqrt{q}}& \frac{-(m_y-y_k)}{\sqrt{q}}& 0\\ \frac{m_y-y_k}{q}& \frac{-(m_x-x_k)}{q}& -1\end{array}\right]so{\mathbf{H}}_k{|}_{{\hat{x}}_{k|k-1}}=\left[\begin{array}{ccc}\frac{-(m_x-{\hat{x}}_{k|k-1})}{\sqrt{q}}& \frac{-(m_y-{\hat{y}}_{k|k-1})}{\sqrt{q}}& 0\\ \frac{m_y-{\hat{y}}_{k|k-1}}{q}& \frac{-(m_x-{\hat{x}}_{k|k-1})}{q}& -1\end{array}\right]$$ $${\mathbf{K}}_k=\frac{{\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^T}{{\mathbf{H}}_k{\mathbf{P}}_{k|k-1}{H}_k^T+{\mathbf{R}}_k}$$ $${\mathbf{P}}_{k|k}=(\mathbf{I}-{\mathbf{H}}_k{\mathbf{K}}_k){\mathbf{P}}_{k|k-1}$$ $${\hat{\mathbf{x}}}_{k|k}={\hat{\mathbf{x}}}_{k|k-1}+{\mathbf{K}}_k({\mathbf{z}}_k-h({\hat{\mathbf{x}}}_{k|k-1}))$$

Rinse and repeat

worldModeling