From ce3f835c637338086a18307c61deb35ccddbee05 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Tue, 15 Jan 2013 23:36:01 -0500 Subject: Mag and velocity measurement fix. Fault detection working. --- apps/examples/kalman_demo/KalmanNav.cpp | 79 ++++++++++++++------------------- apps/examples/kalman_demo/KalmanNav.hpp | 23 +++++----- apps/examples/kalman_demo/params.c | 25 ++++++----- 3 files changed, 59 insertions(+), 68 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 41e948ae2..d93a7bdc6 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -73,15 +73,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _att(&getPublications(), ORB_ID(vehicle_attitude)), // timestamps _pubTimeStamp(hrt_absolute_time()), - _fastTimeStamp(hrt_absolute_time()), - _slowTimeStamp(hrt_absolute_time()), + _predictTimeStamp(hrt_absolute_time()), _attTimeStamp(hrt_absolute_time()), _outTimeStamp(hrt_absolute_time()), // frame count _navFrames(0), // miss counts - _missFast(0), - _missSlow(0), + _miss(0), // accelerations fN(0), fE(0), fD(0), // state @@ -96,9 +94,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _rGpsPos(this, "R_GPS_POS"), _rGpsAlt(this, "R_GPS_ALT"), _rAccel(this, "R_ACCEL"), - _magDip(this, "MAG_DIP"), - _magDec(this, "MAG_DEC"), - _g(this, "G"), + _magDip(this, "ENV_MAG_DIP"), + _magDec(this, "ENV_MAG_DEC"), + _g(this, "ENV_G"), _faultPos(this, "FAULT_POS"), _faultAtt(this, "FAULT_ATT"), _positionInitialized(false) @@ -193,29 +191,21 @@ void KalmanNav::update() lat, lon, alt); } - // fast prediciton step - // note, using sensors timestamp so we can account - // for packet lag - float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f; - printf("dtFast: %15.10f\n", double(dtFast)); - _fastTimeStamp = _sensors.timestamp; - - if (dtFast < 1.0f) { - predictFast(dtFast); - predictSlow(dtFast); + // prediciton step + // using sensors timestamp so we can account for packet lag + float dt= (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; + //printf("dt: %15.10f\n", double(dtFast)); + _predictTimeStamp = _sensors.timestamp; + // don't predict if time greater than a second + if (dt < 1.0f) { + predictState(dt); + predictStateCovariance(dt); // count fast frames _navFrames += 1; + } - } else _missFast++; - - // slow prediction step - //float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f; - - //if (dtSlow > 1.0f / 100) { // 100 Hz - //_slowTimeStamp = _sensors.timestamp; - //if (dtSlow < 1.0f / 50) predictSlow(dtSlow); - //else _missSlow ++; - //} + // count times 100 Hz rate isn't met + if (dt > 0.01f) _miss++; // gps correction step if (gpsUpdate) { @@ -237,11 +227,10 @@ void KalmanNav::update() // output if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz _outTimeStamp = newTimeStamp; - printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", - _navFrames / 10, _missFast / 10, _missSlow / 10); + printf("nav: %4d Hz, miss #: %4d\n", + _navFrames / 10, _miss/ 10); _navFrames = 0; - _missFast = 0; - _missSlow = 0; + _miss= 0; } } @@ -288,7 +277,7 @@ void KalmanNav::updatePublications() SuperBlock::updatePublications(); } -void KalmanNav::predictFast(float dt) +void KalmanNav::predictState(float dt) { using namespace math; Vector3 w(_sensors.gyro_rad_s); @@ -336,20 +325,13 @@ void KalmanNav::predictFast(float dt) float lDot = vE / (cosLSing * R); float rotRate = 2 * omega + lDot; float vNDot = fN - vE * rotRate * sinL + - vD * LDot; + vD * LDot; float vDDot = fD - vE * rotRate * cosL - - vN * LDot + _g.get(); + vN * LDot + _g.get(); 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)); + vDDot * rotRate * cosL; // rectangular integration - printf("dt: %8.4f\n", double(dt)); - printf("fN: %8.4f, fE: %8.4f, fD: %8.4f\n", double(fN), double(fE), double(fD)); - printf("vN: %8.4f, vE: %8.4f, vD: %8.4f\n", double(vN), double(vE), double(vD)); vN += vNDot * dt; vE += vEDot * dt; vD += vDDot * dt; @@ -358,7 +340,7 @@ void KalmanNav::predictFast(float dt) alt += double(-vD * dt); } -void KalmanNav::predictSlow(float dt) +void KalmanNav::predictStateCovariance(float dt) { using namespace math; @@ -473,6 +455,8 @@ void KalmanNav::correctAtt() // mag measurement Vector3 zMag(_sensors.magnetometer_ga); + //float magNorm = zMag.norm(); + zMag = zMag.unit(); // mag predicted measurement // choosing some typical magnetic field properties, @@ -588,9 +572,9 @@ void KalmanNav::correctAtt() psi += xCorrect(PSI); // attitude also affects nav velocities - //vN += xCorrect(VN); - //vE += xCorrect(VE); - //vD += xCorrect(VD); + vN += xCorrect(VN); + vE += xCorrect(VE); + vD += xCorrect(VD); // update state covariance // http://en.wikipedia.org/wiki/Extended_Kalman_filter @@ -602,6 +586,9 @@ void KalmanNav::correctAtt() if (beta > _faultAtt.get()) { printf("fault in attitude: beta = %8.4f\n", (double)beta); printf("y:\n"); y.print(); + printf("zMagHat:\n"); zMagHat.print(); + printf("zMag:\n"); zMag.print(); + printf("bNav:\n"); bNav.print(); } // update quaternions from euler diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index d7068beec..da1a7ee10 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -83,18 +83,23 @@ public: */ void update(); + /** - * Fast prediction - * Contains: state integration + * Publication update */ virtual void updatePublications(); - void predictFast(float dt); /** - * Slow prediction - * Contains: covariance integration + * State prediction + * Continuous, non-linear + */ + void predictState(float dt); + + /** + * State covariance prediction + * Continuous, linear */ - void predictSlow(float dt); + void predictStateCovariance(float dt); /** * Attitude correction @@ -133,15 +138,13 @@ protected: control::UOrbPublication _att; /**< attitude pub. */ // time stamps uint64_t _pubTimeStamp; /**< output data publication time stamp */ - uint64_t _fastTimeStamp; /**< fast prediction time stamp */ - uint64_t _slowTimeStamp; /**< slow prediction time stamp */ + uint64_t _predictTimeStamp; /**< prediction time stamp */ uint64_t _attTimeStamp; /**< attitude correction time stamp */ uint64_t _outTimeStamp; /**< output time stamp */ // frame count uint16_t _navFrames; /**< navigation frames completed in output cycle */ // miss counts - uint16_t _missFast; /**< number of times fast prediction loop missed */ - uint16_t _missSlow; /**< number of times slow prediction loop missed */ + uint16_t _miss; /**< number of times fast prediction loop missed */ // accelerations float fN, fE, fD; /**< navigation frame acceleration */ // states diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 2cdbc1435..58ebeba3c 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -1,15 +1,16 @@ #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_R_MAG, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_MAG_DIP, 60.0f); -PARAM_DEFINE_FLOAT(KF_MAG_DEC, 0.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1000.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); -PARAM_DEFINE_FLOAT(KF_G, 9.806f); +PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.1f); +PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.1f); +PARAM_DEFINE_FLOAT(KF_R_MAG, 0.1f); +PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.1f); +PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f); +PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f); +PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.1f); +PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 1.0f); +PARAM_DEFINE_FLOAT(KF_ENV_G, 9.806f); +PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); +PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); + -- cgit v1.2.3