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:

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:

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: