Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 52:ab42c0497088
- Parent:
- 51:a5af3b280d23
- Child:
- 53:8b551358a7e3
diff -r a5af3b280d23 -r ab42c0497088 solaESKF.cpp --- a/solaESKF.cpp Wed Nov 03 09:27:58 2021 +0000 +++ b/solaESKF.cpp Thu Nov 04 09:41:31 2021 +0000 @@ -131,7 +131,7 @@ 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- vihat(3,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); @@ -188,16 +188,16 @@ void solaESKF::updateGPSPosition(Matrix posgps,Matrix R) { - Matrix H(3,nState); + Matrix H(2,nState); H(1,1) = 1.0f; H(2,2) = 1.0f; - H(3,3) = 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(3,1); - z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << posgps(3,1) - pihat(3,1); + 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);