7
$\begingroup$

I understand the basic principles involved in Kalman filtering and I have spent some time implementing several algorithms in MATLAB. The problem I'm facing now is to check if the algorithm and my code actually do the right thing.

I know that there are statistical tests, such as the normalized estimation error squared (NEES) test and the normalized innovation squared (NIS) test. Their principle is described in the literature, but the description of implementation and interpretation of the results are pretty vague. In the simulations I currently do, I can't get the NEES test to pass even for the perfectly matched model (while the NIS test normally passes)!

So my question is: Does anyone have tips, tricks, hints or references on how to check the consistency of the filter and debug the code if needed (especially regarding the interpretation of test outcomes)?

(Remark: I first posted this on signal processing but figured it might receive more attention here. If more detailed information is needed, I'm happy to make my post more specific!)

$\endgroup$

2 Answers 2

5
$\begingroup$
  1. Generate your own measurements, in a known scenario, that fulfills the assumptions
  2. Test it in the known scenario. If you do it in 1 or 2 dimensions, you can even do it by hand.
  3. Look at the properties. The autocorrelation of the innovation must be white if I am not mistaken (in a linear scenario, not 100% sure, but I know that in non-linear scenarios it is not. If some), so if you take the autocorrelation it should look approximately like a delta function.
  4. There are implementations online, it won't tell you that yours is right but it can make you think about the differences if there are.
  5. Check if what you get is what you are expecting to get. What is this? Well, in 2D, you are expecting to get an estimation which balances the prediction and the measurement, right? So If the result is not sort of in the middle, it is probably wrong.
  6. Be suspicious about everything you can.

And then if you have any particular point where you are not sure, post it.

There is a very nice description here, at least I liked it very much. https://cs.adelaide.edu.au/~ianr/Teaching/Estimation/LectureNotes2.pdf And since the link can break at any point, it is the file Estimation II from Ian Reid, Hillary Term 2001

$\endgroup$
0
$\begingroup$

As a follow up to @Marcel's answer, here is a more detailed explanation of how to debug and check the consistency of a Kalman filter. This explanation is an expansion of the one from section 2.2.3, page 18, of the lecture notes titled Estimation II written by Ian Reid at Oxford in 2001, which is the same set of lecture notes that @Marcel links to in his answer.

Overview

Recall that the Kalman filter is an optimal observer for a linear, time-varying, and discrete-time system that evolves according to the following state equation $$ \begin{align} x_{k+1} &= A_k x_k + B_k u_k + w_k \end{align} $$ where

  • $x_k \in \mathbb R^{d_x}$ is the state vector at time-step $k$,
  • $A_k \in \mathbb R^{d_x \times d_x}$ is the state-transition matrix at time-step $k$,
  • $u_k \in \mathbb R^{d_u}$ is the control input at time-step $k$,
  • $B_k \in \mathbb R^{d_x \times d_u}$ is the control matrix at time-step $k$,
  • $w_k \sim N(0,\Sigma_{w_k})$ is a multivariate Gaussian random vector with covariance matrix $\Sigma_{w_k} \in \mathbb R^{d_x \times d_x}$, and
  • $\text{Cov}(w_k,w_\ell) = 0$ if $k \neq \ell$ and $\text{Cov}(w_k,w_\ell) = \Sigma_{w_k}$ if $k = \ell$.

We assume that the states $x_k$, for $k \in \{0,\dots,N-1\}$, are not observed directly, but via measurements $y_k$, which are related to $x_k$ by the following observation equation $$ y_k = C_k x_k + v_k $$ where

  • $y_k \in \mathbb R^{d_y}$ is the measurement of $x_k$ at time-step $k$,
  • $C_k \in \mathbb R^{d_y \times d_x}$ is the observation matrix at time-step $k$,
  • $v_k \sim N(0,\Sigma_{v_k})$ is a multivariate Gaussian random vector with covariance matrix $\Sigma_{v_k} \in \mathbb R^{d_y \times d_y}$, and
  • $\text{Cov}(v_k,v_\ell) = 0$ if $k \neq \ell$ and $\text{Cov}(v_k,v_\ell) = \Sigma_{v_k}$ if $k = \ell$.

The state of the Kalman filter, denoted $\hat x_{k/k-1}$, and the covariance of $\hat x_{k/k-1}$ conditioned on the observed measurements, denoted $\widehat \Sigma_{k/k-1}$, evolve according to the following measurement and time update equations $$ \begin{align} \text{Measurement update equations} \\ K_k &= \Sigma_{k/k-1} C_{k}^T (C_k \Sigma_{k/k-1} C_k^T + \Sigma_{v_{k}})^{-1} \\ \hat x_{k/k} &= \hat x_{k/k-1} + K_k (y_k - C_k \hat x_{k/k-1}) \label{eq:innov} \tag{1} \\ \widehat \Sigma_{k/k} &= (I - K_k C_k) \widehat \Sigma_{k/k-1} (I - K_k C_k)^T + K_k \Sigma_{v_{k}} K_k^T \\ \text{Time update equations} \\ \hat x_{k/k-1} &= A_k \hat x_{k-1/k-1} + B_k u_k \\ \widehat \Sigma_{k/k-1} &= A_k \widehat \Sigma_{k-1/k-1} A_k^T + \Sigma_{w_k} \end{align} $$

The Innovation Sequence

The term $\nu_{k/k-1} = y_k - C_k \hat x_{k/k-1}$ in equation $\eqref{eq:innov}$ is known as the innovation at time-step $k$, which is the difference between the measurement $y_k$ and an estimate of $y_k$ obtained using previous measurements, which is denoted as $$ \begin{align} \hat y_{k/k-1} &= E[y_k \mid y_{k-1},\dots,y_0] \\ &= E[C_k x_k + v_k \mid y_{k-1},\dots,y_0] \\ &= E[C_k x_k \mid y_{k-1},\dots,y_0] + E[v_k \mid y_{k-1},\dots,y_0] \\ &= C_k E[x_k \mid y_{k-1},\dots,y_0] + E[v_k] \\ &= C_k \hat x_{k/k-1} \end{align} $$ Then, the innovation is $\nu_{k/k-1} = y_k - \hat y_{k/k-1}$. Intuitively, the innovation represents the amount of new information we get by observing $y_k$, since $\hat y_{k/k-1}$ was estimated using only previous measurements up to time-step $k-1$. Moreover, note that the innovation can also be re-written as $$ \begin{align} \nu_{k/k-1} &= y_k - C_k \hat x_{k/k-1} \\ &= C_kx_k + v_k - C_k \hat x_{k/k-1} \\ &= C_k (x_k - \hat x_{k/k-1}) + v_k \end{align} $$ We see that when $x_k = \hat x_{k/k-1}$, the only new information we obtain is the observation noise $v_k$. Ideally, this is what we want. The innovation sequence $\nu_{k/k-1}$, for $k \in \{0,\dots,N-1\}$, has several nice properties that can allow us to verify that a Kalman filter is operating as intended.

One of the the properties of the innovation sequence $\nu_{k/k-1}$ is that it has zero mean conditioned on the previous measurements, since $$ \begin{align} E[\nu_{k/k-1} \mid y_{k-1},\dots,y_0] &= E[y_k - C_k \hat x_{k/k-1} \mid y_{k-1},\dots,y_0] \\ &= E[y_k \mid y_{k-1},\dots,y_0] - E[C_k \hat x_{k/k-1} \mid y_{k-1},\dots,y_0] \\ &= \hat y_{k/k-1} - C_k \hat x_{k/k-1} \\ &= 0 \end{align} $$ Another important property is that $\nu_{k/k-1}$ is uncorrelated, given previous measurements, from $\nu_{\ell/\ell-1}$ for $k \neq \ell$. To prove this, first suppose that $k < \ell$. Then, $$ \begin{align} \text{Cov}(\nu_{k/k-1},\nu_{\ell/\ell-1} \mid y_{\ell-1},\dots,y_k,\dots,y_0) &= E[(\nu_{k/k-1} - E[\nu_{k/k-1} \mid y_{\ell-1},\dots,y_k,\dots,y_0])(\nu_{\ell/\ell-1} - E[\nu_{\ell/\ell-1} \mid y_{\ell-1},\dots,y_k,\dots,y_0])^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] \\ &= E[(\nu_{k/k-1} - (y_k - C_k \hat x_{k/k-1}))\nu_{\ell/\ell-1}^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] \\ &= E[\nu_{k/k-1}\nu_{\ell/\ell-1}^T - y_k\nu_{\ell/\ell-1}^T + C_k \hat x_{k/k-1}\nu_{\ell/\ell-1}^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] \\ &= E[\nu_{k/k-1}\nu_{\ell/\ell-1}^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] - y_k E[\nu_{\ell/\ell-1}^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] + C_k \hat x_{k/k-1}E[\nu_{\ell/\ell-1}^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] \\ &= E[\nu_{k/k-1}\nu_{\ell/\ell-1}^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] \\ &= E[(y_k - \hat y_{k/k-1})(y_\ell - \hat y_{\ell/\ell-1})^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] \\ &= E[y_k y_\ell^T - y_k \hat y_{\ell/\ell-1}^T - \hat y_{k/k-1} y_\ell^T + \hat y_{k/k-1} \hat y_{\ell/\ell-1}^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] \\ &= E[y_k y_\ell^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] - E[y_k \hat y_{\ell/\ell-1}^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] - E[\hat y_{k/k-1} y_\ell^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] + E[\hat y_{k/k-1} \hat y_{\ell/\ell-1}^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] \\ &= y_k E[y_\ell^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] - y_k E[\hat y_{\ell/\ell-1}^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] - E[\hat y_{k/k-1} y_\ell^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] + E[\hat y_{k/k-1} \hat y_{\ell/\ell-1}^T \mid y_{\ell-1},\dots,y_k,\dots,y_0] \\ &= y_k \hat y_{\ell/\ell-1}^T - y_k \hat y_{\ell/\ell-1}^T - \hat y_{k/k-1} \hat y_{\ell/\ell-1}^T + \hat y_{k/k-1} \hat y_{\ell/\ell-1}^T \\ &= 0 \end{align} $$

A similar proof can be obtained for the case when $k > \ell$. Therefore, $\text{Cov}(\nu_{k/k-1},\nu_{\ell/\ell-1} \mid y_{\ell-1},\dots,y_k,\dots,y_0) = 0$ when $k \neq \ell$. To determine $\text{Cov}(\nu_{k/k-1},\nu_{k/k-1} \mid y_{k-1},\dots,y_0)$, first note that, as mentioned above, the innovation can be re-written as $$ \begin{align} \nu_{k/k-1} &= C_k (x_k - \hat x_{k/k-1}) + v_k \\ &= C_k e_k + v_k \end{align} $$ where $e_k = x_k - \hat x_{k/k-1}$, $E[e_k \mid y_{k-1},\dots,y_0] = 0$, and $E[e_k e_k^T \mid y_{k-1},\dots,y_0] = \widehat \Sigma_{k/k-1}$. Then, $$ \begin{align} \text{Cov}(\nu_{k/k-1},\nu_{k/k-1} \mid y_{k-1},\dots,y_0) &= E[\nu_{k/k-1} \nu_{k/k-1}^T\mid y_{k-1},\dots,y_0] \\ &= E[(C_k e_k + v_k) (C_k e_k + v_k)^T \mid y_{k-1},\dots,y_0] \\ &= E[C_k e_k e_k^T C_k^T + C_k e_k v_k^T + v_k e_k^T C_k^T + v_k v_k^T \mid y_{k-1},\dots,y_0] \\ &= C_k E[e_k e_k^T \mid y_{k-1},\dots,y_0] C_k^T + E[v_k v_k^T \mid y_{k-1},\dots,y_0] \\ &= C_k \widehat \Sigma_{k/k-1} C_k^T + \Sigma_{v_k} %&= E[(y_k - \hat y_{k/k-1}) (y_k - \hat y_{k/k-1})^T\mid y_{k-1},\dots,y_0] \\ %&= E[y_k y_k^T - y_k \hat y_{k/k-1}^T - \hat y_{k/k-1} y_k^T + \hat y_{k/k-1} \hat y_{k/k-1}^T\mid y_{k-1},\dots,y_0] \\ %&= E[y_k y_k^T \mid y_{k-1},\dots,y_0] - E[y_k \hat y_{k/k-1}^T \mid y_{k-1},\dots,y_0] - E[\hat y_{k/k-1} y_k^T \mid y_{k-1},\dots,y_0] + E[\hat y_{k/k-1} \hat y_{k/k-1}^T \mid y_{k-1},\dots,y_0] \\ %&= E[(C_k x_k + v_k) (C_k x_k + v_k)^T \mid y_{k-1},\dots,y_0] - E[y_k \mid y_{k-1},\dots,y_0]\hat y_{k/k-1}^T - \hat y_{k/k-1} E[y_k^T \mid y_{k-1},\dots,y_0] + \hat y_{k/k-1} \hat y_{k/k-1}^T \\ %&= E[C_k x_k x_k^T C_k^T + C_k x_k v_k + v_k x_k^T C_k^T + v_k v_k^T \mid y_{k-1},\dots,y_0] - E[C_k x_k + v_k \mid y_{k-1},\dots,y_0]\hat y_{k/k-1}^T - \hat y_{k/k-1} E[x_k^T C_k^T + v_k^T \mid y_{k-1},\dots,y_0] + \hat y_{k/k-1} \hat y_{k/k-1}^T \\ %&= C_k E[x_k x_k^T \mid y_{k-1},\dots,y_0] C_k^T + E[v_k v_k^T] - (C_k E[x_k \mid y_{k-1},\dots,y_0] + E[v_k]) \hat y_{k/k-1}^T - \hat y_{k/k-1} (E[x_k^T \mid y_{k-1},\dots,y_0] C_k^T + E[v_k^T]) + \hat y_{k/k-1} \hat y_{k/k-1}^T \\ %&= C_k \widehat \Sigma_{k/k-1} C_k^T + \Sigma_{v_k} - C_k \hat x_{k/k-1} \hat x_{k/k-1}^T C_k^T - C_k \hat x_{k/k-1} \hat x_{k/k-1}^T C_k^T + C_k \hat x_{k/k-1} \hat x_{k/k-1}^T C_k^T \\ \end{align} $$ Because $x_k$ is Gaussian, then the innovation $\nu_{k/k-1}$ is also Gaussian, with mean $0$ and covariance $C_k \widehat \Sigma_{k/k-1} C_k^T + \Sigma_{v_k}$. Additionally, because each $\nu_{k/k-1}$ are uncorrelated, and because $a\nu_{k/k-1} + b\nu_{\ell/\ell-1}$ is Gaussian distributed for $a,b \in \mathbb R$ and $k \neq \ell$, then $\nu_{k/k-1}$ is independent of $\nu_{\ell/\ell-1}$ for $k \neq \ell$. Furthermore, if we let $$ q_{k/k-1} = \Sigma_{\nu_{k}}^{-\frac{1}{2}} \nu_{k/k-1} $$ where $\Sigma_{\nu_v} = C_k \widehat \Sigma_{k/k-1} C_k^T + \Sigma_{v_k}$ is the covariance of $\nu_{k/k-1}$ and $\Sigma_{\nu_{k}}^{-\frac{1}{2}}$ is the inverse of its square root, then $q_{k/k-1}$ is Gaussian distributed with mean $0$ and covariance $$ \begin{align} E[q_{k/k-1} q_{k/k-1}^T] &= E\left[\Sigma_{\nu_{k}}^{-\frac{1}{2}} \nu_{k/k-1} \nu_{k/k-1}^T \Sigma_{\nu_{k}}^{-\frac{1}{2}^{T}}\right] \\ &= \Sigma_{\nu_{k}}^{-\frac{1}{2}} E[\nu_{k/k-1} \nu_{k/k-1}^T] \Sigma_{\nu_{k}}^{-\frac{1}{2}^{T}} \\ &= \Sigma_{\nu_{k}}^{-\frac{1}{2}} \Sigma_{\nu_k} \Sigma_{\nu_{k}}^{-\frac{1}{2}^{T}} \\ &= \Sigma_{\nu_{k}}^{-\frac{1}{2}} \Sigma_{\nu_{k}}^{\frac{1}{2}} \Sigma_{\nu_{k}}^{\frac{1}{2}^{T}} \Sigma_{\nu_{k}}^{-\frac{1}{2}^{T}} \\ &= I \end{align} $$ for each $k$. That is, $q_{k/k-1}$ is a sequence of independent and identically distributed (i.i.d) random vectors. Because of this, we can consider the entire sequence of $q_{k/k-1}$, for $k \in \{0,\dots,N-1\}$, to be i.i.d samples of a single random vector $q$.

To summarize, the important properties of $\nu_{k/k-1}$ and $q_{k/k-1}$ are:

  • $\nu_{k/k-1} \sim N(0,C_k \widehat \Sigma_{k/k-1} C_k^T + \Sigma_{v_k})$
  • $\nu_{k/k-1}$ is independent of (and uncorrelated to) $\nu_{\ell/\ell-1}$ for $k \neq \ell$
  • $q_{k/k-1}$ is a sequence of independent (and uncorrelated) and identically distributed Gaussian random vectors with mean $0$ and covariance $I$.

The Tests

If a Kalman filter is implemented correctly, then the properties of $q_{k/k-1}$ mentioned above should be true. Equivalently, if the properties of $q_{k/k-1}$ mentioned above are not true, then the Kalman filter is not implemented correctly. Therefore, we need to check that:

  1. $q_{k/k-1}$ is independent of (or uncorrelated to) $q_{\ell/\ell-1}$ for $k \neq \ell$.
  2. That each $q_{k/k-1}$ has mean $0$ and covariance $I$.
  3. That each $q_{k/k-1}$ is Gaussian distributed.

For steps on how to perform these tests, see section 2.2.3, page 18, of the lecture notes mentioned at the top of this answer.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.