Re: [Paparazzi-devel] Error in Kalman Filter Implementation

[Felix Ruess]

Hi Gisela,

seems to me that you're right! Although I doubt the impact is huge.
I just committed the fix.

Cheers, Felix

On Mon, Oct 12, 2009 at 4:55 PM, gisela.noci
<address@hidden> wrote:
> 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
>
>
>
> _______________________________________________
> Paparazzi-devel mailing list
> address@hidden
> http://lists.nongnu.org/mailman/listinfo/paparazzi-devel
>
>