![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
Revision 24:e5188a2d72ca, committed 2020-02-05
- Comitter:
- pmic
- Date:
- Wed Feb 05 15:35:01 2020 +0000
- Parent:
- 23:226f18dd30a5
- Commit message:
- Update EKF_RP 6 states to EKF_RP 8 states.
Changed in this revision
EKF_RP.cpp | Show annotated file Show diff for this revision Revisions of this file |
EKF_RP.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 226f18dd30a5 -r e5188a2d72ca EKF_RP.cpp --- a/EKF_RP.cpp Thu Jan 30 14:14:50 2020 +0000 +++ b/EKF_RP.cpp Wed Feb 05 15:35:01 2020 +0000 @@ -6,19 +6,31 @@ EKF_RP::EKF_RP(float Ts) { this->Ts = Ts; - /* [n_gyro; n_b_g; n_v] */ - var_fx << 0.1f, 0.1f, 0.002f, 0.002f, 0.002f, 0.002f; - /* [n_acc] */ - var_gy << 40.0f, 40.0f; - rho = 1.0f; - // kv = 0.5f; /* QK3 k1/m */ - kv = 0.26f; /* QK4 k1/m */ - g = 9.81f; - scale_P0 = 1000.0f; + set_para(); reset(); } EKF_RP::~EKF_RP() {} + +void EKF_RP::set_para() +{ + // kv = 0.5f; /* QK3 k1/m */ + kv = 0.25f; /* QK4*/ + g = 9.81f; + wg = 0.5026549f; // 2*pi*0.08 + wa = 1.2566371f; // 2*pi*0.2 + scale_P0 = 100.0f; + 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, + 0.0000000e+00f, 4.0000265e-03f, 0.0000000e+00f, -1.9799947e-06f, 3.9172179e-04f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, + -1.9799947e-06f, 0.0000000e+00f, 1.9800281e-04f, 0.0000000e+00f, 0.0000000e+00f, 1.2981776e-07f, 0.0000000e+00f, 0.0000000e+00f, + 0.0000000e+00f, -1.9799947e-06f, 0.0000000e+00f, 1.9800281e-04f, -1.2981776e-07f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, + 0.0000000e+00f, 3.9172179e-04f, 0.0000000e+00f, -1.2981776e-07f, 2.5008971e-04f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, + -3.9172179e-04f, 0.0000000e+00f, 1.2981776e-07f, 0.0000000e+00f, 0.0000000e+00f, 2.5008971e-04f, 0.0000000e+00f, 0.0000000e+00f, + 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 1.9505688e-04f, 0.0000000e+00f, + 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 0.0000000e+00f, 1.9505688e-04f; + R << 3.5000000e+02f, 0.0000000e+00f, + 0.0000000e+00f, 3.5000000e+02f; +} void EKF_RP::reset() { @@ -28,8 +40,6 @@ update_angles(); calc_F(); calc_H(); - initialize_Q(); - initialize_R(); K.setZero(); I.setIdentity(); e.setZero(); @@ -43,12 +53,14 @@ float EKF_RP::get_est_state(uint8_t i) { - /* x = [ang; v; b_g] = [0: phi - 1: theta - 2: vx - 3: vy - 4: b_gx - 5: b_gy] */ + /* x = [ang; b_g; v; b_a] = [0: phi + 1: theta + 2: b_gx + 3: b_gy + 4: vx + 5: vy] + 6: b_ax] + 7: b_ay] */ return x(i); } @@ -60,25 +72,34 @@ calc_F(); // calc_H(); /* H remains constant */ - calc_Q(); x = fxd(); P = F * P * F.transpose() + Q; e = y - gy(); - /* inversion faster 184 mus < 207 mus recursion */ + /* RP 6 states on PES-board: inversion faster 184 mus < 207 mus recursion, mbed online */ + /* RPY 8 states on PES-board: inversion faster 356 mus < 432 mus recursion, mbed online */ + /* RP 6 states on QK-board: inversion 1204 mus , Keil muVision5 */ + /* RPY 8 states on QK-board: inversion 2582 mus , Keil muVision5 */ + + /* RP 8 states on PES-board: inversion faster 284 mus , mbed online */ + /* RP 8 states on qk-board: inversion faster 1907 mus < 2318 mus recursion, Keil muVision5 */ + /* RPY 12 states on qk-board: inversion faster 5295 mus < 8575 mus recursion, Keil muVision5 */ + + /* inversion */ K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse(); x = x + K * e; - P = (I - K * H) * P; + P -= K * H * P; // (I - K * H) * P; - /* only valid if R is diagonal (uncorrelated noise) */ + /* recursion: only valid if R is diagonal (uncorrelated noise) */ /* for(uint8_t i = 0; i < 2; i++) { - K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) ); - x = x + K.col(i) * e(i); - P = (I - K.col(i)*H.row(i)) * P; + K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) ); + x = x + K.col(i) * e(i); + P -= K.col(i) * H.row(i) * P; // P = (I - K.col(i) * H.row(i)) * P; } */ + } void EKF_RP::update_angles() @@ -91,61 +112,41 @@ void EKF_RP::calc_F() { - 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, - -Ts*s1*(u(1) - x(3)), 1.0f, 0.0f, -Ts*c1, 0.0f, 0.0f, - 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, - 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, - 0.0f, Ts*c2*g, 0.0f, 0.0f, 1.0f - Ts*kv, 0.0f, - -Ts*c1*c2*g, Ts*g*s1*s2, 0.0f, 0.0f, 0.0f, 1.0f - Ts*kv; + 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, + -Ts*s1*(u(1) - x(3)), 1.0f, 0.0f, -Ts*c1, 0.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, + 0.0f, Ts*c2*g, 0.0f, 0.0f, 1.0f - Ts*kv, 0.0f, 0.0f, 0.0f, + -Ts*c1*c2*g, Ts*g*s1*s2, 0.0f, 0.0f, 0.0f, 1.0f - Ts*kv, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f - Ts*wa, 0.0f, + 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f - Ts*wa; } void EKF_RP::calc_H() { - H << 0.0f, 0.0f, 0.0f, 0.0f, -kv, 0.0f, - 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -kv; -} - -void EKF_RP::initialize_R() -{ - R << rho*var_gy(0)/Ts, 0.0f, - 0.0f, rho*var_gy(1)/Ts; + H << 0.0f, 0.0f, 0.0f, 0.0f, -kv, 0.0f, 1.0f, 0.0f, + 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -kv, 0.0f, 1.0f; } -void EKF_RP::initialize_Q() -{ - 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, - Ts*c1*s1*s2*var_fx(1)/c2, Ts*c1*c1*var_fx(1), 0.0f, 0.0f, 0.0f, 0.0f, - 0.0f, 0.0f, Ts*var_fx(2), 0.0f, 0.0f, 0.0f, - 0.0f, 0.0f, 0.0f, Ts*var_fx(3), 0.0f, 0.0f, - 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(4), 0.0f, - 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(5); -} - -void EKF_RP::calc_Q() -{ - Q(0,0) = Ts*(var_fx(0) + s1*s1*s2*s2*var_fx(1)/(c2*c2)); - Q(0,1) = Ts*c1*s1*s2*var_fx(1)/c2; - Q(1,0) = Q(0,1); - Q(1,1) = Ts*c1*c1*var_fx(1); -} - -Matrix <float, 6, 1> EKF_RP::fxd() +Matrix <float, 8, 1> EKF_RP::fxd() { - Matrix <float, 6, 1> retval; + Matrix <float, 8, 1> retval; retval << x(0) + Ts*(u(0) - x(2) + (s1*s2*(u(1) - x(3)))/c2), - x(1) + Ts*c1*(u(1) - x(3)), - x(2), - x(3), - x(4) + Ts*(g*s2 - kv*x(4)), - x(5) - Ts*(kv*x(5) + c2*g*s1); + x(1) + Ts*c1*(u(1) - x(3)), + x(2), + x(3), + x(4) + Ts*(g*s2 - kv*x(4)), + x(5) - Ts*(kv*x(5) + c2*g*s1), + x(6) - Ts*x(6)*wa, + x(7) - Ts*x(7)*wa; return retval; } Matrix <float, 2, 1> EKF_RP::gy() { Matrix <float, 2, 1> retval; - retval << -kv*x(4), - -kv*x(5); + retval << x(6) - kv*x(4), + x(7) - kv*x(5); return retval; }
diff -r 226f18dd30a5 -r e5188a2d72ca EKF_RP.h --- a/EKF_RP.h Thu Jan 30 14:14:50 2020 +0000 +++ b/EKF_RP.h Wed Feb 05 15:35:01 2020 +0000 @@ -14,46 +14,37 @@ virtual ~EKF_RP(); - void reset(); - void increase_diag_P(); + void set_para(); + void reset(); + void increase_diag_P(); float get_est_state(uint8_t i); - void update(float gyro_x, float gyro_y, float accel_x, float accel_y); + void update(float gyro_x, float gyro_y, float accel_x, float accel_y); private: - - float s1; - float c1; - float s2; - float c2; - + + float Ts; + float kv, g, wg, wa; float scale_P0; - float g; - float kv; - float Ts; - float rho; - Matrix <float, 2, 1> var_gy; - Matrix <float, 6, 1> var_fx; + + float s1, c1, s2, c2; Matrix <float, 2, 1> u; Matrix <float, 2, 1> y; - Matrix <float, 6, 1> x; - Matrix <float, 6, 6> F; - Matrix <float, 2, 6> H; - Matrix <float, 6, 6> Q; + Matrix <float, 8, 1> x; + Matrix <float, 8, 8> F; + Matrix <float, 2, 8> H; + Matrix <float, 8, 8> Q; Matrix <float, 2, 2> R; - Matrix <float, 6, 6> P; - Matrix <float, 6, 2> K; - Matrix <float, 6, 6> I; + Matrix <float, 8, 8> P; + Matrix <float, 8, 2> K; + Matrix <float, 8, 8> I; Matrix <float, 2, 1> e; - + void update_angles(); void calc_F(); void calc_H(); - void initialize_R(); - void initialize_Q(); - void calc_Q(); - Matrix <float, 6, 1> fxd(); + Matrix <float, 8, 1> fxd(); Matrix <float, 2, 1> gy(); };