altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

EKF_RP.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_RP_H_
pmic 0:a0e9705be9c4 2 #define EKF_RP_H_
pmic 0:a0e9705be9c4 3
pmic 0:a0e9705be9c4 4 #include <mbed.h>
pmic 6:f9569a07aff5 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_RP
pmic 0:a0e9705be9c4 10 {
pmic 0:a0e9705be9c4 11 public:
pmic 0:a0e9705be9c4 12
pmic 0:a0e9705be9c4 13 EKF_RP(float Ts);
pmic 0:a0e9705be9c4 14
pmic 0:a0e9705be9c4 15 virtual ~EKF_RP();
pmic 0:a0e9705be9c4 16
pmic 24:e5188a2d72ca 17 void set_para();
pmic 24:e5188a2d72ca 18 void reset();
pmic 24:e5188a2d72ca 19 void increase_diag_P();
pmic 0:a0e9705be9c4 20 float get_est_state(uint8_t i);
pmic 24:e5188a2d72ca 21 void update(float gyro_x, float gyro_y, float accel_x, float accel_y);
pmic 0:a0e9705be9c4 22
pmic 0:a0e9705be9c4 23 private:
pmic 24:e5188a2d72ca 24
pmic 24:e5188a2d72ca 25 float Ts;
pmic 24:e5188a2d72ca 26 float kv, g, wg, wa;
pmic 19:ccb6fc8bf872 27 float scale_P0;
pmic 24:e5188a2d72ca 28
pmic 24:e5188a2d72ca 29 float s1, c1, s2, c2;
pmic 0:a0e9705be9c4 30
pmic 0:a0e9705be9c4 31 Matrix <float, 2, 1> u;
pmic 0:a0e9705be9c4 32 Matrix <float, 2, 1> y;
pmic 24:e5188a2d72ca 33 Matrix <float, 8, 1> x;
pmic 24:e5188a2d72ca 34 Matrix <float, 8, 8> F;
pmic 24:e5188a2d72ca 35 Matrix <float, 2, 8> H;
pmic 24:e5188a2d72ca 36 Matrix <float, 8, 8> Q;
pmic 0:a0e9705be9c4 37 Matrix <float, 2, 2> R;
pmic 24:e5188a2d72ca 38 Matrix <float, 8, 8> P;
pmic 24:e5188a2d72ca 39 Matrix <float, 8, 2> K;
pmic 24:e5188a2d72ca 40 Matrix <float, 8, 8> I;
pmic 13:2e03d9c03409 41 Matrix <float, 2, 1> e;
pmic 24:e5188a2d72ca 42
pmic 0:a0e9705be9c4 43 void update_angles();
pmic 4:e50e18eac72b 44 void calc_F();
pmic 4:e50e18eac72b 45 void calc_H();
pmic 0:a0e9705be9c4 46
pmic 24:e5188a2d72ca 47 Matrix <float, 8, 1> fxd();
pmic 0:a0e9705be9c4 48 Matrix <float, 2, 1> gy();
pmic 0:a0e9705be9c4 49
pmic 0:a0e9705be9c4 50 };
pmic 0:a0e9705be9c4 51
pmic 0:a0e9705be9c4 52 #endif