Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 50:dadad0567349
- Parent:
- 47:2467de40951f
- Child:
- 51:a5af3b280d23
diff -r 2467de40951f -r dadad0567349 solaESKF.cpp --- a/solaESKF.cpp Fri Oct 29 13:30:03 2021 +0000 +++ b/solaESKF.cpp Mon Nov 01 09:17:52 2021 +0000 @@ -105,7 +105,7 @@ Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Qd; } -void solaESKF::updateAccConstraints(Matrix acc,Matrix R) +void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R) { Matrix accm = acc - accBias; Matrix tdcm(3,3); @@ -114,7 +114,7 @@ Matrix tdcm_g = tdcm*gravity; Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1)); - Matrix H(3,nState); + 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); @@ -124,15 +124,18 @@ 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 z = -accm-tdcm*gravity; + Matrix zacc = -accm-tdcm*gravity; + Matrix z(4,1); + z << zacc(1,1) << zacc(2,1) << zacc(3,1) << palt- vihat(3,1); errState = K * z; - //Phat = (MatrixMath::Eye(nState)-K*H)*Phat; + 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); + //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); } void solaESKF::updateGyroConstraints(Matrix gyro,Matrix R) @@ -178,9 +181,9 @@ 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 = (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); + //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) @@ -196,9 +199,9 @@ 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 = (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); + //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)