altb_pmic
/
AHRS
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Eigen
Embed:
(wiki syntax)
Show/hide line numbers
EKF_RPY.cpp
00001 #include "EKF_RPY.h" 00002 00003 using namespace std; 00004 using namespace Eigen; 00005 00006 EKF_RPY::EKF_RPY(float Ts, float mx0, float my0, float mz0) 00007 { 00008 this->Ts = Ts; 00009 set_para(); 00010 m0 << mx0, my0, mz0; 00011 reset(); 00012 } 00013 00014 EKF_RPY::EKF_RPY(float Ts) 00015 { 00016 this->Ts = Ts; 00017 set_para(); 00018 reset(); 00019 } 00020 00021 EKF_RPY::~EKF_RPY() {} 00022 00023 void EKF_RPY::set_para() 00024 { 00025 /* [n_gyro; n_v; n_b_g] */ 00026 var_fx << 0.1f, 0.1f, 0.1f, 0.002f, 0.002f, 0.002f, 0.002f, 0.002f; 00027 /* [n_acc; n_mag] */ 00028 var_gy << 40.0f, 40.0f, 1.0f, 1.0f; 00029 rho = 1.0f; 00030 // kv = 0.5f; /* QK3 k1/m */ 00031 kv = 0.26f; /* QK4 k1/m */ 00032 g = 9.81f; 00033 scale_P0 = 1000.0f; 00034 } 00035 00036 void EKF_RPY::reset() 00037 { 00038 u.setZero(); 00039 y.setZero(); 00040 x.setZero(); 00041 update_angles(); 00042 calc_F(); 00043 calc_H(); 00044 initialize_Q(); 00045 initialize_R(); 00046 K.setZero(); 00047 I.setIdentity(); 00048 e.setZero(); 00049 P = scale_P0 * I; 00050 } 00051 00052 void EKF_RPY::increase_diag_P() 00053 { 00054 P = P + scale_P0 * I; 00055 } 00056 00057 void EKF_RPY::set_m0(float mx0, float my0, float mz0) 00058 { 00059 m0 << mx0, my0, mz0; 00060 } 00061 00062 float EKF_RPY::get_est_state(uint8_t i) 00063 { 00064 /* x = [ang; v; b_g] = [0: phi 00065 1: theta 00066 2: psi 00067 3: b_gx 00068 4: b_gy 00069 5: b_gz 00070 6: vx 00071 7: vy] */ 00072 return x(i); 00073 } 00074 00075 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) 00076 { 00077 u << gyro_x, gyro_y, gyro_z; 00078 y << accel_x, accel_y, magnet_x, magnet_y; 00079 update_angles(); 00080 00081 calc_F(); 00082 calc_H(); 00083 calc_Q(); 00084 00085 x = fxd(); 00086 P = F * P * F.transpose() + Q; 00087 e = y - gy(); 00088 00089 /* inversion faster 356 mus < 432 mus recursion */ 00090 K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse(); 00091 x = x + K * e; 00092 P = (I - K * H) * P; 00093 00094 /* only valid if R is diagonal (uncorrelated noise) */ 00095 /* 00096 for(uint8_t i = 0; i < 4; i++) { 00097 K.col(i) = ( P * (H.row(i)).transpose() ) / ( H.row(i) * P * (H.row(i)).transpose() + R(i,i) ); 00098 x = x + K.col(i) * e(i); 00099 P = (I - K.col(i)*H.row(i)) * P; 00100 } 00101 */ 00102 } 00103 00104 void EKF_RPY::update_angles() 00105 { 00106 s1 = sinf(x(0)); 00107 c1 = cosf(x(0)); 00108 s2 = sinf(x(1)); 00109 c2 = cosf(x(1)); 00110 s3 = sinf(x(2)); 00111 c3 = cosf(x(2)); 00112 } 00113 00114 void EKF_RPY::calc_F() 00115 { 00116 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, 00117 -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, 00118 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, 00119 0.0f, Ts*c2*g, 0.0f, 1.0f - Ts*kv, Ts*(u(2) - x(7)), 0.0f, 0.0f, -Ts*x(4), 00120 -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), 00121 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 00122 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 00123 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f; 00124 } 00125 00126 void EKF_RPY::calc_H() 00127 { 00128 H << 0.0f, 0.0f, 0.0f, -kv, u(2) - x(7), 0.0f, 0.0f, -x(4), 00129 0.0f, 0.0f, 0.0f, x(7) - u(2), -kv, 0.0f, 0.0f, x(3), 00130 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, 00131 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; 00132 } 00133 00134 void EKF_RPY::initialize_R() 00135 { 00136 R << rho*var_gy(0)/Ts, 0.0f, 0.0f, 0.0f, 00137 0.0f, rho*var_gy(1)/Ts, 0.0f, 0.0f, 00138 0.0f, 0.0f, rho*var_gy(2)/Ts, 0.0f, 00139 0.0f, 0.0f, 0.0f, rho*var_gy(3)/Ts; 00140 } 00141 00142 void EKF_RPY::initialize_Q() 00143 { 00144 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, 00145 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, 00146 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, 00147 0.0f, 0.0f, 0.0f, Ts*var_fx(3), 0.0f, 0.0f, 0.0f, 0.0f, 00148 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(4), 0.0f, 0.0f, 0.0f, 00149 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(5), 0.0f, 0.0f, 00150 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(6), 0.0f, 00151 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, Ts*var_fx(7); 00152 } 00153 00154 void EKF_RPY::calc_Q() 00155 { 00156 Q(0,0) = Ts*(var_fx(0) + (c1*c1*var_fx(2) + s1*s1*var_fx(1))*s2*s2/(c2*c2)); 00157 Q(0,1) = Ts*(var_fx(1) - var_fx(2))*c1*s1*s2/c2; 00158 Q(0,2) = Ts*(c1*c1*var_fx(2) + s1*s1*var_fx(1))*s2/(c2*c2); 00159 Q(1,0) = Q(0,1); 00160 Q(1,1) = Ts*(var_fx(1)*c1*c1 + var_fx(2)*s1*s1); 00161 Q(1,2) = Ts*(var_fx(1) - var_fx(2))*c1*s1/c2; 00162 Q(2,0) = Q(0,2); 00163 Q(2,1) = Q(1,2); 00164 Q(2,2) = Ts*(c1*c1*var_fx(2) + s1*s1*var_fx(1))/(c2*c2); 00165 } 00166 00167 Matrix <float, 8, 1> EKF_RPY::fxd() 00168 { 00169 Matrix <float, 8, 1> retval; 00170 retval << x(0) + Ts*(u(0) - x(5) + (c1*s2*(u(2) - x(7)))/c2 + (s1*s2*(u(1) - x(6)))/c2), 00171 x(1) + Ts*(c1*(u(1) - x(6)) - s1*(u(2) - x(7))), 00172 x(2) + Ts*((c1*(u(2) - x(7)))/c2 + (s1*(u(1) - x(6)))/c2), 00173 x(3) + Ts*(g*s2 - kv*x(3) + x(4)*(u(2) - x(7))), 00174 x(4) - Ts*(kv*x(4) + x(3)*(u(2) - x(7)) + c2*g*s1), 00175 x(5), 00176 x(6), 00177 x(7); 00178 return retval; 00179 } 00180 00181 Matrix <float, 4, 1> EKF_RPY::gy() 00182 { 00183 Matrix <float, 4, 1> retval; 00184 retval << x(4)*(u(2) - x(7)) - kv*x(3), 00185 - kv*x(4) - x(3)*(u(2) - x(7)), 00186 - m0(2)*s2 + c2*c3*m0(0) + c2*m0(1)*s3, 00187 - m0(0)*(c1*s3 - c3*s1*s2) + m0(1)*(c1*c3 + s1*s2*s3) + c2*m0(2)*s1; 00188 return retval; 00189 } 00190
Generated on Thu Jul 14 2022 22:08:52 by
1.7.2