1 minute read

As explained in the previous post, a Kalman filter is used as a state predictor for linear systems. To deal with nonlinear systems given by nonlinear state prediction $x’=f(x)$ and/or nonlinear measurement function $z = h(x)$ the extended Kalman filter can be used. The essence of this extended approach is the usage of the Taylor series, to linearize the nonliner system around an operating point.

The extended Kalman filter requires the calculation of the Jacobian of a nonlinear function as part of every single iteration, since the mean (which is the point that at which the nonlinear funciton is linearize about) is updated.

Differences to the linear Kalman Filter

The Kalman filter consists of an prediction and an measurement update step. In the case of a nonlinear model the state is still updated with the nonlinear state equation $x’= f(x)$. However, the covariance cannot be updated using the nonlinear function as it would result in a non-Gaussian distribution. To update the state covariance, the nonlinear function $\mathbf{f}(\mathbf{x})$ needs to be linearized around a small section at the operation point $\mathbf{x}$, which results in the Jacobian matrix $\mathbf{F}$. The linearization is achieved by the taking the first two terms of the Taylor Series of the function centered around the mean.

\[\mathbf{F} = \mathbf{f}(\mathbf{\mu}) + \frac{\partial \mathbf{f}(\mathbf{\mu})}{\partial \mathbf{x}} (\mathbf{x} - \mathbf{\mu})\]

Because the measurement function is assumed to be nonlinear the residual results in the following equation

\[y = z - h(x')\]

To update the measurement covariance matrix $\mathbf{S}$, the measuremtn function $\mathbf{h}$ needs to be linearized around the mean. This is done again with the Taylor series or the Jacobian in the multi-dimensional case.

\(\mathbf{h}(\mathbf{x}) \approx \mathbf{h}(\mathbf{\mu}) + (\mathbf{x} - \mathbf{\mu}) J_{\mathbf{h}}(\mathbf{\mu})\) either the measurement update or the state prediction.