Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 55:21611d4cf7e8
- Parent:
- 54:cd514d9d4b19
- Child:
- 56:c10f1168bd4a
diff -r cd514d9d4b19 -r 21611d4cf7e8 solaESKF.cpp --- a/solaESKF.cpp Sun Nov 07 05:38:04 2021 +0000 +++ b/solaESKF.cpp Mon Nov 08 09:19:55 2021 +0000 @@ -30,7 +30,7 @@ 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.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項なし @@ -141,7 +141,7 @@ 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 a2v = dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1)); Matrix H(2,nState); for (int i = 1; i < 3; i++){ @@ -163,7 +163,32 @@ //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) {