AEKF (Adaptive extended Kalman filter)

Last updated on Feb 23, 2023

The Kalman filter is a popular algorithm used for estimation and control in various fields, such as robotics, aerospace, and navigation. It provides a means to estimate the state of a dynamic system based on noisy measurements of its output. The extended Kalman filter (EKF) is a variant of the Kalman filter that can handle nonlinear systems by linearizing them at each time step. However, the EKF has limitations in terms of its ability to handle nonlinearities, noise statistics, and parameter uncertainties. The adaptive extended Kalman filter (AEKF) is an improved version of the EKF that addresses these limitations by adapting the Kalman gain and the noise statistics based on the system's behavior.

The AEKF is an algorithm that provides an estimate of the state of a nonlinear dynamic system based on a sequence of measurements of the system's output. The algorithm consists of two main steps: prediction and update. In the prediction step, the state and covariance of the system are predicted based on the previous state estimate and the system's dynamics. In the update step, the predicted state and covariance are corrected based on the measurement and the measurement noise statistics.

The prediction step of the AEKF is similar to that of the EKF. Given the previous state estimate and the system's dynamics, the algorithm predicts the current state and covariance using the following equations:

State prediction: \begin{equation} \hat{x}{k|k-1} = f(\hat{x}{k-1|k-1}, u_{k-1}) \end{equation}

Covariance prediction: \begin{equation} P_{k|k-1} = F_{k-1}P_{k-1|k-1}F_{k-1}^T + Q_{k-1} \end{equation}

where:

$\hat{x}_{k|k-1}$ is the predicted state estimate at time $k$ based on measurements up to time $k-1$,
$\hat{x}_{k-1|k-1}$ is the previous state estimate at time $k-1$ based on measurements up to time $k-2$,
$u_{k-1}$ is the control input at time $k-1$,
$f(\cdot)$ is the nonlinear system function that relates the state at time $k-1$ to the state at time $k$ based on the control input,
$F_{k-1}$ is the Jacobian of the system function evaluated at the previous state estimate,
$P_{k|k-1}$ is the predicted state covariance at time $k$ based on measurements up to time $k-1$,
$P_{k-1|k-1}$ is the previous state covariance at time $k-1$ based on measurements up to time $k-2$,
$Q_{k-1}$ is the process noise covariance at time $k-1$.

The update step of the AEKF is where the algorithm differs from the EKF. In the EKF, the Kalman gain and the measurement noise covariance are fixed and determined based on the system's dynamics and the measurement noise statistics. In the AEKF, the Kalman gain and the measurement noise covariance are adapted based on the system's behavior. The adaptation is achieved using a recursive least squares (RLS) algorithm that estimates the system's parameters and noise statistics.

The update step of the AEKF is as follows:

Measurement innovation: \begin{equation} y_k = z_k - h(\hat{x}_{k|k-1}) \end{equation}

Innovation covariance: \begin{equation} S_k = H_k P_{k|k-1} H_k^T + R_k \end{equation}

Kalman gain: \begin{equation} K_k = P_{k|k-1} H_k^T S_k^{-1} \end{equation}

State update: \begin{equation} \hat{x}{k|k} = \hat{x}{k|k-1} + K_k y_k \end{equation}

Covariance update: \begin{equation} P_{k|k} = (I - K_k H_k) P_{k|k-1} \end{equation}

Parameter estimation: \begin{equation} \hat{\theta}k = \hat{\theta}{k-1} + \Gamma_k y_k \end{equation}

where:

$z_k$ is the measurement at time $k$,
$h(\cdot)$ is the measurement function that relates the state to the measurement,
$y_k$ is the measurement innovation or residual, which is the difference between the actual measurement and the predicted measurement based on the state estimate,
$H_k$ is the Jacobian of the measurement function evaluated at the predicted state estimate,
$R_k$ is the measurement noise covariance at time $k$,
$S_k$ is the innovation covariance, which represents the uncertainty in the measurement due to both the measurement noise and the state estimation error,
$K_k$ is the Kalman gain, which determines how much the state estimate is corrected based on the measurement,
$\hat{\theta}_k$ is the parameter estimate at time $k$,
$\hat{\theta}_{k-1}$ is the previous parameter estimate at time $k-1$,
$\Gamma_k$ is the adaptation gain, which determines how much the parameter estimate is updated based on the measurement innovation.

The parameter estimation step of the AEKF is based on the RLS algorithm, which recursively updates the parameter estimate and the noise statistics based on the measurement innovation. The RLS algorithm is an online learning algorithm that can adapt to changes in the system's behavior over time. The algorithm updates the parameter estimate and the noise statistics using the following equations:

Parameter estimate update: \begin{equation} \hat{\theta}k = \hat{\theta}{k-1} + K_\theta y_k \end{equation}

Parameter covariance update: \begin{equation} P_\theta = (I - K_\theta H_\theta) P_{\theta,k-1} \end{equation}

Noise covariance update: \begin{equation} P_n = (I - K_n H_n) P_{n,k-1} \end{equation}

where:

$\hat{\theta}_k$ is the parameter estimate at time $k$,
$K_\theta$ is the Kalman gain for the parameter estimation,
$H_\theta$ is the Jacobian of the parameter function evaluated at the predicted state estimate,
$P_{\theta,k-1}$ is the previous parameter covariance at time $k-1$,
$K_n$ is the Kalman gain for the noise statistics estimation,
$H_n$ is the Jacobian of the noise function evaluated at the predicted state estimate,
$P_{n,k-1}$ is the previous noise covariance at time $k-1$.

The AEKF has several advantages over the EKF. First, it can handle nonlinear systems more effectively by adapting the Kalman gain and the noise statistics based on the system's behavior. Second, it can handle parameter uncertainties by estimating the system's parameters and updating the Kalman gain and noise statistics based on the estimated parameters. Third, it can adapt to changes in the system's behavior over time by continuously updating the parameter estimate and the noise statistics using the RLS algorithm.

The AEKF can be used in a wide range of applications, including robotics, control systems, and signal processing. For example, it can be used to estimate the position and orientation of a mobile robot based on sensor measurements, or to estimate the parameters of a control system based on input-output data.

One important consideration when using the AEKF is the choice of the adaptation gain $\Gamma_k$. The adaptation gain determines how much the parameter estimate is updated based on the measurement innovation. A small adaptation gain can lead to slow convergence of the parameter estimate, while a large adaptation gain can lead to instability and divergence. Therefore, the adaptation gain must be carefully tuned based on the specific application and system dynamics.

Another consideration when using the AEKF is the computational complexity of the algorithm. The AEKF requires the computation of several matrix operations, including the Jacobian matrices, the Kalman gain, and the covariance updates. These operations can be computationally expensive, especially for high-dimensional systems. Therefore, efficient implementations of the AEKF are necessary to reduce the computational complexity and improve the real-time performance of the algorithm.

In conclusion, the AEKF is a powerful algorithm for state and parameter estimation in nonlinear systems. It combines the Kalman filter with the RLS algorithm to adaptively estimate the system's parameters and noise statistics based on the measurement innovation. The AEKF can handle parameter uncertainties and adapt to changes in the system's behavior over time, making it a useful tool for a wide range of applications in robotics, control systems, and signal processing.