altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

EKF_RP.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_RP_H_
altb2 21:31e01d3e0143 2 #define EKF_RP_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_RP
altb2 21:31e01d3e0143 10 {
altb2 21:31e01d3e0143 11 public:
altb2 21:31e01d3e0143 12
altb2 21:31e01d3e0143 13 EKF_RP(float Ts);
altb2 21:31e01d3e0143 14
altb2 21:31e01d3e0143 15 virtual ~EKF_RP();
altb2 21:31e01d3e0143 16
altb2 21:31e01d3e0143 17 void reset();
altb2 21:31e01d3e0143 18 float get_est_state(uint8_t i);
altb2 21:31e01d3e0143 19 void update(float gyro_x, float gyro_y, float accel_x, float accel_y);
altb2 21:31e01d3e0143 20
altb2 21:31e01d3e0143 21 private:
altb2 21:31e01d3e0143 22
altb2 21:31e01d3e0143 23 float s1;
altb2 21:31e01d3e0143 24 float c1;
altb2 21:31e01d3e0143 25 float s2;
altb2 21:31e01d3e0143 26 float c2;
altb2 21:31e01d3e0143 27
altb2 21:31e01d3e0143 28 float g;
altb2 21:31e01d3e0143 29 float kv;
altb2 21:31e01d3e0143 30 float Ts;
altb2 21:31e01d3e0143 31 float rho;
altb2 21:31e01d3e0143 32 Matrix <float, 2, 1> var_gy;
altb2 21:31e01d3e0143 33 Matrix <float, 6, 1> var_fx;
altb2 21:31e01d3e0143 34
altb2 21:31e01d3e0143 35 Matrix <float, 2, 1> u;
altb2 21:31e01d3e0143 36 Matrix <float, 2, 1> y;
altb2 21:31e01d3e0143 37 Matrix <float, 6, 1> x;
altb2 21:31e01d3e0143 38 Matrix <float, 6, 6> F;
altb2 21:31e01d3e0143 39 Matrix <float, 2, 6> H;
altb2 21:31e01d3e0143 40 Matrix <float, 6, 6> Q;
altb2 21:31e01d3e0143 41 Matrix <float, 2, 2> R;
altb2 21:31e01d3e0143 42 Matrix <float, 6, 6> P;
altb2 21:31e01d3e0143 43 Matrix <float, 6, 2> K;
altb2 21:31e01d3e0143 44 Matrix <float, 6, 6> I;
altb2 21:31e01d3e0143 45
altb2 21:31e01d3e0143 46 void update_angles();
altb2 21:31e01d3e0143 47 void calc_F();
altb2 21:31e01d3e0143 48 void calc_H();
altb2 21:31e01d3e0143 49 void initialize_R();
altb2 21:31e01d3e0143 50 void initialize_Q();
altb2 21:31e01d3e0143 51 void calc_Q();
altb2 21:31e01d3e0143 52
altb2 21:31e01d3e0143 53 Matrix <float, 6, 1> fxd();
altb2 21:31e01d3e0143 54 Matrix <float, 2, 1> gy();
altb2 21:31e01d3e0143 55
altb2 21:31e01d3e0143 56 };
altb2 21:31e01d3e0143 57
altb2 21:31e01d3e0143 58 #endif