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

Part 17: A Nonlinear Kalman Filter

So what can we do with this first derivative? Well, here is the set of equations for our linear Kalman Filter, using a model with no state transition or control signal, some process noise, a single sensor, and a single state value:

Model: \[x_k = x_{k-1} + w_k \] \[z_k = c x_k + v_k \]

Predict: \[\hat{x}_k = \hat{x}_{k-1}\] \[p_k = p_{k-1} + q\] Update: \[ g_k = p_k c (c p_k c + r)^{-1} \] \[ \hat{x}_k \leftarrow \hat{x}_k + g_k(z_k - c \hat{x}_k) \] \[ p_k \leftarrow (1 - g_k c) p_k \] Now we'll modify these equations to reflect the nonlinearity of our sensor. Using a function $h$ to stand in for any nonlinear function (like $log_2$ in our example) [16] , and $c_k$ to stand for its first derivative at timestep $k$, we get:

Model: \[x_k = x_{k-1} + w_k \]

$z_k = $ $h(x_k)$ $ + v_k$

Predict: \[\hat{x}_k = \hat{x}_{k-1}\] \[p_k = p_{k-1} + q\] Update:

$g_k = p_k$ $c_k$ $($ $c_k$ $p_k$ $c_k$ $+ r)^{-1}$

$\hat{x}_k \leftarrow \hat{x}_k + g_k(z_k-$ $h(\hat{x}_k)$)

$p_k \leftarrow (1 - g_k$ $c_k$ $) p_k$

Like our sensor fusion demo, the demo below shows a time-varying temperature fluctuation, but with a single sensor having nonlinear response and no bias. You can select from three different nonlinear sensor functions and compare our nonlinear Kalman filter to the linear version. As you can see, no amount of adjustment of the $c$ parameter in the linear version is adequate to get as good a fit to the original signal as you can get with the nonlinear version. The plot on the right shows the shape of the nonlinear function by itself, for reference:

$x_k$    $z_k$    $\hat{x}_k$    $\hat{x}_k~~linear$    $\hspace{3in}h(x)$

$h(x) = e^{2x}$ $h(x) = 10 log_{10}(x)$ $h(x) = 10 e^{-x/10}$

Linear approximation $c$ = 0

Previous:       Next:

[16] Why not call this function $f$? We will see why in a couple more pages.