altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

EKF_RP.h@30:9b0cd3caf0ec, 2020-01-27 (annotated)

Committer:
pmic
Date:
Mon Jan 27 10:54:13 2020 +0000
Revision:
30:9b0cd3caf0ec
Parent:
23:71996bfe68eb 
Correct magnetometer.

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 23:71996bfe68eb 18 void increase_diag_P();
altb2 21:31e01d3e0143 19 float get_est_state(uint8_t i);
altb2 21:31e01d3e0143 20 void update(float gyro_x, float gyro_y, float accel_x, float accel_y);
altb2 21:31e01d3e0143 21
altb2 21:31e01d3e0143 22 private:
altb2 21:31e01d3e0143 23
altb2 21:31e01d3e0143 24 float s1;
altb2 21:31e01d3e0143 25 float c1;
altb2 21:31e01d3e0143 26 float s2;
altb2 21:31e01d3e0143 27 float c2;
altb2 21:31e01d3e0143 28
altb2 23:71996bfe68eb 29 float scale_P0;
altb2 21:31e01d3e0143 30 float g;
altb2 21:31e01d3e0143 31 float kv;
altb2 21:31e01d3e0143 32 float Ts;
altb2 21:31e01d3e0143 33 float rho;
altb2 21:31e01d3e0143 34 Matrix <float, 2, 1> var_gy;
altb2 21:31e01d3e0143 35 Matrix <float, 6, 1> var_fx;
altb2 21:31e01d3e0143 36
altb2 21:31e01d3e0143 37 Matrix <float, 2, 1> u;
altb2 21:31e01d3e0143 38 Matrix <float, 2, 1> y;
altb2 21:31e01d3e0143 39 Matrix <float, 6, 1> x;
altb2 21:31e01d3e0143 40 Matrix <float, 6, 6> F;
altb2 21:31e01d3e0143 41 Matrix <float, 2, 6> H;
altb2 21:31e01d3e0143 42 Matrix <float, 6, 6> Q;
altb2 21:31e01d3e0143 43 Matrix <float, 2, 2> R;
altb2 21:31e01d3e0143 44 Matrix <float, 6, 6> P;
altb2 21:31e01d3e0143 45 Matrix <float, 6, 2> K;
altb2 21:31e01d3e0143 46 Matrix <float, 6, 6> I;
altb2 22:495a419e474c 47 Matrix <float, 2, 1> e;
altb2 21:31e01d3e0143 48
altb2 21:31e01d3e0143 49 void update_angles();
altb2 21:31e01d3e0143 50 void calc_F();
altb2 21:31e01d3e0143 51 void calc_H();
altb2 21:31e01d3e0143 52 void initialize_R();
altb2 21:31e01d3e0143 53 void initialize_Q();
altb2 21:31e01d3e0143 54 void calc_Q();
altb2 21:31e01d3e0143 55
altb2 21:31e01d3e0143 56 Matrix <float, 6, 1> fxd();
altb2 21:31e01d3e0143 57 Matrix <float, 2, 1> gy();
altb2 21:31e01d3e0143 58
altb2 21:31e01d3e0143 59 };
altb2 21:31e01d3e0143 60
altb2 21:31e01d3e0143 61 #endif