Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 80:b241c058df83
- Parent:
- 79:365ea9277167
- Child:
- 81:230a3d2b0493
diff -r 365ea9277167 -r b241c058df83 solaESKF.cpp --- a/solaESKF.cpp Fri Dec 10 10:42:09 2021 +0000 +++ b/solaESKF.cpp Fri Jun 24 05:43:46 2022 +0000 @@ -58,7 +58,7 @@ Matrix3f a2v = -dcm*solaESKF::Matrixcross(accm)*att_dt; Matrix3f a2v2 = 0.5f*a2v*att_dt; - MatrixXf Fx(nState,nState); + MatrixXf Fx = MatrixXf::Zero(nState, nState); //position Fx(0,0) = 1.0f; Fx(1,1) = 1.0f; @@ -119,7 +119,7 @@ Fx(16,16) = 1.0f; Fx(17,17) = 1.0f; - errState = Fx * errState; + //errState = Fx * errState; Phat = Fx*Phat*Fx.transpose(); for (int i = 0; i < nState; i++){ if(i>2 && i<9){ @@ -139,7 +139,7 @@ Vector3f tdcm_g = tdcm*gravity; Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g); - MatrixXf H(3,nState); + MatrixXf H = MatrixXf::Zero(3,nState); for (int i = 0; i < 3; i++){ for (int j = 0; j < 3; j++){ H(i,j+6) = rotgrav(i,j); @@ -170,48 +170,48 @@ float q3 = qhat(3); bool canUseA = false; - const float SA0 = 2*q3; - const float SA1 = 2*q2; + const float SA0 = 2.0f*q3; + const float SA1 = 2.0f*q2; const float SA2 = SA0*q0 + SA1*q1; const float SA3 = q0*q0 + q1*q1 - q2*q2 - q3*q3; float SA4, SA5_inv; if ((SA3*SA3) > 1e-6f) { - SA4 = 1.0F/(SA3*SA3); - SA5_inv = SA2*SA2*SA4 + 1; + SA4 = 1.0f/(SA3*SA3); + SA5_inv = SA2*SA2*SA4 + 1.0f; canUseA = std::abs(SA5_inv) > 1e-6f; } bool canUseB = false; - const float SB0 = 2*q0; - const float SB1 = 2*q1; + const float SB0 = 2.0f*q0; + const float SB1 = 2.0f*q1; const float SB2 = SB0*q3 + SB1*q2; const float SB4 = q0*q0 + q1*q1 - q2*q2 - q3*q3; float SB3, SB5_inv; if ((SB2*SB2) > 1e-6f) { - SB3 = 1.0F/(SB2*SB2); + SB3 = 1.0f/(SB2*SB2); SB5_inv = SB3*SB4*SB4 + 1; canUseB = std::abs(SB5_inv) > 1e-6f; } - Matrix<float, 1, 4> Hh; + MatrixXf Hh = MatrixXf::Zero(1,4); if (canUseA && (!canUseB || std::abs(SA5_inv) >= std::abs(SB5_inv))) { - const float SA5 = 1.0F/SA5_inv; - const float SA6 = 1.0F/SA3; + const float SA5 = 1.0f/SA5_inv; + const float SA6 = 1.0f/SA3; const float SA7 = SA2*SA4; - const float SA8 = 2*SA7; - const float SA9 = 2*SA6; + const float SA8 = 2.0f*SA7; + const float SA9 = 2.0f*SA6; Hh(0,0) = SA5*(SA0*SA6 - SA8*q0); Hh(0,1) = SA5*(SA1*SA6 - SA8*q1); Hh(0,2) = SA5*(SA1*SA7 + SA9*q1); Hh(0,3) = SA5*(SA0*SA7 + SA9*q0); } else if (canUseB && (!canUseA || std::abs(SB5_inv) > std::abs(SA5_inv))) { - const float SB5 = 1.0F/SB5_inv; - const float SB6 = 1.0F/SB2; + const float SB5 = 1.0f/SB5_inv; + const float SB6 = 1.0f/SB2; const float SB7 = SB3*SB4; - const float SB8 = 2*SB7; - const float SB9 = 2*SB6; + const float SB8 = 2.0f*SB7; + const float SB9 = 2.0f*SB6; Hh(0,0) = -SB5*(SB0*SB6 - SB8*q3); Hh(0,1) = -SB5*(SB1*SB6 - SB8*q2); @@ -221,14 +221,14 @@ return; } - Matrix<float, 4, 3> Hdq; + MatrixXf Hdq = MatrixXf::Zero(4,3); Hdq << -0.5f*q1, -0.5f*q2, -0.5f*q3, 0.5f*q0, -0.5f*q3, 0.5f*q2, 0.5f*q3, 0.5f*q0, -0.5f*q1, -0.5f*q2, 0.5f*q1, 0.5f*q0; - Matrix<float, 1, 3> Hpart = Hh*Hdq; - MatrixXf H(1,nState); + MatrixXf Hpart = Hh*Hdq; + MatrixXf H=MatrixXf::Zero(1,nState); for(int j=0; j<3; j++){ H(0,j+6) = Hpart(0,j); } @@ -242,9 +242,110 @@ fuseErr2Nominal(); } + +void solaESKF::updateIMU(Vector3f acc,float heading, Matrix<float, 4, 4> R) +{ + + Vector3f accm = acc - accBias; + Matrix3f dcm; + computeDcm(dcm, qhat); + Matrix3f tdcm = dcm.transpose(); + 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(0,9) = -1.0f; + H(1,10) = -1.0f; + H(2,11) = -1.0f; + + float q0 = qhat(0); + float q1 = qhat(1); + float q2 = qhat(2); + float q3 = qhat(3); + + bool canUseA = false; + const float SA0 = 2.0f*q3; + const float SA1 = 2.0f*q2; + const float SA2 = SA0*q0 + SA1*q1; + const float SA3 = q0*q0 + q1*q1 - q2*q2 - q3*q3; + float SA4, SA5_inv; + if ((SA3*SA3) > 1e-6f) { + SA4 = 1.0f/(SA3*SA3); + SA5_inv = SA2*SA2*SA4 + 1.0f; + canUseA = std::abs(SA5_inv) > 1e-6f; + } + + bool canUseB = false; + const float SB0 = 2.0f*q0; + const float SB1 = 2.0f*q1; + const float SB2 = SB0*q3 + SB1*q2; + const float SB4 = q0*q0 + q1*q1 - q2*q2 - q3*q3; + float SB3, SB5_inv; + if ((SB2*SB2) > 1e-6f) { + SB3 = 1.0f/(SB2*SB2); + SB5_inv = SB3*SB4*SB4 + 1; + canUseB = std::abs(SB5_inv) > 1e-6f; + } + + MatrixXf Hh = MatrixXf::Zero(1,4); + + if (canUseA && (!canUseB || std::abs(SA5_inv) >= std::abs(SB5_inv))) { + const float SA5 = 1.0f/SA5_inv; + const float SA6 = 1.0f/SA3; + const float SA7 = SA2*SA4; + const float SA8 = 2.0f*SA7; + const float SA9 = 2.0f*SA6; + + Hh(0,0) = SA5*(SA0*SA6 - SA8*q0); + Hh(0,1) = SA5*(SA1*SA6 - SA8*q1); + Hh(0,2) = SA5*(SA1*SA7 + SA9*q1); + Hh(0,3) = SA5*(SA0*SA7 + SA9*q0); + } else if (canUseB && (!canUseA || std::abs(SB5_inv) > std::abs(SA5_inv))) { + const float SB5 = 1.0f/SB5_inv; + const float SB6 = 1.0f/SB2; + const float SB7 = SB3*SB4; + const float SB8 = 2.0f*SB7; + const float SB9 = 2.0f*SB6; + + Hh(0,0) = -SB5*(SB0*SB6 - SB8*q3); + Hh(0,1) = -SB5*(SB1*SB6 - SB8*q2); + Hh(0,2) = -SB5*(-SB1*SB7 - SB9*q2); + Hh(0,3) = -SB5*(-SB0*SB7 - SB9*q3); + } else { + return; + } + + MatrixXf Hdq = MatrixXf::Zero(4,3); + Hdq << -0.5f*q1, -0.5f*q2, -0.5f*q3, + 0.5f*q0, -0.5f*q3, 0.5f*q2, + 0.5f*q3, 0.5f*q0, -0.5f*q1, + -0.5f*q2, 0.5f*q1, 0.5f*q0; + + MatrixXf Hpart = Hh*Hdq; + for(int j=0; j<3; j++){ + H(3,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)); + MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); + errState = K * z; + Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; + + fuseErr2Nominal(); +} void solaESKF::updateGPSPosition(Vector3f posgps,float palt,Matrix3f R) { - MatrixXf H(3,nState); + MatrixXf H = MatrixXf::Zero(3,nState); H(0,0) = 1.0f; H(1,1) = 1.0f; H(2,2) = 1.0f; @@ -262,7 +363,7 @@ } void solaESKF::updateGPSVelocity(Vector3f velgps,Matrix3f R) { - MatrixXf H(3,nState); + MatrixXf H = MatrixXf::Zero(3,nState); H(0,3) = 1.0f; H(1,4) = 1.0f; H(2,5) = 1.0f; @@ -279,9 +380,116 @@ fuseErr2Nominal(); } +void solaESKF::updateWhole(Vector3f posgps, float palt, Vector3f velgps,Vector3f acc,float heading, MatrixXf R) +{ + MatrixXf H = MatrixXf::Zero(9,nState); + H(0,0) = 1.0f; + H(1,1) = 1.0f; + H(2,2) = 1.0f; + H(3,3) = 1.0f; + H(4,4) = 1.0f; + + Vector3f accm = acc - accBias; + Matrix3f dcm; + computeDcm(dcm, qhat); + Matrix3f tdcm = dcm.transpose(); + Vector3f tdcm_g = tdcm*gravity; + Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g); + + for (int i = 0; i < 3; i++){ + for (int j = 0; j < 3; j++){ + H(i+5,j+6) = rotgrav(i,j); + H(i+5,j+15) = tdcm(i,j); + } + } + + H(5,9) = -1.0f; + H(6,10) = -1.0f; + H(7,11) = -1.0f; + + float q0 = qhat(0); + float q1 = qhat(1); + float q2 = qhat(2); + float q3 = qhat(3); + + bool canUseA = false; + const float SA0 = 2.0f*q3; + const float SA1 = 2.0f*q2; + const float SA2 = SA0*q0 + SA1*q1; + const float SA3 = q0*q0 + q1*q1 - q2*q2 - q3*q3; + float SA4, SA5_inv; + if ((SA3*SA3) > 1e-6f) { + SA4 = 1.0f/(SA3*SA3); + SA5_inv = SA2*SA2*SA4 + 1.0f; + canUseA = std::abs(SA5_inv) > 1e-6f; + } + + bool canUseB = false; + const float SB0 = 2.0f*q0; + const float SB1 = 2.0f*q1; + const float SB2 = SB0*q3 + SB1*q2; + const float SB4 = q0*q0 + q1*q1 - q2*q2 - q3*q3; + float SB3, SB5_inv; + if ((SB2*SB2) > 1e-6f) { + SB3 = 1.0f/(SB2*SB2); + SB5_inv = SB3*SB4*SB4 + 1; + canUseB = std::abs(SB5_inv) > 1e-6f; + } + + MatrixXf Hh = MatrixXf::Zero(1,4); + + if (canUseA && (!canUseB || std::abs(SA5_inv) >= std::abs(SB5_inv))) { + const float SA5 = 1.0f/SA5_inv; + const float SA6 = 1.0f/SA3; + const float SA7 = SA2*SA4; + const float SA8 = 2.0f*SA7; + const float SA9 = 2.0f*SA6; + + Hh(0,0) = SA5*(SA0*SA6 - SA8*q0); + Hh(0,1) = SA5*(SA1*SA6 - SA8*q1); + Hh(0,2) = SA5*(SA1*SA7 + SA9*q1); + Hh(0,3) = SA5*(SA0*SA7 + SA9*q0); + } else if (canUseB && (!canUseA || std::abs(SB5_inv) > std::abs(SA5_inv))) { + const float SB5 = 1.0f/SB5_inv; + const float SB6 = 1.0f/SB2; + const float SB7 = SB3*SB4; + const float SB8 = 2.0f*SB7; + const float SB9 = 2.0f*SB6; + + Hh(0,0) = -SB5*(SB0*SB6 - SB8*q3); + Hh(0,1) = -SB5*(SB1*SB6 - SB8*q2); + Hh(0,2) = -SB5*(-SB1*SB7 - SB9*q2); + Hh(0,3) = -SB5*(-SB0*SB7 - SB9*q3); + } else { + return; + } + + MatrixXf Hdq = MatrixXf::Zero(4,3); + Hdq << -0.5f*q1, -0.5f*q2, -0.5f*q3, + 0.5f*q0, -0.5f*q3, 0.5f*q2, + 0.5f*q3, 0.5f*q0, -0.5f*q1, + -0.5f*q2, 0.5f*q1, 0.5f*q0; + + MatrixXf Hpart = Hh*Hdq; + for(int j=0; j<3; j++){ + H(8,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(9); + 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)); + MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); + errState = K * z; + Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; + + fuseErr2Nominal(); + +} + void solaESKF::updateGPS(Vector3f posgps, float palt, Vector3f velgps, MatrixXf R) { - MatrixXf H(5, nState); + MatrixXf H = MatrixXf::Zero(5,nState); H(0,0) = 1.0f; H(1,1) = 1.0f; H(2,2) = 1.0f; @@ -397,9 +605,9 @@ Vector3f solaESKF::calcDynAcc(Vector3f acc) { Vector3f accm = acc - accBias; - Matrix3f tdcm(3,3); - computeDcm(tdcm, qhat); - tdcm = tdcm.transpose(); + Matrix3f dcm; + computeDcm(dcm, qhat); + Matrix3f tdcm = dcm.transpose(); Vector3f dynAcc = accm+tdcm*gravity; return dynAcc; @@ -443,13 +651,42 @@ return errState; } -void solaESKF::setPhatPosition(float val) +VectorXf solaESKF::getState() { - setBlockDiag(Phat, val, 0,2); + VectorXf state = VectorXf::Zero(nState+1); + for (int i = 0; i < 3; i++){ + state(i) = pihat(i); + state(i+3) = vihat(i); + } + for (int i = 0; i < 4; i++){ + state(i+6) = qhat(i); + } + for (int i = 0; i < 3; i++){ + state(i+10) = accBias(i); + state(i+13) = gyroBias(i); + state(i+16) = gravity(i); + } + return state; } -void solaESKF::setPhatVelocity(float val) + +VectorXf solaESKF::getVariance() { - setBlockDiag(Phat, val, 3,5); + VectorXf variance = VectorXf::Zero(nState); + for (int i = 0; i < nState; i++){ + variance(i) = Phat(i,i); + } + return variance; +} + +void solaESKF::setPhatPosition(float valNE,float valD) +{ + setBlockDiag(Phat, valNE, 0,2); + Phat(2,2) = valD; +} +void solaESKF::setPhatVelocity(float valNE,float valD) +{ + setBlockDiag(Phat, valNE, 3,5); + Phat(5,5) = valD; } void solaESKF::setPhatAngleError(float val) { @@ -469,9 +706,10 @@ } -void solaESKF::setQVelocity(float val) +void solaESKF::setQVelocity(float valNE,float valD) { - setBlockDiag(Q, val, 3, 5); + setBlockDiag(Q, valNE, 3, 5); + Q(5,5) = valD; } void solaESKF::setQAngleError(float val) { @@ -486,14 +724,31 @@ setBlockDiag(Q, val, 12, 14); } - -void solaESKF::setDiag(MatrixXf& mat, float val){ +void solaESKF::setDiag(Matrix3f& mat, float val){ + for (int i = 0; i < mat.cols(); i++){ + for (int j = 0; j < mat.rows(); j++){ + mat(i,j) = 0.0f; + } + } for (int i = 0; i < mat.cols(); i++){ mat(i,i) = val; } } +void solaESKF::setDiag(MatrixXf& mat, float val){ + for (int i = 0; i < mat.cols(); i++){ + for (int j = 0; j < mat.rows(); j++){ + mat(i,j) = 0.0f; + } + } + for (int i = 0; i < mat.cols(); i++) + { + mat(i, i) = val; + } +} + void solaESKF::setBlockDiag(MatrixXf& mat, float val, int startIndex, int endIndex){ + for (int i = startIndex; i < endIndex+1; i++){ mat(i,i) = val; }