[paparazzi-commits] [4425] fix computation order of Pk matrix in alt_kalman

[Felix Ruess]

Revision: 4425
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4425
Author:   flixr
Date:     2010-01-14 09:02:29 +0000 (Thu, 14 Jan 2010)
Log Message:
-----------
fix computation order of Pk matrix in alt_kalman

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/estimator.c

Modified: paparazzi3/trunk/sw/airborne/estimator.c
===================================================================
--- paparazzi3/trunk/sw/airborne/estimator.c    2010-01-12 13:24:07 UTC (rev 
4424)
+++ paparazzi3/trunk/sw/airborne/estimator.c    2010-01-14 09:02:29 UTC (rev 
4425)
@@ -53,7 +53,7 @@
 float estimator_q;
 
 /* flight time in seconds */
-uint16_t estimator_flight_time; 
+uint16_t estimator_flight_time;
 /* flight time in seconds */
 float estimator_t;
 
@@ -107,7 +107,7 @@
 
 
 void estimator_propagate_state( void ) {
-  
+
 }
 
 bool_t alt_kalman_enabled;
@@ -184,15 +184,15 @@
     float k_0 = p[0][0] / e;
     float k_1 =  p[1][0] / e;
     e = gps_z - estimator_z;
-    
+
     /* correction */
     estimator_z += k_0 * e;
     estimator_z_dot += k_1 * e;
-    
+
+    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);
-    p[1][0] = -p[0][0]*k_1+p[1][0];
-    p[1][1] = -p[0][1]*k_1+p[1][1];
   }
 
 #ifdef DEBUG_ALT_KALMAN