Robotics Fundamentals - Extended Kalman Filter (EKF) - Rico贾若童的博客

Welcome To EKF - A Diff Drive Example

EKF (Extended Kalman Filter) is widely applied in robotics to smooth sensor noises to get a better estimate of a car’s position.

Now let’s take a look at a diff (differential) drive example.

Imagine our world has two cones with known locations: c1, c2, and a diff drive with wheel encoders and a cone detector.

A wheel encoder tells us how far (in m/s) a wheel has travelled within a known time window.
The cone detector can detect the range d and bearing $β$ of each cone at any given time.

The goal is to estimate the $[x, y, θ]$ of the vehicle.

Mathematical Formulation

Wheel Encoder

The wheel encoder is able to give us a noisy estimate of the linear $v$ and angular velocity $w$ of the robot:

\begin{matrix} \begin{aligned} v = \frac{l + r}{c} \\ w = \frac{l - r}{2} \\ R = \frac{2 (l + r)}{c (r - l)} \end{aligned} \end{matrix}

Where:

$v$ is the linear velocity of the robot’s center in m/s
$w$ is the angular velocity of the robot’s rotation in rad/s
$l, r$ are the wheel encoder increments within time window $Δ t$ in m/s.
$c$ is the distance between the two wheels (wheel base).

One can also notice that $v = w R$ in this case!

Conventionally, we use $x$ to represent our state vector as:

\begin{matrix} \begin{aligned} x = [\begin{array}{c} x \\ y \\ θ \end{array}] \end{aligned} \end{matrix}

And we deem the linear and angular velocities as control input , $u$ : $\begin{matrix} \begin{array}{r} u = [\begin{array}{c} v \\ w \end{array}] \end{array} \end{matrix}$

Motion Model

In discrete time $t + 1 = t + Δ t$ , we assume the robot follows a circular motion. That is, it has a constant linear and angular velocity.

Accordingly,

