The Extended Kalman Filter: An Interactive Tutorial for Non-Experts – Part 7

The Extended Kalman Filter: An Interactive Tutorial for Non-Experts

Part 7: Running the Filter

So now we have everything we need to run our Kalman Filter:

Predict:
\[ \hat{x}_k = a \hat{x}_{k-1} \]
\[ p_k = a p_{k-1} a \]
Update:
\[ g_k = p_k / (p_k + r) \]
\[ \hat{x}_k \leftarrow \hat{x}_k + g_k(z_k – \hat{x}_k)\]
\[ p_k \leftarrow (1 – g_k)p_k \]

Note that I’ve used the assignment (arrow) symbol, instead of the standard equal sign, in the last two lines of the update. Altough this usage is nontraditional, it gets across the idea that the update to $\hat{x}_k$ and $p_k$ is modifying their current values (from the prediction step), rather than defining those values in terms of a previous step (as prediction does). [9]

To try out our filter, we’ll need:

  • A sequence of observations $z_k$
  • An initial value (base case) $\hat{x}_0$ for the state estimates. This can just be our first observation $z_0$.
  • An initial value $p_0$ for the prediction error. It can’t be 0, otherwise $p_k$ would stay 0 forever by multiplication. So we arbitrarily set it to 1.

For our observations, rather than trying to observe an actual system (like a plane coming in for a landing), we’ll fake up some observations based on adding random noise
[10] $v_k$ in the interval [-200,+200] to the idealized values $x_k = 0.75 x_{k-1}$ , starting with $x_0 = 1000$:

$k$ 0 1 2 3 4 5 6 7 8 9
$x_k$

$z_k$

$p_k$
$\hat{x}_k$

$g_k$

$x_0 = $
 
$r = $
 
$a = $

Once you’re ready to run the filter, hit the Run button to see how the Kalman filter produces a smooth version (green) of the noisy signal (red) that is often remarkably close to the original clean signal (blue). You can also try different values for $x_0$, $r$, and $a$.

Previous:
Next:


[9] Indeed, if you look at the source code for this page, you will see that the JavaScript for the prediction and update is even simpler than the formulas:

    // Predict
    xhat = a * xhat;
    p    = a * p * a;

    // Update
    g    = p  / (p  + r);
    xhat = xhat + g * (z - xhat);
    p    = (1 - g) * p;
    

I thank Marco Camurri and John Mahoney for pointing out inconsistencies in my use of these equations, in earlier versions of this tutorial.

[10] I have added noise from a uniform distribution rather than the Gaussian (normal) distribution assumed by the Kalman Filter, but it doesn’t make much difference from the perspective of this demo.