![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
Diff: main.cpp
- Revision:
- 1:6b803652d032
- Parent:
- 0:a0e9705be9c4
- Child:
- 2:756446014084
--- a/main.cpp Fri Oct 18 14:55:53 2019 +0000 +++ b/main.cpp Fri Oct 18 21:15:52 2019 +0000 @@ -1,4 +1,5 @@ #include "mbed.h" +#include "iostream" //#include "Eigen/Dense.h" #include "Eigen/Core.h" #include "Eigen/Geometry.h" @@ -15,11 +16,50 @@ uint32_t counter; -Matrix<float, 3, 3> A; -Matrix<float, 3, 1> b; -DiagonalMatrix<float, 3, 3> I; +Matrix<float, 8, 8> A; +Matrix<float, 8, 1> b; +// Matrix<float, 8, 8> I; + +/******************************************************************************/ + + void reset(); + float get_est_state(uint8_t i); + void update(float gyro_x, float gyro_y, float accel_x, float accel_y); + + float s1; + float c1; + float s2; + float c2; + + float g; + float kv; + float Ts; + float rho; + Matrix <float, 2, 1> var_gy; + Matrix <float, 6, 1> var_fx; -// Matrix<float,4,1> mat = state.block(6,0,4,1); + 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, 2, 2> R; + Matrix <float, 6, 6> P; + Matrix <float, 6, 2> K; + Matrix <float, 6, 6> I; + + void update_angles(); + void update_F(); + void update_H(); + void initialize_R(); + void initialize_Q(); + void update_Q(); + + Matrix <float, 6, 1> fxd(); + Matrix <float, 2, 1> gy(); + +/******************************************************************************/ int main() { @@ -29,28 +69,70 @@ counter = 0; - A << 1, 2, 3, - 4, 5, 6, - 7, 8, 10; + /* + A << 1, 2, 3, 5, 1, 8,10, 1, 3, + 4, 5, 6, 8, 4, 2, 1, 9, 4, + 7, 8,10, 5, 6, 8, 4, 5, 1, + 4, 2,10, 8,10, 5, 6, 7, 8, + 1, 8, 7, 3, 4, 6, 5, 1, 7, + 4, 2, 7, 5, 7, 6, 9, 2, 1, + 5, 5, 1, 7, 4, 2, 1, 1, 9, + 8, 9, 7, 4, 5, 6, 1, 2, 2, + 1, 5, 9, 4, 8, 7, 2, 6, 3; b << 3, 3, - 4; + 4, + 2, + 7, + 5, + 1, + 8, + 1; + */ + + A << 1, 2, 3, 5, 1, 8,10, 1, + 4, 5, 6, 8, 4, 2, 1, 9, + 7, 8,10, 5, 6, 8, 4, 5, + 4, 2,10, 8,10, 5, 6, 7, + 1, 8, 7, 3, 4, 6, 5, 1, + 4, 2, 7, 5, 7, 6, 9, 2, + 5, 5, 1, 7, 4, 2, 1, 1, + 8, 9, 7, 4, 5, 6, 1, 2; + b << 3, + 3, + 4, + 2, + 7, + 5, + 1, + 8; // I.setIdentity(); - I.diagonal()[0] = 1; - I.diagonal()[1] = 1; - I.diagonal()[2] = 1; - +/******************************************************************************/ + + // this->Ts = Ts; + Ts = 0.05; + /* [n_gyro; n_b_g; n_v] */ + var_fx << 1000, 1000, 20, 20, 10, 10; + /* [n_acc] */ + var_gy << 40, 40; + rho = 5000; + kv = 0.5; /* k1/m */ + g = 9.81; + reset(); + +/******************************************************************************/ + while(1) { - Vector3f x = I * A.inverse() * b; + Matrix<float, 8, 1> x = A.inverse() * b; // ekf_rp.update(0.0f, 0.0f, 0.0f, 0.0f); dt = timer.read(); timer.reset(); - pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), 1.0f/dt); + pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), dt); pc.printf("%i; %.6f; %.6f; %.6f;\r\n", counter, x(0), x(1), x(2)); counter++; @@ -59,3 +141,108 @@ } } +void reset() +{ + u.setZero(); + y.setZero(); + x.setZero(); + update_F(); + update_H(); + initialize_Q(); + initialize_R(); + P = Q; + K.setZero(); + I.setIdentity(); +} + +void update(float gyro_x, float gyro_y, float accel_x, float accel_y) +{ + u << gyro_x, gyro_y; + y << accel_x, accel_y; + + update_F(); + // update_H(); /* H remains constant */ + update_Q(); + + x = fxd(); + 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; +} + +float get_est_state(uint8_t i) +{ + // x = [ang; b_g; v] + return x(i); +} + +void update_angles() +{ + s1 = sin(x(0)); + c1 = cos(x(0)); + s2 = sin(x(1)); + c2 = cos(x(1)); +} + +void update_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; +} + +void update_H() +{ + H << 0, 0, 0, 0, -kv, 0, + 0, 0, 0, 0, 0, -kv; +} + +void initialize_R() +{ + R << rho*var_gy(0)/Ts, 0, + 0, rho*var_gy(1)/Ts; +} + +void 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); +} + +void update_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> fxd() +{ + Matrix <float, 6, 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); + return retval; +} + +Matrix <float, 2, 1> gy() +{ + Matrix <float, 2, 1> retval; + retval << -kv*x(4), + -kv*x(5); + return retval; +}