altb_pmic
/
Test_ekf
Testing ekf implementation for Quadro_1.
main.cpp
- Committer:
- pmic
- Date:
- 2019-10-18
- Revision:
- 2:756446014084
- Parent:
- 1:6b803652d032
- Child:
- 3:121647a7cddf
File content as of revision 2:756446014084:
#include "mbed.h" #include "iostream" //#include "Eigen/Dense.h" #include "Eigen/Core.h" #include "Eigen/Geometry.h" #include "EKF_RP.h" using namespace Eigen; Serial pc(SERIAL_TX, SERIAL_RX); EKF_RP ekf_rp(0.02f); // initialize counter on PB_6 and PB_7 Timer timer; // timer for time measurement float dt = 0.0f; uint32_t counter; 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, 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() { pc.baud(2000000); timer.start(); counter = 0; /* 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, 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(); /******************************************************************************/ // 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) { 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), dt); pc.printf("%i; %.6f; %.6f; %.6f;\r\n", counter, x(0), x(1), x(2)); counter++; wait_us(1000000); } } 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; }