Kalman filter equation derivation

Question

I'm studying the Kalman Filter for tracking and smoothing. Even if I have understood the Bayesian filter concept, and I can efficiently use some of Kalman Filter implementation I'm stucked on understand the math behind it in an easy way.

So, I'm looking for an easy to understand derivation of Kalman Filter equations ( (1) update step, (2) prediction step and (3) Kalman Filter gain) from the Bayes rules and Chapman- Kolmogorov formula, knowing that:

• Temporal model is expressed by: $$ \textbf{X}_t = A\textbf{X}_{t-1} + \mu_p + \epsilon_p$$ where $A$ is transition matrix $D_\textbf{X} \times D_\textbf{X}$, $\mu_p$ is the $D_\textbf{X} \times 1$ control signal vector and $\epsilon_p$ is a transition gaussian noise with covariance $\Sigma_m$, and in probabilistic term could be expressed by: $$ p(\textbf{X}_t | \textbf{X}_{t-1}) = Norm_{\textbf{X}_t}[\textbf{X}_{t-1} + \mu_p, \Sigma_p] $$ and

• Measurement model is expressed by: $$ \textbf{y}_t = H\textbf{X}_t + \mu_m + \epsilon_m $$ where $H$ the $D_y \times D_x$ observation matrix, that maps real state space to observation space, $\mu_m$ is a $D_\textbf{y} \times1$ mean vector, and $\epsilon_m$ is the observation noise with covariance $\Sigma_m$ that in probabilistic term could be expressed by $$ p(\textbf{y}_t | \textbf{X}_t) = Norm_{\textbf{y}_t}[ H\textbf{X}_t + \mu_m, \epsilon_m] $$

Answer 1

There is a simple, straightforward derivation that starts with the assumptions of the Kalman filter and requires a little Algebra to arrive at the update and extrapolation equations as well as some properties regarding the measurement residuals (difference between the predicted state and the measurement). To start, the Kalman Filter is a linear, unbiased estimator that uses a predictor/corrector process to estimate the state given a sequence of measurements. This means that the general process involves predicting the state and then correcting the state based upon the difference between that prediction and the observed measurement (also known as the residual). The question becomes how to update the state prediction with the observed measurement such that the resulting state estimate is: (1) a linear combination of the predicted state "x" and the observed measurement "z" and (2) has an error with zero mean (unbiased). Base upon these assumptions, the Kalman Filter can be derived.

State and Measurement Model Notation and Assumptions

The state dynamics model for the state vector $\bar x_k$ at time $k$ is given by the state transition matrix $F_{k-1}$ and the state vector $\bar x_{k-1}$ at a previous time $k-1$. The state dynamics model also includes process noise given by $\bar v_{k-1}$ at time $k-1$.

The measurement model for the measurement vector $\bar z_k$ at time $k$ is given by the observation matrix $H_k$ and the state vector $\bar x_k$ at time $k$. The measurement model also includes measurement noise given by $\bar w_k$ at time $k$.

The Kalman Filter derivation is easier if we make the Linear Gaussian assumptions and assume that the measurement noise and process noises are statistically independent (uncorrelated):

State Estimation and Error Notiations

Now, we wish to find the state estimate $\hat x$ given a time series of measurements and define the following notation:

$\hat x_{k|k}$ is the state estimate at time $k$ after updating the Kalman Filter with all measurements through time $k$. That is, it is the updated/filtered state estimate.

$\hat x_{k|k-1}$ is the state estimate at time $k$ after updating the Kalman Filter with all but the most recent measurement. That is, it is the predicted state estimate.

$\tilde x_{j|k}$ is the estimation error in the state, which is given by:

$\tilde x_{j|k} = x_j - \hat x_{j|k}$

$P_{k|k}$ is the state estimate error covariance matrix at time $k$ after updating the Kalman Filter with all measurements through time $k$. That is, it is the error covariance for the updated/filtered state estimate.

$P_{k|k-1}$ is the state estimate at time $k$ after updating the Kalman Filter with all but the most recent measurement. That is, it is the error covariance for the predicted state estimate.

$P_{j|k}$ is the state estimate error covariance matrix, which is given by:

$P_{j|k} = E[\tilde x_{j|k} \tilde x_{j|k}^{\prime}]$

