Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 78:e36a7b844fb5
- Parent:
- 77:780ce6556041
- Child:
- 79:365ea9277167
diff -r 780ce6556041 -r e36a7b844fb5 solaESKF.cpp --- a/solaESKF.cpp Tue Nov 30 11:26:56 2021 +0000 +++ b/solaESKF.cpp Mon Dec 06 08:24:13 2021 +0000 @@ -1,80 +1,73 @@ #include "solaESKF.hpp" - - solaESKF::solaESKF() - :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) +// :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) + :errState(18,1),Phat(18,18),Q(18,18) { - pihat << 0.0f << 0.0f << 0.0f; - vihat << 0.0f << 0.0f << 0.0f; - qhat << 1.0f << 0.0f << 0.0f << 0.0f; - accBias << 0.0f << 0.0f << 0.0f; - gyroBias << 0.0f << 0.0f << 0.0f; - gravity << 0.0f << 0.0f << 9.8f; - - nState = errState.getRows(); + pihat << 0.0f, 0.0f, 0.0f; + vihat << 0.0f, 0.0f, 0.0f; + qhat << 1.0f, 0.0f, 0.0f, 0.0f; + accBias << 0.0f, 0.0f, 0.0f; + gyroBias << 0.0f, 0.0f, 0.0f; + gravity << 0.0f, 0.0, 9.8f; - for (int i = 1; i < nState+1; i++){ - errState(i,1) = 0.0f; - for (int j = 1; j < nState+1; j++){ - Phat(i,j) = 0.0f; - Q(i,j) = 0.0f; - } - } + nState = 18; + errState = VectorXf::Zero(nState); + Phat = MatrixXf::Zero(nState, nState); + Q = MatrixXf::Zero(nState, nState); - setBlockDiag(Phat,0.1f,1,3);//position - setBlockDiag(Phat,0.1f,4,6);//velocity - setBlockDiag(Phat,0.1f,7,9);//angle error - setBlockDiag(Phat,0.1f,10,12);//acc bias - setBlockDiag(Phat,0.1f,13,15);//gyro bias - setBlockDiag(Phat,0.00000001f,16,18);//gravity - setBlockDiag(Q,0.00025f,4,6);//velocity - setBlockDiag(Q,0.005f/57.0f,7,9);//angle error - setBlockDiag(Q,0.001f,10,12);//acc bias - setBlockDiag(Q,0.001f,13,15);//gyro bias//positionとgravityはQ項なし - + setBlockDiag(Phat, 0.1f, 0, 2);//position + setBlockDiag(Phat, 0.1f, 3, 5);//velocity + setBlockDiag(Phat, 0.1f, 6, 8);//angle error + setBlockDiag(Phat, 0.1f, 9, 11);//acc bias + setBlockDiag(Phat, 0.1f, 12, 14);//gyro bias + setBlockDiag(Phat, 0.00000001f, 15, 17);//gravity + setBlockDiag(Q, 0.00025f, 3, 5);//velocity + setBlockDiag(Q, 0.005f/57.0f, 6, 8);//angle error + setBlockDiag(Q, 0.001f, 9, 11);//acc bias + setBlockDiag(Q, 0.001f, 12, 14);//gyro bias//positionとgravityはQ項なし } -void solaESKF::updateNominal(Matrix acc, Matrix gyro,float att_dt) +void solaESKF::updateNominal(Vector3f acc, Vector3f gyro,float att_dt) { - Matrix gyrom = gyro - gyroBias; - Matrix accm = acc - accBias; + Vector3f gyrom = gyro - gyroBias; + Vector3f accm = acc - accBias; - Matrix qint(4,1); - qint << 1.0f << 0.5f*gyrom(1,1)*att_dt << 0.5f*gyrom(2,1)*att_dt << 0.5f*gyrom(3,1)*att_dt; - qhat = quatmultiply(qhat,qint); - float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); - qhat *= (1.0f/ qnorm); + Vector4f qint; + qint << 1.0f, 0.5f*gyrom(0)*att_dt, 0.5f*gyrom(1)*att_dt, 0.5f*gyrom(2)*att_dt; + qhat = quatmultiply(qhat, qint); + qhat.normalize(); - Matrix dcm(3,3); + Matrix3f dcm; computeDcm(dcm, qhat); - Matrix accned = dcm*accm+gravity; + Vector3f accned = dcm*accm + gravity; vihat += accned*att_dt; - pihat += vihat*att_dt+0.5f*accned*att_dt*att_dt; + pihat += vihat*att_dt + 0.5f*accned*att_dt*att_dt; } -void solaESKF::updateErrState(Matrix acc,Matrix gyro,float att_dt) +void solaESKF::updateErrState(Vector3f acc, Vector3f gyro, float att_dt) { - Matrix gyrom = gyro - gyroBias; - Matrix accm = acc - accBias; + Vector3f gyrom = gyro - gyroBias; + Vector3f accm = acc - accBias; - Matrix dcm(3,3); + Matrix3f dcm; computeDcm(dcm, qhat); - Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1))*att_dt; - Matrix a2v2 = 0.5f*a2v*att_dt; +// Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1))*att_dt; + Matrix3f a2v = -dcm*solaESKF::Matrixcross(accm)*att_dt; + Matrix3f a2v2 = 0.5f*a2v*att_dt; - Matrix Fx(nState,nState); + MatrixXf Fx(nState,nState); //position + Fx(0,0) = 1.0f; Fx(1,1) = 1.0f; Fx(2,2) = 1.0f; - Fx(3,3) = 1.0f; + Fx(0,3) = 1.0f*att_dt; Fx(1,4) = 1.0f*att_dt; Fx(2,5) = 1.0f*att_dt; - Fx(3,6) = 1.0f*att_dt; - for (int i = 1; i < 4; i++){ - for (int j = 1; j < 4; j++){ + for (int i = 0; i < 3; i++){ + for (int j = 0; j < 3; j++){ Fx(i,j+6) = a2v2(i,j); Fx(i,j+9) = -0.5f*dcm(i,j)*att_dt*att_dt; } @@ -83,97 +76,98 @@ //velocity + Fx(3,3) = 1.0f; Fx(4,4) = 1.0f; Fx(5,5) = 1.0f; - Fx(6,6) = 1.0f; - for (int i = 1; i < 4; i++){ - for (int j = 1; j < 4; j++){ + for (int i = 0; i < 3; i++){ + for (int j = 0; j < 3; j++){ Fx(i+3,j+6) = a2v(i,j); Fx(i+3,j+9) = -dcm(i,j)*att_dt; Fx(i+3,j+12) = -a2v2(i,j); } } + Fx(3,15) = 1.0f*att_dt; Fx(4,16) = 1.0f*att_dt; Fx(5,17) = 1.0f*att_dt; - Fx(6,18) = 1.0f*att_dt; //angulat error + Fx(6,6) = 1.0f; Fx(7,7) = 1.0f; Fx(8,8) = 1.0f; - Fx(9,9) = 1.0f; - Fx(7,8) = gyrom(3,1)*att_dt; - Fx(7,9) = -gyrom(2,1)*att_dt; - Fx(8,7) = -gyrom(3,1)*att_dt; - Fx(8,9) = gyrom(1,1)*att_dt; - Fx(9,7) = gyrom(2,1)*att_dt; - Fx(9,8) = -gyrom(1,1)*att_dt; + Fx(6,7) = gyrom(2)*att_dt; + Fx(6,8) = -gyrom(1)*att_dt; + Fx(7,6) = -gyrom(2)*att_dt; + Fx(7,8) = gyrom(0)*att_dt; + Fx(8,6) = gyrom(1)*att_dt; + Fx(8,7) = -gyrom(0)*att_dt; + Fx(6,12) = -1.0f*att_dt; Fx(7,13) = -1.0f*att_dt; Fx(8,14) = -1.0f*att_dt; - Fx(9,15) = -1.0f*att_dt; //acc bias + Fx(9,9) = 1.0f; Fx(10,10) = 1.0f; Fx(11,11) = 1.0f; - Fx(12,12) = 1.0f; //gyro bias + Fx(12,12) = 1.0f; Fx(13,13) = 1.0f; Fx(14,14) = 1.0f; - Fx(15,15) = 1.0f; //gravity bias + Fx(15,15) = 1.0f; Fx(16,16) = 1.0f; Fx(17,17) = 1.0f; - Fx(18,18) = 1.0f; errState = Fx * errState; - Phat = Fx*Phat*MatrixMath::Transpose(Fx); - for (int i = 1; i < nState+1; i++){ - if(i>3 && i<10){ + Phat = Fx*Phat*Fx.transpose(); + for (int i = 0; i < nState; i++){ + if(i>2 && i<9){ Phat(i,i) += Q(i,i)*att_dt; - }else if(i>9 && i<16){ + }else if(i>8 && i<15){ Phat(i,i) += Q(i,i)* att_dt*att_dt; } } } -void solaESKF::updateAcc(Matrix acc,Matrix R) +void solaESKF::updateAcc(Vector3f acc, Matrix3f R) { - Matrix accm = acc - accBias; - Matrix dcm(3,3); + Vector3f accm = acc - accBias; + Matrix3f dcm; computeDcm(dcm, qhat); - Matrix tdcm = MatrixMath::Transpose(dcm); - Matrix tdcm_g = tdcm*gravity; - Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1)); + Matrix3f tdcm = dcm.transpose(); + Vector3f tdcm_g = tdcm*gravity; + Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g); - Matrix H(3,nState); - for (int i = 1; i < 4; i++){ - for (int j = 1; j < 4; j++){ + MatrixXf H(3,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; - H(3,12) = -1.0f; - Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); - Matrix zacc = -accm-tdcm*gravity; - Matrix z(3,1); - z << zacc(1,1) << zacc(2,1) << zacc(3,1); +// Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); + Vector3f zacc = -accm-tdcm*gravity; + Vector3f z; + z = zacc; errState = K * z; - Phat = (MatrixMath::Eye(nState)-K*H)*Phat; + Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; fuseErr2Nominal(); } -void solaESKF::updateHeading(float a,Matrix R) +void solaESKF::updateHeading(float a, Matrix3f R) { - float q0 = qhat(1,1); - float q1 = qhat(2,1); - float q2 = qhat(3,1); - float q3 = qhat(4,1); + float q0 = qhat(0); + float q1 = qhat(1); + float q2 = qhat(2); + float q3 = qhat(3); bool canUseA = false; const float SA0 = 2*q3; @@ -184,7 +178,7 @@ if ((SA3*SA3) > 1e-6f) { SA4 = 1.0F/(SA3*SA3); SA5_inv = SA2*SA2*SA4 + 1; - canUseA = fabsf(SA5_inv) > 1e-6f; + canUseA = std::abs(SA5_inv) > 1e-6f; } bool canUseB = false; @@ -196,119 +190,119 @@ if ((SB2*SB2) > 1e-6f) { SB3 = 1.0F/(SB2*SB2); SB5_inv = SB3*SB4*SB4 + 1; - canUseB = fabsf(SB5_inv) > 1e-6f; + canUseB = std::abs(SB5_inv) > 1e-6f; } - Matrix Hh(1,4); + Matrix<float, 1, 4> Hh; - if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) { + 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*SA7; const float SA9 = 2*SA6; - Hh(1,1) = SA5*(SA0*SA6 - SA8*q0); - Hh(1,2) = SA5*(SA1*SA6 - SA8*q1); - Hh(1,3) = SA5*(SA1*SA7 + SA9*q1); - Hh(1,4) = SA5*(SA0*SA7 + SA9*q0); - } else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) { + 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*SB7; const float SB9 = 2*SB6; - Hh(1,1) = -SB5*(SB0*SB6 - SB8*q3); - Hh(1,2) = -SB5*(SB1*SB6 - SB8*q2); - Hh(1,3) = -SB5*(-SB1*SB7 - SB9*q2); - Hh(1,4) = -SB5*(-SB0*SB7 - SB9*q3); + 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; } - Matrix Hdq(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, 4, 3> Hdq; + 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 Hpart = Hh*Hdq; - Matrix H(1,nState); - for(int j=1;j<4;j++){ - H(1,j+6) = Hpart(1,j); + Matrix<float, 1, 3> Hpart = Hh*Hdq; + MatrixXf H(1,nState); + for(int j=0; j<3; j++){ + H(0,j+6) = Hpart(0,j); } - const float psi = atan2f(qhat(2,1)*qhat(3,1) + qhat(1,1)*qhat(4,1), 0.5f - qhat(3,1)*qhat(3,1) - qhat(4,1)*qhat(4,1)); - Matrix z(1,1); - z << atan2f(sin(a-psi),cos(a-psi)); - Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + const float psi = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3)); + Matrix<float, 1, 1> z; + z << std::atan2(std::sin(a-psi), std::cos(a-psi)); + MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); errState = K * z; - Phat = (MatrixMath::Eye(nState)-K*H)*Phat; + Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; fuseErr2Nominal(); } -void solaESKF::updateGPSPosition(Matrix posgps,float palt,Matrix R) +void solaESKF::updateGPSPosition(Vector3f posgps,float palt,Matrix3f R) { - Matrix H(3,nState); + MatrixXf H(3,nState); + H(0,0) = 1.0f; H(1,1) = 1.0f; H(2,2) = 1.0f; - H(3,3) = 1.0f; //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); - Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); - Matrix z(3,1); - z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1); + MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); + Vector3f z; + z << posgps(0)-pihat(0), posgps(1)-pihat(1), palt - pihat(2); errState = K * z; - Phat = (MatrixMath::Eye(nState)-K*H)*Phat; + Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K); //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); fuseErr2Nominal(); } -void solaESKF::updateGPSVelocity(Matrix velgps,Matrix R) +void solaESKF::updateGPSVelocity(Vector3f velgps,Matrix3f R) { - Matrix H(3,nState); + MatrixXf H(3,nState); + H(0,3) = 1.0f; H(1,4) = 1.0f; H(2,5) = 1.0f; - H(3,6) = 1.0f; //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); - Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); - Matrix z(3,1); - z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1)<< velgps(3,1)-vihat(3,1); + MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); + Vector3f z; + z << velgps(0)-vihat(0), velgps(1)-vihat(1), velgps(2)-vihat(2); errState = K * z; - Phat = (MatrixMath::Eye(nState)-K*H)*Phat; + Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K); //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); fuseErr2Nominal(); } -void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R) +void solaESKF::updateGPS(Vector3f posgps, float palt, Vector3f velgps, MatrixXf R) { - Matrix H(5,nState); + MatrixXf H(5, 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; - H(5,5) = 1.0f; - Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); - Matrix z(5,1); - z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1); + MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse(); + Matrix<float, 5, 1> z; + z << posgps(0)-pihat(0), posgps(1)-pihat(1), palt-pihat(2), velgps(0)-vihat(0), velgps(1)-vihat(1); errState = K * z; - Phat = (MatrixMath::Eye(nState)-K*H)*Phat; + Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat; fuseErr2Nominal(); } -Matrix solaESKF::computeAngles() +Vector3f solaESKF::computeAngles() { - Matrix euler(3,1); - euler(1,1) = atan2f(qhat(1,1)*qhat(2,1) + qhat(3,1)*qhat(4,1), 0.5f - qhat(2,1)*qhat(2,1) - qhat(3,1)*qhat(3,1)); - euler(2,1) = asinf(-2.0f * (qhat(2,1)*qhat(4,1) - qhat(1,1)*qhat(3,1))); - euler(3,1) = atan2f(qhat(2,1)*qhat(3,1) + qhat(1,1)*qhat(4,1), 0.5f - qhat(3,1)*qhat(3,1) - qhat(4,1)*qhat(4,1)); + Vector3f euler; + euler(0) = std::atan2(qhat(0)*qhat(1) + qhat(2)*qhat(3), 0.5f - qhat(1)*qhat(1) - qhat(2)*qhat(2)); + euler(1) = std::asin(-2.0f * (qhat(1)*qhat(3) - qhat(0)*qhat(2))); + euler(2) = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3)); return euler; } @@ -316,190 +310,190 @@ void solaESKF::fuseErr2Nominal() { //position - pihat(1,1) += errState(1,1); - pihat(2,1) += errState(2,1); - pihat(3,1) += errState(3,1); + pihat(0) += errState(0); + pihat(1) += errState(1); + pihat(2) += errState(2); //velocity - vihat(1,1) += errState(4,1); - vihat(2,1) += errState(5,1); - vihat(3,1) += errState(6,1); + vihat(0) += errState(3); + vihat(1) += errState(4); + vihat(2) += errState(5); //angle error - Matrix qerr(4,1); - qerr << 1.0f << 0.5f*errState(7,1) << 0.5f*errState(8,1) << 0.5f*errState(9,1); + Vector4f qerr; + qerr << 1.0f, 0.5f*errState(6), 0.5f*errState(7), 0.5f*errState(8); qhat = quatmultiply(qhat, qerr); - float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); - qhat *= (1.0f/ qnorm); + qhat.normalize(); //acc bias - accBias(1,1) += errState(10,1); - accBias(2,1) += errState(11,1); - accBias(3,1) += errState(12,1); + accBias(0) += errState(9); + accBias(1) += errState(10); + accBias(2) += errState(11); //gyro bias - gyroBias(1,1) += errState(13,1); - gyroBias(2,1) += errState(14,1); - gyroBias(3,1) += errState(15,1); + gyroBias(0) += errState(12); + gyroBias(1) += errState(13); + gyroBias(2) += errState(14); //gravity bias - gravity(1,1) += errState(16,1); - gravity(2,1) += errState(17,1); - gravity(3,1) += errState(18,1); - + gravity(0) += errState(15); + gravity(1) += errState(16); + gravity(2) += errState(17); - - for (int i = 1; i < nState+1; i++){ - errState(i,1) = 0.0f; - } - + errState = VectorXf::Zero(nState); } -Matrix solaESKF::quatmultiply(Matrix p, Matrix q) +Vector4f solaESKF::quatmultiply(Vector4f p, Vector4f q) { - - Matrix qout(4,1); - qout(1,1) = p(1,1)*q(1,1) - p(2,1)*q(2,1) - p(3,1)*q(3,1) - p(4,1)*q(4,1); - qout(2,1) = p(1,1)*q(2,1) + p(2,1)*q(1,1) + p(3,1)*q(4,1) - p(4,1)*q(3,1); - qout(3,1) = p(1,1)*q(3,1) - p(2,1)*q(4,1) + p(3,1)*q(1,1) + p(4,1)*q(2,1); - qout(4,1) = p(1,1)*q(4,1) + p(2,1)*q(3,1) - p(3,1)*q(2,1) + p(4,1)*q(1,1); - float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout)); - qout *= (1.0f/ qnorm); + Vector4f qout; + qout(0) = p(0)*q(0) - p(1)*q(1) - p(2)*q(2) - p(3)*q(3); + qout(1) = p(0)*q(1) + p(1)*q(0) + p(2)*q(3) - p(3)*q(2); + qout(2) = p(0)*q(2) - p(1)*q(3) + p(2)*q(0) + p(3)*q(1); + qout(3) = p(0)*q(3) + p(1)*q(2) - p(2)*q(1) + p(3)*q(0); +// qout.normalize(); return qout; } -void solaESKF::computeDcm(Matrix& dcm, Matrix quat) +void solaESKF::computeDcm(Matrix3f& dcm, Vector4f quat) { - - 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); - dcm(1,2) = 2.0f*(quat(2,1)*quat(3,1) - quat(1,1)*quat(4,1)); - dcm(1,3) = 2.0f*(quat(2,1)*quat(4,1) + quat(1,1)*quat(3,1)); - dcm(2,1) = 2.0f*(quat(2,1)*quat(3,1) + quat(1,1)*quat(4,1)); - 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); - dcm(2,3) = 2.0f*(quat(3,1)*quat(4,1) - quat(1,1)*quat(2,1)); - dcm(3,1) = 2.0f*(quat(2,1)*quat(4,1) - quat(1,1)*quat(3,1)); - dcm(3,2) = 2.0f*(quat(3,1)*quat(4,1) + quat(1,1)*quat(2,1)); - 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); - +// 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); +// dcm(1,2) = 2.0f*(quat(2,1)*quat(3,1) - quat(1,1)*quat(4,1)); +// dcm(1,3) = 2.0f*(quat(2,1)*quat(4,1) + quat(1,1)*quat(3,1)); +// dcm(2,1) = 2.0f*(quat(2,1)*quat(3,1) + quat(1,1)*quat(4,1)); +// 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); +// dcm(2,3) = 2.0f*(quat(3,1)*quat(4,1) - quat(1,1)*quat(2,1)); +// dcm(3,1) = 2.0f*(quat(2,1)*quat(4,1) - quat(1,1)*quat(3,1)); +// dcm(3,2) = 2.0f*(quat(3,1)*quat(4,1) + quat(1,1)*quat(2,1)); +// 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); + + dcm(0,0) = quat(0)*quat(0) + quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3); + dcm(0,1) = 2.0f*(quat(1)*quat(2) - quat(0)*quat(3)); + dcm(0,2) = 2.0f*(quat(1)*quat(3) + quat(0)*quat(2)); + dcm(1,0) = 2.0f*(quat(1)*quat(2) + quat(0)*quat(3)); + dcm(1,1) = quat(0)*quat(0) - quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3); + dcm(1,2) = 2.0f*(quat(2)*quat(3) - quat(0)*quat(1)); + dcm(2,0) = 2.0f*(quat(1)*quat(3) - quat(0)*quat(2)); + dcm(2,1) = 2.0f*(quat(2)*quat(3) + quat(0)*quat(1)); + dcm(2,2) = quat(0)*quat(0) - quat(1)*quat(1) - quat(2)*quat(2) + quat(3)*quat(3); } void solaESKF::setQhat(float ex,float ey,float ez) { - float cos_z_2 = cosf(0.5f*ez); - float cos_y_2 = cosf(0.5f*ey); - float cos_x_2 = cosf(0.5f*ex); + float cos_z_2 = std::cos(0.5f*ez); + float cos_y_2 = std::cos(0.5f*ey); + float cos_x_2 = std::cos(0.5f*ex); - float sin_z_2 = sinf(0.5f*ez); - float sin_y_2 = sinf(0.5f*ey); - float sin_x_2 = sinf(0.5f*ex); + float sin_z_2 = std::sin(0.5f*ez); + float sin_y_2 = std::sin(0.5f*ey); + float sin_x_2 = std::sin(0.5f*ex); // and now compute quaternion - qhat(1,1) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2; - qhat(2,1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2; - qhat(3,1) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2; - qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; + qhat(0) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2; + qhat(1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2; + qhat(2) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2; + qhat(3) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; } -Matrix solaESKF::calcDynAcc(Matrix acc) +Vector3f solaESKF::calcDynAcc(Vector3f acc) { - Matrix accm = acc - accBias; - Matrix tdcm(3,3); + Vector3f accm = acc - accBias; + Matrix3f tdcm(3,3); computeDcm(tdcm, qhat); - tdcm = MatrixMath::Transpose(tdcm); + tdcm = tdcm.transpose(); - Matrix dynAcc = accm+tdcm*gravity; + Vector3f dynAcc = accm+tdcm*gravity; return dynAcc; } void solaESKF::setGravity(float gx,float gy,float gz) { - gravity(1,1) = gx; - gravity(2,1) = gy; - gravity(3,1) = gz; + gravity(0) = gx; + gravity(1) = gy; + gravity(2) = gz; } -Matrix solaESKF::getPihat() +Vector3f solaESKF::getPihat() { return pihat; } -Matrix solaESKF::getVihat() +Vector3f solaESKF::getVihat() { return vihat; } -Matrix solaESKF::getQhat() +Vector4f solaESKF::getQhat() { return qhat; } -Matrix solaESKF::getAccBias() +Vector3f solaESKF::getAccBias() { return accBias; } -Matrix solaESKF::getGyroBias() +Vector3f solaESKF::getGyroBias() { return gyroBias; } -Matrix solaESKF::getGravity() +Vector3f solaESKF::getGravity() { return gravity; } -Matrix solaESKF::getErrState() +VectorXf solaESKF::getErrState() { return errState; } void solaESKF::setPhatPosition(float val) { - setBlockDiag(Phat,val,1,3); + setBlockDiag(Phat, val, 0,2); } void solaESKF::setPhatVelocity(float val) { - setBlockDiag(Phat,val,4,6); + setBlockDiag(Phat, val, 3,5); } void solaESKF::setPhatAngleError(float val) { - setBlockDiag(Phat,val,7,9); + setBlockDiag(Phat, val, 6,8); } void solaESKF::setPhatAccBias(float val) { - setBlockDiag(Phat,val,10,12); + setBlockDiag(Phat, val, 9,11); } void solaESKF::setPhatGyroBias(float val) { - setBlockDiag(Phat,val,13,15); + setBlockDiag(Phat, val, 12,14); } void solaESKF::setPhatGravity(float val) { - setBlockDiag(Phat,val,16,18); + setBlockDiag(Phat, val, 15,17); } void solaESKF::setQVelocity(float val) { - setBlockDiag(Q,val,4,6); + setBlockDiag(Q, val, 3, 5); } void solaESKF::setQAngleError(float val) { - setBlockDiag(Q,val,7,9); + setBlockDiag(Q, val, 6, 8); } void solaESKF::setQAccBias(float val) { - setBlockDiag(Q,val,10,12); + setBlockDiag(Q, val, 9, 11); } void solaESKF::setQGyroBias(float val) { - setBlockDiag(Q,val,13,15); + setBlockDiag(Q, val, 12, 14); } -void solaESKF::setDiag(Matrix& mat, float val){ - for (int i = 1; i < mat.getCols()+1; i++){ +void solaESKF::setDiag(MatrixXf& mat, float val){ + for (int i = 0; i < mat.cols(); i++){ mat(i,i) = val; } } -void solaESKF::setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex){ +void solaESKF::setBlockDiag(MatrixXf& mat, float val, int startIndex, int endIndex){ for (int i = startIndex; i < endIndex+1; i++){ mat(i,i) = val; } @@ -507,11 +501,19 @@ void solaESKF::setPihat(float pi_x, float pi_y) { - pihat(1,1) = pi_x; - pihat(2,1) = pi_y; + pihat(0) = pi_x; + pihat(1) = pi_y; //pihat(3,1) = pi_z; } +Matrix3f solaESKF::Matrixcross(Vector3f v) +{ + Matrix3f m; + m << 0.0f, -v(2), v(1), + v(2), 0.0f, -v(0), + -v(1), v(0), 0.0f; + return m; +} /* void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R) {