EKF Overview

The basic Extended Kalman Filter(EKF) with Error Correction variables are defined below.

$$ \begin{aligned} \hat{x}_{k|k-1} = x^{-}_{k} \qquad &\longmapsto \qquad \text{Predicted (a Priori) State Estimate} \\ \boldsymbol{P}_{k|k-1} = \boldsymbol{P}^-_k \qquad &\longmapsto \qquad \text{Predicted (a Priori) State Estimate} \\ \boldsymbol{Q}_k = \boldsymbol{Q}_{k-1} \qquad &\longmapsto \qquad \text{Process Noise Covariance} \\ \tilde{y}_k = d_k \qquad &\longmapsto \qquad \text{Measurement Innovation} \\ h(*) \qquad &\longmapsto \qquad \text{Measurement Function mapping} \\ \boldsymbol{H}_k \qquad &\longmapsto \qquad \text{Jacobian of Measurement Function} \\ \boldsymbol{S}_k \qquad &\longmapsto \qquad \text{Innovation Covariance} \\ \boldsymbol{R}_k \qquad &\longmapsto \qquad \text{Measurement Noise Covariance} \\ \boldsymbol{K}_k \qquad &\longmapsto \qquad \text{Kalman Gain} \\ \hat{x}_{k|k} = x^+_k \qquad &\longmapsto \qquad \text{Updated (a Posteriori) State Estimate} \\ \boldsymbol{P}_{k|k} = \boldsymbol{P}^+_k \qquad &\longmapsto \qquad \text{Updated (a Posteriori) Covariance Estimate}\\ \varepsilon_k \qquad & \longmapsto \qquad \text{System Residual Error}\\ \end{aligned} $$

The basic EKF is defined below in two common methods, in this tutorial we will typically use the second method when referring to variables.

EKF Algorithm Overview

0. Initialization (k=0)

$$\large \bold{\text{Initial State Estimate}}$$ $$ \qquad \qquad x^+_0 = E[x_0] = \frac{1}{N} \sum_{i=0}^{N}x_i $$

$$\large \bold{\text{Initial Covariance Estimate}}$$ $$ \boldsymbol{P}^+_0 = E \left[ (x_0 - x^+_0)(x_0 - x^+_0)^T \right] $$

1. Prediction Step (k=1, 2..)

$$\large \bold{\text{Predicted State Estimate}}$$ $$ \hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1}, u_k) \qquad \rightleftharpoons \qquad \begin{aligned} x_k^- & = \boldsymbol{\Phi}(x^+_{k-1}, u_{k-1}) \\ & = \boldsymbol{A}_{k-1} x^+_{k-1} + \boldsymbol{B}_{k-1}u_{k-1} \end{aligned} $$

$$\large \bold{\text{Predicted Covariance Estimate}}$$ $$ \boldsymbol{P}_{k|k-1} = \boldsymbol{F}_k \boldsymbol{P}_{k-1|k-1} \boldsymbol{F}^T_k + \boldsymbol{Q}_k \qquad \rightleftharpoons \qquad \begin{aligned} \boldsymbol{P}^-_k & = \boldsymbol{\Phi}_{k-1} \boldsymbol{P}^+_{k-1} \boldsymbol{\Phi}^T_{k-1} + \boldsymbol{Q}_{k-1} \\ &= \boldsymbol{A}_{k-1} \boldsymbol{P}_{k-1}^+ \boldsymbol{A}^T_{k-1} + \boldsymbol{Q}_{k-1} \end{aligned} $$

2. Update Step (k=1, 2..)

$$\large \bold{\text{Measurement Innovation}}$$ $$ \tilde{y}_k = \left[ z_k - h_k(\hat{x}_{k|k-1}) \right] \qquad \rightleftharpoons \qquad d_k = \left[ z_k - h_k(x^-_k)V^{ref}_x \right] = \left[ z_k - d^-_k \right] $$

$$ $$

$$\large \bold{\text{Measurement Function Mapping}}$$ $$ h(q_k) = h(\hat{x}_\mathrm{k|k-1}) = h(x^-_k) = R^b_n $$

