2
$\begingroup$

If I have a discrete time process model of the form:

$$x_{k+1} = x_{k} + v_{k}\cos(\theta_{k})dt$$ $$y_{k+1} = y_{k} + v_{k}\sin(\theta_{k})dt$$ $$v_{k+1} = v_{k} $$ $$\theta_{k+1} = \theta_{k}$$

Which gives me a state transition matrix of the form: $$F = \begin{bmatrix} 1 & 0 & \cos(\theta)dt & -v\sin(\theta)dt \\ 0 & 1 & \sin(\theta)dt & v\cos(\theta)dt \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}$$

And if I use an EKF and start with a diagonal state covariance matrix:

$$P = \begin{bmatrix} p_{xx} & 0 & 0 & 0 \\ 0 & p_{yy} & 0 & 0 \\ 0 & 0 & p_{vv} & 0 \\ 0 & 0 & 0 & p_{\theta \theta} \\ \end{bmatrix}$$

Should I expect to have the $P$ matrix only have non-zero values in the index locations where we have non-zero entries in the state transition matrix $F$ (and the corresponding "reflections" to keep $P$ symmetric), that is:

$$P = \begin{bmatrix} p_{xx} & 0 & p_{xv} & p_{x\theta} \\ 0 & p_{yy} & p_{yv} & p_{y\theta} \\ p_{vx} & p_{vy} & p_{vv} & 0 \\ p_{\theta x} & 0 & 0 & p_{\theta \theta} \\ \end{bmatrix}$$

or could we expect the entire $P$ matrix to become filled with non-zero elements?

Intuition tells me that the former should be true, but experimentation shows me that the latter is true, which makes me wonder if my EKF implementation in incorrect somewhere. Any insight into the deeper workings of how covariance matrices should evolve with time would be much appreciated.

EDIT: I should have also mentioned that I'm using diagonal process and measurement noise covariances $Q$ and $R$, and a diagonal measurement matrix $H$.

$\endgroup$
7
  • $\begingroup$ What noise covariance matrices are you using? $\endgroup$
    – fibonatic
    May 23 '19 at 11:01
  • $\begingroup$ I am using diagonal noise covariances for Q and R, and a diagonal observation matrix H. $\endgroup$
    – indigoblue
    May 23 '19 at 16:42
  • $\begingroup$ Have you tried applying one iteration and see what happens? $\endgroup$
    – Royi
    May 23 '19 at 18:47
  • $\begingroup$ Yup, it starts out as expected, but over time it seems that all of the off-diagonal terms in the covariance matrix become non-zero. $\endgroup$
    – indigoblue
    May 23 '19 at 21:52
  • $\begingroup$ First, a nit -- your state transition matrix is what you get after you go to an EKF. Before that, you say "oh, this is a nonlinear system, I cannot implement a Kalman filter for it". $\endgroup$
    – TimWescott
    Oct 7 at 15:06
0
$\begingroup$

I had the same experience with Kalman filters, I think the suspect is a kind of numerical error. Besides using floating-point numbers (which are inaccurate), I would check the innovation covariance, Kalman gain, especially if the calculation uses some numerical procedure, including (pseudo)inverse (you can experiment with the same inverse procedure on its own to see whether you get something different).

EDIT: A very important point I missed: as your covariance is not diagonal anymore after the first update step, as you transform it in subsequent iterations with $FPF^T +Q$.

When you calculate the innovation covariance as $S=HPH^T +R$ then the Kalman gain as $K=PH^TS^{-1}$, then in the last step you multiply two symmetric matrices ($H$ is not important here as it is diagonal).

Even when you assume that $S^{-1}$ has 0s where $P$ then if you calculate e.g. $K_{12}$ (which is 0 in $P$), then it will be different from 0.

So when you update $P$ as $(I-KH)P$, then the multiplying factor will be a general matrix.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

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