altb_pmic
/
Test_ekf
Testing ekf implementation for Quadro_1.
main.cpp@1:6b803652d032, 2019-10-18 (annotated)
- Committer:
- pmic
- Date:
- Fri Oct 18 21:15:52 2019 +0000
- Revision:
- 1:6b803652d032
- Parent:
- 0:a0e9705be9c4
- Child:
- 2:756446014084
Test ekf in main.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pmic | 0:a0e9705be9c4 | 1 | #include "mbed.h" |
pmic | 1:6b803652d032 | 2 | #include "iostream" |
pmic | 0:a0e9705be9c4 | 3 | //#include "Eigen/Dense.h" |
pmic | 0:a0e9705be9c4 | 4 | #include "Eigen/Core.h" |
pmic | 0:a0e9705be9c4 | 5 | #include "Eigen/Geometry.h" |
pmic | 0:a0e9705be9c4 | 6 | #include "EKF_RP.h" |
pmic | 0:a0e9705be9c4 | 7 | |
pmic | 0:a0e9705be9c4 | 8 | using namespace Eigen; |
pmic | 0:a0e9705be9c4 | 9 | |
pmic | 0:a0e9705be9c4 | 10 | Serial pc(SERIAL_TX, SERIAL_RX); |
pmic | 0:a0e9705be9c4 | 11 | |
pmic | 0:a0e9705be9c4 | 12 | // EKF_RP ekf_rp(0.02f); // initialize counter on PB_6 and PB_7 |
pmic | 0:a0e9705be9c4 | 13 | |
pmic | 0:a0e9705be9c4 | 14 | Timer timer; // timer for time measurement |
pmic | 0:a0e9705be9c4 | 15 | float dt = 0.0f; |
pmic | 0:a0e9705be9c4 | 16 | |
pmic | 0:a0e9705be9c4 | 17 | uint32_t counter; |
pmic | 0:a0e9705be9c4 | 18 | |
pmic | 1:6b803652d032 | 19 | Matrix<float, 8, 8> A; |
pmic | 1:6b803652d032 | 20 | Matrix<float, 8, 1> b; |
pmic | 1:6b803652d032 | 21 | // Matrix<float, 8, 8> I; |
pmic | 1:6b803652d032 | 22 | |
pmic | 1:6b803652d032 | 23 | /******************************************************************************/ |
pmic | 1:6b803652d032 | 24 | |
pmic | 1:6b803652d032 | 25 | void reset(); |
pmic | 1:6b803652d032 | 26 | float get_est_state(uint8_t i); |
pmic | 1:6b803652d032 | 27 | void update(float gyro_x, float gyro_y, float accel_x, float accel_y); |
pmic | 1:6b803652d032 | 28 | |
pmic | 1:6b803652d032 | 29 | float s1; |
pmic | 1:6b803652d032 | 30 | float c1; |
pmic | 1:6b803652d032 | 31 | float s2; |
pmic | 1:6b803652d032 | 32 | float c2; |
pmic | 1:6b803652d032 | 33 | |
pmic | 1:6b803652d032 | 34 | float g; |
pmic | 1:6b803652d032 | 35 | float kv; |
pmic | 1:6b803652d032 | 36 | float Ts; |
pmic | 1:6b803652d032 | 37 | float rho; |
pmic | 1:6b803652d032 | 38 | Matrix <float, 2, 1> var_gy; |
pmic | 1:6b803652d032 | 39 | Matrix <float, 6, 1> var_fx; |
pmic | 0:a0e9705be9c4 | 40 | |
pmic | 1:6b803652d032 | 41 | Matrix <float, 2, 1> u; |
pmic | 1:6b803652d032 | 42 | Matrix <float, 2, 1> y; |
pmic | 1:6b803652d032 | 43 | Matrix <float, 6, 1> x; |
pmic | 1:6b803652d032 | 44 | Matrix <float, 6, 6> F; |
pmic | 1:6b803652d032 | 45 | Matrix <float, 2, 6> H; |
pmic | 1:6b803652d032 | 46 | Matrix <float, 6, 6> Q; |
pmic | 1:6b803652d032 | 47 | Matrix <float, 2, 2> R; |
pmic | 1:6b803652d032 | 48 | Matrix <float, 6, 6> P; |
pmic | 1:6b803652d032 | 49 | Matrix <float, 6, 2> K; |
pmic | 1:6b803652d032 | 50 | Matrix <float, 6, 6> I; |
pmic | 1:6b803652d032 | 51 | |
pmic | 1:6b803652d032 | 52 | void update_angles(); |
pmic | 1:6b803652d032 | 53 | void update_F(); |
pmic | 1:6b803652d032 | 54 | void update_H(); |
pmic | 1:6b803652d032 | 55 | void initialize_R(); |
pmic | 1:6b803652d032 | 56 | void initialize_Q(); |
pmic | 1:6b803652d032 | 57 | void update_Q(); |
pmic | 1:6b803652d032 | 58 | |
pmic | 1:6b803652d032 | 59 | Matrix <float, 6, 1> fxd(); |
pmic | 1:6b803652d032 | 60 | Matrix <float, 2, 1> gy(); |
pmic | 1:6b803652d032 | 61 | |
pmic | 1:6b803652d032 | 62 | /******************************************************************************/ |
pmic | 0:a0e9705be9c4 | 63 | |
pmic | 0:a0e9705be9c4 | 64 | int main() |
pmic | 0:a0e9705be9c4 | 65 | { |
pmic | 0:a0e9705be9c4 | 66 | pc.baud(2000000); |
pmic | 0:a0e9705be9c4 | 67 | |
pmic | 0:a0e9705be9c4 | 68 | timer.start(); |
pmic | 0:a0e9705be9c4 | 69 | |
pmic | 0:a0e9705be9c4 | 70 | counter = 0; |
pmic | 0:a0e9705be9c4 | 71 | |
pmic | 1:6b803652d032 | 72 | /* |
pmic | 1:6b803652d032 | 73 | A << 1, 2, 3, 5, 1, 8,10, 1, 3, |
pmic | 1:6b803652d032 | 74 | 4, 5, 6, 8, 4, 2, 1, 9, 4, |
pmic | 1:6b803652d032 | 75 | 7, 8,10, 5, 6, 8, 4, 5, 1, |
pmic | 1:6b803652d032 | 76 | 4, 2,10, 8,10, 5, 6, 7, 8, |
pmic | 1:6b803652d032 | 77 | 1, 8, 7, 3, 4, 6, 5, 1, 7, |
pmic | 1:6b803652d032 | 78 | 4, 2, 7, 5, 7, 6, 9, 2, 1, |
pmic | 1:6b803652d032 | 79 | 5, 5, 1, 7, 4, 2, 1, 1, 9, |
pmic | 1:6b803652d032 | 80 | 8, 9, 7, 4, 5, 6, 1, 2, 2, |
pmic | 1:6b803652d032 | 81 | 1, 5, 9, 4, 8, 7, 2, 6, 3; |
pmic | 0:a0e9705be9c4 | 82 | b << 3, |
pmic | 0:a0e9705be9c4 | 83 | 3, |
pmic | 1:6b803652d032 | 84 | 4, |
pmic | 1:6b803652d032 | 85 | 2, |
pmic | 1:6b803652d032 | 86 | 7, |
pmic | 1:6b803652d032 | 87 | 5, |
pmic | 1:6b803652d032 | 88 | 1, |
pmic | 1:6b803652d032 | 89 | 8, |
pmic | 1:6b803652d032 | 90 | 1; |
pmic | 1:6b803652d032 | 91 | */ |
pmic | 1:6b803652d032 | 92 | |
pmic | 1:6b803652d032 | 93 | A << 1, 2, 3, 5, 1, 8,10, 1, |
pmic | 1:6b803652d032 | 94 | 4, 5, 6, 8, 4, 2, 1, 9, |
pmic | 1:6b803652d032 | 95 | 7, 8,10, 5, 6, 8, 4, 5, |
pmic | 1:6b803652d032 | 96 | 4, 2,10, 8,10, 5, 6, 7, |
pmic | 1:6b803652d032 | 97 | 1, 8, 7, 3, 4, 6, 5, 1, |
pmic | 1:6b803652d032 | 98 | 4, 2, 7, 5, 7, 6, 9, 2, |
pmic | 1:6b803652d032 | 99 | 5, 5, 1, 7, 4, 2, 1, 1, |
pmic | 1:6b803652d032 | 100 | 8, 9, 7, 4, 5, 6, 1, 2; |
pmic | 1:6b803652d032 | 101 | b << 3, |
pmic | 1:6b803652d032 | 102 | 3, |
pmic | 1:6b803652d032 | 103 | 4, |
pmic | 1:6b803652d032 | 104 | 2, |
pmic | 1:6b803652d032 | 105 | 7, |
pmic | 1:6b803652d032 | 106 | 5, |
pmic | 1:6b803652d032 | 107 | 1, |
pmic | 1:6b803652d032 | 108 | 8; |
pmic | 0:a0e9705be9c4 | 109 | |
pmic | 0:a0e9705be9c4 | 110 | // I.setIdentity(); |
pmic | 0:a0e9705be9c4 | 111 | |
pmic | 1:6b803652d032 | 112 | /******************************************************************************/ |
pmic | 1:6b803652d032 | 113 | |
pmic | 1:6b803652d032 | 114 | // this->Ts = Ts; |
pmic | 1:6b803652d032 | 115 | Ts = 0.05; |
pmic | 1:6b803652d032 | 116 | /* [n_gyro; n_b_g; n_v] */ |
pmic | 1:6b803652d032 | 117 | var_fx << 1000, 1000, 20, 20, 10, 10; |
pmic | 1:6b803652d032 | 118 | /* [n_acc] */ |
pmic | 1:6b803652d032 | 119 | var_gy << 40, 40; |
pmic | 1:6b803652d032 | 120 | rho = 5000; |
pmic | 1:6b803652d032 | 121 | kv = 0.5; /* k1/m */ |
pmic | 1:6b803652d032 | 122 | g = 9.81; |
pmic | 1:6b803652d032 | 123 | reset(); |
pmic | 1:6b803652d032 | 124 | |
pmic | 1:6b803652d032 | 125 | /******************************************************************************/ |
pmic | 1:6b803652d032 | 126 | |
pmic | 0:a0e9705be9c4 | 127 | while(1) { |
pmic | 0:a0e9705be9c4 | 128 | |
pmic | 1:6b803652d032 | 129 | Matrix<float, 8, 1> x = A.inverse() * b; |
pmic | 0:a0e9705be9c4 | 130 | |
pmic | 0:a0e9705be9c4 | 131 | // ekf_rp.update(0.0f, 0.0f, 0.0f, 0.0f); |
pmic | 0:a0e9705be9c4 | 132 | dt = timer.read(); |
pmic | 0:a0e9705be9c4 | 133 | timer.reset(); |
pmic | 0:a0e9705be9c4 | 134 | |
pmic | 1:6b803652d032 | 135 | pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), dt); |
pmic | 0:a0e9705be9c4 | 136 | pc.printf("%i; %.6f; %.6f; %.6f;\r\n", counter, x(0), x(1), x(2)); |
pmic | 0:a0e9705be9c4 | 137 | |
pmic | 0:a0e9705be9c4 | 138 | counter++; |
pmic | 0:a0e9705be9c4 | 139 | |
pmic | 0:a0e9705be9c4 | 140 | wait_us(1000000); |
pmic | 0:a0e9705be9c4 | 141 | } |
pmic | 0:a0e9705be9c4 | 142 | } |
pmic | 0:a0e9705be9c4 | 143 | |
pmic | 1:6b803652d032 | 144 | void reset() |
pmic | 1:6b803652d032 | 145 | { |
pmic | 1:6b803652d032 | 146 | u.setZero(); |
pmic | 1:6b803652d032 | 147 | y.setZero(); |
pmic | 1:6b803652d032 | 148 | x.setZero(); |
pmic | 1:6b803652d032 | 149 | update_F(); |
pmic | 1:6b803652d032 | 150 | update_H(); |
pmic | 1:6b803652d032 | 151 | initialize_Q(); |
pmic | 1:6b803652d032 | 152 | initialize_R(); |
pmic | 1:6b803652d032 | 153 | P = Q; |
pmic | 1:6b803652d032 | 154 | K.setZero(); |
pmic | 1:6b803652d032 | 155 | I.setIdentity(); |
pmic | 1:6b803652d032 | 156 | } |
pmic | 1:6b803652d032 | 157 | |
pmic | 1:6b803652d032 | 158 | void update(float gyro_x, float gyro_y, float accel_x, float accel_y) |
pmic | 1:6b803652d032 | 159 | { |
pmic | 1:6b803652d032 | 160 | u << gyro_x, gyro_y; |
pmic | 1:6b803652d032 | 161 | y << accel_x, accel_y; |
pmic | 1:6b803652d032 | 162 | |
pmic | 1:6b803652d032 | 163 | update_F(); |
pmic | 1:6b803652d032 | 164 | // update_H(); /* H remains constant */ |
pmic | 1:6b803652d032 | 165 | update_Q(); |
pmic | 1:6b803652d032 | 166 | |
pmic | 1:6b803652d032 | 167 | x = fxd(); |
pmic | 1:6b803652d032 | 168 | P = F * P * F.transpose() + Q; |
pmic | 1:6b803652d032 | 169 | |
pmic | 1:6b803652d032 | 170 | K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse(); |
pmic | 1:6b803652d032 | 171 | |
pmic | 1:6b803652d032 | 172 | x = x + K * (y - gy()); |
pmic | 1:6b803652d032 | 173 | P = (I - K * H) * P; |
pmic | 1:6b803652d032 | 174 | } |
pmic | 1:6b803652d032 | 175 | |
pmic | 1:6b803652d032 | 176 | float get_est_state(uint8_t i) |
pmic | 1:6b803652d032 | 177 | { |
pmic | 1:6b803652d032 | 178 | // x = [ang; b_g; v] |
pmic | 1:6b803652d032 | 179 | return x(i); |
pmic | 1:6b803652d032 | 180 | } |
pmic | 1:6b803652d032 | 181 | |
pmic | 1:6b803652d032 | 182 | void update_angles() |
pmic | 1:6b803652d032 | 183 | { |
pmic | 1:6b803652d032 | 184 | s1 = sin(x(0)); |
pmic | 1:6b803652d032 | 185 | c1 = cos(x(0)); |
pmic | 1:6b803652d032 | 186 | s2 = sin(x(1)); |
pmic | 1:6b803652d032 | 187 | c2 = cos(x(1)); |
pmic | 1:6b803652d032 | 188 | } |
pmic | 1:6b803652d032 | 189 | |
pmic | 1:6b803652d032 | 190 | void update_F() |
pmic | 1:6b803652d032 | 191 | { |
pmic | 1:6b803652d032 | 192 | 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 | 1:6b803652d032 | 193 | -Ts*s1*(u(1) - x(3)), 1, 0, -Ts*c1, 0, 0, |
pmic | 1:6b803652d032 | 194 | 0, 0, 1, 0, 0, 0, |
pmic | 1:6b803652d032 | 195 | 0, 0, 0, 1, 0, 0, |
pmic | 1:6b803652d032 | 196 | 0, Ts*c2*g, 0, 0, 1 - Ts*kv, 0, |
pmic | 1:6b803652d032 | 197 | -Ts*c1*c2*g, Ts*g*s1*s2, 0, 0, 0, 1 - Ts*kv; |
pmic | 1:6b803652d032 | 198 | } |
pmic | 1:6b803652d032 | 199 | |
pmic | 1:6b803652d032 | 200 | void update_H() |
pmic | 1:6b803652d032 | 201 | { |
pmic | 1:6b803652d032 | 202 | H << 0, 0, 0, 0, -kv, 0, |
pmic | 1:6b803652d032 | 203 | 0, 0, 0, 0, 0, -kv; |
pmic | 1:6b803652d032 | 204 | } |
pmic | 1:6b803652d032 | 205 | |
pmic | 1:6b803652d032 | 206 | void initialize_R() |
pmic | 1:6b803652d032 | 207 | { |
pmic | 1:6b803652d032 | 208 | R << rho*var_gy(0)/Ts, 0, |
pmic | 1:6b803652d032 | 209 | 0, rho*var_gy(1)/Ts; |
pmic | 1:6b803652d032 | 210 | } |
pmic | 1:6b803652d032 | 211 | |
pmic | 1:6b803652d032 | 212 | void initialize_Q() |
pmic | 1:6b803652d032 | 213 | { |
pmic | 1:6b803652d032 | 214 | 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 | 1:6b803652d032 | 215 | (Ts*c1*s1*s2*var_fx(1))/c2, Ts*c1*c1*var_fx(1), 0, 0, 0, 0, |
pmic | 1:6b803652d032 | 216 | 0, 0, Ts*var_fx(2), 0, 0, 0, |
pmic | 1:6b803652d032 | 217 | 0, 0, 0, Ts*var_fx(3), 0, 0, |
pmic | 1:6b803652d032 | 218 | 0, 0, 0, 0, Ts*var_fx(4), 0, |
pmic | 1:6b803652d032 | 219 | 0, 0, 0, 0, 0, Ts*var_fx(5); |
pmic | 1:6b803652d032 | 220 | } |
pmic | 1:6b803652d032 | 221 | |
pmic | 1:6b803652d032 | 222 | void update_Q() |
pmic | 1:6b803652d032 | 223 | { |
pmic | 1:6b803652d032 | 224 | Q(0,0) = Ts*(var_fx(0) + (s1*s1*s2*s2*var_fx(1))/(c2*c2)); |
pmic | 1:6b803652d032 | 225 | Q(0,1) = Ts*c1*s1*s2*var_fx(1)/c2; |
pmic | 1:6b803652d032 | 226 | Q(1,0) = Q(0,1); |
pmic | 1:6b803652d032 | 227 | Q(1,1) = Ts*c1*c1*var_fx(1); |
pmic | 1:6b803652d032 | 228 | } |
pmic | 1:6b803652d032 | 229 | |
pmic | 1:6b803652d032 | 230 | Matrix <float, 6, 1> fxd() |
pmic | 1:6b803652d032 | 231 | { |
pmic | 1:6b803652d032 | 232 | Matrix <float, 6, 1> retval; |
pmic | 1:6b803652d032 | 233 | retval << x(0) + Ts*(u(0) - x(2) + (s1*s2*(u(1) - x(3)))/c2), |
pmic | 1:6b803652d032 | 234 | x(1) + Ts*c1*(u(1) - x(3)), |
pmic | 1:6b803652d032 | 235 | x(2), |
pmic | 1:6b803652d032 | 236 | x(3), |
pmic | 1:6b803652d032 | 237 | x(4) + Ts*(g*s2 - kv*x(4)), |
pmic | 1:6b803652d032 | 238 | x(5) - Ts*(kv*x(5) + c2*g*s1); |
pmic | 1:6b803652d032 | 239 | return retval; |
pmic | 1:6b803652d032 | 240 | } |
pmic | 1:6b803652d032 | 241 | |
pmic | 1:6b803652d032 | 242 | Matrix <float, 2, 1> gy() |
pmic | 1:6b803652d032 | 243 | { |
pmic | 1:6b803652d032 | 244 | Matrix <float, 2, 1> retval; |
pmic | 1:6b803652d032 | 245 | retval << -kv*x(4), |
pmic | 1:6b803652d032 | 246 | -kv*x(5); |
pmic | 1:6b803652d032 | 247 | return retval; |
pmic | 1:6b803652d032 | 248 | } |