altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

EKF_RPY.cpp@16:c39e084f05ed, 2019-10-24 (annotated)

Committer:
pmic
Date:
Thu Oct 24 06:43:17 2019 +0000
Revision:
16:c39e084f05ed
Parent:
15:53485bd1ff28
Child:
17:1d98928f7681
Get recursion running. Everything is fine now : - )

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 16:c39e084f05ed 82 // K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
pmic 16:c39e084f05ed 83 // x = x + K * e;
pmic 16:c39e084f05ed 84 // P = (I - K * H) * P;
pmic 15:53485bd1ff28 85
pmic 13:2e03d9c03409 86 /* only valid if R is diagonal (uncorrelated noise) */
pmic 16:c39e084f05ed 87 for(uint8_t i = 0; i < 4; i++) {
pmic 16:c39e084f05ed 88 K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) );
pmic 16:c39e084f05ed 89 x = x + K.col(i) * e(i);
pmic 16:c39e084f05ed 90 P = (I - K.col(i)*H.row(i)) * P;
pmic 16:c39e084f05ed 91 }
pmic 0:a0e9705be9c4 92 }
pmic 0:a0e9705be9c4 93
pmic 0:a0e9705be9c4 94 void EKF_RPY::update_angles()
pmic 0:a0e9705be9c4 95 {
pmic 7:bcbcc23983de 96 s1 = sinf(x(0));
pmic 7:bcbcc23983de 97 c1 = cosf(x(0));
pmic 7:bcbcc23983de 98 s2 = sinf(x(1));
pmic 7:bcbcc23983de 99 c2 = cosf(x(1));
pmic 7:bcbcc23983de 100 s3 = sinf(x(2));
pmic 7:bcbcc23983de 101 c3 = cosf(x(2));
pmic 0:a0e9705be9c4 102 }
pmic 0:a0e9705be9c4 103
pmic 7:bcbcc23983de 104 void EKF_RPY::calc_F()
pmic 7:bcbcc23983de 105 {
pmic 7:bcbcc23983de 106 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 107 -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 108 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 109 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 110 -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 111 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
pmic 7:bcbcc23983de 112 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
pmic 7:bcbcc23983de 113 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f;
pmic 7:bcbcc23983de 114 }
pmic 0:a0e9705be9c4 115
pmic 7:bcbcc23983de 116 void EKF_RPY::calc_H()
pmic 7:bcbcc23983de 117 {
pmic 7:bcbcc23983de 118 H << 0.0f, 0.0f, 0.0f, -kv, u(2) - x(7), 0.0f, 0.0f, -x(4),
pmic 7:bcbcc23983de 119 0.0f, 0.0f, 0.0f, x(7) - u(2), -kv, 0.0f, 0.0f, x(3),
pmic 7:bcbcc23983de 120 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 121 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 122 }
pmic 0:a0e9705be9c4 123
pmic 0:a0e9705be9c4 124 void EKF_RPY::initialize_R()
pmic 7:bcbcc23983de 125 {
pmic 7:bcbcc23983de 126 R << rho*var_gy(0)/Ts, 0.0f, 0.0f, 0.0f,
pmic 7:bcbcc23983de 127 0.0f, rho*var_gy(1)/Ts, 0.0f, 0.0f,
pmic 7:bcbcc23983de 128 0.0f, 0.0f, rho*var_gy(2)/Ts, 0.0f,
pmic 7:bcbcc23983de 129 0.0f, 0.0f, 0.0f, rho*var_gy(3)/Ts;
pmic 7:bcbcc23983de 130 }
pmic 0:a0e9705be9c4 131
pmic 0:a0e9705be9c4 132 void EKF_RPY::initialize_Q()
pmic 7:bcbcc23983de 133 {
pmic 7:bcbcc23983de 134 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 135 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 136 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 137 0.0f, 0.0f, 0.0f, Ts*var_fx(3), 0.0f, 0.0f, 0.0f, 0.0f,
pmic 7:bcbcc23983de 138 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(4), 0.0f, 0.0f, 0.0f,
pmic 7:bcbcc23983de 139 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(5), 0.0f, 0.0f,
pmic 7:bcbcc23983de 140 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(6), 0.0f,
pmic 7:bcbcc23983de 141 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(7);
pmic 7:bcbcc23983de 142 }
pmic 0:a0e9705be9c4 143
pmic 7:bcbcc23983de 144 void EKF_RPY::calc_Q()
pmic 7:bcbcc23983de 145 {
pmic 7:bcbcc23983de 146 Q(0,0) = Ts*(var_fx(0) + (c1*c1*var_fx(2) + s1*s1*var_fx(1))*s2*s2/(c2*c2));
pmic 7:bcbcc23983de 147 Q(0,1) = Ts*(var_fx(1) - var_fx(2))*c1*s1*s2/c2;
pmic 7:bcbcc23983de 148 Q(0,2) = Ts*(c1*c1*var_fx(2) + s1*s1*var_fx(1))*s2/(c2*c2);
pmic 0:a0e9705be9c4 149 Q(1,0) = Q(0,1);
pmic 7:bcbcc23983de 150 Q(1,1) = Ts*(var_fx(1)*c1*c1 + var_fx(2)*s1*s1);
pmic 7:bcbcc23983de 151 Q(1,2) = Ts*(var_fx(1) - var_fx(2))*c1*s1/c2;
pmic 0:a0e9705be9c4 152 Q(2,0) = Q(0,2);
pmic 0:a0e9705be9c4 153 Q(2,1) = Q(1,2);
pmic 7:bcbcc23983de 154 Q(2,2) = Ts*(c1*c1*var_fx(2) + s1*s1*var_fx(1))/(c2*c2);
pmic 7:bcbcc23983de 155 }
pmic 0:a0e9705be9c4 156
pmic 7:bcbcc23983de 157 Matrix <float, 8, 1> EKF_RPY::fxd()
pmic 0:a0e9705be9c4 158 {
pmic 7:bcbcc23983de 159 Matrix <float, 8, 1> retval;
pmic 0:a0e9705be9c4 160 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 161 x(1) + Ts*(c1*(u(1) - x(6)) - s1*(u(2) - x(7))),
pmic 0:a0e9705be9c4 162 x(2) + Ts*((c1*(u(2) - x(7)))/c2 + (s1*(u(1) - x(6)))/c2),
pmic 0:a0e9705be9c4 163 x(3) + Ts*(g*s2 - kv*x(3) + x(4)*(u(2) - x(7))),
pmic 0:a0e9705be9c4 164 x(4) - Ts*(kv*x(4) + x(3)*(u(2) - x(7)) + c2*g*s1),
pmic 0:a0e9705be9c4 165 x(5),
pmic 0:a0e9705be9c4 166 x(6),
pmic 7:bcbcc23983de 167 x(7);
pmic 0:a0e9705be9c4 168 return retval;
pmic 0:a0e9705be9c4 169 }
pmic 0:a0e9705be9c4 170
pmic 0:a0e9705be9c4 171 Matrix <float, 4, 1> EKF_RPY::gy()
pmic 0:a0e9705be9c4 172 {
pmic 0:a0e9705be9c4 173 Matrix <float, 4, 1> retval;
pmic 7:bcbcc23983de 174 retval << x(4)*(u(2) - x(7)) - kv*x(3),
pmic 7:bcbcc23983de 175 - kv*x(4) - x(3)*(u(2) - x(7)),
pmic 7:bcbcc23983de 176 - m0(2)*s2 + c2*c3*m0(0) + c2*m0(1)*s3,
pmic 7:bcbcc23983de 177 - m0(0)*(c1*s3 - c3*s1*s2) + m0(1)*(c1*c3 + s1*s2*s3) + c2*m0(2)*s1;
pmic 0:a0e9705be9c4 178 return retval;
pmic 0:a0e9705be9c4 179 }
pmic 0:a0e9705be9c4 180