[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4032] init bias with lower covariance in hfilter
From: |
Felix Ruess |
Subject: |
[paparazzi-commits] [4032] init bias with lower covariance in hfilter |
Date: |
Mon, 31 Aug 2009 18:46:13 +0000 |
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;
}
}
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4032] init bias with lower covariance in hfilter,
Felix Ruess <=