altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

EKF_RP.cpp@24:e5188a2d72ca, 2020-02-05 (annotated)

Committer:
pmic
Date:
Wed Feb 05 15:35:01 2020 +0000
Revision:
24:e5188a2d72ca
Parent:
22:4148af9e3d81 
Update EKF_RP 6 states to EKF_RP 8 states.

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 24:e5188a2d72ca 9 set_para();
pmic 0:a0e9705be9c4 10 reset();
pmic 0:a0e9705be9c4 11 }
pmic 0:a0e9705be9c4 12
pmic 0:a0e9705be9c4 13 EKF_RP::~EKF_RP() {}
pmic 24:e5188a2d72ca 14
pmic 24:e5188a2d72ca 15 void EKF_RP::set_para()
pmic 24:e5188a2d72ca 16 {
pmic 24:e5188a2d72ca 17 // kv = 0.5f; /* QK3 k1/m */
pmic 24:e5188a2d72ca 18 kv = 0.25f; /* QK4*/
pmic 24:e5188a2d72ca 19 g = 9.81f;
pmic 24:e5188a2d72ca 20 wg = 0.5026549f; // 2*pi*0.08
pmic 24:e5188a2d72ca 21 wa = 1.2566371f; // 2*pi*0.2
pmic 24:e5188a2d72ca 22 scale_P0 = 100.0f;
pmic 24:e5188a2d72ca 23 Q << 4.0000265e-03f, 0.0000000e+00f, -1.9799947e-06f, 0.0000000e+00f, 0.0000000e+00f, -3.9172179e-04f, 0.0000000e+00f, 0.0000000e+00f,
pmic 24:e5188a2d72ca 24 0.0000000e+00f, 4.0000265e-03f, 0.0000000e+00f, -1.9799947e-06f, 3.9172179e-04f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f,
pmic 24:e5188a2d72ca 25 -1.9799947e-06f, 0.0000000e+00f, 1.9800281e-04f, 0.0000000e+00f, 0.0000000e+00f, 1.2981776e-07f, 0.0000000e+00f, 0.0000000e+00f,
pmic 24:e5188a2d72ca 26 0.0000000e+00f, -1.9799947e-06f, 0.0000000e+00f, 1.9800281e-04f, -1.2981776e-07f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f,
pmic 24:e5188a2d72ca 27 0.0000000e+00f, 3.9172179e-04f, 0.0000000e+00f, -1.2981776e-07f, 2.5008971e-04f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f,
pmic 24:e5188a2d72ca 28 -3.9172179e-04f, 0.0000000e+00f, 1.2981776e-07f, 0.0000000e+00f, 0.0000000e+00f, 2.5008971e-04f, 0.0000000e+00f, 0.0000000e+00f,
pmic 24:e5188a2d72ca 29 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 1.9505688e-04f, 0.0000000e+00f,
pmic 24:e5188a2d72ca 30 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 1.9505688e-04f;
pmic 24:e5188a2d72ca 31 R << 3.5000000e+02f, 0.0000000e+00f,
pmic 24:e5188a2d72ca 32 0.0000000e+00f, 3.5000000e+02f;
pmic 24:e5188a2d72ca 33 }
pmic 0:a0e9705be9c4 34
pmic 0:a0e9705be9c4 35 void EKF_RP::reset()
pmic 0:a0e9705be9c4 36 {
pmic 0:a0e9705be9c4 37 u.setZero();
pmic 0:a0e9705be9c4 38 y.setZero();
pmic 0:a0e9705be9c4 39 x.setZero();
pmic 4:e50e18eac72b 40 update_angles();
pmic 4:e50e18eac72b 41 calc_F();
pmic 4:e50e18eac72b 42 calc_H();
pmic 0:a0e9705be9c4 43 K.setZero();
pmic 0:a0e9705be9c4 44 I.setIdentity();
pmic 13:2e03d9c03409 45 e.setZero();
pmic 20:0227c82a78c2 46 P = scale_P0 * I;
pmic 19:ccb6fc8bf872 47 }
pmic 19:ccb6fc8bf872 48
pmic 19:ccb6fc8bf872 49 void EKF_RP::increase_diag_P()
pmic 19:ccb6fc8bf872 50 {
pmic 19:ccb6fc8bf872 51 P = P + scale_P0 * I;
pmic 0:a0e9705be9c4 52 }
pmic 0:a0e9705be9c4 53
pmic 4:e50e18eac72b 54 float EKF_RP::get_est_state(uint8_t i)
pmic 4:e50e18eac72b 55 {
pmic 24:e5188a2d72ca 56 /* x = [ang; b_g; v; b_a] = [0: phi
pmic 24:e5188a2d72ca 57 1: theta
pmic 24:e5188a2d72ca 58 2: b_gx
pmic 24:e5188a2d72ca 59 3: b_gy
pmic 24:e5188a2d72ca 60 4: vx
pmic 24:e5188a2d72ca 61 5: vy]
pmic 24:e5188a2d72ca 62 6: b_ax]
pmic 24:e5188a2d72ca 63 7: b_ay] */
pmic 4:e50e18eac72b 64 return x(i);
pmic 4:e50e18eac72b 65 }
pmic 4:e50e18eac72b 66
pmic 0:a0e9705be9c4 67 void EKF_RP::update(float gyro_x, float gyro_y, float accel_x, float accel_y)
pmic 0:a0e9705be9c4 68 {
pmic 4:e50e18eac72b 69 u << gyro_x, gyro_y;
pmic 4:e50e18eac72b 70 y << accel_x, accel_y;
pmic 4:e50e18eac72b 71 update_angles();
pmic 0:a0e9705be9c4 72
pmic 4:e50e18eac72b 73 calc_F();
pmic 5:676cbc33c81b 74 // calc_H(); /* H remains constant */
pmic 0:a0e9705be9c4 75
pmic 4:e50e18eac72b 76 x = fxd();
pmic 5:676cbc33c81b 77 P = F * P * F.transpose() + Q;
pmic 13:2e03d9c03409 78 e = y - gy();
pmic 0:a0e9705be9c4 79
pmic 24:e5188a2d72ca 80 /* RP 6 states on PES-board: inversion faster 184 mus < 207 mus recursion, mbed online */
pmic 24:e5188a2d72ca 81 /* RPY 8 states on PES-board: inversion faster 356 mus < 432 mus recursion, mbed online */
pmic 24:e5188a2d72ca 82 /* RP 6 states on QK-board: inversion 1204 mus , Keil muVision5 */
pmic 24:e5188a2d72ca 83 /* RPY 8 states on QK-board: inversion 2582 mus , Keil muVision5 */
pmic 24:e5188a2d72ca 84
pmic 24:e5188a2d72ca 85 /* RP 8 states on PES-board: inversion faster 284 mus , mbed online */
pmic 24:e5188a2d72ca 86 /* RP 8 states on qk-board: inversion faster 1907 mus < 2318 mus recursion, Keil muVision5 */
pmic 24:e5188a2d72ca 87 /* RPY 12 states on qk-board: inversion faster 5295 mus < 8575 mus recursion, Keil muVision5 */
pmic 24:e5188a2d72ca 88
pmic 24:e5188a2d72ca 89 /* inversion */
pmic 17:1d98928f7681 90 K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
pmic 17:1d98928f7681 91 x = x + K * e;
pmic 24:e5188a2d72ca 92 P -= K * H * P; // (I - K * H) * P;
pmic 15:53485bd1ff28 93
pmic 24:e5188a2d72ca 94 /* recursion: only valid if R is diagonal (uncorrelated noise) */
pmic 17:1d98928f7681 95 /*
pmic 16:c39e084f05ed 96 for(uint8_t i = 0; i < 2; i++) {
pmic 24:e5188a2d72ca 97 K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) );
pmic 24:e5188a2d72ca 98 x = x + K.col(i) * e(i);
pmic 24:e5188a2d72ca 99 P -= K.col(i) * H.row(i) * P; // P = (I - K.col(i) * H.row(i)) * P;
pmic 17:1d98928f7681 100 }
pmic 17:1d98928f7681 101 */
pmic 24:e5188a2d72ca 102
pmic 0:a0e9705be9c4 103 }
pmic 0:a0e9705be9c4 104
pmic 0:a0e9705be9c4 105 void EKF_RP::update_angles()
pmic 0:a0e9705be9c4 106 {
pmic 5:676cbc33c81b 107 s1 = sinf(x(0));
pmic 5:676cbc33c81b 108 c1 = cosf(x(0));
pmic 5:676cbc33c81b 109 s2 = sinf(x(1));
pmic 5:676cbc33c81b 110 c2 = cosf(x(1));
pmic 0:a0e9705be9c4 111 }
pmic 0:a0e9705be9c4 112
pmic 4:e50e18eac72b 113 void EKF_RP::calc_F()
pmic 0:a0e9705be9c4 114 {
pmic 24:e5188a2d72ca 115 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, 0.0f, 0.0f,
pmic 24:e5188a2d72ca 116 -Ts*s1*(u(1) - x(3)), 1.0f, 0.0f, -Ts*c1, 0.0f, 0.0f, 0.0f, 0.0f,
pmic 24:e5188a2d72ca 117 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
pmic 24:e5188a2d72ca 118 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
pmic 24:e5188a2d72ca 119 0.0f, Ts*c2*g, 0.0f, 0.0f, 1.0f - Ts*kv, 0.0f, 0.0f, 0.0f,
pmic 24:e5188a2d72ca 120 -Ts*c1*c2*g, Ts*g*s1*s2, 0.0f, 0.0f, 0.0f, 1.0f - Ts*kv, 0.0f, 0.0f,
pmic 24:e5188a2d72ca 121 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f - Ts*wa, 0.0f,
pmic 24:e5188a2d72ca 122 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f - Ts*wa;
pmic 0:a0e9705be9c4 123 }
pmic 0:a0e9705be9c4 124
pmic 4:e50e18eac72b 125 void EKF_RP::calc_H()
pmic 0:a0e9705be9c4 126 {
pmic 24:e5188a2d72ca 127 H << 0.0f, 0.0f, 0.0f, 0.0f, -kv, 0.0f, 1.0f, 0.0f,
pmic 24:e5188a2d72ca 128 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -kv, 0.0f, 1.0f;
pmic 0:a0e9705be9c4 129 }
pmic 0:a0e9705be9c4 130
pmic 24:e5188a2d72ca 131 Matrix <float, 8, 1> EKF_RP::fxd()
pmic 0:a0e9705be9c4 132 {
pmic 24:e5188a2d72ca 133 Matrix <float, 8, 1> retval;
pmic 0:a0e9705be9c4 134 retval << x(0) + Ts*(u(0) - x(2) + (s1*s2*(u(1) - x(3)))/c2),
pmic 24:e5188a2d72ca 135 x(1) + Ts*c1*(u(1) - x(3)),
pmic 24:e5188a2d72ca 136 x(2),
pmic 24:e5188a2d72ca 137 x(3),
pmic 24:e5188a2d72ca 138 x(4) + Ts*(g*s2 - kv*x(4)),
pmic 24:e5188a2d72ca 139 x(5) - Ts*(kv*x(5) + c2*g*s1),
pmic 24:e5188a2d72ca 140 x(6) - Ts*x(6)*wa,
pmic 24:e5188a2d72ca 141 x(7) - Ts*x(7)*wa;
pmic 0:a0e9705be9c4 142 return retval;
pmic 0:a0e9705be9c4 143 }
pmic 0:a0e9705be9c4 144
pmic 0:a0e9705be9c4 145 Matrix <float, 2, 1> EKF_RP::gy()
pmic 0:a0e9705be9c4 146 {
pmic 0:a0e9705be9c4 147 Matrix <float, 2, 1> retval;
pmic 24:e5188a2d72ca 148 retval << x(6) - kv*x(4),
pmic 24:e5188a2d72ca 149 x(7) - kv*x(5);
pmic 0:a0e9705be9c4 150 return retval;
pmic 0:a0e9705be9c4 151 }
pmic 0:a0e9705be9c4 152