[Paparazzi-devel] Error in Kalman Filter Implementation

[gisela.noci]

Regarding the current implementation of the Kalman Filter, I believe that there is a bug in the last 4 lines. The purpose of these lines is to update the error covariance matrix P.

 

The typical formula is Pk = Pk - K*H*Pk, where Pk, K and H are all matrices. The Pk on the right of the equation is the current value of the matrix, while the Pk on the left of the equation is the newly computed matrix.

 

In the code, the computation has been simplified into the following lines of code. The problem here is that lines 3 and 4 are using the value of p[0][0] and p[0][1] computed in lines 1 and 2 (ie. elements of the new Pk matrix), instead of the values form the current Pk matrix.

 

    p[0][0] = p[0][0] * (1-k_0);

    p[0][1] = p[0][1] * (1-k_0);

    p[1][0] = -p[0][0]*k_1+p[1][0];

    p[1][1] = -p[0][1]*k_1+p[1][1];

 

To be correct, the sequence of these lines of code should be as follows:

 

    p[1][0] = -p[0][0]*k_1+p[1][0];

    p[1][1] = -p[0][1]*k_1+p[1][1];

    p[0][0] = p[0][0] * (1-k_0);

    p[0][1] = p[0][1] * (1-k_0);

 

I don’t think it will have a major impact on performance, but cannot be sure.

Gisela