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 \]
Predict:
\[\hat{x}_k = \hat{x}_{k-1}\]
\[p_k = p_{k-1} + q\]
Update:
$\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)$ |
| Linear approximation $c$ = |
Previous: Nonlinearity
Next: Computing the Derivative
[16] Why not call this function $f$? We will see why in a couple more pages.