altb_pmic
Testing ekf implementation for Quadro_1.
Diff: EKF_RP.cpp
- Revision:
- 7:bcbcc23983de
- Parent:
- 6:f9569a07aff5
- Child:
- 8:a72cd6356bab
--- a/EKF_RP.cpp Sun Oct 20 10:20:05 2019 +0000
+++ b/EKF_RP.cpp Sun Oct 20 13:16:13 2019 +0000
@@ -35,7 +35,12 @@
float EKF_RP::get_est_state(uint8_t i)
{
- /* x = [ang; b_g; v] = [phi; theta; b_gx; b_gy; vx; vy] */
+ /* x = [ang; v; b_g] = [0: phi
+ 1: theta
+ 2: vx
+ 3: vy
+ 4: b_gx
+ 5: b_gy] */
return x(i);
}
@@ -58,11 +63,6 @@
P = (I - K * H) * P;
}
-float EKF_RP::read_Q(uint8_t i, uint8_t j)
-{
- return Q(i,j);
-}
-
void EKF_RP::update_angles()
{
s1 = sinf(x(0));
@@ -73,12 +73,12 @@
void EKF_RP::calc_F()
{
- 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;
+ 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;
}
void EKF_RP::calc_H()