AHRS
Dependencies: Eigen
EKF_RP.cpp@30:9b0cd3caf0ec, 2020-01-27 (annotated)
- Committer:
- pmic
- Date:
- Mon Jan 27 10:54:13 2020 +0000
- Revision:
- 30:9b0cd3caf0ec
- Parent:
- 29:cd963a6d31c5
Correct magnetometer.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 21:31e01d3e0143 | 1 | #include "EKF_RP.h" |
altb2 | 21:31e01d3e0143 | 2 | |
altb2 | 21:31e01d3e0143 | 3 | using namespace std; |
altb2 | 21:31e01d3e0143 | 4 | using namespace Eigen; |
altb2 | 21:31e01d3e0143 | 5 | |
altb2 | 21:31e01d3e0143 | 6 | EKF_RP::EKF_RP(float Ts) |
altb2 | 21:31e01d3e0143 | 7 | { |
altb2 | 21:31e01d3e0143 | 8 | this->Ts = Ts; |
altb2 | 21:31e01d3e0143 | 9 | /* [n_gyro; n_b_g; n_v] */ |
altb2 | 22:495a419e474c | 10 | var_fx << 0.1f, 0.1f, 0.002f, 0.002f, 0.002f, 0.002f; |
altb2 | 21:31e01d3e0143 | 11 | /* [n_acc] */ |
altb2 | 22:495a419e474c | 12 | var_gy << 40.0f, 40.0f; |
altb2 | 22:495a419e474c | 13 | rho = 1.0f; |
pmic | 29:cd963a6d31c5 | 14 | // kv = 0.5f; /* QK3 k1/m */ |
pmic | 29:cd963a6d31c5 | 15 | kv = 0.26f; /* QK4 k1/m */ |
altb2 | 21:31e01d3e0143 | 16 | g = 9.81f; |
altb2 | 23:71996bfe68eb | 17 | scale_P0 = 1000.0f; |
altb2 | 21:31e01d3e0143 | 18 | reset(); |
altb2 | 21:31e01d3e0143 | 19 | } |
altb2 | 21:31e01d3e0143 | 20 | |
altb2 | 21:31e01d3e0143 | 21 | EKF_RP::~EKF_RP() {} |
altb2 | 21:31e01d3e0143 | 22 | |
altb2 | 21:31e01d3e0143 | 23 | void EKF_RP::reset() |
altb2 | 21:31e01d3e0143 | 24 | { |
altb2 | 21:31e01d3e0143 | 25 | u.setZero(); |
altb2 | 21:31e01d3e0143 | 26 | y.setZero(); |
altb2 | 21:31e01d3e0143 | 27 | x.setZero(); |
altb2 | 21:31e01d3e0143 | 28 | update_angles(); |
altb2 | 21:31e01d3e0143 | 29 | calc_F(); |
altb2 | 21:31e01d3e0143 | 30 | calc_H(); |
altb2 | 21:31e01d3e0143 | 31 | initialize_Q(); |
altb2 | 21:31e01d3e0143 | 32 | initialize_R(); |
altb2 | 21:31e01d3e0143 | 33 | K.setZero(); |
altb2 | 21:31e01d3e0143 | 34 | I.setIdentity(); |
altb2 | 22:495a419e474c | 35 | e.setZero(); |
altb2 | 23:71996bfe68eb | 36 | P = scale_P0 * I; |
altb2 | 23:71996bfe68eb | 37 | } |
altb2 | 23:71996bfe68eb | 38 | |
altb2 | 23:71996bfe68eb | 39 | void EKF_RP::increase_diag_P() |
altb2 | 23:71996bfe68eb | 40 | { |
altb2 | 23:71996bfe68eb | 41 | P = P + scale_P0 * I; |
altb2 | 21:31e01d3e0143 | 42 | } |
altb2 | 21:31e01d3e0143 | 43 | |
altb2 | 21:31e01d3e0143 | 44 | float EKF_RP::get_est_state(uint8_t i) |
altb2 | 21:31e01d3e0143 | 45 | { |
altb2 | 21:31e01d3e0143 | 46 | /* x = [ang; v; b_g] = [0: phi |
altb2 | 21:31e01d3e0143 | 47 | 1: theta |
altb2 | 21:31e01d3e0143 | 48 | 2: vx |
altb2 | 21:31e01d3e0143 | 49 | 3: vy |
altb2 | 21:31e01d3e0143 | 50 | 4: b_gx |
altb2 | 21:31e01d3e0143 | 51 | 5: b_gy] */ |
altb2 | 21:31e01d3e0143 | 52 | return x(i); |
altb2 | 21:31e01d3e0143 | 53 | } |
altb2 | 21:31e01d3e0143 | 54 | |
altb2 | 21:31e01d3e0143 | 55 | void EKF_RP::update(float gyro_x, float gyro_y, float accel_x, float accel_y) |
altb2 | 21:31e01d3e0143 | 56 | { |
altb2 | 21:31e01d3e0143 | 57 | u << gyro_x, gyro_y; |
altb2 | 21:31e01d3e0143 | 58 | y << accel_x, accel_y; |
altb2 | 21:31e01d3e0143 | 59 | update_angles(); |
altb2 | 21:31e01d3e0143 | 60 | |
altb2 | 21:31e01d3e0143 | 61 | calc_F(); |
altb2 | 21:31e01d3e0143 | 62 | // calc_H(); /* H remains constant */ |
altb2 | 21:31e01d3e0143 | 63 | calc_Q(); |
altb2 | 21:31e01d3e0143 | 64 | |
altb2 | 21:31e01d3e0143 | 65 | x = fxd(); |
altb2 | 21:31e01d3e0143 | 66 | P = F * P * F.transpose() + Q; |
altb2 | 22:495a419e474c | 67 | e = y - gy(); |
altb2 | 21:31e01d3e0143 | 68 | |
altb2 | 22:495a419e474c | 69 | /* inversion faster 184 mus < 207 mus recursion */ |
altb2 | 21:31e01d3e0143 | 70 | K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse(); |
altb2 | 22:495a419e474c | 71 | x = x + K * e; |
altb2 | 22:495a419e474c | 72 | P = (I - K * H) * P; |
altb2 | 21:31e01d3e0143 | 73 | |
altb2 | 22:495a419e474c | 74 | /* only valid if R is diagonal (uncorrelated noise) */ |
altb2 | 22:495a419e474c | 75 | /* |
altb2 | 22:495a419e474c | 76 | for(uint8_t i = 0; i < 2; i++) { |
altb2 | 22:495a419e474c | 77 | K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) ); |
altb2 | 22:495a419e474c | 78 | x = x + K.col(i) * e(i); |
altb2 | 22:495a419e474c | 79 | P = (I - K.col(i)*H.row(i)) * P; |
altb2 | 22:495a419e474c | 80 | } |
altb2 | 22:495a419e474c | 81 | */ |
altb2 | 21:31e01d3e0143 | 82 | } |
altb2 | 21:31e01d3e0143 | 83 | |
altb2 | 21:31e01d3e0143 | 84 | void EKF_RP::update_angles() |
altb2 | 21:31e01d3e0143 | 85 | { |
altb2 | 21:31e01d3e0143 | 86 | s1 = sinf(x(0)); |
altb2 | 21:31e01d3e0143 | 87 | c1 = cosf(x(0)); |
altb2 | 21:31e01d3e0143 | 88 | s2 = sinf(x(1)); |
altb2 | 21:31e01d3e0143 | 89 | c2 = cosf(x(1)); |
altb2 | 21:31e01d3e0143 | 90 | } |
altb2 | 21:31e01d3e0143 | 91 | |
altb2 | 21:31e01d3e0143 | 92 | void EKF_RP::calc_F() |
altb2 | 21:31e01d3e0143 | 93 | { |
altb2 | 21:31e01d3e0143 | 94 | 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, |
altb2 | 21:31e01d3e0143 | 95 | -Ts*s1*(u(1) - x(3)), 1.0f, 0.0f, -Ts*c1, 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 96 | 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 97 | 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 98 | 0.0f, Ts*c2*g, 0.0f, 0.0f, 1.0f - Ts*kv, 0.0f, |
altb2 | 21:31e01d3e0143 | 99 | -Ts*c1*c2*g, Ts*g*s1*s2, 0.0f, 0.0f, 0.0f, 1.0f - Ts*kv; |
altb2 | 21:31e01d3e0143 | 100 | } |
altb2 | 21:31e01d3e0143 | 101 | |
altb2 | 21:31e01d3e0143 | 102 | void EKF_RP::calc_H() |
altb2 | 21:31e01d3e0143 | 103 | { |
altb2 | 21:31e01d3e0143 | 104 | H << 0.0f, 0.0f, 0.0f, 0.0f, -kv, 0.0f, |
altb2 | 21:31e01d3e0143 | 105 | 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -kv; |
altb2 | 21:31e01d3e0143 | 106 | } |
altb2 | 21:31e01d3e0143 | 107 | |
altb2 | 21:31e01d3e0143 | 108 | void EKF_RP::initialize_R() |
altb2 | 21:31e01d3e0143 | 109 | { |
altb2 | 21:31e01d3e0143 | 110 | R << rho*var_gy(0)/Ts, 0.0f, |
altb2 | 21:31e01d3e0143 | 111 | 0.0f, rho*var_gy(1)/Ts; |
altb2 | 21:31e01d3e0143 | 112 | } |
altb2 | 21:31e01d3e0143 | 113 | |
altb2 | 21:31e01d3e0143 | 114 | void EKF_RP::initialize_Q() |
altb2 | 21:31e01d3e0143 | 115 | { |
altb2 | 21:31e01d3e0143 | 116 | 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, |
altb2 | 21:31e01d3e0143 | 117 | Ts*c1*s1*s2*var_fx(1)/c2, Ts*c1*c1*var_fx(1), 0.0f, 0.0f, 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 118 | 0.0f, 0.0f, Ts*var_fx(2), 0.0f, 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 119 | 0.0f, 0.0f, 0.0f, Ts*var_fx(3), 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 120 | 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(4), 0.0f, |
altb2 | 21:31e01d3e0143 | 121 | 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(5); |
altb2 | 21:31e01d3e0143 | 122 | } |
altb2 | 21:31e01d3e0143 | 123 | |
altb2 | 21:31e01d3e0143 | 124 | void EKF_RP::calc_Q() |
altb2 | 21:31e01d3e0143 | 125 | { |
altb2 | 21:31e01d3e0143 | 126 | Q(0,0) = Ts*(var_fx(0) + s1*s1*s2*s2*var_fx(1)/(c2*c2)); |
altb2 | 21:31e01d3e0143 | 127 | Q(0,1) = Ts*c1*s1*s2*var_fx(1)/c2; |
altb2 | 21:31e01d3e0143 | 128 | Q(1,0) = Q(0,1); |
altb2 | 21:31e01d3e0143 | 129 | Q(1,1) = Ts*c1*c1*var_fx(1); |
altb2 | 21:31e01d3e0143 | 130 | } |
altb2 | 21:31e01d3e0143 | 131 | |
altb2 | 21:31e01d3e0143 | 132 | Matrix <float, 6, 1> EKF_RP::fxd() |
altb2 | 21:31e01d3e0143 | 133 | { |
altb2 | 21:31e01d3e0143 | 134 | Matrix <float, 6, 1> retval; |
altb2 | 21:31e01d3e0143 | 135 | retval << x(0) + Ts*(u(0) - x(2) + (s1*s2*(u(1) - x(3)))/c2), |
altb2 | 21:31e01d3e0143 | 136 | x(1) + Ts*c1*(u(1) - x(3)), |
altb2 | 21:31e01d3e0143 | 137 | x(2), |
altb2 | 21:31e01d3e0143 | 138 | x(3), |
altb2 | 21:31e01d3e0143 | 139 | x(4) + Ts*(g*s2 - kv*x(4)), |
altb2 | 21:31e01d3e0143 | 140 | x(5) - Ts*(kv*x(5) + c2*g*s1); |
altb2 | 21:31e01d3e0143 | 141 | return retval; |
altb2 | 21:31e01d3e0143 | 142 | } |
altb2 | 21:31e01d3e0143 | 143 | |
altb2 | 21:31e01d3e0143 | 144 | Matrix <float, 2, 1> EKF_RP::gy() |
altb2 | 21:31e01d3e0143 | 145 | { |
altb2 | 21:31e01d3e0143 | 146 | Matrix <float, 2, 1> retval; |
altb2 | 21:31e01d3e0143 | 147 | retval << -kv*x(4), |
altb2 | 21:31e01d3e0143 | 148 | -kv*x(5); |
altb2 | 21:31e01d3e0143 | 149 | return retval; |
altb2 | 21:31e01d3e0143 | 150 | } |
altb2 | 21:31e01d3e0143 | 151 |