Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 53:8b551358a7e3
- Parent:
- 52:ab42c0497088
- Child:
- 54:cd514d9d4b19
diff -r ab42c0497088 -r 8b551358a7e3 solaESKF.cpp --- a/solaESKF.cpp Thu Nov 04 09:41:31 2021 +0000 +++ b/solaESKF.cpp Fri Nov 05 13:48:03 2021 +0000 @@ -207,21 +207,22 @@ void solaESKF::updateGPS(Matrix posgps,Matrix velgps,Matrix R) { - Matrix H(6,nState); + Matrix H(5,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; - H(6,6) = 1.0f; + H(3,4) = 1.0f; + H(4,5) = 1.0f; + H(5,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(6))*MatrixMath::Transpose(A)); - Matrix z(6,1); - z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << posgps(3,1) - pihat(3,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << velgps(3,1) - vihat(3,1); + //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); 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); + //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); } Matrix solaESKF::computeAngles()