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
Fixing square root kalman (K, S, and off-diagonal elements of P) #243
I was trying to work through the math used in the square root kalman filter to make sure I understood it while comparing to various references such as BDO Anderson and JB Moore. Optimal filtering. Prentice-Hall (1979) and found a couple issues in the update method.
The QR decomposition of M was stored in S, which is clearly not right since that is a (dim_x + dim_z, dim_x + dim_z) matrix when S should be (dim_z, dim_z). Instead, the transpose of sqrt S is in the top-left corner of the result.
K was set to the top-right corner of the QR decomposition result, even though that still needs to be right multiplied by sqrt S.
P, Q, and R were computed by multiplying their square root and its transpose int he wrong order, which could be seen by the off-diagonal elements of P not matching up with the standard kalman filter.
To fix this, I did the following
Stored the QR decomposition of M in a temporary array
Extract sqrt S from the QR decomposition
Compute the inverse of sqrt S and store it
Compute the K from the top-right corner of the QR decomposition result and the inverse of sqrt S
Added a get property to compute S from sqrt S.
Changed the order of multiplications for get properties of P, Q, and R.
I also went ahead and
Added a get property for SI
Made the test for the square root filter check all elements of P and with a smaller tolerance
Made the test for the square root filter check all elements of S and SI against the non-sqrt filter
Increased z_dim to 2 for the test for the square root filter to better catch any future errors in S and SI (many errors would survive scrutiny for 1x1 matrices that wouldn't for 2x2).
I was trying to work through the math used in the square root kalman filter to make sure I understood it while comparing to various references such as BDO Anderson and JB Moore. Optimal filtering. Prentice-Hall (1979) and found a couple issues in the update method.
To fix this, I did the following
I also went ahead and