EKF partial state update question

Question

This is a follow up to

Maintaining positive-definite property for covariance in an unscented Kalman filter update

...but it's deserving of its own question, I think.

I am processing measurements in my EKF for a subset of the variables in my state. My state vector is of cardinality 12. I am directly measuring my state variables, which means my state-to-measurement function $h$ is the identity. I am trying to update the first two variables in my state vector, which are the x and y position of my robot. My Kalman update matrices currently look like this:

State $x$ (just test values): $$ \left(\begin{array}{ccc} 0.4018 & 0.0760 \end{array} \right) $$

Covariance matrix $P$ (pulled from log file): $$ \left(\begin{array}{ccc} 0.1015 & -0.0137 & -0.2900 & 0 & 0 & 0 & 0.0195 & 0.0233 & 0.1004 & 0 & 0 & 0 \\ -0.0137 & 0.5825 & -0.0107 & 0 & 0 & 0 & 0.0002 & -0.7626 & -0.0165 & 0 & 0 & 0 \\ -0.2900 & -0.0107 & 9.6257 & 0 & 0 & 0 & 0.0015 & 0.0778 & -2.9359 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0.0100 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0.0100 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0.0100 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0.0195 & 0.0002 & 0.0015 & 0 & 0 & 0 & 0.0100 & 0 & 0 & 0 & 0 & 0 \\ 0.0233 & -0.7626 & 0.0778 & 0 & 0 & 0 & 0 & 1.0000 & 0 & 0 & 0 & 0 \\ 0.1004 & -0.0165 & -2.9359 & 0 & 0 & 0 & 0 & 0 & 1.0000 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.0100 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.0100 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.0100 \\ \end{array} \right) $$

Measurement $z$ (just test values): $$ \left(\begin{array}{ccc} 2 & 2 \end{array} \right) $$

"Jacobean" $J$: $$ \left(\begin{array}{ccc} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ \end{array} \right) $$

Measurement covariance $R$ (just test values): $$ \left(\begin{array}{ccc} 5 & 0 \\ 0 & 5 \\ \end{array} \right) $$

Kalman gain $K = PJ^T(JPJ^T + R)^{-1}$:

$$ \left(\begin{array}{ccc} 0.0199 & -0.0024 \\ -0.0024 & 0.1043 \\ -0.0569 & -0.0021 \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 0.0038 & 0.0000 \\ 0.0042 & -0.1366 \\ 0.0197 & -0.0029 \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ \end{array} \right) $$

$K$ is 12x2, meaning that my innovation - and therefore both measurement and current state - would need to be 2x1 in order to have a 12x1 result to add to the current full state:

$x' = x + K(z - h(x_s))$

where $x_s$ is a vector containing only the parts of the full state vector that I am measuring.

Here's my question: $K(z - h(x_s))$ yields

$$ \left(\begin{array}{ccc} 0.0272 \\ 0.1969 \\ -0.0948 \\ 0 \\ 0 \\ 0 \\ 0.0062 \\ -0.2561 \\ 0.0258 \\ 0 \\ 0 \\ 0 \\ \end{array} \right) $$

Does it make sense that this vector, which I will add to the current state, has non-zero values in positions other that 1 and 2 (the x and y positions of my robot)? The other non-zero locations correspond to the robot's z location, and the x, y, and z velocities. It seems strange to me that a measurement of x and y should yield changes to other variables in the state vector. Am I incorrect in this assumption?

Incidentally, the covariance update works very well with the Jacobean in this form, and maintains its positive-definite property.

Answer 1

According to your covariance matrix it does make sense. The third column of the first row is sigma_xz, and is nonzero, which means, according to your covariance matrix, the random variable z is correlated to the random variable x, and so changes in x will affect z. If you further analyze your covariance matrix, it seems that Z is correlated to Y (element (2,3) of covariance matrix is nonzero) and, of course, itself (element (3,3) of covariance matrix). For the velocities, it certainly makes sense that updates in position will update your velocity because the EKF is simply capturing information regarding changes in state. Again if you look at the covariance matrix, I assume the 7th, 8th and 9th elements of your state vector are the velocity in x, y and z. In the covariance matrix, the elements (1,7), (1,8), and (1,9) are all nonzero so the 3 velocities are correlated to your x position...further analysis shows that they're also correlated to your Y and Z position by similar arguments.