HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Embed:
(wiki syntax)
Show/hide line numbers
solaESKF.cpp
00001 #include "solaESKF.hpp" 00002 00003 solaESKF::solaESKF() 00004 // :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),errState(18,1),Phat(18,18),Q(18,18) 00005 :errState(18,1),Phat(18,18),Q(18,18) 00006 { 00007 pihat << 0.0f, 0.0f, 0.0f; 00008 vihat << 0.0f, 0.0f, 0.0f; 00009 qhat << 1.0f, 0.0f, 0.0f, 0.0f; 00010 accBias << 0.0f, 0.0f, 0.0f; 00011 gyroBias << 0.0f, 0.0f, 0.0f; 00012 gravity << 0.0f, 0.0, 9.8f; 00013 00014 nState = 18; 00015 errState = VectorXf::Zero(nState); 00016 Phat = MatrixXf::Zero(nState, nState); 00017 Q = MatrixXf::Zero(nState, nState); 00018 00019 setBlockDiag(Phat, 0.1f, 0, 2);//position 00020 setBlockDiag(Phat, 0.1f, 3, 5);//velocity 00021 setBlockDiag(Phat, 0.1f, 6, 8);//angle error 00022 setBlockDiag(Phat, 0.1f, 9, 11);//acc bias 00023 setBlockDiag(Phat, 0.1f, 12, 14);//gyro bias 00024 setBlockDiag(Phat, 0.00000001f, 15, 17);//gravity 00025 setBlockDiag(Q, 0.00025f, 3, 5);//velocity 00026 setBlockDiag(Q, 0.005f/57.0f, 6, 8);//angle error 00027 setBlockDiag(Q, 0.001f, 9, 11);//acc bias 00028 setBlockDiag(Q, 0.001f, 12, 14);//gyro bias//positionとgravityはQ項なし 00029 } 00030 00031 00032 void solaESKF::updateNominal(Vector3f acc, Vector3f gyro,float att_dt) 00033 { 00034 Vector3f gyrom = gyro - gyroBias; 00035 Vector3f accm = acc - accBias; 00036 00037 Vector4f qint; 00038 qint << 1.0f, 0.5f*gyrom(0)*att_dt, 0.5f*gyrom(1)*att_dt, 0.5f*gyrom(2)*att_dt; 00039 qhat = quatmultiply(qhat, qint); 00040 qhat.normalize(); 00041 00042 Matrix3f dcm; 00043 computeDcm(dcm, qhat); 00044 Vector3f accned = dcm*accm + gravity; 00045 vihat += accned*att_dt; 00046 00047 pihat += vihat*att_dt + 0.5f*accned*att_dt*att_dt; 00048 } 00049 00050 void solaESKF::updateErrState(Vector3f acc, Vector3f gyro, float att_dt) 00051 { 00052 Vector3f gyrom = gyro - gyroBias; 00053 Vector3f accm = acc - accBias; 00054 00055 Matrix3f dcm; 00056 computeDcm(dcm, qhat); 00057 // Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1))*att_dt; 00058 Matrix3f a2v = -dcm*solaESKF::Matrixcross(accm)*att_dt; 00059 Matrix3f a2v2 = 0.5f*a2v*att_dt; 00060 00061 MatrixXf Fx = MatrixXf::Zero(nState, nState); 00062 //position 00063 Fx(0,0) = 1.0f; 00064 Fx(1,1) = 1.0f; 00065 Fx(2,2) = 1.0f; 00066 Fx(0,3) = 1.0f*att_dt; 00067 Fx(1,4) = 1.0f*att_dt; 00068 Fx(2,5) = 1.0f*att_dt; 00069 for (int i = 0; i < 3; i++){ 00070 for (int j = 0; j < 3; j++){ 00071 Fx(i,j+6) = a2v2(i,j); 00072 Fx(i,j+9) = -0.5f*dcm(i,j)*att_dt*att_dt; 00073 } 00074 Fx(i,i+15) = 0.5f*att_dt*att_dt; 00075 } 00076 00077 00078 //velocity 00079 Fx(3,3) = 1.0f; 00080 Fx(4,4) = 1.0f; 00081 Fx(5,5) = 1.0f; 00082 for (int i = 0; i < 3; i++){ 00083 for (int j = 0; j < 3; j++){ 00084 Fx(i+3,j+6) = a2v(i,j); 00085 Fx(i+3,j+9) = -dcm(i,j)*att_dt; 00086 Fx(i+3,j+12) = -a2v2(i,j); 00087 } 00088 } 00089 Fx(3,15) = 1.0f*att_dt; 00090 Fx(4,16) = 1.0f*att_dt; 00091 Fx(5,17) = 1.0f*att_dt; 00092 00093 //angulat error 00094 Fx(6,6) = 1.0f; 00095 Fx(7,7) = 1.0f; 00096 Fx(8,8) = 1.0f; 00097 Fx(6,7) = gyrom(2)*att_dt; 00098 Fx(6,8) = -gyrom(1)*att_dt; 00099 Fx(7,6) = -gyrom(2)*att_dt; 00100 Fx(7,8) = gyrom(0)*att_dt; 00101 Fx(8,6) = gyrom(1)*att_dt; 00102 Fx(8,7) = -gyrom(0)*att_dt; 00103 Fx(6,12) = -1.0f*att_dt; 00104 Fx(7,13) = -1.0f*att_dt; 00105 Fx(8,14) = -1.0f*att_dt; 00106 00107 //acc bias 00108 Fx(9,9) = 1.0f; 00109 Fx(10,10) = 1.0f; 00110 Fx(11,11) = 1.0f; 00111 00112 //gyro bias 00113 Fx(12,12) = 1.0f; 00114 Fx(13,13) = 1.0f; 00115 Fx(14,14) = 1.0f; 00116 00117 //gravity bias 00118 Fx(15,15) = 1.0f; 00119 Fx(16,16) = 1.0f; 00120 Fx(17,17) = 1.0f; 00121 00122 //errState = Fx * errState; 00123 Phat = Fx*Phat*Fx.transpose(); 00124 for (int i = 0; i < nState; i++){ 00125 if(i>2 && i<9){ 00126 Phat(i,i) += Q(i,i)*att_dt; 00127 }else if(i>8 && i<15){ 00128 Phat(i,i) += Q(i,i)* att_dt*att_dt; 00129 } 00130 } 00131 } 00132 00133 void solaESKF::updateAcc(Vector3f acc, Matrix3f R) 00134 { 00135 Vector3f accm = acc - accBias; 00136 Matrix3f dcm; 00137 computeDcm(dcm, qhat); 00138 Matrix3f tdcm = dcm.transpose(); 00139 Vector3f tdcm_g = tdcm*gravity; 00140 Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g); 00141 00142 MatrixXf H = MatrixXf::Zero(3,nState); 00143 for (int i = 0; i < 3; i++){ 00144 for (int j = 0; j < 3; j++){ 00145 H(i,j+6) = rotgrav(i,j); 00146 H(i,j+15) = tdcm(i,j); 00147 } 00148 } 00149 00150 H(0,9) = -1.0f; 00151 H(1,10) = -1.0f; 00152 H(2,11) = -1.0f; 00153 00154 // Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); 00155 MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); 00156 Vector3f zacc = -accm-tdcm*gravity; 00157 Vector3f z; 00158 z = zacc; 00159 errState = K * z; 00160 Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; 00161 00162 fuseErr2Nominal(); 00163 } 00164 00165 void solaESKF::updateHeading(float a, Matrix<float, 1, 1> R) 00166 { 00167 float q0 = qhat(0); 00168 float q1 = qhat(1); 00169 float q2 = qhat(2); 00170 float q3 = qhat(3); 00171 00172 bool canUseA = false; 00173 const float SA0 = 2.0f*q3; 00174 const float SA1 = 2.0f*q2; 00175 const float SA2 = SA0*q0 + SA1*q1; 00176 const float SA3 = q0*q0 + q1*q1 - q2*q2 - q3*q3; 00177 float SA4, SA5_inv; 00178 if ((SA3*SA3) > 1e-6f) { 00179 SA4 = 1.0f/(SA3*SA3); 00180 SA5_inv = SA2*SA2*SA4 + 1.0f; 00181 canUseA = std::abs(SA5_inv) > 1e-6f; 00182 } 00183 00184 bool canUseB = false; 00185 const float SB0 = 2.0f*q0; 00186 const float SB1 = 2.0f*q1; 00187 const float SB2 = SB0*q3 + SB1*q2; 00188 const float SB4 = q0*q0 + q1*q1 - q2*q2 - q3*q3; 00189 float SB3, SB5_inv; 00190 if ((SB2*SB2) > 1e-6f) { 00191 SB3 = 1.0f/(SB2*SB2); 00192 SB5_inv = SB3*SB4*SB4 + 1; 00193 canUseB = std::abs(SB5_inv) > 1e-6f; 00194 } 00195 00196 MatrixXf Hh = MatrixXf::Zero(1,4); 00197 00198 if (canUseA && (!canUseB || std::abs(SA5_inv) >= std::abs(SB5_inv))) { 00199 const float SA5 = 1.0f/SA5_inv; 00200 const float SA6 = 1.0f/SA3; 00201 const float SA7 = SA2*SA4; 00202 const float SA8 = 2.0f*SA7; 00203 const float SA9 = 2.0f*SA6; 00204 00205 Hh(0,0) = SA5*(SA0*SA6 - SA8*q0); 00206 Hh(0,1) = SA5*(SA1*SA6 - SA8*q1); 00207 Hh(0,2) = SA5*(SA1*SA7 + SA9*q1); 00208 Hh(0,3) = SA5*(SA0*SA7 + SA9*q0); 00209 } else if (canUseB && (!canUseA || std::abs(SB5_inv) > std::abs(SA5_inv))) { 00210 const float SB5 = 1.0f/SB5_inv; 00211 const float SB6 = 1.0f/SB2; 00212 const float SB7 = SB3*SB4; 00213 const float SB8 = 2.0f*SB7; 00214 const float SB9 = 2.0f*SB6; 00215 00216 Hh(0,0) = -SB5*(SB0*SB6 - SB8*q3); 00217 Hh(0,1) = -SB5*(SB1*SB6 - SB8*q2); 00218 Hh(0,2) = -SB5*(-SB1*SB7 - SB9*q2); 00219 Hh(0,3) = -SB5*(-SB0*SB7 - SB9*q3); 00220 } else { 00221 return; 00222 } 00223 00224 MatrixXf Hdq = MatrixXf::Zero(4,3); 00225 Hdq << -0.5f*q1, -0.5f*q2, -0.5f*q3, 00226 0.5f*q0, -0.5f*q3, 0.5f*q2, 00227 0.5f*q3, 0.5f*q0, -0.5f*q1, 00228 -0.5f*q2, 0.5f*q1, 0.5f*q0; 00229 00230 MatrixXf Hpart = Hh*Hdq; 00231 MatrixXf H=MatrixXf::Zero(1,nState); 00232 for(int j=0; j<3; j++){ 00233 H(0,j+6) = Hpart(0,j); 00234 } 00235 00236 const float psi = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3)); 00237 Matrix<float, 1, 1> z; 00238 z << std::atan2(std::sin(a-psi), std::cos(a-psi)); 00239 MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); 00240 errState = K * z; 00241 Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; 00242 00243 fuseErr2Nominal(); 00244 } 00245 00246 void solaESKF::updateIMU(float palt,Vector3f acc,float heading, Matrix<float, 5, 5> R) 00247 { 00248 MatrixXf H = MatrixXf::Zero(5,nState); 00249 00250 00251 H(0,2) = 1.0f; 00252 00253 Vector3f accm = acc - accBias; 00254 Matrix3f dcm; 00255 computeDcm(dcm, qhat); 00256 Matrix3f tdcm = dcm.transpose(); 00257 Vector3f tdcm_g = tdcm*gravity; 00258 Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g); 00259 00260 for (int i = 0; i < 3; i++){ 00261 for (int j = 0; j < 3; j++){ 00262 H(i+1,j+6) = rotgrav(i,j); 00263 H(i+1,j+15) = tdcm(i,j); 00264 } 00265 } 00266 00267 H(1,9) = -1.0f; 00268 H(2,10) = -1.0f; 00269 H(3,11) = -1.0f; 00270 00271 float q0 = qhat(0); 00272 float q1 = qhat(1); 00273 float q2 = qhat(2); 00274 float q3 = qhat(3); 00275 00276 bool canUseA = false; 00277 const float SA0 = 2.0f*q3; 00278 const float SA1 = 2.0f*q2; 00279 const float SA2 = SA0*q0 + SA1*q1; 00280 const float SA3 = q0*q0 + q1*q1 - q2*q2 - q3*q3; 00281 float SA4, SA5_inv; 00282 if ((SA3*SA3) > 1e-6f) { 00283 SA4 = 1.0f/(SA3*SA3); 00284 SA5_inv = SA2*SA2*SA4 + 1.0f; 00285 canUseA = std::abs(SA5_inv) > 1e-6f; 00286 } 00287 00288 bool canUseB = false; 00289 const float SB0 = 2.0f*q0; 00290 const float SB1 = 2.0f*q1; 00291 const float SB2 = SB0*q3 + SB1*q2; 00292 const float SB4 = q0*q0 + q1*q1 - q2*q2 - q3*q3; 00293 float SB3, SB5_inv; 00294 if ((SB2*SB2) > 1e-6f) { 00295 SB3 = 1.0f/(SB2*SB2); 00296 SB5_inv = SB3*SB4*SB4 + 1; 00297 canUseB = std::abs(SB5_inv) > 1e-6f; 00298 } 00299 00300 MatrixXf Hh = MatrixXf::Zero(1,4); 00301 00302 if (canUseA && (!canUseB || std::abs(SA5_inv) >= std::abs(SB5_inv))) { 00303 const float SA5 = 1.0f/SA5_inv; 00304 const float SA6 = 1.0f/SA3; 00305 const float SA7 = SA2*SA4; 00306 const float SA8 = 2.0f*SA7; 00307 const float SA9 = 2.0f*SA6; 00308 00309 Hh(0,0) = SA5*(SA0*SA6 - SA8*q0); 00310 Hh(0,1) = SA5*(SA1*SA6 - SA8*q1); 00311 Hh(0,2) = SA5*(SA1*SA7 + SA9*q1); 00312 Hh(0,3) = SA5*(SA0*SA7 + SA9*q0); 00313 } else if (canUseB && (!canUseA || std::abs(SB5_inv) > std::abs(SA5_inv))) { 00314 const float SB5 = 1.0f/SB5_inv; 00315 const float SB6 = 1.0f/SB2; 00316 const float SB7 = SB3*SB4; 00317 const float SB8 = 2.0f*SB7; 00318 const float SB9 = 2.0f*SB6; 00319 00320 Hh(0,0) = -SB5*(SB0*SB6 - SB8*q3); 00321 Hh(0,1) = -SB5*(SB1*SB6 - SB8*q2); 00322 Hh(0,2) = -SB5*(-SB1*SB7 - SB9*q2); 00323 Hh(0,3) = -SB5*(-SB0*SB7 - SB9*q3); 00324 } else { 00325 return; 00326 } 00327 00328 MatrixXf Hdq = MatrixXf::Zero(4,3); 00329 Hdq << -0.5f*q1, -0.5f*q2, -0.5f*q3, 00330 0.5f*q0, -0.5f*q3, 0.5f*q2, 00331 0.5f*q3, 0.5f*q0, -0.5f*q1, 00332 -0.5f*q2, 0.5f*q1, 0.5f*q0; 00333 00334 MatrixXf Hpart = Hh*Hdq; 00335 for(int j=0; j<3; j++){ 00336 H(4,j+6) = Hpart(0,j); 00337 } 00338 00339 00340 00341 const float psi = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3)); 00342 Vector3f zacc = -accm-tdcm*gravity; 00343 VectorXf z = VectorXf::Zero(5); 00344 z << palt-pihat(2),zacc(0),zacc(1),zacc(2),std::atan2(std::sin(heading-psi), std::cos(heading-psi)); 00345 MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); 00346 errState = K * z; 00347 Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; 00348 00349 fuseErr2Nominal(); 00350 } 00351 void solaESKF::updateGPSPosition(Vector3f posgps,float palt,Matrix3f R) 00352 { 00353 MatrixXf H = MatrixXf::Zero(3,nState); 00354 H(0,0) = 1.0f; 00355 H(1,1) = 1.0f; 00356 H(2,2) = 1.0f; 00357 00358 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; 00359 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); 00360 MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); 00361 Vector3f z; 00362 z << posgps(0)-pihat(0), posgps(1)-pihat(1), palt - pihat(2); 00363 errState = K * z; 00364 Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; 00365 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K); 00366 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); 00367 fuseErr2Nominal(); 00368 } 00369 void solaESKF::updateGPSVelocity(Vector3f velgps,Matrix3f R) 00370 { 00371 MatrixXf H = MatrixXf::Zero(3,nState); 00372 H(0,3) = 1.0f; 00373 H(1,4) = 1.0f; 00374 H(2,5) = 1.0f; 00375 00376 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; 00377 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); 00378 MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); 00379 Vector3f z; 00380 z << velgps(0)-vihat(0), velgps(1)-vihat(1), velgps(2)-vihat(2); 00381 errState = K * z; 00382 Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; 00383 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K); 00384 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); 00385 fuseErr2Nominal(); 00386 } 00387 00388 void solaESKF::updateWhole(Vector3f posgps, float palt, Vector3f velgps,Vector3f acc,float heading, MatrixXf R) 00389 { 00390 MatrixXf H = MatrixXf::Zero(9,nState); 00391 H(0,0) = 1.0f; 00392 H(1,1) = 1.0f; 00393 H(2,2) = 1.0f; 00394 H(3,3) = 1.0f; 00395 H(4,4) = 1.0f; 00396 00397 Vector3f accm = acc - accBias; 00398 Matrix3f dcm; 00399 computeDcm(dcm, qhat); 00400 Matrix3f tdcm = dcm.transpose(); 00401 Vector3f tdcm_g = tdcm*gravity; 00402 Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g); 00403 00404 for (int i = 0; i < 3; i++){ 00405 for (int j = 0; j < 3; j++){ 00406 H(i+5,j+6) = rotgrav(i,j); 00407 H(i+5,j+15) = tdcm(i,j); 00408 } 00409 } 00410 00411 H(5,9) = -1.0f; 00412 H(6,10) = -1.0f; 00413 H(7,11) = -1.0f; 00414 00415 float q0 = qhat(0); 00416 float q1 = qhat(1); 00417 float q2 = qhat(2); 00418 float q3 = qhat(3); 00419 00420 bool canUseA = false; 00421 const float SA0 = 2.0f*q3; 00422 const float SA1 = 2.0f*q2; 00423 const float SA2 = SA0*q0 + SA1*q1; 00424 const float SA3 = q0*q0 + q1*q1 - q2*q2 - q3*q3; 00425 float SA4, SA5_inv; 00426 if ((SA3*SA3) > 1e-6f) { 00427 SA4 = 1.0f/(SA3*SA3); 00428 SA5_inv = SA2*SA2*SA4 + 1.0f; 00429 canUseA = std::abs(SA5_inv) > 1e-6f; 00430 } 00431 00432 bool canUseB = false; 00433 const float SB0 = 2.0f*q0; 00434 const float SB1 = 2.0f*q1; 00435 const float SB2 = SB0*q3 + SB1*q2; 00436 const float SB4 = q0*q0 + q1*q1 - q2*q2 - q3*q3; 00437 float SB3, SB5_inv; 00438 if ((SB2*SB2) > 1e-6f) { 00439 SB3 = 1.0f/(SB2*SB2); 00440 SB5_inv = SB3*SB4*SB4 + 1; 00441 canUseB = std::abs(SB5_inv) > 1e-6f; 00442 } 00443 00444 MatrixXf Hh = MatrixXf::Zero(1,4); 00445 00446 if (canUseA && (!canUseB || std::abs(SA5_inv) >= std::abs(SB5_inv))) { 00447 const float SA5 = 1.0f/SA5_inv; 00448 const float SA6 = 1.0f/SA3; 00449 const float SA7 = SA2*SA4; 00450 const float SA8 = 2.0f*SA7; 00451 const float SA9 = 2.0f*SA6; 00452 00453 Hh(0,0) = SA5*(SA0*SA6 - SA8*q0); 00454 Hh(0,1) = SA5*(SA1*SA6 - SA8*q1); 00455 Hh(0,2) = SA5*(SA1*SA7 + SA9*q1); 00456 Hh(0,3) = SA5*(SA0*SA7 + SA9*q0); 00457 } else if (canUseB && (!canUseA || std::abs(SB5_inv) > std::abs(SA5_inv))) { 00458 const float SB5 = 1.0f/SB5_inv; 00459 const float SB6 = 1.0f/SB2; 00460 const float SB7 = SB3*SB4; 00461 const float SB8 = 2.0f*SB7; 00462 const float SB9 = 2.0f*SB6; 00463 00464 Hh(0,0) = -SB5*(SB0*SB6 - SB8*q3); 00465 Hh(0,1) = -SB5*(SB1*SB6 - SB8*q2); 00466 Hh(0,2) = -SB5*(-SB1*SB7 - SB9*q2); 00467 Hh(0,3) = -SB5*(-SB0*SB7 - SB9*q3); 00468 } else { 00469 return; 00470 } 00471 00472 MatrixXf Hdq = MatrixXf::Zero(4,3); 00473 Hdq << -0.5f*q1, -0.5f*q2, -0.5f*q3, 00474 0.5f*q0, -0.5f*q3, 0.5f*q2, 00475 0.5f*q3, 0.5f*q0, -0.5f*q1, 00476 -0.5f*q2, 0.5f*q1, 0.5f*q0; 00477 00478 MatrixXf Hpart = Hh*Hdq; 00479 for(int j=0; j<3; j++){ 00480 H(8,j+6) = Hpart(0,j); 00481 } 00482 00483 const float psi = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3)); 00484 Vector3f zacc = -accm-tdcm*gravity; 00485 VectorXf z = VectorXf::Zero(9); 00486 z << posgps(0)-pihat(0), posgps(1)-pihat(1), palt-pihat(2), velgps(0)-vihat(0), velgps(1)-vihat(1), zacc(0),zacc(1),zacc(2),std::atan2(std::sin(heading-psi), std::cos(heading-psi)); 00487 MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); 00488 errState = K * z; 00489 Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; 00490 00491 fuseErr2Nominal(); 00492 00493 } 00494 00495 void solaESKF::updateGPS(Vector3f posgps, float palt, Vector3f velgps, MatrixXf R) 00496 { 00497 MatrixXf H = MatrixXf::Zero(5,nState); 00498 H(0,0) = 1.0f; 00499 H(1,1) = 1.0f; 00500 H(2,2) = 1.0f; 00501 H(3,3) = 1.0f; 00502 H(4,4) = 1.0f; 00503 MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); 00504 Matrix<float, 5, 1> z; 00505 z << posgps(0)-pihat(0), posgps(1)-pihat(1), palt-pihat(2), velgps(0)-vihat(0), velgps(1)-vihat(1); 00506 errState = K * z; 00507 Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; 00508 fuseErr2Nominal(); 00509 } 00510 00511 00512 Vector3f solaESKF::computeAngles() 00513 { 00514 00515 Vector3f euler; 00516 euler(0) = std::atan2(qhat(0)*qhat(1) + qhat(2)*qhat(3), 0.5f - qhat(1)*qhat(1) - qhat(2)*qhat(2)); 00517 euler(1) = std::asin(-2.0f * (qhat(1)*qhat(3) - qhat(0)*qhat(2))); 00518 euler(2) = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3)); 00519 return euler; 00520 } 00521 00522 00523 void solaESKF::fuseErr2Nominal() 00524 { 00525 //position 00526 pihat(0) += errState(0); 00527 pihat(1) += errState(1); 00528 pihat(2) += errState(2); 00529 00530 //velocity 00531 vihat(0) += errState(3); 00532 vihat(1) += errState(4); 00533 vihat(2) += errState(5); 00534 00535 //angle error 00536 Vector4f qerr; 00537 qerr << 1.0f, 0.5f*errState(6), 0.5f*errState(7), 0.5f*errState(8); 00538 qhat = quatmultiply(qhat, qerr); 00539 qhat.normalize(); 00540 00541 //acc bias 00542 accBias(0) += errState(9); 00543 accBias(1) += errState(10); 00544 accBias(2) += errState(11); 00545 00546 //gyro bias 00547 gyroBias(0) += errState(12); 00548 gyroBias(1) += errState(13); 00549 gyroBias(2) += errState(14); 00550 00551 //gravity bias 00552 gravity(0) += errState(15); 00553 gravity(1) += errState(16); 00554 gravity(2) += errState(17); 00555 00556 errState = VectorXf::Zero(nState); 00557 } 00558 00559 void solaESKF::fuseCenter2Nominal(VectorXf errVal) 00560 { 00561 //position 00562 pihat(0) += errVal(0); 00563 pihat(1) += errVal(1); 00564 pihat(2) += errVal(2); 00565 00566 //velocity 00567 vihat(0) += errVal(3); 00568 vihat(1) += errVal(4); 00569 vihat(2) += errVal(5); 00570 00571 //angle error 00572 Vector4f qerr; 00573 qerr << 1.0f, 0.5f*errVal(6), 0.5f*errVal(7), 0.5f*errVal(8); 00574 qhat = quatmultiply(qhat, qerr); 00575 qhat.normalize(); 00576 00577 //acc bias 00578 accBias(0) += errVal(9); 00579 accBias(1) += errVal(10); 00580 accBias(2) += errVal(11); 00581 00582 //gyro bias 00583 gyroBias(0) += errVal(12); 00584 gyroBias(1) += errVal(13); 00585 gyroBias(2) += errVal(14); 00586 00587 //gravity bias 00588 //gravity(0) += errVal(15); 00589 //gravity(1) += errVal(16); 00590 //gravity(2) += errVal(17); 00591 } 00592 00593 Vector4f solaESKF::quatmultiply(Vector4f p, Vector4f q) 00594 { 00595 Vector4f qout; 00596 qout(0) = p(0)*q(0) - p(1)*q(1) - p(2)*q(2) - p(3)*q(3); 00597 qout(1) = p(0)*q(1) + p(1)*q(0) + p(2)*q(3) - p(3)*q(2); 00598 qout(2) = p(0)*q(2) - p(1)*q(3) + p(2)*q(0) + p(3)*q(1); 00599 qout(3) = p(0)*q(3) + p(1)*q(2) - p(2)*q(1) + p(3)*q(0); 00600 // qout.normalize(); 00601 return qout; 00602 } 00603 00604 void solaESKF::computeDcm(Matrix3f& dcm, Vector4f quat) 00605 { 00606 // dcm(1,1) = quat(1,1)*quat(1,1) + quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1); 00607 // dcm(1,2) = 2.0f*(quat(2,1)*quat(3,1) - quat(1,1)*quat(4,1)); 00608 // dcm(1,3) = 2.0f*(quat(2,1)*quat(4,1) + quat(1,1)*quat(3,1)); 00609 // dcm(2,1) = 2.0f*(quat(2,1)*quat(3,1) + quat(1,1)*quat(4,1)); 00610 // dcm(2,2) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) + quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1); 00611 // dcm(2,3) = 2.0f*(quat(3,1)*quat(4,1) - quat(1,1)*quat(2,1)); 00612 // dcm(3,1) = 2.0f*(quat(2,1)*quat(4,1) - quat(1,1)*quat(3,1)); 00613 // dcm(3,2) = 2.0f*(quat(3,1)*quat(4,1) + quat(1,1)*quat(2,1)); 00614 // dcm(3,3) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) + quat(4,1)*quat(4,1); 00615 00616 dcm(0,0) = quat(0)*quat(0) + quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3); 00617 dcm(0,1) = 2.0f*(quat(1)*quat(2) - quat(0)*quat(3)); 00618 dcm(0,2) = 2.0f*(quat(1)*quat(3) + quat(0)*quat(2)); 00619 dcm(1,0) = 2.0f*(quat(1)*quat(2) + quat(0)*quat(3)); 00620 dcm(1,1) = quat(0)*quat(0) - quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3); 00621 dcm(1,2) = 2.0f*(quat(2)*quat(3) - quat(0)*quat(1)); 00622 dcm(2,0) = 2.0f*(quat(1)*quat(3) - quat(0)*quat(2)); 00623 dcm(2,1) = 2.0f*(quat(2)*quat(3) + quat(0)*quat(1)); 00624 dcm(2,2) = quat(0)*quat(0) - quat(1)*quat(1) - quat(2)*quat(2) + quat(3)*quat(3); 00625 } 00626 00627 void solaESKF::setQhat(float ex,float ey,float ez) 00628 { 00629 float cos_z_2 = std::cos(0.5f*ez); 00630 float cos_y_2 = std::cos(0.5f*ey); 00631 float cos_x_2 = std::cos(0.5f*ex); 00632 00633 float sin_z_2 = std::sin(0.5f*ez); 00634 float sin_y_2 = std::sin(0.5f*ey); 00635 float sin_x_2 = std::sin(0.5f*ex); 00636 00637 // and now compute quaternion 00638 qhat(0) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2; 00639 qhat(1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2; 00640 qhat(2) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2; 00641 qhat(3) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; 00642 } 00643 00644 Vector3f solaESKF::calcDynAcc(Vector3f acc) 00645 { 00646 Vector3f accm = acc - accBias; 00647 Matrix3f dcm; 00648 computeDcm(dcm, qhat); 00649 Matrix3f tdcm = dcm.transpose(); 00650 00651 Vector3f dynAcc = accm+tdcm*gravity; 00652 return dynAcc; 00653 } 00654 00655 Vector3f solaESKF::vector2Body(Vector3f veci) 00656 { 00657 Matrix3f dcm; 00658 computeDcm(dcm, qhat); 00659 Matrix3f tdcm = dcm.transpose(); 00660 00661 return tdcm*veci ; 00662 } 00663 00664 Vector3f solaESKF::vector2NED(Vector3f vecb) 00665 { 00666 Matrix3f dcm; 00667 computeDcm(dcm, qhat); 00668 00669 return dcm*vecb; 00670 } 00671 00672 00673 00674 void solaESKF::setGravity(float gx,float gy,float gz) 00675 { 00676 gravity(0) = gx; 00677 gravity(1) = gy; 00678 gravity(2) = gz; 00679 } 00680 00681 Vector3f solaESKF::getPihat() 00682 { 00683 return pihat; 00684 } 00685 Vector3f solaESKF::getVihat() 00686 { 00687 return vihat; 00688 } 00689 Vector4f solaESKF::getQhat() 00690 { 00691 return qhat; 00692 } 00693 Vector3f solaESKF::getAccBias() 00694 { 00695 return accBias; 00696 } 00697 Vector3f solaESKF::getGyroBias() 00698 { 00699 return gyroBias; 00700 } 00701 Vector3f solaESKF::getGravity() 00702 { 00703 return gravity; 00704 } 00705 00706 VectorXf solaESKF::getErrState() 00707 { 00708 return errState; 00709 } 00710 00711 VectorXf solaESKF::getState() 00712 { 00713 VectorXf state = VectorXf::Zero(nState+1); 00714 for (int i = 0; i < 3; i++){ 00715 state(i) = pihat(i); 00716 state(i+3) = vihat(i); 00717 } 00718 for (int i = 0; i < 4; i++){ 00719 state(i+6) = qhat(i); 00720 } 00721 for (int i = 0; i < 3; i++){ 00722 state(i+10) = accBias(i); 00723 state(i+13) = gyroBias(i); 00724 state(i+16) = gravity(i); 00725 } 00726 return state; 00727 } 00728 00729 VectorXf solaESKF::getVariance() 00730 { 00731 VectorXf variance = VectorXf::Zero(nState); 00732 for (int i = 0; i < nState; i++){ 00733 variance(i) = Phat(i,i); 00734 } 00735 return variance; 00736 } 00737 00738 void solaESKF::setPhatPosition(float valNE,float valD) 00739 { 00740 setBlockDiag(Phat, valNE, 0,2); 00741 Phat(2,2) = valD; 00742 } 00743 void solaESKF::setPhatVelocity(float valNE,float valD) 00744 { 00745 setBlockDiag(Phat, valNE, 3,5); 00746 Phat(5,5) = valD; 00747 } 00748 void solaESKF::setPhatAngleError(float val) 00749 { 00750 setBlockDiag(Phat, val, 6,8); 00751 } 00752 void solaESKF::setPhatAccBias(float val) 00753 { 00754 setBlockDiag(Phat, val, 9,11); 00755 } 00756 void solaESKF::setPhatGyroBias(float val) 00757 { 00758 setBlockDiag(Phat, val, 12,14); 00759 } 00760 void solaESKF::setPhatGravity(float val) 00761 { 00762 setBlockDiag(Phat, val, 15,17); 00763 } 00764 00765 00766 void solaESKF::setQVelocity(float valNE,float valD) 00767 { 00768 setBlockDiag(Q, valNE, 3, 5); 00769 Q(5,5) = valD; 00770 } 00771 void solaESKF::setQAngleError(float val) 00772 { 00773 setBlockDiag(Q, val, 6, 8); 00774 } 00775 void solaESKF::setQAccBias(float val) 00776 { 00777 setBlockDiag(Q, val, 9, 11); 00778 } 00779 void solaESKF::setQGyroBias(float val) 00780 { 00781 setBlockDiag(Q, val, 12, 14); 00782 } 00783 00784 void solaESKF::setDiag(Matrix3f& mat, float val){ 00785 for (int i = 0; i < mat.cols(); i++){ 00786 for (int j = 0; j < mat.rows(); j++){ 00787 mat(i,j) = 0.0f; 00788 } 00789 } 00790 for (int i = 0; i < mat.cols(); i++){ 00791 mat(i,i) = val; 00792 } 00793 } 00794 00795 void solaESKF::setDiag(MatrixXf& mat, float val){ 00796 for (int i = 0; i < mat.cols(); i++){ 00797 for (int j = 0; j < mat.rows(); j++){ 00798 mat(i,j) = 0.0f; 00799 } 00800 } 00801 for (int i = 0; i < mat.cols(); i++) 00802 { 00803 mat(i, i) = val; 00804 } 00805 } 00806 00807 void solaESKF::setBlockDiag(MatrixXf& mat, float val, int startIndex, int endIndex){ 00808 00809 for (int i = startIndex; i < endIndex+1; i++){ 00810 mat(i,i) = val; 00811 } 00812 } 00813 00814 void solaESKF::setPihat(float pi_x, float pi_y, float pi_z) 00815 { 00816 pihat(0) = pi_x; 00817 pihat(1) = pi_y; 00818 pihat(2) = pi_z; 00819 } 00820 00821 Matrix3f solaESKF::Matrixcross(Vector3f v) 00822 { 00823 Matrix3f m = Matrix3f::Zero(); 00824 m << 0.0f, -v(2), v(1), 00825 v(2), 0.0f, -v(0), 00826 -v(1), v(0), 0.0f; 00827 return m; 00828 } 00829 /* 00830 void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R) 00831 { 00832 Matrix accm = acc - accBias; 00833 Matrix tdcm(3,3); 00834 computeDcm(tdcm, qhat); 00835 tdcm = MatrixMath::Transpose(tdcm); 00836 Matrix tdcm_g = tdcm*gravity; 00837 Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1)); 00838 00839 Matrix H(4,nState); 00840 for (int i = 1; i < 4; i++){ 00841 for (int j = 1; j < 4; j++){ 00842 H(i,j+6) = a2v(i,j); 00843 H(i,j+15) = tdcm(i,j); 00844 } 00845 } 00846 H(1,10) = -1.0f; 00847 H(2,11) = -1.0f; 00848 H(3,12) = -1.0f; 00849 H(4,3) = 1.0f; 00850 00851 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; 00852 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); 00853 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); 00854 Matrix zacc = -accm-tdcm*gravity; 00855 Matrix z(4,1); 00856 z << zacc(1,1) << zacc(2,1) << zacc(3,1) << palt - pihat(3,1); 00857 errState = K * z; 00858 Phat = (MatrixMath::Eye(nState)-K*H)*Phat; 00859 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K); 00860 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); 00861 fuseErr2Nominal(); 00862 } 00863 00864 void solaESKF::updateGyroConstraints(Matrix gyro,Matrix R) 00865 { 00866 Matrix gyrom = gyro - gyroBias; 00867 Matrix dcm(3,3); 00868 computeDcm(dcm, qhat); 00869 Matrix a2v = dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1)); 00870 00871 Matrix H(2,nState); 00872 for (int i = 1; i < 3; i++){ 00873 for (int j = 1; j < 4; j++){ 00874 H(i,j+6) = a2v(i,j); 00875 H(i,j+12) = -dcm(i,j); 00876 } 00877 } 00878 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; 00879 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A)); 00880 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); 00881 00882 Matrix z3 = -dcm*gyrom; 00883 Matrix z(2,1); 00884 z << z3(1,1) << z3(2,1); 00885 errState = K * z; 00886 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat; 00887 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K); 00888 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); 00889 fuseErr2Nominal(); 00890 } 00891 void solaESKF::updateMag(Matrix mag, Matrix R) 00892 { 00893 Matrix dcm(3,3); 00894 computeDcm(dcm, qhat); 00895 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1)); 00896 00897 Matrix H(2,nState); 00898 for (int i = 1; i < 3; i++){ 00899 for (int j = 1; j < 4; j++){ 00900 H(i,j+6) = a2v(i,j); 00901 } 00902 } 00903 H(1,19) = 1.0f; 00904 //H(3,20) = 1.0f; 00905 00906 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; 00907 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); 00908 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); 00909 Matrix zmag = -dcm*mag-magField; 00910 Matrix z(2,1); 00911 z << zmag(1,1) << zmag(2,1); 00912 errState = K * z; 00913 Phat = (MatrixMath::Eye(nState)-K*H)*Phat; 00914 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K); 00915 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); 00916 fuseErr2Nominal(); 00917 } 00918 void solaESKF::updateMag(Matrix mag,float palt, Matrix R) 00919 { 00920 Matrix dcm(3,3); 00921 computeDcm(dcm, qhat); 00922 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1)); 00923 00924 Matrix H(3,nState); 00925 for (int i = 1; i < 3; i++){ 00926 for (int j = 1; j < 4; j++){ 00927 H(i,j+6) = a2v(i,j); 00928 } 00929 } 00930 H(1,19) = 1.0f; 00931 H(3,3) = 1.0f; 00932 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; 00933 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); 00934 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); 00935 Matrix zmag = -dcm*mag-magField; 00936 Matrix z(3,1); 00937 z << zmag(1,1) << zmag(2,1) << palt - pihat(3,1); 00938 errState = K * z; 00939 Phat = (MatrixMath::Eye(nState)-K*H)*Phat; 00940 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K); 00941 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); 00942 00943 fuseErr2Nominal(); 00944 } 00945 void solaESKF::updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R) 00946 { 00947 00948 Matrix dcm(3,3); 00949 computeDcm(dcm, qhat); 00950 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1)); 00951 00952 00953 Matrix H(3,nState); 00954 H(1,4) = 1.0f; 00955 H(2,5) = 1.0f; 00956 00957 for (int j = 1; j < 4; j++){ 00958 H(3,j+6) = a2v(2,j); 00959 } 00960 //H(3,19) = 1.0f; 00961 00962 Matrix zmag = -dcm*mag-magField; 00963 00964 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; 00965 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); 00966 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); 00967 Matrix z(3,1); 00968 z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << zmag(2,1); 00969 errState = K * z; 00970 Phat = (MatrixMath::Eye(nState)-K*H)*Phat; 00971 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K); 00972 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); 00973 fuseErr2Nominal(); 00974 } 00975 00976 void solaESKF::updateGPSPosition(Matrix posgps,float palt,Matrix R) 00977 { 00978 Matrix H(3,nState); 00979 H(1,1) = 1.0f; 00980 H(2,2) = 1.0f; 00981 H(3,3) = 1.0f; 00982 00983 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; 00984 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); 00985 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); 00986 Matrix z(3,1); 00987 z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1); 00988 errState = K * z; 00989 Phat = (MatrixMath::Eye(nState)-K*H)*Phat; 00990 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K); 00991 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); 00992 fuseErr2Nominal(); 00993 } 00994 00995 void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R) 00996 { 00997 Matrix accm = acc - accBias; 00998 Matrix magm = mag - magBias; 00999 Matrix dcm(3,3); 01000 computeDcm(dcm, qhat); 01001 Matrix tdcm = MatrixMath::Transpose(dcm); 01002 Matrix tdcm_g = tdcm*gravity; 01003 Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1)); 01004 Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1)); 01005 01006 Matrix H(5,nState); 01007 for (int i = 1; i < 4; i++){ 01008 for (int j = 1; j < 4; j++){ 01009 H(i,j+6) = rotgrav(i,j); 01010 } 01011 H(i,16) = tdcm(i,3); 01012 } 01013 01014 H(1,10) = -1.0f; 01015 H(2,11) = -1.0f; 01016 H(3,12) = -1.0f; 01017 01018 Matrix magned = dcm*magm; 01019 float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1)); 01020 01021 for(int j = 1; j < 4; j++){ 01022 H(4,j+6) = rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx; 01023 H(4,j+16) = -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx; 01024 H(5,j+6) = rotmag(2,j); 01025 H(5,j+16) = -dcm(2,j); 01026 } 01027 01028 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); 01029 Matrix zacc = -accm-tdcm*gravity; 01030 Matrix zmag = dcm*magm; 01031 Matrix z(5,1); 01032 z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx) << -zmag(2,1); 01033 twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx)); 01034 errState = K * z; 01035 Phat = (MatrixMath::Eye(nState)-K*H)*Phat; 01036 01037 fuseErr2Nominal(); 01038 } 01039 float q0 = qhat(1,1); 01040 float q1 = qhat(2,1); 01041 float q2 = qhat(3,1); 01042 float q3 = qhat(4,1); 01043 01044 float d0 = (-q3*q3-q2*q2+q1*q1+q0*q0); 01045 float q0q3q1q2 = (q0*q3+q1*q2); 01046 float h1lower = 2.0f*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f); 01047 01048 float d1 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f); 01049 float d2 = d0*d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f); 01050 float d3 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f); 01051 01052 01053 01054 Matrix Hh(2,4); 01055 Hh(1,1) = -(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower; 01056 Hh(1,2) = -(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower; 01057 Hh(1,3) = -(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower; 01058 Hh(1,4) = -(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower; 01059 01060 Hh(2,1) = 2.0f*q3/d1-4.0f*q0*q0q3q1q2/d2-q0q3q1q2*(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3; 01061 Hh(2,2) = 2.0f*q2/d1-4.0f*q1*q0q3q1q2/d2-q0q3q1q2*(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3; 01062 Hh(2,3) = 2.0f*q1/d1+4.0f*q2*q0q3q1q2/d2-q0q3q1q2*(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3; 01063 Hh(2,4) = 2.0f*q0/d1+4.0f*q3*q0q3q1q2/d2-q0q3q1q2*(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3; 01064 */
Generated on Wed Jan 11 2023 04:11:21 by
1.7.2