diff options
| author | James Goppert <james.goppert@gmail.com> | 2013-01-13 17:09:22 -0500 |
|---|---|---|
| committer | James Goppert <james.goppert@gmail.com> | 2013-01-13 17:25:45 -0500 |
| commit | 464c5245f264b974646b146b5cc1ddea44fbcbf6 (patch) | |
| tree | 7864c7f3ebb71d762a89a6ddcbfb0acf6b494ddd /apps/examples/kalman_demo/KalmanNav.hpp | |
| parent | 0fdf9623561225940189ce4f419b4347f29e11a1 (diff) | |
| download | px4-firmware-464c5245f264b974646b146b5cc1ddea44fbcbf6.tar.gz px4-firmware-464c5245f264b974646b146b5cc1ddea44fbcbf6.tar.bz2 px4-firmware-464c5245f264b974646b146b5cc1ddea44fbcbf6.zip | |
Rebase of changes to sensor_hil_fixedwing branch.
Diffstat (limited to 'apps/examples/kalman_demo/KalmanNav.hpp')
| -rw-r--r-- | apps/examples/kalman_demo/KalmanNav.hpp | 17 |
1 files changed, 12 insertions, 5 deletions
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index dc4a81f4a..1ea46d40f 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -60,6 +60,11 @@ #include <poll.h> #include <unistd.h> +/** + * Kalman filter navigation class + * http://en.wikipedia.org/wiki/Extended_Kalman_filter + * Discrete-time extended Kalman filter + */ class KalmanNav : public control::SuperBlock { public: @@ -70,7 +75,7 @@ public: void predictFast(float dt); void predictSlow(float dt); void correctAtt(); - void correctGps(); + void correctPos(); virtual void updateParams(); protected: math::Matrix F; @@ -79,8 +84,8 @@ protected: math::Matrix V; math::Matrix HAtt; math::Matrix RAtt; - math::Matrix HGps; - math::Matrix RGps; + math::Matrix HPos; + math::Matrix RPos; math::Dcm C_nb; math::Quaternion q; control::UOrbSubscription<sensor_combined_s> _sensors; @@ -94,6 +99,8 @@ protected: uint64_t _attTimeStamp; uint64_t _outTimeStamp; uint16_t _navFrames; + uint16_t _missFast; + uint16_t _missSlow; float fN, fE, fD; // states enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; @@ -103,8 +110,8 @@ protected: control::BlockParam<float> _vGyro; control::BlockParam<float> _vAccel; control::BlockParam<float> _rMag; - control::BlockParam<float> _rGpsV; - control::BlockParam<float> _rGpsGeo; + control::BlockParam<float> _rGpsVel; + control::BlockParam<float> _rGpsPos; control::BlockParam<float> _rGpsAlt; control::BlockParam<float> _rAccel; int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } |