altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

EKF_RP.cpp@17:1d98928f7681, 2019-10-25 (annotated)

Committer:
pmic
Date:
Fri Oct 25 08:44:32 2019 +0000
Revision:
17:1d98928f7681
Parent:
16:c39e084f05ed
Child:
18:f374ca9a3e75
Recursion and inversion deliver same results. Time measurements show inversion is faster (???). So EKF changes back to inversion.

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 13:2e03d9c03409 10 var_fx << 0.1f, 0.1f, 0.002f, 0.002f, 0.002f, 0.002f;
pmic 0:a0e9705be9c4 11 /* [n_acc] */
pmic 13:2e03d9c03409 12 var_gy << 40.0f, 40.0f;
pmic 13:2e03d9c03409 13 rho = 1.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 13:2e03d9c03409 34 e.setZero();
pmic 0:a0e9705be9c4 35 }
pmic 0:a0e9705be9c4 36
pmic 4:e50e18eac72b 37 float EKF_RP::get_est_state(uint8_t i)
pmic 4:e50e18eac72b 38 {
pmic 7:bcbcc23983de 39 /* x = [ang; v; b_g] = [0: phi
pmic 7:bcbcc23983de 40 1: theta
pmic 7:bcbcc23983de 41 2: vx
pmic 7:bcbcc23983de 42 3: vy
pmic 7:bcbcc23983de 43 4: b_gx
pmic 7:bcbcc23983de 44 5: b_gy] */
pmic 4:e50e18eac72b 45 return x(i);
pmic 4:e50e18eac72b 46 }
pmic 4:e50e18eac72b 47
pmic 0:a0e9705be9c4 48 void EKF_RP::update(float gyro_x, float gyro_y, float accel_x, float accel_y)
pmic 0:a0e9705be9c4 49 {
pmic 4:e50e18eac72b 50 u << gyro_x, gyro_y;
pmic 4:e50e18eac72b 51 y << accel_x, accel_y;
pmic 4:e50e18eac72b 52 update_angles();
pmic 0:a0e9705be9c4 53
pmic 4:e50e18eac72b 54 calc_F();
pmic 5:676cbc33c81b 55 // calc_H(); /* H remains constant */
pmic 4:e50e18eac72b 56 calc_Q();
pmic 0:a0e9705be9c4 57
pmic 4:e50e18eac72b 58 x = fxd();
pmic 5:676cbc33c81b 59 P = F * P * F.transpose() + Q;
pmic 13:2e03d9c03409 60 e = y - gy();
pmic 0:a0e9705be9c4 61
pmic 17:1d98928f7681 62 /* inversion faster 184 mus < 207 mus recursion */
pmic 17:1d98928f7681 63 K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
pmic 17:1d98928f7681 64 x = x + K * e;
pmic 17:1d98928f7681 65 P = (I - K * H) * P;
pmic 15:53485bd1ff28 66
pmic 13:2e03d9c03409 67 /* only valid if R is diagonal (uncorrelated noise) */
pmic 17:1d98928f7681 68 /*
pmic 16:c39e084f05ed 69 for(uint8_t i = 0; i < 2; i++) {
pmic 16:c39e084f05ed 70 K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) );
pmic 16:c39e084f05ed 71 x = x + K.col(i) * e(i);
pmic 16:c39e084f05ed 72 P = (I - K.col(i)*H.row(i)) * P;
pmic 17:1d98928f7681 73 }
pmic 17:1d98928f7681 74 */
pmic 0:a0e9705be9c4 75 }
pmic 0:a0e9705be9c4 76
pmic 0:a0e9705be9c4 77 void EKF_RP::update_angles()
pmic 0:a0e9705be9c4 78 {
pmic 5:676cbc33c81b 79 s1 = sinf(x(0));
pmic 5:676cbc33c81b 80 c1 = cosf(x(0));
pmic 5:676cbc33c81b 81 s2 = sinf(x(1));
pmic 5:676cbc33c81b 82 c2 = cosf(x(1));
pmic 0:a0e9705be9c4 83 }
pmic 0:a0e9705be9c4 84
pmic 4:e50e18eac72b 85 void EKF_RP::calc_F()
pmic 0:a0e9705be9c4 86 {
pmic 7:bcbcc23983de 87 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 88 -Ts*s1*(u(1) - x(3)), 1.0f, 0.0f, -Ts*c1, 0.0f, 0.0f,
pmic 7:bcbcc23983de 89 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
pmic 7:bcbcc23983de 90 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
pmic 7:bcbcc23983de 91 0.0f, Ts*c2*g, 0.0f, 0.0f, 1.0f - Ts*kv, 0.0f,
pmic 7:bcbcc23983de 92 -Ts*c1*c2*g, Ts*g*s1*s2, 0.0f, 0.0f, 0.0f, 1.0f - Ts*kv;
pmic 0:a0e9705be9c4 93 }
pmic 0:a0e9705be9c4 94
pmic 4:e50e18eac72b 95 void EKF_RP::calc_H()
pmic 0:a0e9705be9c4 96 {
pmic 5:676cbc33c81b 97 H << 0.0f, 0.0f, 0.0f, 0.0f, -kv, 0.0f,
pmic 5:676cbc33c81b 98 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -kv;
pmic 0:a0e9705be9c4 99 }
pmic 0:a0e9705be9c4 100
pmic 0:a0e9705be9c4 101 void EKF_RP::initialize_R()
pmic 0:a0e9705be9c4 102 {
pmic 5:676cbc33c81b 103 R << rho*var_gy(0)/Ts, 0.0f,
pmic 5:676cbc33c81b 104 0.0f, rho*var_gy(1)/Ts;
pmic 0:a0e9705be9c4 105 }
pmic 0:a0e9705be9c4 106
pmic 0:a0e9705be9c4 107 void EKF_RP::initialize_Q()
pmic 0:a0e9705be9c4 108 {
pmic 5:676cbc33c81b 109 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 110 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 111 0.0f, 0.0f, Ts*var_fx(2), 0.0f, 0.0f, 0.0f,
pmic 5:676cbc33c81b 112 0.0f, 0.0f, 0.0f, Ts*var_fx(3), 0.0f, 0.0f,
pmic 5:676cbc33c81b 113 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(4), 0.0f,
pmic 5:676cbc33c81b 114 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(5);
pmic 0:a0e9705be9c4 115 }
pmic 0:a0e9705be9c4 116
pmic 4:e50e18eac72b 117 void EKF_RP::calc_Q()
pmic 0:a0e9705be9c4 118 {
pmic 4:e50e18eac72b 119 Q(0,0) = Ts*(var_fx(0) + s1*s1*s2*s2*var_fx(1)/(c2*c2));
pmic 0:a0e9705be9c4 120 Q(0,1) = Ts*c1*s1*s2*var_fx(1)/c2;
pmic 0:a0e9705be9c4 121 Q(1,0) = Q(0,1);
pmic 0:a0e9705be9c4 122 Q(1,1) = Ts*c1*c1*var_fx(1);
pmic 0:a0e9705be9c4 123 }
pmic 0:a0e9705be9c4 124
pmic 0:a0e9705be9c4 125 Matrix <float, 6, 1> EKF_RP::fxd()
pmic 0:a0e9705be9c4 126 {
pmic 0:a0e9705be9c4 127 Matrix <float, 6, 1> retval;
pmic 0:a0e9705be9c4 128 retval << x(0) + Ts*(u(0) - x(2) + (s1*s2*(u(1) - x(3)))/c2),
pmic 0:a0e9705be9c4 129 x(1) + Ts*c1*(u(1) - x(3)),
pmic 0:a0e9705be9c4 130 x(2),
pmic 0:a0e9705be9c4 131 x(3),
pmic 0:a0e9705be9c4 132 x(4) + Ts*(g*s2 - kv*x(4)),
pmic 0:a0e9705be9c4 133 x(5) - Ts*(kv*x(5) + c2*g*s1);
pmic 0:a0e9705be9c4 134 return retval;
pmic 0:a0e9705be9c4 135 }
pmic 0:a0e9705be9c4 136
pmic 0:a0e9705be9c4 137 Matrix <float, 2, 1> EKF_RP::gy()
pmic 0:a0e9705be9c4 138 {
pmic 0:a0e9705be9c4 139 Matrix <float, 2, 1> retval;
pmic 0:a0e9705be9c4 140 retval << -kv*x(4),
pmic 0:a0e9705be9c4 141 -kv*x(5);
pmic 0:a0e9705be9c4 142 return retval;
pmic 0:a0e9705be9c4 143 }
pmic 0:a0e9705be9c4 144