# Kalman Filter State Covariance Matrix

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$$.

• What noise covariance matrices are you using? – fibonatic May 23 at 11:01
• I am using diagonal noise covariances for Q and R, and a diagonal observation matrix H. – indigoblue May 23 at 16:42
• Have you tried applying one iteration and see what happens? – Royi May 23 at 18:47
• 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. – indigoblue May 23 at 21:52