altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

EKF_RPY.cpp@1:6b803652d032, 2019-10-18 (annotated)

Committer:
pmic
Date:
Fri Oct 18 21:15:52 2019 +0000
Revision:
1:6b803652d032
Parent:
0:a0e9705be9c4
Child:
7:bcbcc23983de
Test ekf in main.

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 0:a0e9705be9c4 9 /* [n_gyro; n_v; n_b_g; n_b_m] */
pmic 0:a0e9705be9c4 10 var_fx << 1000, 1000, 1000, 10, 10, 20, 20, 20, 20, 20;
pmic 0:a0e9705be9c4 11 /* [n_acc; n_mag] */
pmic 0:a0e9705be9c4 12 var_gy << 40, 40, 0.1, 0.1;
pmic 0:a0e9705be9c4 13 rho = 5000;
pmic 0:a0e9705be9c4 14 kv = 0.5; /* k1/m */
pmic 0:a0e9705be9c4 15 wm = 0.0062832; /* 2*pi*0.001 */
pmic 0:a0e9705be9c4 16 g = 9.81;
pmic 0:a0e9705be9c4 17 m0 << mx0, my0, mz0;
pmic 0:a0e9705be9c4 18 reset();
pmic 0:a0e9705be9c4 19 }
pmic 0:a0e9705be9c4 20
pmic 0:a0e9705be9c4 21 EKF_RPY::~EKF_RPY() {}
pmic 0:a0e9705be9c4 22
pmic 0:a0e9705be9c4 23 void EKF_RPY::reset()
pmic 0:a0e9705be9c4 24 {
pmic 0:a0e9705be9c4 25 u.setZero();
pmic 0:a0e9705be9c4 26 y.setZero();
pmic 0:a0e9705be9c4 27 x.setZero();
pmic 0:a0e9705be9c4 28 update_F();
pmic 0:a0e9705be9c4 29 update_H();
pmic 0:a0e9705be9c4 30 initialize_Q();
pmic 0:a0e9705be9c4 31 initialize_R();
pmic 0:a0e9705be9c4 32 // P = Q;
pmic 0:a0e9705be9c4 33 /*
pmic 0:a0e9705be9c4 34 for(uint8_t i = 0; i < 10; i++) {
pmic 0:a0e9705be9c4 35 for(uint8_t j = 0; i < 10; j++) {
pmic 0:a0e9705be9c4 36 P(i,j) = Q(i,j);
pmic 0:a0e9705be9c4 37 }
pmic 0:a0e9705be9c4 38 }
pmic 0:a0e9705be9c4 39 */
pmic 0:a0e9705be9c4 40 K.setZero();
pmic 0:a0e9705be9c4 41 // I.setIdentity();
pmic 0:a0e9705be9c4 42 /*
pmic 0:a0e9705be9c4 43 for(uint8_t i = 0; i < 10; i++) {
pmic 0:a0e9705be9c4 44 for(uint8_t j = 0; i < 10; j++) {
pmic 0:a0e9705be9c4 45 if(i == j) I(i,j) = 1;
pmic 0:a0e9705be9c4 46 else I(i,j) = 0;
pmic 0:a0e9705be9c4 47 }
pmic 0:a0e9705be9c4 48 }
pmic 0:a0e9705be9c4 49 */
pmic 0:a0e9705be9c4 50 }
pmic 0:a0e9705be9c4 51
pmic 0:a0e9705be9c4 52 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 0:a0e9705be9c4 53 {/*
pmic 0:a0e9705be9c4 54 u << gyro_x, gyro_y, gyro_z;
pmic 0:a0e9705be9c4 55 y << accel_x, accel_y, magnet_x, magnet_y;
pmic 0:a0e9705be9c4 56
pmic 0:a0e9705be9c4 57 update_F();
pmic 0:a0e9705be9c4 58 update_H();
pmic 0:a0e9705be9c4 59 update_Q();
pmic 0:a0e9705be9c4 60
pmic 0:a0e9705be9c4 61 x = fxd();
pmic 0:a0e9705be9c4 62 P = F * P * F.transpose() + Q;
pmic 0:a0e9705be9c4 63
pmic 0:a0e9705be9c4 64 K = P * H.transpose() * ( H * P * H.transpose() + R ); //.inverse();
pmic 0:a0e9705be9c4 65
pmic 0:a0e9705be9c4 66 x = x + K * (y - gy());
pmic 0:a0e9705be9c4 67 P = (I - K * H) * P;
pmic 0:a0e9705be9c4 68 */}
pmic 0:a0e9705be9c4 69
pmic 0:a0e9705be9c4 70 float EKF_RPY::get_est_state(uint8_t i)
pmic 0:a0e9705be9c4 71 {
pmic 0:a0e9705be9c4 72 /* x = [ang; v; b_g; n_b_m] */
pmic 0:a0e9705be9c4 73 return x(i);
pmic 0:a0e9705be9c4 74 }
pmic 0:a0e9705be9c4 75
pmic 0:a0e9705be9c4 76 void EKF_RPY::update_angles()
pmic 0:a0e9705be9c4 77 {
pmic 1:6b803652d032 78 s1 = sin(x(0));
pmic 1:6b803652d032 79 c1 = cos(x(0));
pmic 1:6b803652d032 80 s2 = sin(x(1));
pmic 1:6b803652d032 81 c2 = cos(x(1));
pmic 1:6b803652d032 82 s3 = sin(x(2));
pmic 1:6b803652d032 83 c3 = cos(x(2));
pmic 0:a0e9705be9c4 84 }
pmic 0:a0e9705be9c4 85
pmic 0:a0e9705be9c4 86 void EKF_RPY::update_F()
pmic 0:a0e9705be9c4 87 {/*
pmic 0:a0e9705be9c4 88 F << Ts*((c1*s2*(u(1) - x(6)))/c2 - (s1*s2*(u(2) - x(7)))/c2) + 1, (Ts*(c1*u(2) - c1*x(7) + s1*u(1) - s1*x(6)))/(c2*c2), 0, 0, 0, -Ts, -(Ts*s1*s2)/c2, -(Ts*c1*s2)/c2, 0, 0,
pmic 0:a0e9705be9c4 89 -Ts*(c1*(u(2) - x(7)) + s1*(u(1) - x(6))), 1, 0, 0, 0, 0, -Ts*c1, Ts*s1, 0, 0,
pmic 0:a0e9705be9c4 90 Ts*((c1*(u(1) - x(6)))/c2 - (s1*(u(2) - x(7)))/c2), Ts*((c1*s2*(u(2) - x(7)))/(c2*c2) + (s1*s2*(u(1) - x(6)))/(c2*c2)), 1, 0, 0, 0, -(Ts*s1)/c2, -(Ts*c1)/c2, 0, 0,
pmic 0:a0e9705be9c4 91 0, Ts*c2*g, 0, 1 - Ts*kv, Ts*(u(2) - x(7)), 0, 0, -Ts*x(4), 0, 0,
pmic 0:a0e9705be9c4 92 -Ts*c1*c2*g, Ts*g*s1*s2, 0, -Ts*(u(2) - x(7)), 1 - Ts*kv, 0, 0, Ts*x(3), 0, 0,
pmic 0:a0e9705be9c4 93 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
pmic 0:a0e9705be9c4 94 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
pmic 0:a0e9705be9c4 95 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
pmic 0:a0e9705be9c4 96 0, 0, 0, 0, 0, 0, 0, 0, 1 - Ts*wm, 0,
pmic 0:a0e9705be9c4 97 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 - Ts*wm;
pmic 0:a0e9705be9c4 98 */}
pmic 0:a0e9705be9c4 99
pmic 0:a0e9705be9c4 100 void EKF_RPY::update_H()
pmic 0:a0e9705be9c4 101 {/*
pmic 0:a0e9705be9c4 102 H << 0, 0, 0, -kv, u(2) - x(7), 0, 0, -x(4), 0, 0,
pmic 0:a0e9705be9c4 103 0, 0, 0, x(7) - u(2), -kv, 0, 0, x(3), 0, 0,
pmic 0:a0e9705be9c4 104 0, - c2*m0(2) - c3*m0(0)*s2 - m0(1)*s2*s3, c2*(c3*m0(1) - m0(0)*s3), 0, 0, 0, 0, 0, 1, 0,
pmic 0:a0e9705be9c4 105 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, 0, 0, 0, 0, 0, 1;
pmic 0:a0e9705be9c4 106 */}
pmic 0:a0e9705be9c4 107
pmic 0:a0e9705be9c4 108 void EKF_RPY::initialize_R()
pmic 0:a0e9705be9c4 109 {/*
pmic 0:a0e9705be9c4 110 R << rho*var_gy(0)/Ts, 0, 0, 0,
pmic 0:a0e9705be9c4 111 0, rho*var_gy(1)/Ts, 0, 0,
pmic 0:a0e9705be9c4 112 0, 0, rho*var_gy(2)/Ts, 0,
pmic 0:a0e9705be9c4 113 0, 0, 0, rho*var_gy(3)/Ts;
pmic 0:a0e9705be9c4 114 */}
pmic 0:a0e9705be9c4 115
pmic 0:a0e9705be9c4 116 void EKF_RPY::initialize_Q()
pmic 0:a0e9705be9c4 117 {/*
pmic 0:a0e9705be9c4 118 Q << (Ts*(var_fx(2)*c1*c1*s2*s2 + var_fx(0)*c2*c2 + var_fx(1)*s1*s1*s2*s2))/(c2*c2), (Ts*c1*s1*s2*(var_fx(1) - var_fx(2)))/c2, -(Ts*s2*(var_fx(2) + s1*s1*(var_fx(1) - var_fx(2))))/(s2*s2 - 1), 0, 0, 0, 0, 0, 0, 0,
pmic 0:a0e9705be9c4 119 (Ts*c1*s1*s2*(var_fx(1) - var_fx(2)))/c2, Ts*(var_fx(1) - s1*s1*(var_fx(1) + var_fx(2))), Ts*(c1*s1*(var_fx(1) - var_fx(2)))/c2, 0, 0, 0, 0, 0, 0, 0,
pmic 0:a0e9705be9c4 120 -(Ts*s2*(var_fx(2) + s1*s1*(var_fx(1) - var_fx(2))))/(s2*s2 - 1), Ts*(c1*s1*(var_fx(1) - var_fx(2)))/c2, -(Ts*(var_fx(2) + s1*s1*(var_fx(1) - var_fx(2))))/(s2*s2 - 1), 0, 0, 0, 0, 0, 0, 0,
pmic 0:a0e9705be9c4 121 0, 0, 0, Ts*var_fx(3), 0, 0, 0, 0, 0, 0,
pmic 0:a0e9705be9c4 122 0, 0, 0, 0, Ts*var_fx(4), 0, 0, 0, 0, 0,
pmic 0:a0e9705be9c4 123 0, 0, 0, 0, 0, Ts*var_fx(5), 0, 0, 0, 0,
pmic 0:a0e9705be9c4 124 0, 0, 0, 0, 0, 0, Ts*var_fx(6), 0, 0, 0,
pmic 0:a0e9705be9c4 125 0, 0, 0, 0, 0, 0, 0, Ts*var_fx(7), 0, 0,
pmic 0:a0e9705be9c4 126 0, 0, 0, 0, 0, 0, 0, 0, Ts*var_fx(8), 0,
pmic 0:a0e9705be9c4 127 0, 0, 0, 0, 0, 0, 0, 0, 0, Ts*var_fx(9);
pmic 0:a0e9705be9c4 128 */}
pmic 0:a0e9705be9c4 129
pmic 0:a0e9705be9c4 130 void EKF_RPY::update_Q()
pmic 0:a0e9705be9c4 131 {/*
pmic 0:a0e9705be9c4 132 Q(0,0) = (Ts*(var_fx(2)*c1*c1*s2*s2 + var_fx(0)*c2*c2 + var_fx(1)*s1*s1*s2*s2))/(c2*c2);
pmic 0:a0e9705be9c4 133 Q(0,1) = (Ts*c1*s1*s2*(var_fx(1) - var_fx(2)))/c2;
pmic 0:a0e9705be9c4 134 Q(0,2) = -(Ts*s2*(var_fx(2) + s1*s1*(var_fx(1) - var_fx(2))))/(s2*s2 - 1);
pmic 0:a0e9705be9c4 135 Q(1,0) = Q(0,1);
pmic 0:a0e9705be9c4 136 Q(1,1) = Ts*(var_fx(1) - s1*s1*(var_fx(1) + var_fx(2)));
pmic 0:a0e9705be9c4 137 Q(1,2) = Ts*(c1*s1*(var_fx(1) - var_fx(2)))/c2;
pmic 0:a0e9705be9c4 138 Q(2,0) = Q(0,2);
pmic 0:a0e9705be9c4 139 Q(2,1) = Q(1,2);
pmic 0:a0e9705be9c4 140 Q(2,2) = -(Ts*(var_fx(2) + s1*s1*(var_fx(1) - var_fx(2))))/(s2*s2 - 1);
pmic 0:a0e9705be9c4 141 */}
pmic 0:a0e9705be9c4 142
pmic 0:a0e9705be9c4 143 Matrix <float, 10, 1> EKF_RPY::fxd()
pmic 0:a0e9705be9c4 144 {
pmic 0:a0e9705be9c4 145 Matrix <float, 10, 1> retval;
pmic 0:a0e9705be9c4 146 retval.setZero();
pmic 0:a0e9705be9c4 147 /*
pmic 0:a0e9705be9c4 148 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 149 x(1) + Ts*(c1*(u(1) - x(6)) - s1*(u(2) - x(7))),
pmic 0:a0e9705be9c4 150 x(2) + Ts*((c1*(u(2) - x(7)))/c2 + (s1*(u(1) - x(6)))/c2),
pmic 0:a0e9705be9c4 151 x(3) + Ts*(g*s2 - kv*x(3) + x(4)*(u(2) - x(7))),
pmic 0:a0e9705be9c4 152 x(4) - Ts*(kv*x(4) + x(3)*(u(2) - x(7)) + c2*g*s1),
pmic 0:a0e9705be9c4 153 x(5),
pmic 0:a0e9705be9c4 154 x(6),
pmic 0:a0e9705be9c4 155 x(7),
pmic 0:a0e9705be9c4 156 x(8) - Ts*wm*x(8),
pmic 0:a0e9705be9c4 157 x(9) - Ts*wm*x(9);
pmic 0:a0e9705be9c4 158 */
pmic 0:a0e9705be9c4 159 return retval;
pmic 0:a0e9705be9c4 160 }
pmic 0:a0e9705be9c4 161
pmic 0:a0e9705be9c4 162 Matrix <float, 4, 1> EKF_RPY::gy()
pmic 0:a0e9705be9c4 163 {
pmic 0:a0e9705be9c4 164 Matrix <float, 4, 1> retval;
pmic 0:a0e9705be9c4 165 retval.setZero();
pmic 0:a0e9705be9c4 166 /*
pmic 0:a0e9705be9c4 167 retval << x(4)*(u(2) - x(7)) - kv*x(3),
pmic 0:a0e9705be9c4 168 - kv*x(4) - x(3)*(u(2) - x(7)),
pmic 0:a0e9705be9c4 169 x(8) - m0(2)*s2 + c2*c3*m0(0) + c2*m0(1)*s3,
pmic 0:a0e9705be9c4 170 x(9) - m0(0)*(c1*s3 - c3*s1*s2) + m0(1)*(c1*c3 + s1*s2*s3) + c2*m0(2)*s1;
pmic 0:a0e9705be9c4 171 */
pmic 0:a0e9705be9c4 172 return retval;
pmic 0:a0e9705be9c4 173 }
pmic 0:a0e9705be9c4 174