diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index a735de406f..5f71da58e7 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -82,8 +82,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : // miss counts _missFast(0), _missSlow(0), - // state + // accelerations fN(0), fE(0), fD(0), + // state phi(0), theta(0), psi(0), vN(0), vE(0), vD(0), lat(0), lon(0), alt(0), @@ -94,13 +95,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _rGpsVel(this, "R_GPS_VEL"), _rGpsPos(this, "R_GPS_POS"), _rGpsAlt(this, "R_GPS_ALT"), - _rAccel(this, "R_ACCEL") + _rAccel(this, "R_ACCEL"), + _magDip(this, "MAG_DIP"), + _magDec(this, "MAG_DEC") { using namespace math; // initial state covariance matrix P0 = Matrix::identity(9) * 1.0f; - P = P0; + P = P0; // wait for gps lock while (1) { @@ -132,12 +135,12 @@ 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); + 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)); @@ -342,10 +345,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)); + 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)); @@ -371,7 +374,7 @@ void KalmanNav::predictSlow(float dt) // prepare for matrix float R = R0 + float(alt); - float RSq = R*R; + float RSq = R * R; // F Matrix // Titterton pg. 291 @@ -472,8 +475,8 @@ void KalmanNav::correctAtt() // mag predicted measurement // choosing some typical magnetic field properties, // TODO dip/dec depend on lat/ lon/ time - static const float dip = 0.0f / M_RAD_TO_DEG_F; // dip, inclination with level - static const float dec = 0.0f / M_RAD_TO_DEG_F; // declination, clockwise rotation from north + static const float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level + static const float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north float bN = cosf(dip) * cosf(dec); float bE = cosf(dip) * sinf(dec); float bD = sinf(dip); @@ -593,7 +596,7 @@ void KalmanNav::correctAtt() P = P - K * HAtt * P; // fault detection - float beta = y.dot(S.inverse()*y); + float beta = y.dot(S.inverse() * y); if (beta > 1000.0f) { printf("fault in attitude: beta = %8.4f\n", (double)beta); @@ -671,7 +674,7 @@ void KalmanNav::correctPos() P = P - K * HPos * P; // fault detetcion - float beta = y.dot(S.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 c074cb7c31..af4588e20e 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -68,53 +68,98 @@ class KalmanNav : public control::SuperBlock { public: + /** + * Constructor + */ KalmanNav(SuperBlock *parent, const char *name); + + /** + * Deconstuctor + */ + virtual ~KalmanNav() {}; + /** + * The main callback function for the class + */ void update(); + + /** + * Fast prediction + * Contains: state integration + */ virtual void updatePublications(); void predictFast(float dt); + + /** + * Slow prediction + * Contains: covariance integration + */ void predictSlow(float dt); + + /** + * Attitude correction + */ void correctAtt(); + + /** + * Position correction + */ void correctPos(); + + /** + * Overloaded update parameters + */ virtual void updateParams(); protected: - math::Matrix F; - math::Matrix G; - math::Matrix P; - math::Matrix P0; - math::Matrix V; - math::Matrix HAtt; - math::Matrix RAtt; - math::Matrix HPos; - math::Matrix RPos; - math::Dcm C_nb; - math::Quaternion q; - control::UOrbSubscription _sensors; - control::UOrbSubscription _gps; - control::UOrbSubscription _param_update; - control::UOrbPublication _pos; - control::UOrbPublication _att; - uint64_t _pubTimeStamp; - uint64_t _fastTimeStamp; - uint64_t _slowTimeStamp; - uint64_t _attTimeStamp; - uint64_t _outTimeStamp; - uint16_t _navFrames; - uint16_t _missFast; - uint16_t _missSlow; - float fN, fE, fD; + // kalman filter + math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ + math::Matrix G; /**< noise shaping matrix for gyro/accel */ + math::Matrix P; /**< state covariance matrix */ + math::Matrix P0; /**< initial state covariance matrix */ + math::Matrix V; /**< gyro/ accel noise matrix */ + math::Matrix HAtt; /**< attitude measurement matrix */ + math::Matrix RAtt; /**< attitude measurement noise matrix */ + math::Matrix HPos; /**< position measurement jacobian matrix */ + math::Matrix RPos; /**< position measurement noise matrix */ + // attitude + math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */ + math::Quaternion q; /**< quaternion from body to nav frame */ + // subscriptions + control::UOrbSubscription _sensors; /**< sensors sub. */ + control::UOrbSubscription _gps; /**< gps sub. */ + control::UOrbSubscription _param_update; /**< parameter update sub. */ + // publications + control::UOrbPublication _pos; /**< position pub. */ + 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 _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 */ + // accelerations + float fN, fE, fD; /**< navigation frame acceleration */ // states - enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; - float phi, theta, psi; - float vN, vE, vD; - double lat, lon, alt; - control::BlockParam _vGyro; - control::BlockParam _vAccel; - control::BlockParam _rMag; - control::BlockParam _rGpsVel; - control::BlockParam _rGpsPos; - control::BlockParam _rGpsAlt; - control::BlockParam _rAccel; + enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */ + float phi, theta, psi; /**< 3-2-1 euler angles */ + float vN, vE, vD; /**< navigation velocity, m/s */ + double lat, lon, alt; /**< lat, lon, alt, radians */ + // parameters + control::BlockParam _vGyro; /**< gyro process noise */ + control::BlockParam _vAccel; /**< accelerometer process noise */ + control::BlockParam _rMag; /**< magnetometer measurement noise */ + control::BlockParam _rGpsVel; /**< gps velocity measurement noise */ + control::BlockParam _rGpsPos; /**< gps position measurement noise */ + control::BlockParam _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParam _rAccel; /**< accelerometer measurement noise */ + control::BlockParam _magDip; /**< magnetic inclination with level */ + control::BlockParam _magDec; /**< magnetic declination, clockwise rotation */ + // accessors int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; } int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }