Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 56:c10f1168bd4a
- Parent:
- 55:21611d4cf7e8
- Child:
- 58:93ba28cf5cb3
diff -r 21611d4cf7e8 -r c10f1168bd4a solaESKF.cpp --- a/solaESKF.cpp Mon Nov 08 09:19:55 2021 +0000 +++ b/solaESKF.cpp Wed Nov 10 05:21:36 2021 +0000 @@ -3,7 +3,7 @@ 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),magField(3,1),errState(19,1),Phat(19,19),Q(19,19) { pihat << 0.0f << 0.0f << 0.0f; vihat << 0.0f << 0.0f << 0.0f; @@ -11,10 +11,11 @@ accBias << 0.0f << 0.0f << 0.0f; gyroBias << 0.0f << 0.0f << 0.0f; gravity << 0.0f << 0.0f << 9.8f; + magField << -0.2f << -0.0f << 0.0f; - for (int i = 1; i < 19; i++){ + for (int i = 1; i < 20; i++){ errState(i,1) = 0.0f; - for (int j = 1; j < 19; j++){ + for (int j = 1; j < 20; j++){ Phat(i,j) = 0.0f; Q(i,j) = 0.0f; } @@ -28,11 +29,13 @@ 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(Phat,0.00000001f,16,18);//gravity + setBlockDiag(Phat,0.1f,19,19);//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(Q,0.001f,13,15);//gyro bias + setBlockDiag(Q,0.001f,19,19);//mag field //positionとgravityはQ項なし } @@ -103,149 +106,66 @@ Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Qd; } -void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R) +void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R) { Matrix accm = acc - accBias; - Matrix tdcm(3,3); - computeDcm(tdcm, qhat); - tdcm = MatrixMath::Transpose(tdcm); + Matrix dcm(3,3); + computeDcm(dcm, qhat); + Matrix tdcm = MatrixMath::Transpose(dcm); Matrix tdcm_g = tdcm*gravity; Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1)); + Matrix a2v2 = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1)); - Matrix H(4,nState); - for (int i = 1; i < 4; i++){ - for (int j = 1; j < 4; j++){ + Matrix H(5,nState); + for (int j = 1; j < 4; j++){ + for (int i = 1; i < 4; i++){ H(i,j+6) = a2v(i,j); H(i,j+15) = tdcm(i,j); } + H(4,j+6) = a2v2(1,j); + H(5,j+6) = a2v2(2,j); } + H(1,10) = -1.0f; H(2,11) = -1.0f; H(3,12) = -1.0f; - H(4,3) = 1.0f; + H(4,19) = 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 zacc = -accm-tdcm*gravity; - Matrix z(4,1); - z << zacc(1,1) << zacc(2,1) << zacc(3,1) << palt - pihat(3,1); + Matrix zmag = -dcm*mag-magField; + Matrix z(5,1); + z << zacc(1,1) << zacc(2,1) << zacc(3,1) << zmag(1,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::updateGyroConstraints(Matrix gyro,Matrix R) -{ - Matrix gyrom = gyro - gyroBias; - Matrix dcm(3,3); - computeDcm(dcm, qhat); - Matrix a2v = dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1)); - - Matrix H(2,nState); - for (int i = 1; i < 3; i++){ - for (int j = 1; j < 4; j++){ - H(i,j+6) = a2v(i,j); - H(i,j+12) = -dcm(i,j); - } - } - - //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; - //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A)); - Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); - - Matrix z3 = -dcm*gyrom; - Matrix z(2,1); - z << z3(1,1) << z3(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); -} -void solaESKF::updateMag(Matrix mag,Matrix R) -{ - float magnorm = sqrt(mag(1,1)*mag(1,1)+mag(2,1)*mag(2,1)+mag(3,1)*mag(3,1)); - Matrix magm = mag; - Matrix dcm(3,3); - computeDcm(dcm, qhat); - Matrix a2v = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1)); - - Matrix H(2,nState); - for (int j = 1; j < 4; j++){ - H(1,j+6) = a2v(1,j); - H(2,j+6) = a2v(2,j); - } - - //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; - //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A)); - Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); - - Matrix zmag = dcm*magm; - Matrix z(2,1); - z << 0.5f-zmag(1,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); -} -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); -} - -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); -} - -void solaESKF::updateGPS(Matrix posgps,Matrix velgps,Matrix R) +void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R) { Matrix H(5,nState); H(1,1) = 1.0f; H(2,2) = 1.0f; - H(3,4) = 1.0f; - H(4,5) = 1.0f; - H(5,6) = 1.0f; + H(3,3) = 1.0f; + H(4,4) = 1.0f; + H(5,5) = 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(5,1); - z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << velgps(3,1) - vihat(3,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); 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(); } Matrix solaESKF::computeAngles() @@ -293,7 +213,12 @@ gravity(2,1) += errState(17,1); gravity(3,1) += errState(18,1); - for (int i = 1; i < 19; i++){ + //gravity bias + magField(1,1) += errState(19,1); + magField(2,1) = 0.0f; + magField(3,1) = 0.0f; + + for (int i = 1; i < 20; i++){ errState(i,1) = 0.0f; } @@ -376,6 +301,10 @@ { return gravity; } +Matrix solaESKF::getMagField() +{ + return magField; +} Matrix solaESKF::getErrState() { return errState; @@ -405,6 +334,10 @@ { setBlockDiag(Phat,val,16,18); } +void solaESKF::setPhatMagField(float val) +{ + setBlockDiag(Phat,val,19,19); +} void solaESKF::setQVelocity(float val) @@ -423,6 +356,10 @@ { setBlockDiag(Q,val,13,15); } +void solaESKF::setQMagField(float val) +{ + setBlockDiag(Q,val,19,19); +} void solaESKF::setDiag(Matrix& mat, float val){ @@ -439,5 +376,143 @@ void solaESKF::setPihat(float pi_x, float pi_y, float pi_z) { - pihat << pi_x << pi_y << pi_z; -} \ No newline at end of file + pihat(1,1) = pi_x; + pihat(2,1) = pi_y; + pihat(3,1) = pi_z; +} +void solaESKF::setMagField(float hx, float hy, float hz) +{ + magField(1,1) = -sqrt(hx*hx+hy*hy); + magField(2,1) = 0.0f; + magField(3,1) = 0.0f; +} + +/* +void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R) +{ + Matrix accm = acc - accBias; + Matrix tdcm(3,3); + computeDcm(tdcm, qhat); + tdcm = MatrixMath::Transpose(tdcm); + Matrix tdcm_g = tdcm*gravity; + Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1)); + + Matrix H(4,nState); + for (int i = 1; i < 4; i++){ + for (int j = 1; j < 4; j++){ + H(i,j+6) = a2v(i,j); + H(i,j+15) = tdcm(i,j); + } + } + H(1,10) = -1.0f; + H(2,11) = -1.0f; + H(3,12) = -1.0f; + H(4,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 zacc = -accm-tdcm*gravity; + Matrix z(4,1); + z << zacc(1,1) << zacc(2,1) << zacc(3,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::updateGyroConstraints(Matrix gyro,Matrix R) +{ + Matrix gyrom = gyro - gyroBias; + Matrix dcm(3,3); + computeDcm(dcm, qhat); + Matrix a2v = dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1)); + + Matrix H(2,nState); + for (int i = 1; i < 3; i++){ + for (int j = 1; j < 4; j++){ + H(i,j+6) = a2v(i,j); + H(i,j+12) = -dcm(i,j); + } + } + //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; + //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A)); + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + + Matrix z3 = -dcm*gyrom; + Matrix z(2,1); + z << z3(1,1) << z3(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::updateMag(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(2,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,20) = 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(2,1); + z << zmag(1,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 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