From a13cf90e0ac89527830b6426e70913d67ec093b2 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 14 Jan 2013 13:12:01 -0500 Subject: Increased KF process noise. --- apps/examples/kalman_demo/KalmanNav.cpp | 21 ++++++--------------- apps/examples/kalman_demo/params.c | 4 ++-- 2 files changed, 8 insertions(+), 17 deletions(-) (limited to 'apps') 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 /*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); -- cgit v1.2.3