4 Kalman filters
Kalman filters are a powerful mathematical technique that is used to estimate the state of a system from noisy measurements. Originally developed by Rudolf Kalman in 1960 [1], Kalman filters have become widely used in a variety of fields, including engineering, finance, and robotics. The Kalman filter is an optimal estimator, which means it provides the best estimate of the system state given the available measurements and knowledge of the synamics of the system. The filter uses a state-space model of the system to predict the evolution of the system over time and to address measurement errors. The Kalman filter is particularly useful in applications where measurements are noisy or incomplete, or where the system dynamics are complex or difficult to model. In this way, the Kalman filter is an essential tool for many real-world applications that require accurate state estimation and control.
The algorithm involves two steps: prediction and update. The prediction step uses the previous state estimate and the dynamic model of the system to predict its current state and its uncertainty. The update step uses an obtained measurement and the measurement model of the system to correct the predicted state estimate and its uncertainty using a weighted average. The weight is determined by the Kalman gain, which balances the trust between the prediction and the measurement. The Kalman gain is computed on the basis of the covariance matrices of the prediction error and the measurement error. Prediction and update steps are performed recursively for each new measurement.
The linear Kalman filter assumes that the system and the measurement models are linear and can be represented by matrices. The extended Kalman filter extends the linear Kalman filter to handle non-linear systems by linearizing them around the current estimate using Taylor series expansion. The unscented Kalman filter improves the extended Kalman filter by using a set of sigma points to capture the mean and covariance of the nonlinear system without requiring linearization or Jacobian matrices [3,5]. The unscented Kalman filter is more accurate and robust than the extended Kalman filter, especially for highly nonlinear systems [5] [6].
4.1 Linear Kalman filter
A linear Kalman filter is a recursive algorithm that uses the Bode-Shannon representation of random processes and the state-transition method of analysis of dynamic systems to estimate the state of a system from noisy measurements. In the prediction step 4.1, the algorithm uses a state transition matrix to project the current state and its covariance matrix to the next time step.
\[ \begin{aligned} \textit{Predict step} \\ \hline \mathbf{\hat{x}} &= \mathbf{F x + Bu} \\ \mathbf{\hat{P}} &= \mathbf{F P F^{T} + Q} \end{aligned} \tag{4.1}\] Where: \[ \begin{aligned} \mathbf{\hat{x}}, \mathbf{\hat{P}} &= \text{State mean and covariance} \\ \mathbf{F} &= \text{Transition matrix} \\ \mathbf{Q} &= \text{Process matrix} \\ \mathbf{B, u} &= \text{Input to the system} \end{aligned} \]
In the correction step 4.4, the algorithm uses a measurement matrix to update the predicted state and its covariance matrix with the new measurement. The algorithm optimizes the estimation by minimizing the mean squared error between the true state and the estimated state [1].
\[ \begin{aligned} \textit{Update step} \\ \hline \mathbf{y} &= \mathbf{z - H x} \\ \mathbf{K} &= \mathbf{P H^T (H P H^T + R)^{-1}} \\ \mathbf{\hat{x}} &= \mathbf{x + K y} \\ \mathbf{\hat{P}} &= \mathbf{(I - K H)P} \\ \end{aligned} \tag{4.2}\] Where: \[ \begin{aligned} \mathbf{z}, \mathbf{R} &= \text{Measurement and convariance} \\ \mathbf{H} &= \text{Measurement function} \\ \end{aligned} \]
4.2 Extended Kalman filter
The Kalman filter is an optimal estimation algorithm that combines measurements from sensors with predictions from a mathematical model to estimate the true state of a system. However, the Kalman filter assumes that both the system dynamics and the measurement model are linear. In real-world applications, many systems exhibit non-linear behavior, and the extended Kalman filter is used to handle such cases.
\[ \begin{aligned} \textit{Predict step} \\ \hline \mathbf{F} &= \frac{\partial f(\mathbf{x_t}, \mathbf{u_t})}{\partial \mathbf{x}}\bigg\vert_{\mathbf{x_t}, \mathbf{u_t}} \\ \mathbf{\hat{x}} &= f(\mathbf{x}, \mathbf{u}) \\ \mathbf{\hat{P}} &= \mathbf{F P F^{T} + Q} \end{aligned} \tag{4.3}\]
The extended Kalman filter linearizes the system and measurement models using Taylor series expansions. It uses the first-order derivatives (Jacobian matrices) to linearize the equations \(f(x)\) at each time step. The filter then performs the standard Kalman filter prediction and update steps using these linearized models.
\[ \begin{aligned} \textit{Update step} \\ \hline \mathbf{H} &= \frac{\partial h(\mathbf{x_t})}{\partial \mathbf{x}}\bigg\vert_{\mathbf{x_t}} \\ \mathbf{y} &= \mathbf{z} - h(\mathbf{x}) \\ \mathbf{K} &= \mathbf{P H^T (H P H^T + R)^{-1}} \\ \mathbf{\hat{x}} &= \mathbf{x + K y} \\ \mathbf{\hat{P}} &= \mathbf{(I - K H)P} \\ \end{aligned} \tag{4.4}\]
It’s worth noting that the extended Kalman filter has some limitations. Since it uses linear approximations, it may not perform well for highly non-linear systems or when the linearization is inaccurate [2]. In such cases, other techniques, such as the unscented Kalman filter or particle filters, may be more appropriate.
4.3 Unscented Kalman filter
The unscented Kalman filter (UKF) is a technique for nonlinear estimation that uses a deterministic sampling approach to propagate a Gaussian random variable through system dynamics [6]. The UKF employs the unscented transform (UT), which generates a set of sample points that capture the mean and covariance of the original distribution.
The unscented transform is a technique for approximating the outcome of applying a nonlinear function to a probability distribution that is described by a mean and a covariance matrix. It does this by choosing a set of points, known as sigma points Figure 4.1.
Merwe describes generating the sigma points in the paper The unscented Kalman filter for nonlinear estimation[6]. These sigma points are chosen to capture the mean and covariance of the current estimated state and are transformed through the non-linear model to predict the state at the next time step. The sigma points \(\mathbfcal{X}_i\) as well as the weights \(\omega_i\) for each point are calculated using equation 4.5. The weights \(\omega_i\) must follow the equation \(\sum_{i=0}^{2L} \omega_i = 1\). \[ \lambda = \alpha^2 (L + \kappa) - L \] \[ \begin{aligned} \mathbfcal{X_0} &= \hat{x} & \omega^{m}_0 &= \frac{\lambda}{L + \lambda} & i &= 0 \\ \mathbfcal{X_i} &= \hat{x} + \left( \sqrt{(L+\lambda) \mathbf{P_x}} \right)_i & \omega^{c}_i &= \frac{1}{2(L + \lambda)} + (1 − \alpha^2 + \beta) & i &= 1,..., L \\ \mathbfcal{X_i} &= \hat{x} - \left( \sqrt{(L+\lambda) \mathbf{P_x}} \right)_i & \omega^{m}_i &= \omega^{c}_i = \frac{1}{2(L +\lambda)} & i &= L+1,...,2L \end{aligned} \tag{4.5}\] Where: \[\begin{gathered} \begin{align*} \mathbf{\hat{x}}, \mathbf{P_x} &= \text{State vector and covariance} \\ L &= \text{Dimension of state vector} \end{align*} \\ 0 \leq \alpha \leq 1 \\ 0 \leq \kappa \\ 0 \leq \beta \end{gathered}\]
The sigma points are used both in the predict step 4.6 and update step 4.7 and need to be regenerated for each new measurement. Instead of the matrix \(\mathbf{F}\) 4.1 and \(\mathbf{H}\) 4.4 known from LKF, the UKF is provided with non-linear functions \(\mathbf{f(} \mathbfcal{X} \mathbf{)}\) and \(\mathbf{h(} \mathbfcal{X} \mathbf{)}\).
\[ \begin{gathered} \textit{Predict step} \\ \hline \mathbfcal{Y} = f( \mathbfcal{X} \mathbf{)} \\ \mathbf{\hat{x}} = \sum_{i=0}^{2L} \omega^m_i \mathbfcal{Y}_i \quad\quad \mathbf{\hat{P}} = \sum_{i=0}^{2L} \omega^c_i (\mathbfcal{Y}_i - \mathbf{x})(\mathbfcal{Y}_i - \mathbf{x})^T + \mathbf{Q} \end{gathered} \tag{4.6}\]
By comparing the predicted measurements with the actual measurements, the UKF computes the Kalman gain, which determines the optimal blend between the predicted state and the measurement updates. Finally, the updated state estimate is obtained by incorporating the measurement information into the predicted state estimate using the Kalman gain. The update step of the UKF enables the filter to adaptively adjust the state estimate based on the measurements, providing a more accurate estimation of the true state of the system.
\[ \begin{gathered} \textit{Update step} \\ \hline \mathbfcal{Z} = \mathbf{h(} \mathbfcal{X} \mathbf{)} \\ \mathbf{\mu}_z = \sum_{i=0}^{2L} \omega^m_i \mathbfcal{Z}_i \quad\quad \mathbf{P}_z = \sum_{i=0}^{2L} \omega^c_i (\mathbfcal{Z}_i - \mathbf{\mu}_z)(\mathbfcal{Z}_i - \mathbf{\mu}_z)^T + \mathbf{R} \\ \mathbf{y} = \mathbf{z} - \mathbf{\mu}_z \qquad \mathbf{P}_{xz} = \sum_{i=0}^{2L} \omega_i^c (\mathbfcal{X}_i - \mathbf{x}) (\mathbfcal{Z}_i - \mathbf{\mu}_z)^T \\ \mathbf{K} = \mathbf{P}_{xz} \mathbf{P}_z^{-1} \\ \mathbf{\hat{x}} = \mathbf{x} + \mathbf{K} \mathbf{y} \qquad \mathbf{\hat{P}} = \mathbf{P} - \mathbf{K} \mathbf{P}_z \mathbf{K}^T \end{gathered} \tag{4.7}\]
The UKF has been shown to achieve higher accuracy and robustness than the extended Kalman filter (EKF) for various non-linear estimation problems, such as state estimation, parameter estimation, and dual estimation [6].
