altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

EKF_RPY.cpp@22:495a419e474c, 2019-10-28 (annotated)

Committer:
altb2
Date:
Mon Oct 28 07:52:43 2019 +0000
Revision:
22:495a419e474c
Parent:
21:31e01d3e0143
Child:
23:71996bfe68eb
EKF_RPY and EKF_RP from pmic

Who changed what in which revision?

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