altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

EKF_RP.cpp@1:6b803652d032, 2019-10-18 (annotated)

Committer:
pmic
Date:
Fri Oct 18 21:15:52 2019 +0000
Revision:
1:6b803652d032
Parent:
0:a0e9705be9c4
Child:
4:e50e18eac72b
Test ekf in main.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 0:a0e9705be9c4 1 #include "EKF_RP.h"
pmic 0:a0e9705be9c4 2
pmic 0:a0e9705be9c4 3 using namespace std;
pmic 0:a0e9705be9c4 4 using namespace Eigen;
pmic 0:a0e9705be9c4 5
pmic 0:a0e9705be9c4 6 EKF_RP::EKF_RP(float Ts)
pmic 0:a0e9705be9c4 7 {
pmic 0:a0e9705be9c4 8 this->Ts = Ts;
pmic 0:a0e9705be9c4 9 /* [n_gyro; n_b_g; n_v] */
pmic 0:a0e9705be9c4 10 var_fx << 1000, 1000, 20, 20, 10, 10;
pmic 0:a0e9705be9c4 11 /* [n_acc] */
pmic 0:a0e9705be9c4 12 var_gy << 40, 40;
pmic 0:a0e9705be9c4 13 rho = 5000;
pmic 0:a0e9705be9c4 14 kv = 0.5; /* k1/m */
pmic 0:a0e9705be9c4 15 g = 9.81;
pmic 0:a0e9705be9c4 16 reset();
pmic 0:a0e9705be9c4 17 }
pmic 0:a0e9705be9c4 18
pmic 0:a0e9705be9c4 19 EKF_RP::~EKF_RP() {}
pmic 0:a0e9705be9c4 20
pmic 0:a0e9705be9c4 21 void EKF_RP::reset()
pmic 0:a0e9705be9c4 22 {
pmic 0:a0e9705be9c4 23 u.setZero();
pmic 0:a0e9705be9c4 24 y.setZero();
pmic 0:a0e9705be9c4 25 x.setZero();
pmic 0:a0e9705be9c4 26 update_F();
pmic 0:a0e9705be9c4 27 update_H();
pmic 0:a0e9705be9c4 28 initialize_Q();
pmic 0:a0e9705be9c4 29 initialize_R();
pmic 0:a0e9705be9c4 30 P = Q;
pmic 0:a0e9705be9c4 31 K.setZero();
pmic 0:a0e9705be9c4 32 I.setIdentity();
pmic 0:a0e9705be9c4 33 }
pmic 0:a0e9705be9c4 34
pmic 0:a0e9705be9c4 35 void EKF_RP::update(float gyro_x, float gyro_y, float accel_x, float accel_y)
pmic 0:a0e9705be9c4 36 {
pmic 0:a0e9705be9c4 37 // u << gyro_x, gyro_y;
pmic 0:a0e9705be9c4 38 // y << accel_x, accel_y;
pmic 0:a0e9705be9c4 39
pmic 0:a0e9705be9c4 40 // update_F();
pmic 0:a0e9705be9c4 41 // update_H(); /* H remains constant */
pmic 0:a0e9705be9c4 42 // update_Q();
pmic 0:a0e9705be9c4 43
pmic 0:a0e9705be9c4 44 // x = fxd();
pmic 0:a0e9705be9c4 45 // P = F * P * F.transpose() + Q;
pmic 0:a0e9705be9c4 46
pmic 0:a0e9705be9c4 47 // K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
pmic 0:a0e9705be9c4 48
pmic 0:a0e9705be9c4 49 // x = x + K * (y - gy());
pmic 0:a0e9705be9c4 50 // P = (I - K * H) * P;
pmic 0:a0e9705be9c4 51 }
pmic 0:a0e9705be9c4 52
pmic 0:a0e9705be9c4 53 float EKF_RP::get_est_state(uint8_t i)
pmic 0:a0e9705be9c4 54 {
pmic 0:a0e9705be9c4 55 /* x = [ang; b_g; v] */
pmic 0:a0e9705be9c4 56 return x(i);
pmic 0:a0e9705be9c4 57 }
pmic 0:a0e9705be9c4 58
pmic 0:a0e9705be9c4 59 void EKF_RP::update_angles()
pmic 0:a0e9705be9c4 60 {
pmic 1:6b803652d032 61 s1 = sin(x(0));
pmic 1:6b803652d032 62 c1 = cos(x(0));
pmic 1:6b803652d032 63 s2 = sin(x(1));
pmic 1:6b803652d032 64 c2 = cos(x(1));
pmic 0:a0e9705be9c4 65 }
pmic 0:a0e9705be9c4 66
pmic 0:a0e9705be9c4 67 void EKF_RP::update_F()
pmic 0:a0e9705be9c4 68 {
pmic 0:a0e9705be9c4 69 F << (Ts*c1*s2*(u(1) - x(3)))/c2 + 1, -(Ts*s1*(u(1) - x(3)))/(s2*s2 - 1), -Ts, -(Ts*s1*s2)/c2, 0, 0,
pmic 0:a0e9705be9c4 70 -Ts*s1*(u(1) - x(3)), 1, 0, -Ts*c1, 0, 0,
pmic 0:a0e9705be9c4 71 0, 0, 1, 0, 0, 0,
pmic 0:a0e9705be9c4 72 0, 0, 0, 1, 0, 0,
pmic 0:a0e9705be9c4 73 0, Ts*c2*g, 0, 0, 1 - Ts*kv, 0,
pmic 0:a0e9705be9c4 74 -Ts*c1*c2*g, Ts*g*s1*s2, 0, 0, 0, 1 - Ts*kv;
pmic 0:a0e9705be9c4 75 }
pmic 0:a0e9705be9c4 76
pmic 0:a0e9705be9c4 77 void EKF_RP::update_H()
pmic 0:a0e9705be9c4 78 {
pmic 0:a0e9705be9c4 79 H << 0, 0, 0, 0, -kv, 0,
pmic 0:a0e9705be9c4 80 0, 0, 0, 0, 0, -kv;
pmic 0:a0e9705be9c4 81 }
pmic 0:a0e9705be9c4 82
pmic 0:a0e9705be9c4 83 void EKF_RP::initialize_R()
pmic 0:a0e9705be9c4 84 {
pmic 0:a0e9705be9c4 85 R << rho*var_gy(0)/Ts, 0,
pmic 0:a0e9705be9c4 86 0, rho*var_gy(1)/Ts;
pmic 0:a0e9705be9c4 87 }
pmic 0:a0e9705be9c4 88
pmic 0:a0e9705be9c4 89 void EKF_RP::initialize_Q()
pmic 0:a0e9705be9c4 90 {
pmic 0:a0e9705be9c4 91 Q << Ts*(var_fx(0) + (s1*s1*s2*s2*var_fx(1))/(c2*c2)), (Ts*c1*s1*s2*var_fx(1))/c2, 0, 0, 0, 0,
pmic 0:a0e9705be9c4 92 (Ts*c1*s1*s2*var_fx(1))/c2, Ts*c1*c1*var_fx(1), 0, 0, 0, 0,
pmic 1:6b803652d032 93 0, 0, Ts*var_fx(2), 0, 0, 0,
pmic 1:6b803652d032 94 0, 0, 0, Ts*var_fx(3), 0, 0,
pmic 1:6b803652d032 95 0, 0, 0, 0, Ts*var_fx(4), 0,
pmic 1:6b803652d032 96 0, 0, 0, 0, 0, Ts*var_fx(5);
pmic 0:a0e9705be9c4 97 }
pmic 0:a0e9705be9c4 98
pmic 0:a0e9705be9c4 99 void EKF_RP::update_Q()
pmic 0:a0e9705be9c4 100 {
pmic 0:a0e9705be9c4 101 Q(0,0) = Ts*(var_fx(0) + (s1*s1*s2*s2*var_fx(1))/(c2*c2));
pmic 0:a0e9705be9c4 102 Q(0,1) = Ts*c1*s1*s2*var_fx(1)/c2;
pmic 0:a0e9705be9c4 103 Q(1,0) = Q(0,1);
pmic 0:a0e9705be9c4 104 Q(1,1) = Ts*c1*c1*var_fx(1);
pmic 0:a0e9705be9c4 105 }
pmic 0:a0e9705be9c4 106
pmic 0:a0e9705be9c4 107 Matrix <float, 6, 1> EKF_RP::fxd()
pmic 0:a0e9705be9c4 108 {
pmic 0:a0e9705be9c4 109 Matrix <float, 6, 1> retval;
pmic 0:a0e9705be9c4 110 retval << x(0) + Ts*(u(0) - x(2) + (s1*s2*(u(1) - x(3)))/c2),
pmic 0:a0e9705be9c4 111 x(1) + Ts*c1*(u(1) - x(3)),
pmic 0:a0e9705be9c4 112 x(2),
pmic 0:a0e9705be9c4 113 x(3),
pmic 0:a0e9705be9c4 114 x(4) + Ts*(g*s2 - kv*x(4)),
pmic 0:a0e9705be9c4 115 x(5) - Ts*(kv*x(5) + c2*g*s1);
pmic 0:a0e9705be9c4 116 return retval;
pmic 0:a0e9705be9c4 117 }
pmic 0:a0e9705be9c4 118
pmic 0:a0e9705be9c4 119 Matrix <float, 2, 1> EKF_RP::gy()
pmic 0:a0e9705be9c4 120 {
pmic 0:a0e9705be9c4 121 Matrix <float, 2, 1> retval;
pmic 0:a0e9705be9c4 122 retval << -kv*x(4),
pmic 0:a0e9705be9c4 123 -kv*x(5);
pmic 0:a0e9705be9c4 124 return retval;
pmic 0:a0e9705be9c4 125 }
pmic 0:a0e9705be9c4 126