![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
EKF_RP.cpp@0:a0e9705be9c4, 2019-10-18 (annotated)
- Committer:
- pmic
- Date:
- Fri Oct 18 14:55:53 2019 +0000
- Revision:
- 0:a0e9705be9c4
- Child:
- 1:6b803652d032
First commit. Nothing is working.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pmic | 0:a0e9705be9c4 | 1 | #include "EKF_RP.h" |
pmic | 0:a0e9705be9c4 | 2 | |
pmic | 0:a0e9705be9c4 | 3 | using namespace std; |
pmic | 0:a0e9705be9c4 | 4 | using namespace Eigen; |
pmic | 0:a0e9705be9c4 | 5 | |
pmic | 0:a0e9705be9c4 | 6 | EKF_RP::EKF_RP(float Ts) |
pmic | 0:a0e9705be9c4 | 7 | { |
pmic | 0:a0e9705be9c4 | 8 | this->Ts = Ts; |
pmic | 0:a0e9705be9c4 | 9 | /* [n_gyro; n_b_g; n_v] */ |
pmic | 0:a0e9705be9c4 | 10 | var_fx << 1000, 1000, 20, 20, 10, 10; |
pmic | 0:a0e9705be9c4 | 11 | /* [n_acc] */ |
pmic | 0:a0e9705be9c4 | 12 | var_gy << 40, 40; |
pmic | 0:a0e9705be9c4 | 13 | rho = 5000; |
pmic | 0:a0e9705be9c4 | 14 | kv = 0.5; /* k1/m */ |
pmic | 0:a0e9705be9c4 | 15 | g = 9.81; |
pmic | 0:a0e9705be9c4 | 16 | reset(); |
pmic | 0:a0e9705be9c4 | 17 | } |
pmic | 0:a0e9705be9c4 | 18 | |
pmic | 0:a0e9705be9c4 | 19 | EKF_RP::~EKF_RP() {} |
pmic | 0:a0e9705be9c4 | 20 | |
pmic | 0:a0e9705be9c4 | 21 | void EKF_RP::reset() |
pmic | 0:a0e9705be9c4 | 22 | { |
pmic | 0:a0e9705be9c4 | 23 | u.setZero(); |
pmic | 0:a0e9705be9c4 | 24 | y.setZero(); |
pmic | 0:a0e9705be9c4 | 25 | x.setZero(); |
pmic | 0:a0e9705be9c4 | 26 | update_F(); |
pmic | 0:a0e9705be9c4 | 27 | update_H(); |
pmic | 0:a0e9705be9c4 | 28 | initialize_Q(); |
pmic | 0:a0e9705be9c4 | 29 | initialize_R(); |
pmic | 0:a0e9705be9c4 | 30 | P = Q; |
pmic | 0:a0e9705be9c4 | 31 | K.setZero(); |
pmic | 0:a0e9705be9c4 | 32 | I.setIdentity(); |
pmic | 0:a0e9705be9c4 | 33 | } |
pmic | 0:a0e9705be9c4 | 34 | |
pmic | 0:a0e9705be9c4 | 35 | void EKF_RP::update(float gyro_x, float gyro_y, float accel_x, float accel_y) |
pmic | 0:a0e9705be9c4 | 36 | { |
pmic | 0:a0e9705be9c4 | 37 | // u << gyro_x, gyro_y; |
pmic | 0:a0e9705be9c4 | 38 | // y << accel_x, accel_y; |
pmic | 0:a0e9705be9c4 | 39 | |
pmic | 0:a0e9705be9c4 | 40 | // update_F(); |
pmic | 0:a0e9705be9c4 | 41 | // update_H(); /* H remains constant */ |
pmic | 0:a0e9705be9c4 | 42 | // update_Q(); |
pmic | 0:a0e9705be9c4 | 43 | |
pmic | 0:a0e9705be9c4 | 44 | // x = fxd(); |
pmic | 0:a0e9705be9c4 | 45 | // P = F * P * F.transpose() + Q; |
pmic | 0:a0e9705be9c4 | 46 | |
pmic | 0:a0e9705be9c4 | 47 | // K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse(); |
pmic | 0:a0e9705be9c4 | 48 | |
pmic | 0:a0e9705be9c4 | 49 | // x = x + K * (y - gy()); |
pmic | 0:a0e9705be9c4 | 50 | // P = (I - K * H) * P; |
pmic | 0:a0e9705be9c4 | 51 | } |
pmic | 0:a0e9705be9c4 | 52 | |
pmic | 0:a0e9705be9c4 | 53 | float EKF_RP::get_est_state(uint8_t i) |
pmic | 0:a0e9705be9c4 | 54 | { |
pmic | 0:a0e9705be9c4 | 55 | /* x = [ang; b_g; v] */ |
pmic | 0:a0e9705be9c4 | 56 | return x(i); |
pmic | 0:a0e9705be9c4 | 57 | } |
pmic | 0:a0e9705be9c4 | 58 | |
pmic | 0:a0e9705be9c4 | 59 | void EKF_RP::update_angles() |
pmic | 0:a0e9705be9c4 | 60 | { |
pmic | 0:a0e9705be9c4 | 61 | s1 = sinf(x(0)); |
pmic | 0:a0e9705be9c4 | 62 | c1 = cosf(x(0)); |
pmic | 0:a0e9705be9c4 | 63 | s2 = sinf(x(1)); |
pmic | 0:a0e9705be9c4 | 64 | c2 = cosf(x(1)); |
pmic | 0:a0e9705be9c4 | 65 | } |
pmic | 0:a0e9705be9c4 | 66 | |
pmic | 0:a0e9705be9c4 | 67 | void EKF_RP::update_F() |
pmic | 0:a0e9705be9c4 | 68 | { |
pmic | 0:a0e9705be9c4 | 69 | 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, |
pmic | 0:a0e9705be9c4 | 70 | -Ts*s1*(u(1) - x(3)), 1, 0, -Ts*c1, 0, 0, |
pmic | 0:a0e9705be9c4 | 71 | 0, 0, 1, 0, 0, 0, |
pmic | 0:a0e9705be9c4 | 72 | 0, 0, 0, 1, 0, 0, |
pmic | 0:a0e9705be9c4 | 73 | 0, Ts*c2*g, 0, 0, 1 - Ts*kv, 0, |
pmic | 0:a0e9705be9c4 | 74 | -Ts*c1*c2*g, Ts*g*s1*s2, 0, 0, 0, 1 - Ts*kv; |
pmic | 0:a0e9705be9c4 | 75 | } |
pmic | 0:a0e9705be9c4 | 76 | |
pmic | 0:a0e9705be9c4 | 77 | void EKF_RP::update_H() |
pmic | 0:a0e9705be9c4 | 78 | { |
pmic | 0:a0e9705be9c4 | 79 | H << 0, 0, 0, 0, -kv, 0, |
pmic | 0:a0e9705be9c4 | 80 | 0, 0, 0, 0, 0, -kv; |
pmic | 0:a0e9705be9c4 | 81 | } |
pmic | 0:a0e9705be9c4 | 82 | |
pmic | 0:a0e9705be9c4 | 83 | void EKF_RP::initialize_R() |
pmic | 0:a0e9705be9c4 | 84 | { |
pmic | 0:a0e9705be9c4 | 85 | R << rho*var_gy(0)/Ts, 0, |
pmic | 0:a0e9705be9c4 | 86 | 0, rho*var_gy(1)/Ts; |
pmic | 0:a0e9705be9c4 | 87 | } |
pmic | 0:a0e9705be9c4 | 88 | |
pmic | 0:a0e9705be9c4 | 89 | void EKF_RP::initialize_Q() |
pmic | 0:a0e9705be9c4 | 90 | { |
pmic | 0:a0e9705be9c4 | 91 | 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, |
pmic | 0:a0e9705be9c4 | 92 | (Ts*c1*s1*s2*var_fx(1))/c2, Ts*c1*c1*var_fx(1), 0, 0, 0, 0, |
pmic | 0:a0e9705be9c4 | 93 | 0, 0, Ts*var_fx(3), 0, 0, 0, |
pmic | 0:a0e9705be9c4 | 94 | 0, 0, 0, Ts*var_fx(4), 0, 0, |
pmic | 0:a0e9705be9c4 | 95 | 0, 0, 0, 0, Ts*var_fx(5), 0, |
pmic | 0:a0e9705be9c4 | 96 | 0, 0, 0, 0, 0, Ts*var_fx(6); |
pmic | 0:a0e9705be9c4 | 97 | } |
pmic | 0:a0e9705be9c4 | 98 | |
pmic | 0:a0e9705be9c4 | 99 | void EKF_RP::update_Q() |
pmic | 0:a0e9705be9c4 | 100 | { |
pmic | 0:a0e9705be9c4 | 101 | Q(0,0) = Ts*(var_fx(0) + (s1*s1*s2*s2*var_fx(1))/(c2*c2)); |
pmic | 0:a0e9705be9c4 | 102 | Q(0,1) = Ts*c1*s1*s2*var_fx(1)/c2; |
pmic | 0:a0e9705be9c4 | 103 | Q(1,0) = Q(0,1); |
pmic | 0:a0e9705be9c4 | 104 | Q(1,1) = Ts*c1*c1*var_fx(1); |
pmic | 0:a0e9705be9c4 | 105 | } |
pmic | 0:a0e9705be9c4 | 106 | |
pmic | 0:a0e9705be9c4 | 107 | Matrix <float, 6, 1> EKF_RP::fxd() |
pmic | 0:a0e9705be9c4 | 108 | { |
pmic | 0:a0e9705be9c4 | 109 | Matrix <float, 6, 1> retval; |
pmic | 0:a0e9705be9c4 | 110 | retval << x(0) + Ts*(u(0) - x(2) + (s1*s2*(u(1) - x(3)))/c2), |
pmic | 0:a0e9705be9c4 | 111 | x(1) + Ts*c1*(u(1) - x(3)), |
pmic | 0:a0e9705be9c4 | 112 | x(2), |
pmic | 0:a0e9705be9c4 | 113 | x(3), |
pmic | 0:a0e9705be9c4 | 114 | x(4) + Ts*(g*s2 - kv*x(4)), |
pmic | 0:a0e9705be9c4 | 115 | x(5) - Ts*(kv*x(5) + c2*g*s1); |
pmic | 0:a0e9705be9c4 | 116 | return retval; |
pmic | 0:a0e9705be9c4 | 117 | } |
pmic | 0:a0e9705be9c4 | 118 | |
pmic | 0:a0e9705be9c4 | 119 | Matrix <float, 2, 1> EKF_RP::gy() |
pmic | 0:a0e9705be9c4 | 120 | { |
pmic | 0:a0e9705be9c4 | 121 | Matrix <float, 2, 1> retval; |
pmic | 0:a0e9705be9c4 | 122 | retval << -kv*x(4), |
pmic | 0:a0e9705be9c4 | 123 | -kv*x(5); |
pmic | 0:a0e9705be9c4 | 124 | return retval; |
pmic | 0:a0e9705be9c4 | 125 | } |
pmic | 0:a0e9705be9c4 | 126 |