Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 58:93ba28cf5cb3
- Parent:
- 56:c10f1168bd4a
- Child:
- 59:03fe5e16a33c
--- a/solaESKF.cpp Wed Nov 10 06:33:45 2021 +0000 +++ b/solaESKF.cpp Fri Nov 12 09:03:10 2021 +0000 @@ -12,17 +12,19 @@ gyroBias << 0.0f << 0.0f << 0.0f; gravity << 0.0f << 0.0f << 9.8f; magField << -0.2f << -0.0f << 0.0f; + + nState = errState.getRows(); - for (int i = 1; i < 20; i++){ + for (int i = 1; i < nState+1; i++){ errState(i,1) = 0.0f; - for (int j = 1; j < 20; j++){ + for (int j = 1; j < nState+1; j++){ Phat(i,j) = 0.0f; Q(i,j) = 0.0f; } } - nState = errState.getRows(); + setBlockDiag(Phat,0.1f,1,3);//position setBlockDiag(Phat,0.1f,4,6);//velocity @@ -30,12 +32,12 @@ setBlockDiag(Phat,0.1f,10,12);//acc bias setBlockDiag(Phat,0.1f,13,15);//gyro bias setBlockDiag(Phat,0.00000001f,16,18);//gravity - setBlockDiag(Phat,0.1f,19,19);//gravity + setBlockDiag(Phat,0.001f,19,19);//H0 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 - setBlockDiag(Q,0.001f,19,19);//mag field //positionとgravityはQ項なし + setBlockDiag(Q,0.0001f,19,19);//mag field //positionとgravityはQ項なし } @@ -125,6 +127,9 @@ H(4,j+6) = a2v2(1,j); H(5,j+6) = a2v2(2,j); } + for (int i = 1; i < 4; i++){ + + } H(1,10) = -1.0f; H(2,11) = -1.0f; @@ -145,22 +150,107 @@ fuseErr2Nominal(); } - -void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R) +void solaESKF::updateMag(Matrix mag,float palt, Matrix R) { + Matrix dcm(3,3); + computeDcm(dcm, qhat); + Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1)); - Matrix H(5,nState); + Matrix H(3,nState); + for (int i = 1; i < 3; i++){ + for (int j = 1; j < 4; j++){ + H(i,j+6) = a2v(i,j); + } + } + H(1,19) = 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+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + Matrix zmag = -dcm*mag-magField; + Matrix z(3,1); + z << zmag(1,1) << zmag(2,1) << palt - pihat(3,1); + errState = K * z; + Phat = (MatrixMath::Eye(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 mag,Matrix R) +{ + Matrix dcm(3,3); + computeDcm(dcm, qhat); + Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1)); + + Matrix H(6,nState); 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; + for (int j = 1; j < 4; j++){ + H(6,j+6) = a2v(2,j); + } + + Matrix zmag = -dcm*mag-magField; //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); 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); + Matrix z(6,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)<< zmag(2,1); + errState = K * z; + Phat = (MatrixMath::Eye(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 mag,Matrix R) +{ + + Matrix dcm(3,3); + computeDcm(dcm, qhat); + Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1)); + + + Matrix H(3,nState); + H(1,4) = 1.0f; + H(2,5) = 1.0f; + + for (int j = 1; j < 4; j++){ + H(3,j+6) = a2v(2,j); + } + //H(3,19) = 1.0f; + + Matrix zmag = -dcm*mag-magField; + + //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; + //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.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) << zmag(2,1); + errState = K * z; + Phat = (MatrixMath::Eye(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::updateGPSPosition(Matrix posgps,float palt,Matrix R) +{ + Matrix H(3,nState); + 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); errState = K * z; Phat = (MatrixMath::Eye(nState)-K*H)*Phat; //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K); @@ -213,12 +303,12 @@ gravity(2,1) += errState(17,1); gravity(3,1) += errState(18,1); - //gravity bias - magField(1,1) += errState(19,1); + //Magnetic Field + magField(1,1) += errState(19,1); magField(2,1) = 0.0f; magField(3,1) = 0.0f; - for (int i = 1; i < 20; i++){ + for (int i = 1; i < nState+1; i++){ errState(i,1) = 0.0f; } @@ -374,11 +464,11 @@ } } -void solaESKF::setPihat(float pi_x, float pi_y, float pi_z) +void solaESKF::setPihat(float pi_x, float pi_y) { pihat(1,1) = pi_x; pihat(2,1) = pi_y; - pihat(3,1) = pi_z; + //pihat(3,1) = pi_z; } void solaESKF::setMagField(float hx, float hy, float hz) { @@ -477,42 +567,5 @@ fuseErr2Nominal(); } -void solaESKF::updateGPSVelocity(Matrix velgps,Matrix R) -{ - - Matrix H(3,nState); - 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+10.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); - errState = K * z; - Phat = (MatrixMath::Eye(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::updateGPSPosition(Matrix posgps,Matrix R) -{ - Matrix H(2,nState); - 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(2,1); - z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1); //<< posgps(3,1) - pihat(3,1); - errState = K * z; - Phat = (MatrixMath::Eye(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(); -} */ \ No newline at end of file