altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

EKF_RPY.h@22:495a419e474c, 2019-10-28 (annotated)

Committer:
altb2
Date:
Mon Oct 28 07:52:43 2019 +0000
Revision:
22:495a419e474c
Parent:
21:31e01d3e0143
Child:
23:71996bfe68eb
EKF_RPY and EKF_RP from pmic

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 21:31e01d3e0143 1 #ifndef EKF_RPY_H_
altb2 21:31e01d3e0143 2 #define EKF_RPY_H_
altb2 21:31e01d3e0143 3
altb2 21:31e01d3e0143 4 #include <mbed.h>
altb2 21:31e01d3e0143 5 #include "Eigen/Dense.h"
altb2 21:31e01d3e0143 6
altb2 21:31e01d3e0143 7 using namespace Eigen;
altb2 21:31e01d3e0143 8
altb2 21:31e01d3e0143 9 class EKF_RPY
altb2 21:31e01d3e0143 10 {
altb2 21:31e01d3e0143 11 public:
altb2 21:31e01d3e0143 12
altb2 21:31e01d3e0143 13 EKF_RPY(float Ts, float mx0, float my0, float mz0);
altb2 21:31e01d3e0143 14 EKF_RPY(float Ts);
altb2 21:31e01d3e0143 15
altb2 21:31e01d3e0143 16 virtual ~EKF_RPY();
altb2 21:31e01d3e0143 17
altb2 22:495a419e474c 18 void set_para();
altb2 21:31e01d3e0143 19 void reset();
altb2 22:495a419e474c 20 void set_m0(float mx0, float my0, float mz0);
altb2 21:31e01d3e0143 21 float get_est_state(uint8_t i);
altb2 21:31e01d3e0143 22 void update(float gyro_x, float gyro_y, float gyro_z, float accel_x, float accel_y, float magnet_x, float magnet_y);
altb2 21:31e01d3e0143 23
altb2 21:31e01d3e0143 24 private:
altb2 21:31e01d3e0143 25
altb2 21:31e01d3e0143 26 Matrix <float, 3, 1> m0;
altb2 21:31e01d3e0143 27
altb2 21:31e01d3e0143 28 float s1;
altb2 21:31e01d3e0143 29 float c1;
altb2 21:31e01d3e0143 30 float s2;
altb2 21:31e01d3e0143 31 float c2;
altb2 21:31e01d3e0143 32 float s3;
altb2 21:31e01d3e0143 33 float c3;
altb2 21:31e01d3e0143 34
altb2 21:31e01d3e0143 35 float g;
altb2 21:31e01d3e0143 36 float kv;
altb2 21:31e01d3e0143 37 float Ts;
altb2 21:31e01d3e0143 38 float rho;
altb2 21:31e01d3e0143 39 Matrix <float, 4, 1> var_gy;
altb2 21:31e01d3e0143 40 Matrix <float, 8, 1> var_fx;
altb2 21:31e01d3e0143 41
altb2 21:31e01d3e0143 42 Matrix <float, 3, 1> u;
altb2 21:31e01d3e0143 43 Matrix <float, 4, 1> y;
altb2 21:31e01d3e0143 44 Matrix <float, 8, 1> x;
altb2 21:31e01d3e0143 45 Matrix <float, 8, 8> F;
altb2 21:31e01d3e0143 46 Matrix <float, 4, 8> H;
altb2 21:31e01d3e0143 47 Matrix <float, 8, 8> Q;
altb2 21:31e01d3e0143 48 Matrix <float, 4, 4> R;
altb2 21:31e01d3e0143 49 Matrix <float, 8, 8> P;
altb2 21:31e01d3e0143 50 Matrix <float, 8, 4> K;
altb2 21:31e01d3e0143 51 Matrix <float, 8, 8> I;
altb2 22:495a419e474c 52 Matrix <float, 4, 1> e;
altb2 21:31e01d3e0143 53
altb2 21:31e01d3e0143 54 void update_angles();
altb2 21:31e01d3e0143 55 void calc_F();
altb2 21:31e01d3e0143 56 void calc_H();
altb2 21:31e01d3e0143 57 void initialize_R();
altb2 21:31e01d3e0143 58 void initialize_Q();
altb2 21:31e01d3e0143 59 void calc_Q();
altb2 21:31e01d3e0143 60
altb2 21:31e01d3e0143 61 Matrix <float, 8, 1> fxd();
altb2 21:31e01d3e0143 62 Matrix <float, 4, 1> gy();
altb2 21:31e01d3e0143 63
altb2 21:31e01d3e0143 64 };
altb2 21:31e01d3e0143 65
altb2 21:31e01d3e0143 66 #endif