altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

EKF_RPY.h@23:71996bfe68eb, 2019-11-13 (annotated)

Committer:
altb2
Date:
Wed Nov 13 11:40:46 2019 +0000
Revision:
23:71996bfe68eb
Parent:
22:495a419e474c 
Commit before fligth experiments Deutweg Nr.2

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