altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

EKF_RPY.h@24:e5188a2d72ca, 2020-02-05 (annotated)

Committer:
pmic
Date:
Wed Feb 05 15:35:01 2020 +0000
Revision:
24:e5188a2d72ca
Parent:
19:ccb6fc8bf872 
Update EKF_RP 6 states to EKF_RP 8 states.

Who changed what in which revision?

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