Kalman Filters

## Kalman Filters The Kalman filter is an algorithm that uses a series of measurements observed over time, containing statistical noise and other inaccuracies, and produces estimates of unknown variables that tend to be more accurate than those based on a single measurement alone. Named after Rudolf E. Kálmán, this mathematical method is used in a wide range of fields, including control systems, signal processing, time series analysis, and econometrics. **Assumptions:** 1. The true state at time t is evolved from the state at (t-1) via Gaussian transition probabilities. 2. The observed output at time t is generated from the state at time t via Gaussian emission probabilities. 3. The state-space model assumes linearity and Gaussian noise. **Conceptually, the Kalman filter operates in two steps:** 1. Predict: Using the state estimate from the previous time step, the Kalman filter predicts the state at the current step, but it doesn't take the measurements into account. 2. Update (correct): The filter takes the current measurements into account to correct the predicted state, resulting in the final estimate. The real power of the Kalman filter is that it combines both the current and previous states, taking into account the uncertainty of each, to produce the best estimate of the current state. **Mathematically, these steps can be summarized as follows:** Let's denote the following variables: - `x` is the true state. - `x_hat` is the estimate of the state. - `P` is the error covariance matrix, a measure of the estimated accuracy of the state estimate. - `Q` is the process noise covariance matrix, representing the expected magnitude of changes in the state due to the process itself (without taking measurements into account). - `R` is the measurement noise covariance matrix, representing the expected accuracy of the measurements. - `K` is the Kalman gain, balancing the predictions and the measurements. - `A` is the state transition matrix, describing how the state evolves from one time step to the next without the process noise. - `H` is the observation model, describing how the state is observed at each time step. - `z` is the actual measurement. The steps are then: 1. **Predict** State prediction: ``` x_hat_predicted = A * x_hat_previous ``` Covariance prediction: ``` P_predicted = A * P_previous * A' + Q ``` 2. **Update (Correct)** Compute Kalman gain: ``` K = P_predicted * H' * inv(H * P_predicted * H' + R) ``` Update the state estimate using the measurement: ``` x_hat_current = x_hat_predicted + K * (z - H * x_hat_predicted) ``` Update the covariance: ``` P_current = (I - K * H) * P_predicted ``` Here, `*` denotes matrix multiplication, `'` denotes matrix transpose, and `inv` denotes matrix inversion. This recursive filter leads to an equation that doesn't require all past input data. The previous estimate and current measurement are all you need. Remember, the Kalman filter assumes that both the state transition and the measurements are linear with added Gaussian noise. This is not always the case in real-world systems, so extensions or modifications of the Kalman filter may be used in these cases (such as the Extended Kalman Filter or Unscented Kalman Filter). ## Extended Kalman Filters The Extended Kalman Filter (EKF) is a variant of the Kalman Filter that can handle non-linear transition and measurement models. The EKF linearizes the non-linear models at the current state estimate and then uses these linear models in the standard Kalman Filter. The non-linear models are: 1. State transition model: `x = f(x_prev, u) + w` where `x` is the state, `x_prev` is the previous state, `u` is the control input, `f()` is the non-linear transition function, and `w` is the process noise. 2. Measurement model: `z = h(x) + v` where `z` is the measurement, `h()` is the non-linear measurement function, and `v` is the measurement noise. The Extended Kalman Filter proceeds in two steps, similarly to the standard Kalman Filter: 1. **Predict:** State prediction: `x_hat_predicted = f(x_hat_prev, u)` Covariance prediction: `P_predicted = A * P_prev * A' + Q` where `A` is the Jacobian of `f()` with respect to `x`, evaluated at `x_hat_prev`. 2. **Update (Correct):** Compute the Kalman gain: `K = P_predicted * H' * inv(H * P_predicted * H' + R)` where `H` is the Jacobian of `h()` with respect to `x`, evaluated at `x_hat_predicted`. Update the state estimate using the measurement: `x_hat_current = x_hat_predicted + K * (z - h(x_hat_predicted))` Update the covariance: `P_current = (I - K * H) * P_predicted` In the EKF, the Jacobians `A` and `H` are used to linearize the non-linear models around the current state estimate. These Jacobians are typically calculated analytically and then evaluated numerically at each time step. The process noise covariance `Q` and measurement noise covariance `R` may also be functions of state and may need to be linearized. As with the standard Kalman Filter, the EKF gives an optimal estimate if the noise is Gaussian and the models are linear. Since the models are non-linear in the EKF, the estimate is not truly optimal, but is instead a first-order approximation. If the models are highly non-linear, the EKF may not perform well, and other filters such as the Unscented Kalman Filter or particle filter may be needed. ## Unscented kalman filter The Unscented Kalman Filter (UKF) is an extension of the traditional Kalman filter which addresses some of the limitations of the Extended Kalman Filter (EKF). Like the EKF, the UKF is designed to deal with non-linear transition and observation models, but it does so in a different way. Instead of linearizing the non-linear models around the current state estimate, the UKF uses a deterministic sampling approach. It generates a set of sample points (known as sigma points) around the current state estimate, propagates these points through the non-linear models, and then computes the new state estimate and covariance from these transformed points. This method can provide better accuracy than the EKF for highly non-linear systems. The process can be summarized as follows: 1. **Initialization:** Set the initial state estimate `x_hat` and covariance `P`. 2. **Sigma point generation:** Generate a set of 2n+1 sigma points `Xi` from the current state estimate and covariance, where n is the dimension of the state. The sigma points include the current state estimate, n points in the positive direction along the axes of the error ellipse defined by P, and n points in the negative direction. `Xi = [x_hat, x_hat + sqrt((n+lambda)*P), x_hat - sqrt((n+lambda)*P)]` where `lambda` is a scaling parameter, and `sqrt((n+lambda)*P)` is the matrix square root of `(n+lambda)*P`. The sigma points are also associated with weights `Wi` for the mean and covariance. 3. **Sigma point transformation:** Propagate the sigma points through the non-linear transition and observation models: `Xi_predicted = f(Xi, u)` `Zi = h(Xi_predicted)` 4. **State and covariance estimation:** Compute the predicted state estimate and covariance, and the predicted observation and covariance, from the transformed sigma points: `x_hat_predicted = sum(Wi * Xi_predicted)` `P_predicted = sum(Wi * (Xi_predicted - x_hat_predicted) * (Xi_predicted - x_hat_predicted)') + Q` `z_hat = sum(Wi * Zi)` `Pzz = sum(Wi * (Zi - z_hat) * (Zi - z_hat)') + R` 5. **Update (correct):** Compute the Kalman gain and update the state estimate and covariance: `K = Pxz * inv(Pzz)` `x_hat_current = x_hat_predicted + K * (z - z_hat)` `P_current = P_predicted - K * Pzz * K'` Here, `f()` is the non-linear transition function, `h()` is the non-linear observation function, `u` is the control input, `z` is the observation, `Q` is the process noise covariance, and `R` is the observation noise covariance. This unscented transform method provides a more accurate approximation to the true mean and covariance of the non-linear transformations, and hence often performs better than the EKF for highly non-linear systems. :::info [Kalman Filter Python Example – Estimate Velocity From Position](https://thekalmanfilter.com/kalman-filter-python-example/) :::