altb_pmic
/
Test_ekf
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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 |
--- 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;
}
--- 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();
};