\begin{matrix} \begin{aligned} x^{'} = v c o s (θ) \\ y^{'} = v s i n (θ) \\ θ^{'} = w \end{aligned} \end{matrix}

If we integrate the above derivatives, we can get the motion update. This motion update is equivalent to gettting the new coordinates on a circle. In the real world, we have a gaussian noise $η$ from the control update.

\begin{matrix} \begin{aligned} x_{t + 1} = x_{t} - \frac{ω}{v} \sin (θ_{t}) + \frac{ω}{v} \sin (θ_{t} + ω Δ t) + η_{x}, \\ y_{t + 1} = y_{t} + \frac{ω}{v} \cos (θ_{t}) - \frac{ω}{v} \cos (θ_{t} + ω Δ t) + η_{y}, \\ θ_{t + 1} = θ_{t} + ω Δ t + η_{θ} . \\ \Rightarrow \\ x_{t + 1} = f (x_{t}, u, η) \end{aligned} \end{matrix}

The process noise has a covariance matrix $Q$ :

\begin{matrix} \begin{aligned} Q = [\begin{array}{c} v a r (v) & 0 \\ 0 & v a r (θ) \end{array}] \end{aligned} \end{matrix}

However, to do Kalman Filtering, we must be able to compute kalman gain and covariance matrix using a linear form:

\begin{matrix} \begin{aligned} x_{t + 1} = F_{t} x_{t} + B_{t} u_{t} + η \end{aligned} \end{matrix}

However, because of the non linear terms like $\frac{w}{v} s i n (θ)$ , we can’t write the above into a linear form directly. However, we can use the taylor expansion to linearize it. One tricky thing is, the linearization is around an estimate. So the EKF framework is formulated about the deviation from the true value.

We first get a prediction $x_{t + 1}^{*}$ using the full non-linear update model above,
Define Deviation between the last state estimate and the next state estimate:

$\begin{matrix} \begin{aligned} δ x := x_{t + 1, t r u e} - \bar{x_{t}} \end{aligned} \end{matrix}$

Linearize the deviation $δ x$

\begin{matrix} \begin{aligned} x_{t + 1}^{*} = f (\bar{x_{t}}, u_{t}) \\ \Rightarrow \\ x_{t + 1, t r u e} = f (\bar{x_{t}} + δ x) \approx x_{t + 1}^{*} + J_{x} (x_{t + 1} - \bar{x_{t}}) = J_{x} x_{t + 1} + b \end{aligned} \end{matrix}

So $x_{t + 1, t r u e} = J_{x} x_{t + 1} + b$ is linear, w.r.t $x_{t + 1}$ ! This is the foundamental difference from ESKF, where a linear system is built on error $δ x = x_{t + 1, t r u e} - x_{t + 1}^{*}$

This is because we our motion model estimates a single state estimate $x_{t + 1}$ , and still linearizes around $x_{t}$ . However ESKF estimates the error $δ x = x_{t + 1, t r u e} - x_{t + 1}^{*}$

$J_{x}$ is the Jacobian of $\frac{\partial f}{\partial x}$

\begin{matrix} \begin{array}{r} x_{t + 1} \approx [\begin{array}{c} 1 & 0 & - \frac{w}{v} \cos (θ_{t}) + \frac{w}{v} \cos (θ_{t} + Δ t) \\ 0 & 1 & - \frac{w}{v} \sin (θ_{t}) + \frac{w}{v} \sin (θ_{t} + Δ t) Δ t \\ 0 & 0 & 1 \end{array}] x_{t + 1} + b \end{array} \end{matrix}

To be consistent with most other literature, we use $F_{t}$ to represent this Jacobian (a.k.a Prediction Jacobian)

\begin{matrix} \begin{aligned} F_{t} = J_{x} = [\begin{array}{c} 1 & 0 & - \frac{w}{v} \cos (θ_{t}) + \frac{w}{v} \cos (θ_{t} + Δ t) \\ 0 & 1 & - \frac{w}{v} \sin (θ_{t}) + \frac{w}{v} \sin (θ_{t} + Δ t) Δ t \\ 0 & 0 & 1 \end{array}] \end{aligned} \end{matrix}

Observation Model

We assume that we know the positions of each landmark $[c_{x}, c_{y}]$ A cone appears to be \beta, d in our observation. Note that in real life, this observation is subject to observation noise:

\begin{matrix} \begin{aligned} d = \sqrt{(c_{x} - x)^{2} + (c_{y} - y)^{2}} + η_{o d} \\ β = a t a n 2 (\frac{y_{c} - y_{1}}{x_{c} - x_{1}}) - θ + η_{o b} \end{aligned} \end{matrix}

Similarly, we think that deviation of the predicted observation from the real observation comes from the deviation of state estimate $x_{t + 1}^{*}$ and the true state: $x_{t + 1, t r u e}$ . Using Taylor Expansion to linearize the observation model:

\begin{matrix} \begin{aligned} h (x_{t + 1, t r u e}) \approx h (x_{t + 1}^{*}) + h (x)^{'} |_{x = x_{t + 1}^{*}} (x_{t + 1} - x_{t + 1}^{*}) \\ = h (x_{t + 1}^{*}) + H (x) (x_{t + 1} - x_{t + 1}^{*}) \\ \Rightarrow \\ h (x_{t + 1, t r u e}) - h (x_{t + 1}^{*}) = z_{t} - h (x_{t + 1}^{*}) = H (x) (x_{t + 1} - x_{t + 1}^{*}) \end{aligned} \end{matrix}

$h (x_{t + 1, t r u e}) := z_{t}$ is the real observation

Linearizing the above gives us the observation Jacobian $H$ :

\begin{matrix} \begin{aligned} H = \frac{\partial h (x)}{\partial x} |_{x = x_{t + 1}^{*}} = [\begin{array}{c} \frac{x - c_{x}}{d} & \frac{y - c_{y}}{d} & 0 \\ \frac{c_{y} - y}{d^{2}} & \frac{x - c_{x}}{d^{2}} & - 1 \end{array}] \end{aligned} \end{matrix}

This is another difference from ESKF, where

\begin{matrix} \begin{aligned} H = \frac{\partial h (x)}{\partial δ x} = \frac{\partial h (x)}{\partial x} \frac{\partial x}{\partial δ x} \end{aligned} \end{matrix}

This is because we our observation model is still linearized on $x$ (at $x=x_{t+1}^ $), b u t E S K F^{'} s o b s e r v a t i o n m o d e l v i e w s t h e e n t i r e$ h(x_{t+1, true}) - h(x_{t+1}^) $c a u s e d b y t h e e r r o r$ \delta x$

Filtering Process

In EKF, we are implicitly applying the Kalman Filtering on the devivation $δ x$ from true state values. By applying kalman filtering, we can minimize the error covariance on the deviation given observation.

Prediction is

\begin{matrix} \begin{aligned} x_{t + 1}^{*} = f (\bar{x_{t}}, u_{t}) = [\begin{array}{c} x_{t} - \frac{ω}{v} \sin (θ_{t}) + \frac{ω}{v} \sin (θ_{t} + ω Δ t) \\ y_{t} + \frac{ω}{v} \cos (θ_{t}) - \frac{ω}{v} \cos (θ_{t} + ω Δ t) \\ θ_{t} + ω Δ t \end{array}] \\ P_{t + 1}^{*} = F_{t} P_{t} F_{t}^{⊤} + Q, \end{aligned} \end{matrix}

Update is:

\begin{matrix} \begin{aligned} S = H P_{t + 1}^{*} H^{⊤} + R \\ K = P_{t + 1}^{*} H^{⊤} S^{- 1} \\ \bar{x_{t + 1}} = x_{t + 1}^{*} + K (z_{t} - h (x_{t + 1}^{*})) \\ P_{t + 1} = (I - K H) P_{t + 1}^{*} \end{aligned} \end{matrix}

Related Issues not found

Please contact @ricojia to initialize the comment

Robotics Fundamentals - Extended Kalman Filter (EKF)

Kalman Filter Framework

Welcome To EKF - A Diff Drive Example

Mathematical Formulation

Wheel Encoder

Motion Model

Observation Model

Filtering Process

CATALOG

FEATURED TAGS

FRIENDS