altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

EKF_RP.cpp@7:bcbcc23983de, 2019-10-20 (annotated)

Committer:
pmic
Date:
Sun Oct 20 13:16:13 2019 +0000
Revision:
7:bcbcc23983de
Parent:
6:f9569a07aff5
Child:
8:a72cd6356bab
EKF_RPY (8 states) first time succesfully running.

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 5:676cbc33c81b 10 var_fx << 1000.0f, 1000.0f, 20.0f, 20.0f, 10.0f, 10.0f;
pmic 0:a0e9705be9c4 11 /* [n_acc] */
pmic 5:676cbc33c81b 12 var_gy << 40.0f, 40.0f;
pmic 5:676cbc33c81b 13 rho = 5000.0f;
pmic 5:676cbc33c81b 14 kv = 0.5f; /* k1/m */
pmic 5:676cbc33c81b 15 g = 9.81f;
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 4:e50e18eac72b 26 update_angles();
pmic 4:e50e18eac72b 27 calc_F();
pmic 4:e50e18eac72b 28 calc_H();
pmic 0:a0e9705be9c4 29 initialize_Q();
pmic 0:a0e9705be9c4 30 initialize_R();
pmic 0:a0e9705be9c4 31 P = Q;
pmic 0:a0e9705be9c4 32 K.setZero();
pmic 0:a0e9705be9c4 33 I.setIdentity();
pmic 0:a0e9705be9c4 34 }
pmic 0:a0e9705be9c4 35
pmic 4:e50e18eac72b 36 float EKF_RP::get_est_state(uint8_t i)
pmic 4:e50e18eac72b 37 {
pmic 7:bcbcc23983de 38 /* x = [ang; v; b_g] = [0: phi
pmic 7:bcbcc23983de 39 1: theta
pmic 7:bcbcc23983de 40 2: vx
pmic 7:bcbcc23983de 41 3: vy
pmic 7:bcbcc23983de 42 4: b_gx
pmic 7:bcbcc23983de 43 5: b_gy] */
pmic 4:e50e18eac72b 44 return x(i);
pmic 4:e50e18eac72b 45 }
pmic 4:e50e18eac72b 46
pmic 0:a0e9705be9c4 47 void EKF_RP::update(float gyro_x, float gyro_y, float accel_x, float accel_y)
pmic 0:a0e9705be9c4 48 {
pmic 4:e50e18eac72b 49 u << gyro_x, gyro_y;
pmic 4:e50e18eac72b 50 y << accel_x, accel_y;
pmic 4:e50e18eac72b 51 update_angles();
pmic 0:a0e9705be9c4 52
pmic 4:e50e18eac72b 53 calc_F();
pmic 5:676cbc33c81b 54 // calc_H(); /* H remains constant */
pmic 4:e50e18eac72b 55 calc_Q();
pmic 0:a0e9705be9c4 56
pmic 4:e50e18eac72b 57 x = fxd();
pmic 5:676cbc33c81b 58 P = F * P * F.transpose() + Q;
pmic 0:a0e9705be9c4 59
pmic 4:e50e18eac72b 60 K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
pmic 0:a0e9705be9c4 61
pmic 5:676cbc33c81b 62 x = x + K * (y - gy());
pmic 5:676cbc33c81b 63 P = (I - K * H) * P;
pmic 0:a0e9705be9c4 64 }
pmic 0:a0e9705be9c4 65
pmic 0:a0e9705be9c4 66 void EKF_RP::update_angles()
pmic 0:a0e9705be9c4 67 {
pmic 5:676cbc33c81b 68 s1 = sinf(x(0));
pmic 5:676cbc33c81b 69 c1 = cosf(x(0));
pmic 5:676cbc33c81b 70 s2 = sinf(x(1));
pmic 5:676cbc33c81b 71 c2 = cosf(x(1));
pmic 0:a0e9705be9c4 72 }
pmic 0:a0e9705be9c4 73
pmic 4:e50e18eac72b 74 void EKF_RP::calc_F()
pmic 0:a0e9705be9c4 75 {
pmic 7:bcbcc23983de 76 F << Ts*c1*s2*(u(1) - x(3))/c2 + 1.0f, Ts*s1*(u(1) - x(3))/(c2*c2), -Ts, -Ts*s1*s2/c2, 0.0f, 0.0f,
pmic 7:bcbcc23983de 77 -Ts*s1*(u(1) - x(3)), 1.0f, 0.0f, -Ts*c1, 0.0f, 0.0f,
pmic 7:bcbcc23983de 78 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
pmic 7:bcbcc23983de 79 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
pmic 7:bcbcc23983de 80 0.0f, Ts*c2*g, 0.0f, 0.0f, 1.0f - Ts*kv, 0.0f,
pmic 7:bcbcc23983de 81 -Ts*c1*c2*g, Ts*g*s1*s2, 0.0f, 0.0f, 0.0f, 1.0f - Ts*kv;
pmic 0:a0e9705be9c4 82 }
pmic 0:a0e9705be9c4 83
pmic 4:e50e18eac72b 84 void EKF_RP::calc_H()
pmic 0:a0e9705be9c4 85 {
pmic 5:676cbc33c81b 86 H << 0.0f, 0.0f, 0.0f, 0.0f, -kv, 0.0f,
pmic 5:676cbc33c81b 87 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -kv;
pmic 0:a0e9705be9c4 88 }
pmic 0:a0e9705be9c4 89
pmic 0:a0e9705be9c4 90 void EKF_RP::initialize_R()
pmic 0:a0e9705be9c4 91 {
pmic 5:676cbc33c81b 92 R << rho*var_gy(0)/Ts, 0.0f,
pmic 5:676cbc33c81b 93 0.0f, rho*var_gy(1)/Ts;
pmic 0:a0e9705be9c4 94 }
pmic 0:a0e9705be9c4 95
pmic 0:a0e9705be9c4 96 void EKF_RP::initialize_Q()
pmic 0:a0e9705be9c4 97 {
pmic 5:676cbc33c81b 98 Q << Ts*(var_fx(0) + s1*s1*s2*s2*var_fx(1)/(c2*c2)), Ts*c1*s1*s2*var_fx(1)/c2, 0.0f, 0.0f, 0.0f, 0.0f,
pmic 5:676cbc33c81b 99 Ts*c1*s1*s2*var_fx(1)/c2, Ts*c1*c1*var_fx(1), 0.0f, 0.0f, 0.0f, 0.0f,
pmic 5:676cbc33c81b 100 0.0f, 0.0f, Ts*var_fx(2), 0.0f, 0.0f, 0.0f,
pmic 5:676cbc33c81b 101 0.0f, 0.0f, 0.0f, Ts*var_fx(3), 0.0f, 0.0f,
pmic 5:676cbc33c81b 102 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(4), 0.0f,
pmic 5:676cbc33c81b 103 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(5);
pmic 0:a0e9705be9c4 104 }
pmic 0:a0e9705be9c4 105
pmic 4:e50e18eac72b 106 void EKF_RP::calc_Q()
pmic 0:a0e9705be9c4 107 {
pmic 4:e50e18eac72b 108 Q(0,0) = Ts*(var_fx(0) + s1*s1*s2*s2*var_fx(1)/(c2*c2));
pmic 0:a0e9705be9c4 109 Q(0,1) = Ts*c1*s1*s2*var_fx(1)/c2;
pmic 0:a0e9705be9c4 110 Q(1,0) = Q(0,1);
pmic 0:a0e9705be9c4 111 Q(1,1) = Ts*c1*c1*var_fx(1);
pmic 0:a0e9705be9c4 112 }
pmic 0:a0e9705be9c4 113
pmic 0:a0e9705be9c4 114 Matrix <float, 6, 1> EKF_RP::fxd()
pmic 0:a0e9705be9c4 115 {
pmic 0:a0e9705be9c4 116 Matrix <float, 6, 1> retval;
pmic 0:a0e9705be9c4 117 retval << x(0) + Ts*(u(0) - x(2) + (s1*s2*(u(1) - x(3)))/c2),
pmic 0:a0e9705be9c4 118 x(1) + Ts*c1*(u(1) - x(3)),
pmic 0:a0e9705be9c4 119 x(2),
pmic 0:a0e9705be9c4 120 x(3),
pmic 0:a0e9705be9c4 121 x(4) + Ts*(g*s2 - kv*x(4)),
pmic 0:a0e9705be9c4 122 x(5) - Ts*(kv*x(5) + c2*g*s1);
pmic 0:a0e9705be9c4 123 return retval;
pmic 0:a0e9705be9c4 124 }
pmic 0:a0e9705be9c4 125
pmic 0:a0e9705be9c4 126 Matrix <float, 2, 1> EKF_RP::gy()
pmic 0:a0e9705be9c4 127 {
pmic 0:a0e9705be9c4 128 Matrix <float, 2, 1> retval;
pmic 0:a0e9705be9c4 129 retval << -kv*x(4),
pmic 0:a0e9705be9c4 130 -kv*x(5);
pmic 0:a0e9705be9c4 131 return retval;
pmic 0:a0e9705be9c4 132 }
pmic 0:a0e9705be9c4 133