![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
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?
User | Revision | Line number | New 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 |