$$ $$

$$\large \bold{\text{Innovation Covariance}}$$ $$ \boldsymbol{S}_k = \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}^T_k + \boldsymbol{R}_k \qquad \rightleftharpoons \qquad \boldsymbol{S}_k = \boldsymbol{H}_k \boldsymbol{P}^-_k \boldsymbol{H}^T_k + \boldsymbol{R}_k $$

$$ $$

$$\large \bold{\text{Jacobian of Measurement Function}}$$ $$ H_k = \left. \frac{\partial h}{\partial x}\right\vert_\mathrm{\hat{x}_{k|k-1}} = \left.\frac{\partial h (x_k, u_k)}{\partial x}\right\vert_\mathrm{x_k = \hat{x}^-_k} $$ $$ H_k \text{ is the Jacobian matrix of the measurement function, } h(*)$$

$$ $$

$$\large \bold{\text{Kalman Gain}}$$ $$ \boldsymbol{K}_k = \boldsymbol{P}_{k|k-1} \boldsymbol{H}^T_k \boldsymbol{S}^{-1}_k \qquad \rightleftharpoons \qquad \boldsymbol{K}_k = \boldsymbol{P}^{-}_k \boldsymbol{H}^T_k \boldsymbol{S}^{-1}_k $$

$$ $$

$$\large \bold{\text{Updated State Estimate}}$$ $$ \hat{x}_{k|k} = \hat{x}_{k|k-1} + \boldsymbol{K}_k [ \tilde{y}_k ] \qquad \rightleftharpoons \qquad x^+_k = x^-_k + \boldsymbol{K}_k [ d_k ] $$

$$ $$

$$\large \bold{\text{Updated Covariance Estimate}}$$ $$ \boldsymbol{P}_{k|k} = \left( \boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k \right) \boldsymbol{P}_{k|k-1} \qquad \rightleftharpoons \qquad \boldsymbol{P}^+_k = \left( \boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k \right) \boldsymbol{P}^-_k $$

$$ $$

$$\large \bold{\text{System Residual Error}}$$ $$ \varepsilon_k = \left[ z_k - h_k(\hat{x}_\mathrm{k|k-1})V^\mathrm{ref}_x \right] \qquad \rightleftharpoons \qquad \varepsilon_k = \left[ z_k - h_k(x^+_k)V^\mathrm{ref}_x \right] $$

$$ $$

$$\large \bold{\text{Measurement Noise Covariance}}$$ $$ \begin{aligned} \boldsymbol{R}_k &= \alpha\boldsymbol{R}_{k-1} + (1-\alpha)\left( \varepsilon_k \varepsilon^T_k + \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}^T_k \right) \\ &= \alpha\boldsymbol{R}_{k-1} + (1-\alpha)\left( \varepsilon_k \varepsilon^T_k + \boldsymbol{H}_k \boldsymbol{P}^-_{k} \boldsymbol{H}^T_k \right) \end{aligned} $$

$$ $$

$$\large \bold{\text{Process Noise Covariance}}$$ $$ \begin{aligned} \boldsymbol{Q}_k &= \alpha\boldsymbol{Q}_{k-1} + (1 - \alpha)\left( \boldsymbol{K}_k \tilde{y}_k \tilde{y}^T_k \boldsymbol{K}^T_k \right) \\ &= \alpha\boldsymbol{Q}_{k-1} + (1 - \alpha)\left( \boldsymbol{K}_k d_k d^T_k \boldsymbol{K}^T_k \right) \end{aligned} $$

$$ $$

$$\large \bold{\text{Update Previous Variables}}$$ $$ \hat{x}_{k-1|k-1} = \hat{x}_{k|k} \qquad \rightleftharpoons \qquad x^+_{k-1} = x^+_k $$

$$ \boldsymbol{P}_{k-1|k-1} = \boldsymbol{P}_{k|k} \qquad \rightleftharpoons \qquad \boldsymbol{P}^+_{k-1} = \boldsymbol{P}^+_k $$