[paparazzi-commits] [4032] init bias with lower covariance in hfilter

[Felix Ruess]

Revision: 4032
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4032
Author:   flixr
Date:     2009-08-31 18:46:08 +0000 (Mon, 31 Aug 2009)
Log Message:
-----------
init bias with lower covariance in hfilter

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c

Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c      2009-08-31 
16:59:28 UTC (rev 4031)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c      2009-08-31 
18:46:08 UTC (rev 4032)
@@ -38,6 +38,7 @@
 
 /* initial covariance diagonal */
 #define INIT_PXX 1.
+#define INIT_PXX_BIAS 0.1
 /* process noise (is the same for x and y)*/
 #define ACCEL_NOISE 0.5
 #define Q       ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.
@@ -47,6 +48,7 @@
 #define Rpos   5.
 #define Rspeed 1.
 
+/* filter states */
 float b2_hff_x;
 float b2_hff_xbias;
 float b2_hff_xdot;
@@ -57,6 +59,7 @@
 float b2_hff_ydot;
 float b2_hff_ydotdot;
 
+/* filter covariance matrices */
 float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
 float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
 
@@ -91,7 +94,10 @@
   for (i=0; i<B2_HFF_STATE_SIZE; i++) {
     for (j=0; j<B2_HFF_STATE_SIZE; j++)
       b2_hff_xP[i][j] = 0.;
-    b2_hff_xP[i][i] = INIT_PXX;
+       if (i < 2)
+         b2_hff_xP[i][i] = INIT_PXX;
+       else
+         b2_hff_xP[i][i] = INIT_PXX_BIAS;
   }
 
 }
@@ -104,7 +110,10 @@
   for (i=0; i<B2_HFF_STATE_SIZE; i++) {
     for (j=0; j<B2_HFF_STATE_SIZE; j++)
       b2_hff_yP[i][j] = 0.;
-    b2_hff_yP[i][i] = INIT_PXX;
+       if (i < 2)
+         b2_hff_yP[i][i] = INIT_PXX;
+       else
+         b2_hff_yP[i][i] = INIT_PXX_BIAS;
   }
 
 }