Increased KF process noise.

2 files changed, 8 insertions, 17 deletions

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index c3093fe7f..d28cba42c 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -583,7 +583,7 @@ void KalmanNav::correctAtt()
 	// fault detection
 	float beta = y.dot((S*S.transpose()).inverse() * y);
 
-	if (beta > 1.0f) {
+	if (beta > 10.0f) {
 		printf("fault in attitude: beta = %8.4f\n", (double)beta);
 		printf("y:\n"); y.print();
 	}
@@ -613,8 +613,10 @@ void KalmanNav::correctPos()
         else cosLSing = -0.01;
     }
 
+	float noiseVel = _rGpsVel.get();
 	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
 	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
+	float noiseAlt = _rGpsAlt.get();
 	RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7;
 	RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7;
 
@@ -659,21 +661,10 @@ void KalmanNav::correctPos()
 	// fault detetcion
 	float beta = y.dot((S*S.transpose()).inverse() * y);
 
-	if (beta > 1.0f) {
+	if (beta > 10.0f) {
 		printf("fault in gps: beta = %8.4f\n", (double)beta);
-		printf("y:\n"); y.print();
-		//printf("R:\n"); RPos.print();
-		//printf("S:\n"); S.print();
-		//printf("S*S^T:\n"); (S*S.transpose()).print();
-		//printf("(S*S^T)^-1:\n"); ((S*S.transpose()).inverse()).print();
-		//printf("(S*S^T)^-1*y:\n"); ((S*S.transpose()).inverse()*y).print();
-        printf("gps: vN: %8.5f, vE: %8.4f, vD: %8.4f, lat: %15.10f, lon: %15.10f, alt: %15.2f\n", 
-                double(_gps.vel_n), double(_gps.vel_e), double(_gps.vel_d),
-                double(_gps.lat)/ 1.0e7 / M_RAD_TO_DEG,
-                double(_gps.lon)/ 1.0e7 / M_RAD_TO_DEG,
-                double(_gps.alt)/ 1.0e3);
-        printf("x  : vN: %8.5f, vE: %8.4f, vD: %8.4f, lat: %15.10f, lon: %15.10f, alt: %15.2f\n", 
-                double(vN), double(vE), double(vD), lat, lon, alt);
+        printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
+                y(0)/noiseVel, y(1)/noiseVel, y(2)/noiseLatDegE7, y(3)/noiseLonDegE7, y(4)/noiseAlt);
 	}
 }
 
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 03cdca5ed..f36ac334f 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -1,8 +1,8 @@
 #include <systemlib/param/param.h>
 
 /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
-PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
-PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
+PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f);
+PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f);