EKF class
Dependencies: MatrixMath Vector3
HAPS_EKF.hpp@2:771eed5f655a, 2021-05-31 (annotated)
- Committer:
- cocorlow
- Date:
- Mon May 31 07:27:20 2021 +0000
- Revision:
- 2:771eed5f655a
- Parent:
- 0:de56252b419e
rpy
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cocorlow | 0:de56252b419e | 1 | #ifndef __HAPS_EKF_HPP__ |
cocorlow | 0:de56252b419e | 2 | #define __HAPS_EKF_HPP__ |
cocorlow | 0:de56252b419e | 3 | |
cocorlow | 0:de56252b419e | 4 | #include "Matrix.h" |
cocorlow | 0:de56252b419e | 5 | #include "Vector3.hpp" |
cocorlow | 0:de56252b419e | 6 | |
cocorlow | 0:de56252b419e | 7 | |
cocorlow | 0:de56252b419e | 8 | class HAPS_EKF |
cocorlow | 0:de56252b419e | 9 | { |
cocorlow | 0:de56252b419e | 10 | private: |
cocorlow | 0:de56252b419e | 11 | Matrix qhat; |
cocorlow | 0:de56252b419e | 12 | Matrix qhat_gyro; |
cocorlow | 0:de56252b419e | 13 | Matrix Phat; |
cocorlow | 0:de56252b419e | 14 | Matrix Qgyro; |
cocorlow | 0:de56252b419e | 15 | Matrix D; |
cocorlow | 0:de56252b419e | 16 | |
cocorlow | 0:de56252b419e | 17 | public: |
cocorlow | 0:de56252b419e | 18 | Matrix Racc; |
cocorlow | 0:de56252b419e | 19 | Matrix Rmag; |
cocorlow | 0:de56252b419e | 20 | |
cocorlow | 0:de56252b419e | 21 | HAPS_EKF(); |
cocorlow | 0:de56252b419e | 22 | void updateBetweenMeasures(Vector3 gyro, float att_dt); |
cocorlow | 0:de56252b419e | 23 | void updateAcrossMeasures(Vector3 _v, Vector3 _u, Matrix& R); |
cocorlow | 2:771eed5f655a | 24 | void computeAngles(Vector3& rpy, Vector3& rpy_g, Vector3 rpy_align); |
cocorlow | 0:de56252b419e | 25 | void triad(Vector3 fb, Vector3 fn, Vector3 mb, Vector3 mn); |
cocorlow | 0:de56252b419e | 26 | Vector3 calcMagRef(Vector3 m); |
cocorlow | 0:de56252b419e | 27 | Vector3 calcDynAcc(Vector3 LPacc, Vector3 accref); |
cocorlow | 0:de56252b419e | 28 | }; |
cocorlow | 0:de56252b419e | 29 | |
cocorlow | 0:de56252b419e | 30 | #endif |