From 5b0aa490d68741fc067923e7ef801f672dcb5819 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 14 Jan 2013 18:38:17 -0500 Subject: Added P0. Hid some printing. Corrected fault detection. --- apps/examples/kalman_demo/KalmanNav.cpp | 26 +++++++++++++++++++------- apps/examples/kalman_demo/KalmanNav.hpp | 1 + 2 files changed, 20 insertions(+), 7 deletions(-) (limited to 'apps/examples') diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index e804fbca5..a735de406 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -45,8 +45,6 @@ // Titterton pg. 52 static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s static const float R0 = 6378137.0f; // earth radius, m - -static const float RSq = 4.0680631591e+13; // earth radius squared static const float g = 9.806f; // gravitational accel. m/s^2, XXX should be calibrated KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : @@ -55,6 +53,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : F(9, 9), G(9, 6), P(9, 9), + P0(9, 9), V(6, 6), // attitude measurement ekf matrices HAtt(6, 9), @@ -100,7 +99,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : using namespace math; // initial state covariance matrix - P = Matrix::identity(9) * 1.0f; + P0 = Matrix::identity(9) * 1.0f; + P = P0; // wait for gps lock while (1) { @@ -132,6 +132,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : setLonDegE7(_gps.lon); setAltE3(_gps.alt); + printf("[kalman_demo] initializing EKF state with GPS\n"); + printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + double(phi), double(theta), double(psi)); + printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + double(vN), double(vE), double(vD), + lat, lon, alt); + // initialize quaternions q = Quaternion(EulerAngles(phi, theta, psi)); @@ -335,6 +342,10 @@ void KalmanNav::predictFast(float dt) vN * LDot + g; float vEDot = fE + vN * rotRate * sinL + vDDot * rotRate * cosL; + printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n", + double(vNDot), double(vEDot), double(vDDot)); + printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n", + double(LDot), double(lDot), double(rotRate)); // rectangular integration //printf("dt: %8.4f\n", double(dt)); @@ -360,6 +371,7 @@ void KalmanNav::predictSlow(float dt) // prepare for matrix float R = R0 + float(alt); + float RSq = R*R; // F Matrix // Titterton pg. 291 @@ -558,7 +570,7 @@ void KalmanNav::correctAtt() // abort correction and return printf("[kalman_demo] numerical failure in att correction\n"); // reset P matrix to identity - P = Matrix::identity(9) * 1.0f; + P = P0; return; } } @@ -581,7 +593,7 @@ void KalmanNav::correctAtt() P = P - K * HAtt * P; // fault detection - float beta = y.dot((S * S.transpose()).inverse() * y); + float beta = y.dot(S.inverse()*y); if (beta > 1000.0f) { printf("fault in attitude: beta = %8.4f\n", (double)beta); @@ -641,7 +653,7 @@ void KalmanNav::correctPos() setLonDegE7(_gps.lon); setAltE3(_gps.alt); // reset P matrix to identity - P = Matrix::identity(9) * 1.0f; + P = P0; return; } } @@ -659,7 +671,7 @@ void KalmanNav::correctPos() P = P - K * HPos * P; // fault detetcion - float beta = y.dot((S * S.transpose()).inverse() * y); + float beta = y.dot(S.inverse()*y); if (beta > 1000.0f) { printf("fault in gps: beta = %8.4f\n", (double)beta); diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index 1ea46d40f..c074cb7c3 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -81,6 +81,7 @@ protected: math::Matrix F; math::Matrix G; math::Matrix P; + math::Matrix P0; math::Matrix V; math::Matrix HAtt; math::Matrix RAtt; -- cgit v1.2.3