altb_pmic
Testing ekf implementation for Quadro_1.
Diff: EKF_RP.cpp
- Revision:
- 24:e5188a2d72ca
- Parent:
- 22:4148af9e3d81
--- 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;
}