AHRS
Dependencies: Eigen
EKF_RPY.cpp@23:71996bfe68eb, 2019-11-13 (annotated)
- Committer:
- altb2
- Date:
- Wed Nov 13 11:40:46 2019 +0000
- Revision:
- 23:71996bfe68eb
- Parent:
- 22:495a419e474c
- Child:
- 24:7fe0935b3bd2
- Child:
- 29:cd963a6d31c5
Commit before fligth experiments Deutweg Nr.2
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 21:31e01d3e0143 | 1 | #include "EKF_RPY.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_RPY::EKF_RPY(float Ts, float mx0, float my0, float mz0) |
altb2 | 21:31e01d3e0143 | 7 | { |
altb2 | 21:31e01d3e0143 | 8 | this->Ts = Ts; |
altb2 | 22:495a419e474c | 9 | set_para(); |
altb2 | 21:31e01d3e0143 | 10 | m0 << mx0, my0, mz0; |
altb2 | 21:31e01d3e0143 | 11 | reset(); |
altb2 | 21:31e01d3e0143 | 12 | } |
altb2 | 22:495a419e474c | 13 | |
altb2 | 22:495a419e474c | 14 | EKF_RPY::EKF_RPY(float Ts) |
altb2 | 21:31e01d3e0143 | 15 | { |
altb2 | 21:31e01d3e0143 | 16 | this->Ts = Ts; |
altb2 | 22:495a419e474c | 17 | set_para(); |
altb2 | 22:495a419e474c | 18 | reset(); |
altb2 | 22:495a419e474c | 19 | } |
altb2 | 22:495a419e474c | 20 | |
altb2 | 22:495a419e474c | 21 | EKF_RPY::~EKF_RPY() {} |
altb2 | 22:495a419e474c | 22 | |
altb2 | 22:495a419e474c | 23 | void EKF_RPY::set_para() |
altb2 | 22:495a419e474c | 24 | { |
altb2 | 21:31e01d3e0143 | 25 | /* [n_gyro; n_v; n_b_g] */ |
altb2 | 22:495a419e474c | 26 | var_fx << 0.1f, 0.1f, 0.1f, 0.002f, 0.002f, 0.002f, 0.002f, 0.002f; |
altb2 | 21:31e01d3e0143 | 27 | /* [n_acc; n_mag] */ |
altb2 | 22:495a419e474c | 28 | var_gy << 40.0f, 40.0f, 1.0f, 1.0f; |
altb2 | 22:495a419e474c | 29 | rho = 1.0f; |
altb2 | 21:31e01d3e0143 | 30 | kv = 0.5f; /* k1/m */ |
altb2 | 21:31e01d3e0143 | 31 | g = 9.81f; |
altb2 | 23:71996bfe68eb | 32 | scale_P0 = 1000.0f; |
altb2 | 21:31e01d3e0143 | 33 | } |
altb2 | 21:31e01d3e0143 | 34 | |
altb2 | 21:31e01d3e0143 | 35 | void EKF_RPY::reset() |
altb2 | 21:31e01d3e0143 | 36 | { |
altb2 | 21:31e01d3e0143 | 37 | u.setZero(); |
altb2 | 21:31e01d3e0143 | 38 | y.setZero(); |
altb2 | 21:31e01d3e0143 | 39 | x.setZero(); |
altb2 | 21:31e01d3e0143 | 40 | update_angles(); |
altb2 | 21:31e01d3e0143 | 41 | calc_F(); |
altb2 | 21:31e01d3e0143 | 42 | calc_H(); |
altb2 | 21:31e01d3e0143 | 43 | initialize_Q(); |
altb2 | 21:31e01d3e0143 | 44 | initialize_R(); |
altb2 | 21:31e01d3e0143 | 45 | K.setZero(); |
altb2 | 21:31e01d3e0143 | 46 | I.setIdentity(); |
altb2 | 22:495a419e474c | 47 | e.setZero(); |
altb2 | 23:71996bfe68eb | 48 | P = scale_P0 * I; |
altb2 | 23:71996bfe68eb | 49 | } |
altb2 | 23:71996bfe68eb | 50 | |
altb2 | 23:71996bfe68eb | 51 | void EKF_RPY::increase_diag_P() |
altb2 | 23:71996bfe68eb | 52 | { |
altb2 | 23:71996bfe68eb | 53 | P = P + scale_P0 * I; |
altb2 | 22:495a419e474c | 54 | } |
altb2 | 22:495a419e474c | 55 | |
altb2 | 22:495a419e474c | 56 | void EKF_RPY::set_m0(float mx0, float my0, float mz0) |
altb2 | 22:495a419e474c | 57 | { |
altb2 | 22:495a419e474c | 58 | m0 << mx0, my0, mz0; |
altb2 | 21:31e01d3e0143 | 59 | } |
altb2 | 21:31e01d3e0143 | 60 | |
altb2 | 21:31e01d3e0143 | 61 | float EKF_RPY::get_est_state(uint8_t i) |
altb2 | 21:31e01d3e0143 | 62 | { |
altb2 | 21:31e01d3e0143 | 63 | /* x = [ang; v; b_g] = [0: phi |
altb2 | 21:31e01d3e0143 | 64 | 1: theta |
altb2 | 21:31e01d3e0143 | 65 | 2: psi |
altb2 | 21:31e01d3e0143 | 66 | 3: b_gx |
altb2 | 21:31e01d3e0143 | 67 | 4: b_gy |
altb2 | 21:31e01d3e0143 | 68 | 5: b_gz |
altb2 | 21:31e01d3e0143 | 69 | 6: vx |
altb2 | 21:31e01d3e0143 | 70 | 7: vy] */ |
altb2 | 21:31e01d3e0143 | 71 | return x(i); |
altb2 | 21:31e01d3e0143 | 72 | } |
altb2 | 21:31e01d3e0143 | 73 | |
altb2 | 21:31e01d3e0143 | 74 | void EKF_RPY::update(float gyro_x, float gyro_y, float gyro_z, float accel_x, float accel_y, float magnet_x, float magnet_y) |
altb2 | 21:31e01d3e0143 | 75 | { |
altb2 | 21:31e01d3e0143 | 76 | u << gyro_x, gyro_y, gyro_z; |
altb2 | 21:31e01d3e0143 | 77 | y << accel_x, accel_y, magnet_x, magnet_y; |
altb2 | 21:31e01d3e0143 | 78 | update_angles(); |
altb2 | 21:31e01d3e0143 | 79 | |
altb2 | 21:31e01d3e0143 | 80 | calc_F(); |
altb2 | 21:31e01d3e0143 | 81 | calc_H(); |
altb2 | 21:31e01d3e0143 | 82 | calc_Q(); |
altb2 | 21:31e01d3e0143 | 83 | |
altb2 | 21:31e01d3e0143 | 84 | x = fxd(); |
altb2 | 21:31e01d3e0143 | 85 | P = F * P * F.transpose() + Q; |
altb2 | 22:495a419e474c | 86 | e = y - gy(); |
altb2 | 21:31e01d3e0143 | 87 | |
altb2 | 22:495a419e474c | 88 | /* inversion faster 356 mus < 432 mus recursion */ |
altb2 | 21:31e01d3e0143 | 89 | K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse(); |
altb2 | 22:495a419e474c | 90 | x = x + K * e; |
altb2 | 22:495a419e474c | 91 | P = (I - K * H) * P; |
altb2 | 21:31e01d3e0143 | 92 | |
altb2 | 22:495a419e474c | 93 | /* only valid if R is diagonal (uncorrelated noise) */ |
altb2 | 22:495a419e474c | 94 | /* |
altb2 | 22:495a419e474c | 95 | for(uint8_t i = 0; i < 4; i++) { |
altb2 | 22:495a419e474c | 96 | K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) ); |
altb2 | 22:495a419e474c | 97 | x = x + K.col(i) * e(i); |
altb2 | 22:495a419e474c | 98 | P = (I - K.col(i)*H.row(i)) * P; |
altb2 | 22:495a419e474c | 99 | } |
altb2 | 22:495a419e474c | 100 | */ |
altb2 | 21:31e01d3e0143 | 101 | } |
altb2 | 21:31e01d3e0143 | 102 | |
altb2 | 21:31e01d3e0143 | 103 | void EKF_RPY::update_angles() |
altb2 | 21:31e01d3e0143 | 104 | { |
altb2 | 21:31e01d3e0143 | 105 | s1 = sinf(x(0)); |
altb2 | 21:31e01d3e0143 | 106 | c1 = cosf(x(0)); |
altb2 | 21:31e01d3e0143 | 107 | s2 = sinf(x(1)); |
altb2 | 21:31e01d3e0143 | 108 | c2 = cosf(x(1)); |
altb2 | 21:31e01d3e0143 | 109 | s3 = sinf(x(2)); |
altb2 | 21:31e01d3e0143 | 110 | c3 = cosf(x(2)); |
altb2 | 21:31e01d3e0143 | 111 | } |
altb2 | 21:31e01d3e0143 | 112 | |
altb2 | 21:31e01d3e0143 | 113 | void EKF_RPY::calc_F() |
altb2 | 21:31e01d3e0143 | 114 | { |
altb2 | 21:31e01d3e0143 | 115 | F << Ts*(c1*s2*(u(1) - x(6)) - s1*s2*(u(2) - x(7)))/c2 + 1.0f, Ts*(c1*(u(2) - x(7)) + s1*(u(1) - x(6)))/(c2*c2), 0.0f, 0.0f, 0.0f, -Ts, -Ts*s1*s2/c2, -Ts*c1*s2/c2, |
altb2 | 21:31e01d3e0143 | 116 | -Ts*(c1*(u(2) - x(7)) + s1*(u(1) - x(6))), 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -Ts*c1, Ts*s1, |
altb2 | 21:31e01d3e0143 | 117 | Ts*(c1*(u(1) - x(6)) - s1*(u(2) - x(7)))/c2, Ts*(c1*(u(2) - x(7)) + s1*(u(1) - x(6)))*s2/(c2*c2), 1.0f, 0.0f, 0.0f, 0.0f, -Ts*s1/c2, -Ts*c1/c2, |
altb2 | 21:31e01d3e0143 | 118 | 0.0f, Ts*c2*g, 0.0f, 1.0f - Ts*kv, Ts*(u(2) - x(7)), 0.0f, 0.0f, -Ts*x(4), |
altb2 | 21:31e01d3e0143 | 119 | -Ts*c1*c2*g, Ts*g*s1*s2, 0.0f, -Ts*(u(2) - x(7)), 1.0f - Ts*kv, 0.0f, 0.0f, Ts*x(3), |
altb2 | 21:31e01d3e0143 | 120 | 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 121 | 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 122 | 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f; |
altb2 | 21:31e01d3e0143 | 123 | } |
altb2 | 21:31e01d3e0143 | 124 | |
altb2 | 21:31e01d3e0143 | 125 | void EKF_RPY::calc_H() |
altb2 | 21:31e01d3e0143 | 126 | { |
altb2 | 21:31e01d3e0143 | 127 | H << 0.0f, 0.0f, 0.0f, -kv, u(2) - x(7), 0.0f, 0.0f, -x(4), |
altb2 | 21:31e01d3e0143 | 128 | 0.0f, 0.0f, 0.0f, x(7) - u(2), -kv, 0.0f, 0.0f, x(3), |
altb2 | 21:31e01d3e0143 | 129 | 0.0f, - c2*m0(2) - c3*m0(0)*s2 - m0(1)*s2*s3, c2*(c3*m0(1) - m0(0)*s3), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 130 | m0(0)*(s1*s3 + c1*c3*s2) - m0(1)*(c3*s1 - c1*s2*s3) + c1*c2*m0(2), s1*(c2*c3*m0(0) - m0(2)*s2 + c2*m0(1)*s3), - m0(0)*(c1*c3 + s1*s2*s3) - m0(1)*(c1*s3 - c3*s1*s2), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f; |
altb2 | 21:31e01d3e0143 | 131 | } |
altb2 | 21:31e01d3e0143 | 132 | |
altb2 | 21:31e01d3e0143 | 133 | void EKF_RPY::initialize_R() |
altb2 | 21:31e01d3e0143 | 134 | { |
altb2 | 21:31e01d3e0143 | 135 | R << rho*var_gy(0)/Ts, 0.0f, 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 136 | 0.0f, rho*var_gy(1)/Ts, 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 137 | 0.0f, 0.0f, rho*var_gy(2)/Ts, 0.0f, |
altb2 | 21:31e01d3e0143 | 138 | 0.0f, 0.0f, 0.0f, rho*var_gy(3)/Ts; |
altb2 | 21:31e01d3e0143 | 139 | } |
altb2 | 21:31e01d3e0143 | 140 | |
altb2 | 21:31e01d3e0143 | 141 | void EKF_RPY::initialize_Q() |
altb2 | 21:31e01d3e0143 | 142 | { |
altb2 | 21:31e01d3e0143 | 143 | Q << Ts*(var_fx(0) + (c1*c1*var_fx(2) + s1*s1*var_fx(1))*s2*s2/(c2*c2)), Ts*(var_fx(1) - var_fx(2))*c1*s1*s2/c2, Ts*(c1*c1*var_fx(2) + s1*s1*var_fx(1))*s2/(c2*c2), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 144 | Ts*(var_fx(1) - var_fx(2))*c1*s1*s2/c2, Ts*(var_fx(1)*c1*c1 + var_fx(2)*s1*s1), Ts*(var_fx(1) - var_fx(2))*c1*s1/c2, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 145 | Ts*(c1*c1*var_fx(2) + s1*s1*var_fx(1))*s2/(c2*c2), Ts*(var_fx(1) - var_fx(2))*c1*s1/c2, Ts*(c1*c1*var_fx(2) + s1*s1*var_fx(1))/(c2*c2), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 146 | 0.0f, 0.0f, 0.0f, Ts*var_fx(3), 0.0f, 0.0f, 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 147 | 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(4), 0.0f, 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 148 | 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(5), 0.0f, 0.0f, |
altb2 | 21:31e01d3e0143 | 149 | 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(6), 0.0f, |
altb2 | 21:31e01d3e0143 | 150 | 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(7); |
altb2 | 21:31e01d3e0143 | 151 | } |
altb2 | 21:31e01d3e0143 | 152 | |
altb2 | 21:31e01d3e0143 | 153 | void EKF_RPY::calc_Q() |
altb2 | 21:31e01d3e0143 | 154 | { |
altb2 | 21:31e01d3e0143 | 155 | Q(0,0) = Ts*(var_fx(0) + (c1*c1*var_fx(2) + s1*s1*var_fx(1))*s2*s2/(c2*c2)); |
altb2 | 21:31e01d3e0143 | 156 | Q(0,1) = Ts*(var_fx(1) - var_fx(2))*c1*s1*s2/c2; |
altb2 | 21:31e01d3e0143 | 157 | Q(0,2) = Ts*(c1*c1*var_fx(2) + s1*s1*var_fx(1))*s2/(c2*c2); |
altb2 | 21:31e01d3e0143 | 158 | Q(1,0) = Q(0,1); |
altb2 | 21:31e01d3e0143 | 159 | Q(1,1) = Ts*(var_fx(1)*c1*c1 + var_fx(2)*s1*s1); |
altb2 | 21:31e01d3e0143 | 160 | Q(1,2) = Ts*(var_fx(1) - var_fx(2))*c1*s1/c2; |
altb2 | 21:31e01d3e0143 | 161 | Q(2,0) = Q(0,2); |
altb2 | 21:31e01d3e0143 | 162 | Q(2,1) = Q(1,2); |
altb2 | 21:31e01d3e0143 | 163 | Q(2,2) = Ts*(c1*c1*var_fx(2) + s1*s1*var_fx(1))/(c2*c2); |
altb2 | 21:31e01d3e0143 | 164 | } |
altb2 | 21:31e01d3e0143 | 165 | |
altb2 | 21:31e01d3e0143 | 166 | Matrix <float, 8, 1> EKF_RPY::fxd() |
altb2 | 21:31e01d3e0143 | 167 | { |
altb2 | 21:31e01d3e0143 | 168 | Matrix <float, 8, 1> retval; |
altb2 | 21:31e01d3e0143 | 169 | retval << x(0) + Ts*(u(0) - x(5) + (c1*s2*(u(2) - x(7)))/c2 + (s1*s2*(u(1) - x(6)))/c2), |
altb2 | 21:31e01d3e0143 | 170 | x(1) + Ts*(c1*(u(1) - x(6)) - s1*(u(2) - x(7))), |
altb2 | 21:31e01d3e0143 | 171 | x(2) + Ts*((c1*(u(2) - x(7)))/c2 + (s1*(u(1) - x(6)))/c2), |
altb2 | 21:31e01d3e0143 | 172 | x(3) + Ts*(g*s2 - kv*x(3) + x(4)*(u(2) - x(7))), |
altb2 | 21:31e01d3e0143 | 173 | x(4) - Ts*(kv*x(4) + x(3)*(u(2) - x(7)) + c2*g*s1), |
altb2 | 21:31e01d3e0143 | 174 | x(5), |
altb2 | 21:31e01d3e0143 | 175 | x(6), |
altb2 | 21:31e01d3e0143 | 176 | x(7); |
altb2 | 21:31e01d3e0143 | 177 | return retval; |
altb2 | 21:31e01d3e0143 | 178 | } |
altb2 | 21:31e01d3e0143 | 179 | |
altb2 | 21:31e01d3e0143 | 180 | Matrix <float, 4, 1> EKF_RPY::gy() |
altb2 | 21:31e01d3e0143 | 181 | { |
altb2 | 21:31e01d3e0143 | 182 | Matrix <float, 4, 1> retval; |
altb2 | 21:31e01d3e0143 | 183 | retval << x(4)*(u(2) - x(7)) - kv*x(3), |
altb2 | 21:31e01d3e0143 | 184 | - kv*x(4) - x(3)*(u(2) - x(7)), |
altb2 | 21:31e01d3e0143 | 185 | - m0(2)*s2 + c2*c3*m0(0) + c2*m0(1)*s3, |
altb2 | 21:31e01d3e0143 | 186 | - m0(0)*(c1*s3 - c3*s1*s2) + m0(1)*(c1*c3 + s1*s2*s3) + c2*m0(2)*s1; |
altb2 | 21:31e01d3e0143 | 187 | return retval; |
altb2 | 21:31e01d3e0143 | 188 | } |
altb2 | 21:31e01d3e0143 | 189 |