Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 47:2467de40951f
- Parent:
- 46:15988dc41923
- Child:
- 48:06ed39e3376e
- Child:
- 50:dadad0567349
diff -r 15988dc41923 -r 2467de40951f solaESKF.cpp --- a/solaESKF.cpp Thu Oct 28 09:44:34 2021 +0000 +++ b/solaESKF.cpp Fri Oct 29 13:30:03 2021 +0000 @@ -24,20 +24,22 @@ nState = errState.getRows(); + 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.01f,10,12);//acc bias - setBlockDiag(Phat,0.01f,13,15);//gyro bias - setBlockDiag(Phat,0.00001f,16,18);//gravity - setBlockDiag(Q,0.0001f,4,6);//velocity - setBlockDiag(Q,0.00001f,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.0,7,9);//angle error setBlockDiag(Q,0.001f,10,12);//acc bias setBlockDiag(Q,0.001f,13,15);//gyro bias //positionとgravityはQ項なし } + void solaESKF::updateNominal(Matrix acc, Matrix gyro,float att_dt) { Matrix gyrom = gyro - gyroBias; @@ -64,85 +66,43 @@ Matrix dcm(3,3); 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)); - Matrix Fx(nState,nState); + Matrix A(nState,nState); //position - Fx(1,1) = 1.0f; - Fx(2,2) = 1.0f; - Fx(3,3) = 1.0f; - Fx(1,4) = 1.0f*att_dt; - Fx(2,5) = 1.0f*att_dt; - Fx(3,6) = 1.0f*att_dt; + A(1,4) = 1.0f; + A(2,5) = 1.0f; + A(3,6) = 1.0f; + + //velocity for (int i = 1; i < 4; i++){ for (int j = 1; j < 4; j++){ - Fx(i,j+6) = a2v2(i,j); - Fx(i,j+9) = -0.5f*dcm(i,j)*att_dt*att_dt; - } - Fx(i,i+15) = 0.5f*att_dt*att_dt; - } - - - //velocity - 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++){ - 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); + A(i+3,j+6) = a2v(i,j); + A(i+3,j+9) = -dcm(i,j); + } } - Fx(4,16) = 1.0f*att_dt; - Fx(5,17) = 1.0f*att_dt; - Fx(6,18) = 1.0f*att_dt; + A(4,16) = 1.0f; + A(5,17) = 1.0f; + A(6,18) = 1.0f; //angulat error - 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(7,13) = -1.0f*att_dt; - Fx(8,14) = -1.0f*att_dt; - Fx(9,15) = -1.0f*att_dt; + A(7,8) = gyrom(3,1); + A(7,9) = -gyrom(2,1); + A(8,7) = -gyrom(3,1); + A(8,9) = gyrom(1,1); + A(9,7) = gyrom(2,1); + A(9,8) = -gyrom(1,1); + A(7,13) = -1.0f; + A(8,14) = -1.0f; + A(9,15) = -1.0f; - //acc bias - Fx(10,10) = 1.0f; - Fx(11,11) = 1.0f; - Fx(12,12) = 1.0f; - - //gyro bias - Fx(13,13) = 1.0f; - Fx(14,14) = 1.0f; - Fx(15,15) = 1.0f; - - //gravity bias - Fx(16,16) = 1.0f; - Fx(17,17) = 1.0f; - Fx(18,18) = 1.0f; + + Matrix Fx = MatrixMath::Eye(nState) + A * att_dt;// + (0.5f*att_dt*att_dt) * A * A; + Matrix Qd = Q * att_dt + 0.5f*A*Q * att_dt * att_dt+ 0.5f*Q*MatrixMath::Transpose(A) * att_dt * att_dt; errState = Fx * errState; - Matrix Qi(nState,nState); - for (int i = 1; i < 4; i++){ - Qi(i,i) = 0.0f; - } - for (int i = 4; i < 10; i++){ - Qi(i,i) = Q(i,i)*att_dt*att_dt; - } - for (int i = 10; i < 16; i++){ - Qi(i,i) = Q(i,i)*att_dt; - } - for (int i = 16; i < 19; i++){ - Qi(i,i) = 0.0f; - } - Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Qi; + Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Qd; } void solaESKF::updateAccConstraints(Matrix acc,Matrix R) @@ -165,11 +125,13 @@ H(2,11) = -1.0f; H(3,12) = -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 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 = -accm-tdcm*gravity; 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); } @@ -188,13 +150,16 @@ } } - 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 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); } @@ -207,11 +172,13 @@ 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 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); } @@ -223,12 +190,13 @@ 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 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) << 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); } @@ -297,6 +265,10 @@ gravity(1,1) += errState(16,1); gravity(2,1) += errState(17,1); gravity(3,1) += errState(18,1); + + for (int i = 1; i < 19; i++){ + errState(i,1) = 0.0f; + } } @@ -345,13 +317,85 @@ qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; } + + void solaESKF::setGravity(float gx,float gy,float gz) { gravity(1,1) = gx; gravity(2,1) = gy; gravity(3,1) = gz; } +Matrix solaESKF::getPihat() +{ + return pihat; +} +Matrix solaESKF::getVihat() +{ + return vihat; +} +Matrix solaESKF::getQhat() +{ + return qhat; +} +Matrix solaESKF::getAccBias() +{ + return accBias; +} +Matrix solaESKF::getGyroBias() +{ + return gyroBias; +} +Matrix solaESKF::getGravity() +{ + return gravity; +} +Matrix solaESKF::getErrState() +{ + return errState; +} +void solaESKF::setPhatPosition(float val) +{ + setBlockDiag(Phat,val,1,3); +} +void solaESKF::setPhatVelocity(float val) +{ + setBlockDiag(Phat,val,4,6); +} +void solaESKF::setPhatAngleError(float val) +{ + setBlockDiag(Phat,val,7,9); +} +void solaESKF::setPhatAccBias(float val) +{ + setBlockDiag(Phat,val,10,12); +} +void solaESKF::setPhatGyroBias(float val) +{ + setBlockDiag(Phat,val,13,15); +} +void solaESKF::setPhatGravity(float val) +{ + setBlockDiag(Phat,val,16,18); +} + + +void solaESKF::setQVelocity(float val) +{ + setBlockDiag(Q,val,4,6); +} +void solaESKF::setQAngleError(float val) +{ + setBlockDiag(Q,val,7,9); +} +void solaESKF::setQAccBias(float val) +{ + setBlockDiag(Q,val,10,12); +} +void solaESKF::setQGyroBias(float val) +{ + setBlockDiag(Q,val,13,15); +} void solaESKF::setDiag(Matrix& mat, float val){