Fixing square root kalman (K, S, and off-diagonal elements of P)

[frejanordsiek]

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).