Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 83:8f6ae61d47ac
- Parent:
- 82:662b810155e9
- Child:
- 84:fb14acabefd3
diff -r 662b810155e9 -r 8f6ae61d47ac solaESKF.cpp --- a/solaESKF.cpp Wed Jun 29 01:39:26 2022 +0000 +++ b/solaESKF.cpp Wed Jun 29 07:56:42 2022 +0000 @@ -243,8 +243,12 @@ fuseErr2Nominal(); } -void solaESKF::updateIMU(Vector3f acc,float heading, Matrix<float, 4, 4> R) +void solaESKF::updateIMU(float palt,Vector3f acc,float heading, Matrix<float, 5, 5> R) { + MatrixXf H = MatrixXf::Zero(5,nState); + + + H(0,2) = 1.0f; Vector3f accm = acc - accBias; Matrix3f dcm; @@ -253,17 +257,16 @@ Vector3f tdcm_g = tdcm*gravity; Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g); - MatrixXf H = MatrixXf::Zero(4,nState); for (int i = 0; i < 3; i++){ for (int j = 0; j < 3; j++){ - H(i,j+6) = rotgrav(i,j); - H(i,j+15) = tdcm(i,j); + H(i+1,j+6) = rotgrav(i,j); + H(i+1,j+15) = tdcm(i,j); } } - H(0,9) = -1.0f; - H(1,10) = -1.0f; - H(2,11) = -1.0f; + H(1,9) = -1.0f; + H(2,10) = -1.0f; + H(3,11) = -1.0f; float q0 = qhat(0); float q1 = qhat(1); @@ -330,13 +333,15 @@ MatrixXf Hpart = Hh*Hdq; for(int j=0; j<3; j++){ - H(3,j+6) = Hpart(0,j); + H(4,j+6) = Hpart(0,j); } + + const float psi = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3)); Vector3f zacc = -accm-tdcm*gravity; - VectorXf z = VectorXf::Zero(4); - z <<zacc(0),zacc(1),zacc(2),std::atan2(std::sin(heading-psi), std::cos(heading-psi)); + VectorXf z = VectorXf::Zero(5); + z << palt-pihat(2),zacc(0),zacc(1),zacc(2),std::atan2(std::sin(heading-psi), std::cos(heading-psi)); MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); errState = K * z; Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; @@ -613,15 +618,14 @@ return dynAcc; } -Vector3f solaESKF::calcDynAcc(Vector3f acc) +Vector3f solaESKF::calcVb() { - Vector3f accm = acc - accBias; Matrix3f dcm; computeDcm(dcm, qhat); Matrix3f tdcm = dcm.transpose(); Vector3f Vb = tdcm*vihat; - return dynAcc; + return Vb; } @@ -774,7 +778,7 @@ Matrix3f solaESKF::Matrixcross(Vector3f v) { - Matrix3f m; + Matrix3f m = Matrix3f::Zero(); m << 0.0f, -v(2), v(1), v(2), 0.0f, -v(0), -v(1), v(0), 0.0f;