altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

EKF_RPY.cpp@17:1d98928f7681, 2019-10-25 (annotated)

Committer:
pmic
Date:
Fri Oct 25 08:44:32 2019 +0000
Revision:
17:1d98928f7681
Parent:
16:c39e084f05ed
Child:
18:f374ca9a3e75
Recursion and inversion deliver same results. Time measurements show inversion is faster (???). So EKF changes back to inversion.

Who changed what in which revision?

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