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.31k stars 617 forks source link

in _cholesky raise LinAlgError("%d-th leading minor of the array is not positive") #218

Closed whentheworldfallsapart closed 3 years ago

whentheworldfallsapart commented 3 years ago
Traceback (most recent call last):
  File "c:/Users/.../filterpy/testFilterPy.py", line 317, in <module>
    ukf.predict(dt=dt)
  File "c:\Users\...\filterpy\filterpy\kalman\UKF.py", line 400, in predict
an\sigma_points.py", line 168, in sigma_points
    U = self.sqrt((lambda_ + n)*P)
  File "C:\Users\Anaconda3\envs\filterpy\lib\site-packages\scipy\linalg\decomp_cholesky.py", line 88, in cholesky
    c, lower = _cholesky(a, lower=lower, overwrite_a=overwrite_a, clean=True,
  File "C:\Users\Anaconda3\envs\filterpy\lib\site-packages\scipy\linalg\decomp_cholesky.py", line 37, in _cholesky
    raise LinAlgError("%d-th leading minor of the array is not positive "
numpy.linalg.LinAlgError: 4-th leading minor of the array is not positive definite

I am using UKF with a CV model. I initialize P and after the first call to predict, then update, P becomes non positive definite. So on the next measurement when predict I get the above error.

This only happens in master branch not in release 1.4.5. I traced down the issue to this commit: https://github.com/rlabbe/filterpy/commit/94852b4213875f380be1d7cb4d83283af9a4e23a

This is only an issue with the default sqrt function (scipy.linalg.cholesky), if I switch to scipy.linalg.sqrtm then the issues doesnt occur. So I can work around it by either switching the sqrt function or by using the release not master (the only reason i switch to the master is I was playing with IMM, which is outside of this issue as this happens with a single UKF). However, is the implementation in of the UKF in release 1.4.5 valid if the following is missing in the predict function

        # update sigma points to reflect the new variance of the points
        self.sigmas_f = self.points_fn.sigma_points(self.x, self.P) 

Should I be using the master branch to have this correction? (I am assuming it is a correction, I dont have the references to easily check that). This may be a case where it is only my data, but the issue happens after only one measurement update so i dont think it is because the filter is diverging. I thought i would reach out for help, any suggestions are welcome.

 print(ukf.P)
[[1. 0. 0. 0. 0. 0.]
 [0. 1. 0. 0. 0. 0.]
 [0. 0. 1. 0. 0. 0.]
 [0. 0. 0. 1. 0. 0.]
 [0. 0. 0. 0. 1. 0.]
 [0. 0. 0. 0. 0. 1.]]

 ukf.predict(dt=dt)
  print(ukf.P)
[[ 5.69113213e+01  3.19060652e-17  2.23342405e-17  1.35802271e+01
   1.25379135e-23 -2.18996067e-22]
 [ 3.19060562e-17  5.69113213e+01  5.93492221e-17 -1.28864074e-22
   1.35802271e+01 -5.80887585e-22]
 [ 2.23342428e-17  5.93492271e-17  5.69113213e+01 -9.07563229e-23
   2.24870724e-23  1.35802271e+01]
 [ 1.35802271e+01 -1.28714553e-22 -9.06516543e-23  4.30000000e+00
  -5.09837881e-29  8.93779406e-28]
 [ 1.22530657e-23  1.35802271e+01  2.23504789e-23 -4.98971799e-29
   4.30000000e+00 -2.30939030e-28]
 [-2.19028165e-22 -5.80967831e-22  1.35802271e+01  8.94690711e-28
  -2.32350148e-28  4.30000000e+00]]

 ukf.update(z)
 print(ukf.P) #this matrix is non positive definite 
[[-1.52766688e-10  3.19060649e-17  5.03131603e-17 -1.82129867e-11
   2.23981951e-23 -3.16807129e-22]
 [ 3.19060557e-17 -1.12855716e-08  5.93492250e-17 -1.28864074e-22
  -1.34648559e-09 -5.80887588e-22]
 [ 5.03131641e-17  5.93492303e-17  2.23563035e-09  1.91645116e-22
   5.60909643e-23  2.66739519e-10]
 [-1.82129867e-11 -1.28714552e-22  1.90890373e-22  1.06581410e-14
   4.62962155e-29 -8.70023528e-29]
 [ 2.18632960e-23 -1.34648737e-09  5.57957654e-23  5.32536877e-29
   8.88178420e-16 -3.60439822e-28]
 [-3.16687176e-22 -5.80967835e-22  2.66739519e-10 -8.50702167e-29
  -3.56201310e-28  2.66453526e-15]]
rlabbe commented 3 years ago

I can't diagnose this without more data/code. What is in master should be correct.

I didn't realize the last release was so old. I suppose it is possible master has a bug, but it has gone unreported because people are running from the release.

However, for var(x) to go so small (-1e-10) after only one update makes me question the implementation. That much certainty (ignoring it is negative) after one update is not normal.

whentheworldfallsapart commented 3 years ago

Thank you Roger, that answers my questions. I will use the master since it has the fix. And that insight on the var(x), lead me to realize my measurement uncertainty matrix R was too small, with that context it make sense that var(x) would go small. (Init R with np.zeros and dont fill it correctly that is what happens, corrected R and everything looks good. Strangely enough the release 1.4.5 didnt care with the sigma points not getting updated after the prior was computed.)