altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

EKF_RP.cpp@30:9b0cd3caf0ec, 2020-01-27 (annotated)

Committer:
pmic
Date:
Mon Jan 27 10:54:13 2020 +0000
Revision:
30:9b0cd3caf0ec
Parent:
29:cd963a6d31c5 
Correct magnetometer.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 21:31e01d3e0143 1 #include "EKF_RP.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_RP::EKF_RP(float Ts)
altb2 21:31e01d3e0143 7 {
altb2 21:31e01d3e0143 8 this->Ts = Ts;
altb2 21:31e01d3e0143 9 /* [n_gyro; n_b_g; n_v] */
altb2 22:495a419e474c 10 var_fx << 0.1f, 0.1f, 0.002f, 0.002f, 0.002f, 0.002f;
altb2 21:31e01d3e0143 11 /* [n_acc] */
altb2 22:495a419e474c 12 var_gy << 40.0f, 40.0f;
altb2 22:495a419e474c 13 rho = 1.0f;
pmic 29:cd963a6d31c5 14 // kv = 0.5f; /* QK3 k1/m */
pmic 29:cd963a6d31c5 15 kv = 0.26f; /* QK4 k1/m */
altb2 21:31e01d3e0143 16 g = 9.81f;
altb2 23:71996bfe68eb 17 scale_P0 = 1000.0f;
altb2 21:31e01d3e0143 18 reset();
altb2 21:31e01d3e0143 19 }
altb2 21:31e01d3e0143 20
altb2 21:31e01d3e0143 21 EKF_RP::~EKF_RP() {}
altb2 21:31e01d3e0143 22
altb2 21:31e01d3e0143 23 void EKF_RP::reset()
altb2 21:31e01d3e0143 24 {
altb2 21:31e01d3e0143 25 u.setZero();
altb2 21:31e01d3e0143 26 y.setZero();
altb2 21:31e01d3e0143 27 x.setZero();
altb2 21:31e01d3e0143 28 update_angles();
altb2 21:31e01d3e0143 29 calc_F();
altb2 21:31e01d3e0143 30 calc_H();
altb2 21:31e01d3e0143 31 initialize_Q();
altb2 21:31e01d3e0143 32 initialize_R();
altb2 21:31e01d3e0143 33 K.setZero();
altb2 21:31e01d3e0143 34 I.setIdentity();
altb2 22:495a419e474c 35 e.setZero();
altb2 23:71996bfe68eb 36 P = scale_P0 * I;
altb2 23:71996bfe68eb 37 }
altb2 23:71996bfe68eb 38
altb2 23:71996bfe68eb 39 void EKF_RP::increase_diag_P()
altb2 23:71996bfe68eb 40 {
altb2 23:71996bfe68eb 41 P = P + scale_P0 * I;
altb2 21:31e01d3e0143 42 }
altb2 21:31e01d3e0143 43
altb2 21:31e01d3e0143 44 float EKF_RP::get_est_state(uint8_t i)
altb2 21:31e01d3e0143 45 {
altb2 21:31e01d3e0143 46 /* x = [ang; v; b_g] = [0: phi
altb2 21:31e01d3e0143 47 1: theta
altb2 21:31e01d3e0143 48 2: vx
altb2 21:31e01d3e0143 49 3: vy
altb2 21:31e01d3e0143 50 4: b_gx
altb2 21:31e01d3e0143 51 5: b_gy] */
altb2 21:31e01d3e0143 52 return x(i);
altb2 21:31e01d3e0143 53 }
altb2 21:31e01d3e0143 54
altb2 21:31e01d3e0143 55 void EKF_RP::update(float gyro_x, float gyro_y, float accel_x, float accel_y)
altb2 21:31e01d3e0143 56 {
altb2 21:31e01d3e0143 57 u << gyro_x, gyro_y;
altb2 21:31e01d3e0143 58 y << accel_x, accel_y;
altb2 21:31e01d3e0143 59 update_angles();
altb2 21:31e01d3e0143 60
altb2 21:31e01d3e0143 61 calc_F();
altb2 21:31e01d3e0143 62 // calc_H(); /* H remains constant */
altb2 21:31e01d3e0143 63 calc_Q();
altb2 21:31e01d3e0143 64
altb2 21:31e01d3e0143 65 x = fxd();
altb2 21:31e01d3e0143 66 P = F * P * F.transpose() + Q;
altb2 22:495a419e474c 67 e = y - gy();
altb2 21:31e01d3e0143 68
altb2 22:495a419e474c 69 /* inversion faster 184 mus < 207 mus recursion */
altb2 21:31e01d3e0143 70 K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
altb2 22:495a419e474c 71 x = x + K * e;
altb2 22:495a419e474c 72 P = (I - K * H) * P;
altb2 21:31e01d3e0143 73
altb2 22:495a419e474c 74 /* only valid if R is diagonal (uncorrelated noise) */
altb2 22:495a419e474c 75 /*
altb2 22:495a419e474c 76 for(uint8_t i = 0; i < 2; i++) {
altb2 22:495a419e474c 77 K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) );
altb2 22:495a419e474c 78 x = x + K.col(i) * e(i);
altb2 22:495a419e474c 79 P = (I - K.col(i)*H.row(i)) * P;
altb2 22:495a419e474c 80 }
altb2 22:495a419e474c 81 */
altb2 21:31e01d3e0143 82 }
altb2 21:31e01d3e0143 83
altb2 21:31e01d3e0143 84 void EKF_RP::update_angles()
altb2 21:31e01d3e0143 85 {
altb2 21:31e01d3e0143 86 s1 = sinf(x(0));
altb2 21:31e01d3e0143 87 c1 = cosf(x(0));
altb2 21:31e01d3e0143 88 s2 = sinf(x(1));
altb2 21:31e01d3e0143 89 c2 = cosf(x(1));
altb2 21:31e01d3e0143 90 }
altb2 21:31e01d3e0143 91
altb2 21:31e01d3e0143 92 void EKF_RP::calc_F()
altb2 21:31e01d3e0143 93 {
altb2 21:31e01d3e0143 94 F << Ts*c1*s2*(u(1) - x(3))/c2 + 1.0f, Ts*s1*(u(1) - x(3))/(c2*c2), -Ts, -Ts*s1*s2/c2, 0.0f, 0.0f,
altb2 21:31e01d3e0143 95 -Ts*s1*(u(1) - x(3)), 1.0f, 0.0f, -Ts*c1, 0.0f, 0.0f,
altb2 21:31e01d3e0143 96 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
altb2 21:31e01d3e0143 97 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
altb2 21:31e01d3e0143 98 0.0f, Ts*c2*g, 0.0f, 0.0f, 1.0f - Ts*kv, 0.0f,
altb2 21:31e01d3e0143 99 -Ts*c1*c2*g, Ts*g*s1*s2, 0.0f, 0.0f, 0.0f, 1.0f - Ts*kv;
altb2 21:31e01d3e0143 100 }
altb2 21:31e01d3e0143 101
altb2 21:31e01d3e0143 102 void EKF_RP::calc_H()
altb2 21:31e01d3e0143 103 {
altb2 21:31e01d3e0143 104 H << 0.0f, 0.0f, 0.0f, 0.0f, -kv, 0.0f,
altb2 21:31e01d3e0143 105 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -kv;
altb2 21:31e01d3e0143 106 }
altb2 21:31e01d3e0143 107
altb2 21:31e01d3e0143 108 void EKF_RP::initialize_R()
altb2 21:31e01d3e0143 109 {
altb2 21:31e01d3e0143 110 R << rho*var_gy(0)/Ts, 0.0f,
altb2 21:31e01d3e0143 111 0.0f, rho*var_gy(1)/Ts;
altb2 21:31e01d3e0143 112 }
altb2 21:31e01d3e0143 113
altb2 21:31e01d3e0143 114 void EKF_RP::initialize_Q()
altb2 21:31e01d3e0143 115 {
altb2 21:31e01d3e0143 116 Q << Ts*(var_fx(0) + s1*s1*s2*s2*var_fx(1)/(c2*c2)), Ts*c1*s1*s2*var_fx(1)/c2, 0.0f, 0.0f, 0.0f, 0.0f,
altb2 21:31e01d3e0143 117 Ts*c1*s1*s2*var_fx(1)/c2, Ts*c1*c1*var_fx(1), 0.0f, 0.0f, 0.0f, 0.0f,
altb2 21:31e01d3e0143 118 0.0f, 0.0f, Ts*var_fx(2), 0.0f, 0.0f, 0.0f,
altb2 21:31e01d3e0143 119 0.0f, 0.0f, 0.0f, Ts*var_fx(3), 0.0f, 0.0f,
altb2 21:31e01d3e0143 120 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(4), 0.0f,
altb2 21:31e01d3e0143 121 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(5);
altb2 21:31e01d3e0143 122 }
altb2 21:31e01d3e0143 123
altb2 21:31e01d3e0143 124 void EKF_RP::calc_Q()
altb2 21:31e01d3e0143 125 {
altb2 21:31e01d3e0143 126 Q(0,0) = Ts*(var_fx(0) + s1*s1*s2*s2*var_fx(1)/(c2*c2));
altb2 21:31e01d3e0143 127 Q(0,1) = Ts*c1*s1*s2*var_fx(1)/c2;
altb2 21:31e01d3e0143 128 Q(1,0) = Q(0,1);
altb2 21:31e01d3e0143 129 Q(1,1) = Ts*c1*c1*var_fx(1);
altb2 21:31e01d3e0143 130 }
altb2 21:31e01d3e0143 131
altb2 21:31e01d3e0143 132 Matrix <float, 6, 1> EKF_RP::fxd()
altb2 21:31e01d3e0143 133 {
altb2 21:31e01d3e0143 134 Matrix <float, 6, 1> retval;
altb2 21:31e01d3e0143 135 retval << x(0) + Ts*(u(0) - x(2) + (s1*s2*(u(1) - x(3)))/c2),
altb2 21:31e01d3e0143 136 x(1) + Ts*c1*(u(1) - x(3)),
altb2 21:31e01d3e0143 137 x(2),
altb2 21:31e01d3e0143 138 x(3),
altb2 21:31e01d3e0143 139 x(4) + Ts*(g*s2 - kv*x(4)),
altb2 21:31e01d3e0143 140 x(5) - Ts*(kv*x(5) + c2*g*s1);
altb2 21:31e01d3e0143 141 return retval;
altb2 21:31e01d3e0143 142 }
altb2 21:31e01d3e0143 143
altb2 21:31e01d3e0143 144 Matrix <float, 2, 1> EKF_RP::gy()
altb2 21:31e01d3e0143 145 {
altb2 21:31e01d3e0143 146 Matrix <float, 2, 1> retval;
altb2 21:31e01d3e0143 147 retval << -kv*x(4),
altb2 21:31e01d3e0143 148 -kv*x(5);
altb2 21:31e01d3e0143 149 return retval;
altb2 21:31e01d3e0143 150 }
altb2 21:31e01d3e0143 151