altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

EKF_RP.cpp@21:aab1ac72095b, 2020-01-06 (annotated)

Committer:
pmic
Date:
Mon Jan 06 10:20:10 2020 +0000
Revision:
21:aab1ac72095b
Parent:
20:0227c82a78c2
Child:
22:4148af9e3d81
Commit changes in state order (correction).

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