altb_pmic
/
Test_ekf
Testing ekf implementation for Quadro_1.
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?
User | Revision | Line number | New 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 |