search

rlabbe / filterpy

Python Kalman filtering and optimal estimation library. Implements Kalman filter, particle filter, Extended Kalman filter, Unscented Kalman filter, g-h (alpha-beta), least squares, H Infinity, smoothers, and more. Has companion book 'Kalman and Bayesian Filters in Python'.
MIT License
3.34k stars 623 forks source link

Covariance matrix not positive definite #31

Closed matthijsroobeek closed 8 years ago

matthijsroobeek commented 8 years ago

For some reason, the covariance matrix in my system gets entries that are not positive definite. Do you have any ideas on what may be causing this issue?

Thanks in advance!

jaja3721 commented 8 years ago

I got this problem few days ago. I hope someone will answer this question.

rlabbe commented 8 years ago

I don't know what your code looks like, or what problem you are trying to solve, so it is impossible to give an exact answer.

In general though, even if your math is perfect, in the end we are linearizing a nonlinear system, and errors will accrue. In practice you need to do a few things:

1) P = 0.5 P + 0.5P^T. This ensures the matrix is symmetric.

2) P = P + eI, where e is some very small number, and I is the identity matrix. This ensures you don't underflow. there is some content in wikipedia on the condition number of a matrix: https://en.wikipedia.org/wiki/Condition_number

peter-moran commented 8 years ago

I am also having this issue.

I am using the UKF with a constant velocity model to track the position and orientation of a tag that I get x,y,z,r,p,ya measurements from an external CV library. I am simply trying to handle the non-linearity of angles (for now) as I was able to get a linear filter to work.

While I am not 100% it is the root of the problem, I am finding that the predict step is producing perfectly symmetric P matrices, while the update step does not, with P - P.T returning discrepancies along the line of 10^-35. Attempts to correct this with P = 0.5 P + 0.5P^T do not work either.

Can anyone give any help? I am looking into this further. It's also worth mentioning that this happens nearly instantly (after ~10 filter steps) and always happens with the covariance matrices I am using.

peter-moran commented 8 years ago

Never mind that. I forced a symmetric matrix by copying the upper triangle to the lower triangle and still had not positive definite P matrices.

# force P to be symmetric
self.P = np.triu(self.P) + np.triu(self.P, 1).T

Ultimately, I started commenting things out until I had a very basic UKF and determined that it was actually a problem with how I was recursively passing data to my filter but not exiting recursion right.