The predicted measurement that is predicted by the Kalman Filter is found by taking the expectation of the measurement model with the zero mean measurement noise assumption:

$\hat z_{k|k-1} = E[\bar z_k] = E[H_k \bar x_k + \bar w_k] = H_k E[\bar x_k] + E[\bar w_k] = H_k \hat x_{k|k-1}$

Finally, the residual vector is the difference between the observed measurement $z_k$ at time $k$ and the predicted measurement:

$\eta_k = z_k - \hat z_{k|k-1} = H_k \hat x_{k|k-1}$

Kalman Filter Derivation

We assume that the updated state estimate is a linear combination of the predicted state estimate and the observed measurement as:

and we wish to find the weights (gains) $K^{\prime}_k$ and $K_k$ that produce an unbiased estimate with a minimum state estimate error covariance.

Unbiased Estimate Assumption

Applying the unbiased estimation error assumption, we have that:

and with $E[\tilde x_{k|k}] = 0$, this results in:

which results in:

Substituting this relationship between $K^{\prime}_k$ and $K_k$ back into the linear combination assumption, we have:

where $K_k$ is called the Kalman Gain.

Minimizing the State Estimate Error Covariance

We start by computing the algebraic form of the updated covariance matrix:

We then compute the trace of the error covariance $Tr[P_{k|k}]$ and minimize it by: (1) computing the matrix derivative with respect to the Kalman Gain $K_k$ and (2) setting this matrix equation to zero. The solution for the Kalman Gain $K_k$ is given by:

$\frac{\partial Tr[P_{k|k}]}{\partial K_k}$ = 0

results in:

Kalman Update

From the above derivation, the Kalman Update equations are given as:

where

Kalman Extrapolation

The extrapolation equations are simply a result of applying the system dynamics model and applying the definition of the error covariance matrix:

Residual Covariance

The residual covariance is given by applying the formal definition of the expectation of the quadratic form of the residual vector $\eta_k$:

Answer 2

A derivation is given here

https://missingueverymoment.wordpress.com/2019/12/02/derivation-of-kalman-filter/

Basically it assumes the linearly dependent relations, and it minimize the square of prediction error to obtain the form of Kalman gain. Matrix derivation identities are also given in the site.

Answer 3

I think you want $p(\boldsymbol{X}_t|\boldsymbol{X}_{t-1} = N(A\boldsymbol{X}_{t-1} + \mu_p,\ldots)$ in your second equation. Regarding easy-to-follow derivations of the filter, there are many in textbooks such as Durbin & Koopman or Anderson-Moore. You might also look the Kalman filter at the Wikipedia.

Answer 4

I would like to add some intuition towards the Kalman gain

The Kalman gain is given by $$K = \Sigma_pH^T(H\Sigma_pH^T + \Sigma_m)^{-1}$$

A useful way to look at this is $$K = \frac{\Sigma_pH^T}{H\Sigma_pH^T + \Sigma_m}$$ The intuition behind this is that if $\Sigma_m$ was infinitely large, i.e. $\lim\limits_{\Sigma_m \to \infty}$, i.e. our sensors have little credibility, $K \approx0$, meaning we completely discard the sensor observation.

On the other hand, if $\Sigma_m$ was 0, i.e. our sensor observations were absolutely reliable and accurate, $K$ becomes $\frac{\Sigma_pH^T}{A\Sigma_pH^T} \approx H^{-1}$ which means we convert our sensor measurement (more accurately, the discrepency between the sensor measurement and our expected sensor results) back into state space and add it to our current state estimation.

Answer 5

Just a small correction to the excellent answer above. The residual vector can be assessed either before the Kalman correction or after. The pre-fit residual vector is \begin{equation} {\eta}_{k|k-1} = H_k (x_k - \hat{x}_{k|k-1}) + w_k = H_k \tilde{x}_{k|k-1} + w_k \end{equation} and equivalently the post-fit residual \begin{equation} {\eta}_{k|k} = H_k (x_k - \hat{x}_{k|k}) + w_k = H_k \tilde{x}_{k|k} + w_k \end{equation} By taking the expectation of the outer product of each residual vector with itself, one arrives at the definition of the residual covariance as stated above.