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

Question

I have an unscented Kalman filter (UKF) that tracks the state of a robot. The state vector has 12 variables. Each time I carry out a prediction step, my transfer function (naturally) acts on the entire state. However, my sensors provide measurements of different parts of the robot's state, so I may get roll, pitch, yaw and their respective velocities in one measurement, and then linear velocity in another.

My approach to handling this so far has been to simply create sub-matrices for the covariance, carry out my standard UKF update equations, and then stick the resulting values back into the full covariance matrix. However, after a few updates, the UKF yells at me for trying to pass a matrix that isn't positive-definite into a Cholesky Decomposition function. Clearly the covariance is losing its positive-definite properties, and I'm guessing it has to do with my attempts to update subsets of the full covariance matrix.

As an example taken from an actual log file, the following matrix (after the UKF prediction step) is positive-definite:

   1.1969            0            0            0            0            0      0.11567            0            0            0            0            0
        0       1.9682            0            0            0            0            0      0.98395            0            0            0            0
        0            0       1.9682            0            0            0            0            0      0.98395            0            0            0
        0            0            0       1.9682            0            0            0            0            0      0.98395            0            0
        0            0            0            0       1.9682            0            0            0            0            0      0.98395            0
        0            0            0            0            0       1.9682            0            0            0            0            0      0.98395
  0.11567            0            0            0            0            0      0.01468            0            0            0            0            0
        0      0.98395            0            0            0            0            0            1            0            0            0            0
        0            0      0.98395            0            0            0            0            0            1            0            0            0
        0            0            0      0.98395            0            0            0            0            0            1            0            0
        0            0            0            0      0.98395            0            0            0            0            0            1            0
        0            0            0            0            0      0.98395            0            0            0            0            0            1

However, after processing the correction for one variable (in this case, linear X velocity), the matrix becomes:

   1.1969            0            0            0            0            0      0.11567            0            0            0            0            0
        0       1.9682            0            0            0            0            0      0.98395            0            0            0            0
        0            0       1.9682            0            0            0            0            0      0.98395            0            0            0
        0            0            0       1.9682            0            0            0            0            0      0.98395            0            0
        0            0            0            0       1.9682            0            0            0            0            0      0.98395            0
        0            0            0            0            0       1.9682            0            0            0            0            0      0.98395
  0.11567            0            0            0            0            0         0.01            0            0            0            0            0
        0      0.98395            0            0            0            0            0            1            0            0            0            0
        0            0      0.98395            0            0            0            0            0            1            0            0            0
        0            0            0      0.98395            0            0            0            0            0            1            0            0
        0            0            0            0      0.98395            0            0            0            0            0            1            0
        0            0            0            0            0      0.98395            0            0            0            0            0            1

The difference between the two matrices above is

        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0     -0.00468            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0
        0            0            0            0            0            0            0            0            0            0            0            0

As you can see, the only difference between the two is the value in the location of the variance of linear X velocity, which is the measurement I just processed. This difference is enough to "break" my covariance matrix.

I have two questions:

1. Updating a subset of the filter doesn't appear to be the right way to go about things. Is there a better solution?

2. Alternatively, am I missing a step that would keep my covariance matrix as positive-definite?

Thanks!

EDIT:

It looks like I'm not properly placing the values back into the original covariance matrix. Simply copying the values back isn't sufficient. I need to track the correlation coefficients for the covariance matrix, and make sure that when I update a variance value, I update all the values in its row/column to maintain the correlation coefficient value. I have to do some more testing to verify that this is my issue, but some initial analysis in Matlab suggests that it is. If I'm correct, I'll answer my own question.

EDIT 2:

Given the response below and after trying it, I can see that my original edit idea won't fly. However, I have one more question:

As this is a UKF, I don't actually have Jacobian matrices. I think I see how I would make it work within the UKF update equations, but even in an EKF - and I ask because I have one of those as well - my state-to-measurement function $h$ is going to end up being the identity matrix, as I am directly measuring my state variables. In the case, I take it my "Jacobian" would just be an $m \times n$ matrix with ones in the $(i, i)$ location, where $i$ is the index of the measured values in the measurement vector?

Answer 1

A few notes first,

First, as you mentioned, you can't just pull out a submatrix and do an update on it. You can do a propogation step on a submatrix, however.

This is because of the cross-covariance terms (which "spread" information across different parts of the state). This is why having a more accurate estimate of your heading will lead to more accurate position estimates, for example.

However, following your edit, simply updating the cross-correlation (covariance) terms won't do it either, you need to update the whole matrix (unless you know for sure that some elements are independent, conditioned on the state estimate.

Here you go:

To do this, form the Jacobian matrices as before, but note that there should be zeros in all off-diagonal elements when no part of that state is being measured. Then the magic of matrix inversion will spread the innovation corrections to the correct parts of the state. The Jacobian matrix must be of size $n\times m$ for $m$ measured values and $n$ state variables (or $m\times n$ depending on your definition of Jacobian). None of this "update part of the covariance junk" unless you know for sure that the Jacobian elements are equivalent to identity. The safest way is to use the full Jacobian.

Other hacks (once everything is theoretically correct)

However, this still won't ensure PD covariance matrices. I strongly strongly recommend you don't do the following hacks until you fix all the other mistakes. But in the end, for a field-deploy-able system that operates for non-trivial amounts of time, I've found it is almost always necessary to do the following things:

1. After all updates, Let covariance $P$ be $P\gets \frac{1}{2}P + \frac{1}{2}P^{T}$, just to "even out" the off-diagonal terms -- for symmetry

2. Let $P\gets P + \epsilon I_{n\times n}$, where $\epsilon$ is a small scalar to ensure you don't underflow (and wreck the condition number of your matrix)