altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

EKF_RP.h@0:a0e9705be9c4, 2019-10-18 (annotated)

Committer:
pmic
Date:
Fri Oct 18 14:55:53 2019 +0000
Revision:
0:a0e9705be9c4
Child:
4:e50e18eac72b
First commit. Nothing is working.

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 0:a0e9705be9c4 5 // #include "Eigen/Dense.h"
pmic 0:a0e9705be9c4 6 #include "Eigen/Core.h"
pmic 0:a0e9705be9c4 7 #include "Eigen/Geometry.h"
pmic 0:a0e9705be9c4 8
pmic 0:a0e9705be9c4 9 using namespace Eigen;
pmic 0:a0e9705be9c4 10
pmic 0:a0e9705be9c4 11 class EKF_RP
pmic 0:a0e9705be9c4 12 {
pmic 0:a0e9705be9c4 13 public:
pmic 0:a0e9705be9c4 14
pmic 0:a0e9705be9c4 15 EKF_RP(float Ts);
pmic 0:a0e9705be9c4 16
pmic 0:a0e9705be9c4 17 virtual ~EKF_RP();
pmic 0:a0e9705be9c4 18
pmic 0:a0e9705be9c4 19 void reset();
pmic 0:a0e9705be9c4 20 float get_est_state(uint8_t i);
pmic 0:a0e9705be9c4 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 0:a0e9705be9c4 24
pmic 0:a0e9705be9c4 25 float s1;
pmic 0:a0e9705be9c4 26 float c1;
pmic 0:a0e9705be9c4 27 float s2;
pmic 0:a0e9705be9c4 28 float c2;
pmic 0:a0e9705be9c4 29
pmic 0:a0e9705be9c4 30 float g;
pmic 0:a0e9705be9c4 31 float kv;
pmic 0:a0e9705be9c4 32 float Ts;
pmic 0:a0e9705be9c4 33 float rho;
pmic 0:a0e9705be9c4 34 Matrix <float, 2, 1> var_gy;
pmic 0:a0e9705be9c4 35 Matrix <float, 6, 1> var_fx;
pmic 0:a0e9705be9c4 36
pmic 0:a0e9705be9c4 37 Matrix <float, 2, 1> u;
pmic 0:a0e9705be9c4 38 Matrix <float, 2, 1> y;
pmic 0:a0e9705be9c4 39 Matrix <float, 6, 1> x;
pmic 0:a0e9705be9c4 40 Matrix <float, 6, 6> F;
pmic 0:a0e9705be9c4 41 Matrix <float, 2, 6> H;
pmic 0:a0e9705be9c4 42 Matrix <float, 6, 6> Q;
pmic 0:a0e9705be9c4 43 Matrix <float, 2, 2> R;
pmic 0:a0e9705be9c4 44 Matrix <float, 6, 6> P;
pmic 0:a0e9705be9c4 45 Matrix <float, 6, 2> K;
pmic 0:a0e9705be9c4 46 Matrix <float, 6, 6> I;
pmic 0:a0e9705be9c4 47
pmic 0:a0e9705be9c4 48 void update_angles();
pmic 0:a0e9705be9c4 49 void update_F();
pmic 0:a0e9705be9c4 50 void update_H();
pmic 0:a0e9705be9c4 51 void initialize_R();
pmic 0:a0e9705be9c4 52 void initialize_Q();
pmic 0:a0e9705be9c4 53 void update_Q();
pmic 0:a0e9705be9c4 54
pmic 0:a0e9705be9c4 55 Matrix <float, 6, 1> fxd();
pmic 0:a0e9705be9c4 56 Matrix <float, 2, 1> gy();
pmic 0:a0e9705be9c4 57
pmic 0:a0e9705be9c4 58 };
pmic 0:a0e9705be9c4 59
pmic 0:a0e9705be9c4 60 #endif