diff options
author | James Goppert <james.goppert@gmail.com> | 2013-01-15 11:00:52 -0500 |
---|---|---|
committer | James Goppert <james.goppert@gmail.com> | 2013-01-15 11:00:52 -0500 |
commit | d02a24ec82e35016545d519ea88d46881c42df2d (patch) | |
tree | 0bed9b0993e5425ef1df8dba3be44ca32cd42b7b /apps/examples/kalman_demo/KalmanNav.hpp | |
parent | 5b0aa490d68741fc067923e7ef801f672dcb5819 (diff) | |
download | px4-firmware-d02a24ec82e35016545d519ea88d46881c42df2d.tar.gz px4-firmware-d02a24ec82e35016545d519ea88d46881c42df2d.tar.bz2 px4-firmware-d02a24ec82e35016545d519ea88d46881c42df2d.zip |
Adding comments to ekf.
Diffstat (limited to 'apps/examples/kalman_demo/KalmanNav.hpp')
-rw-r--r-- | apps/examples/kalman_demo/KalmanNav.hpp | 117 |
1 files changed, 81 insertions, 36 deletions
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index c074cb7c3..af4588e20 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<sensor_combined_s> _sensors; - control::UOrbSubscription<vehicle_gps_position_s> _gps; - control::UOrbSubscription<parameter_update_s> _param_update; - control::UOrbPublication<vehicle_global_position_s> _pos; - control::UOrbPublication<vehicle_attitude_s> _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<sensor_combined_s> _sensors; /**< sensors sub. */ + control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */ + control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */ + // publications + control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */ + control::UOrbPublication<vehicle_attitude_s> _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<float> _vGyro; - control::BlockParam<float> _vAccel; - control::BlockParam<float> _rMag; - control::BlockParam<float> _rGpsVel; - control::BlockParam<float> _rGpsPos; - control::BlockParam<float> _rGpsAlt; - control::BlockParam<float> _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<float> _vGyro; /**< gyro process noise */ + control::BlockParam<float> _vAccel; /**< accelerometer process noise */ + control::BlockParam<float> _rMag; /**< magnetometer measurement noise */ + control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */ + control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */ + control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */ + control::BlockParam<float> _magDip; /**< magnetic inclination with level */ + control::BlockParam<float> _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); } |