altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

main.cpp@2:756446014084, 2019-10-18 (annotated)

Committer:
pmic
Date:
Fri Oct 18 21:17:11 2019 +0000
Revision:
2:756446014084
Parent:
1:6b803652d032
Child:
3:121647a7cddf
Commit before update Eigen.lib

Who changed what in which revision?

UserRevisionLine numberNew 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 2:756446014084 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 }