altb_pmic
Testing ekf implementation for Quadro_1.
Revision 5:676cbc33c81b, committed 2019-10-20
- Comitter:
- pmic
- Date:
- Sun Oct 20 08:59:31 2019 +0000
- Parent:
- 4:e50e18eac72b
- Child:
- 6:f9569a07aff5
- Commit message:
- Commit before update Eigen.
Changed in this revision
| EKF_RP.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/EKF_RP.cpp Sun Oct 20 08:27:25 2019 +0000
+++ b/EKF_RP.cpp Sun Oct 20 08:59:31 2019 +0000
@@ -7,12 +7,12 @@
{
this->Ts = Ts;
/* [n_gyro; n_b_g; n_v] */
- var_fx << 1000, 1000, 20, 20, 10, 10;
+ var_fx << 1000.0f, 1000.0f, 20.0f, 20.0f, 10.0f, 10.0f;
/* [n_acc] */
- var_gy << 40, 40;
- rho = 5000;
- kv = 0.5; /* k1/m */
- g = 9.81;
+ var_gy << 40.0f, 40.0f;
+ rho = 5000.0f;
+ kv = 0.5f; /* k1/m */
+ g = 9.81f;
reset();
}
@@ -46,16 +46,16 @@
update_angles();
calc_F();
- calc_H(); /* H remains constant */
+ // calc_H(); /* H remains constant */
calc_Q();
x = fxd();
- // P = F * P * F.transpose() + Q;
+ P = F * P * F.transpose() + Q;
K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
- // x = x + K * (y - gy());
- // P = (I - K * H) * P;
+ x = x + K * (y - gy());
+ P = (I - K * H) * P;
}
float EKF_RP::read_Q(uint8_t i, uint8_t j)
@@ -65,42 +65,42 @@
void EKF_RP::update_angles()
{
- s1 = sin(x(0));
- c1 = cos(x(0));
- s2 = sin(x(1));
- c2 = cos(x(1));
+ s1 = sinf(x(0));
+ c1 = cosf(x(0));
+ s2 = sinf(x(1));
+ c2 = cosf(x(1));
}
void EKF_RP::calc_F()
{
- F << (Ts*c1*s2*(u(1) - x(3)))/c2 + 1, -(Ts*s1*(u(1) - x(3)))/(s2*s2 - 1), -Ts, -(Ts*s1*s2)/c2, 0, 0,
- -Ts*s1*(u(1) - x(3)), 1, 0, -Ts*c1, 0, 0,
- 0, 0, 1, 0, 0, 0,
- 0, 0, 0, 1, 0, 0,
- 0, Ts*c2*g, 0, 0, 1 - Ts*kv, 0,
- -Ts*c1*c2*g, Ts*g*s1*s2, 0, 0, 0, 1 - Ts*kv;
+ F << (Ts*c1*s2*(u(1) - x(3)))/c2 + 1.0f, -(Ts*s1*(u(1) - x(3)))/(s2*s2 - 1.0f), -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;
}
void EKF_RP::calc_H()
{
- H << 0, 0, 0, 0, -kv, 0,
- 0, 0, 0, 0, 0, -kv;
+ 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,
- 0, rho*var_gy(1)/Ts;
+ R << rho*var_gy(0)/Ts, 0.0f,
+ 0.0f, rho*var_gy(1)/Ts;
}
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, 0, 0, 0,
- Ts*c1*s1*s2*var_fx(1)/c2, Ts*c1*c1*var_fx(1), 0, 0, 0, 0,
- 0, 0, Ts*var_fx(2), 0, 0, 0,
- 0, 0, 0, Ts*var_fx(3), 0, 0,
- 0, 0, 0, 0, Ts*var_fx(4), 0,
- 0, 0, 0, 0, 0, Ts*var_fx(5);
+ 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()