altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

EKF_RPY.h@21:31e01d3e0143, 2019-10-21 (annotated)

Committer:
altb2
Date:
Mon Oct 21 17:14:27 2019 +0000
Revision:
21:31e01d3e0143
Child:
22:495a419e474c
Added EKF_RP, EKF_RPY from pmic, also EIgen Library

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 void set_m0(float,float,float);
altb2 21:31e01d3e0143 16
altb2 21:31e01d3e0143 17 virtual ~EKF_RPY();
altb2 21:31e01d3e0143 18
altb2 21:31e01d3e0143 19 void reset();
altb2 21:31e01d3e0143 20 float get_est_state(uint8_t i);
altb2 21:31e01d3e0143 21 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 22
altb2 21:31e01d3e0143 23 private:
altb2 21:31e01d3e0143 24
altb2 21:31e01d3e0143 25 Matrix <float, 3, 1> m0;
altb2 21:31e01d3e0143 26
altb2 21:31e01d3e0143 27 float s1;
altb2 21:31e01d3e0143 28 float c1;
altb2 21:31e01d3e0143 29 float s2;
altb2 21:31e01d3e0143 30 float c2;
altb2 21:31e01d3e0143 31 float s3;
altb2 21:31e01d3e0143 32 float c3;
altb2 21:31e01d3e0143 33
altb2 21:31e01d3e0143 34 float g;
altb2 21:31e01d3e0143 35 float kv;
altb2 21:31e01d3e0143 36 float Ts;
altb2 21:31e01d3e0143 37 float rho;
altb2 21:31e01d3e0143 38 Matrix <float, 4, 1> var_gy;
altb2 21:31e01d3e0143 39 Matrix <float, 8, 1> var_fx;
altb2 21:31e01d3e0143 40
altb2 21:31e01d3e0143 41 Matrix <float, 3, 1> u;
altb2 21:31e01d3e0143 42 Matrix <float, 4, 1> y;
altb2 21:31e01d3e0143 43 Matrix <float, 8, 1> x;
altb2 21:31e01d3e0143 44 Matrix <float, 8, 8> F;
altb2 21:31e01d3e0143 45 Matrix <float, 4, 8> H;
altb2 21:31e01d3e0143 46 Matrix <float, 8, 8> Q;
altb2 21:31e01d3e0143 47 Matrix <float, 4, 4> R;
altb2 21:31e01d3e0143 48 Matrix <float, 8, 8> P;
altb2 21:31e01d3e0143 49 Matrix <float, 8, 4> K;
altb2 21:31e01d3e0143 50 Matrix <float, 8, 8> I;
altb2 21:31e01d3e0143 51
altb2 21:31e01d3e0143 52 void update_angles();
altb2 21:31e01d3e0143 53 void calc_F();
altb2 21:31e01d3e0143 54 void calc_H();
altb2 21:31e01d3e0143 55 void initialize_R();
altb2 21:31e01d3e0143 56 void initialize_Q();
altb2 21:31e01d3e0143 57 void calc_Q();
altb2 21:31e01d3e0143 58
altb2 21:31e01d3e0143 59 Matrix <float, 8, 1> fxd();
altb2 21:31e01d3e0143 60 Matrix <float, 4, 1> gy();
altb2 21:31e01d3e0143 61
altb2 21:31e01d3e0143 62 };
altb2 21:31e01d3e0143 63
altb2 21:31e01d3e0143 64 #endif