Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 77:780ce6556041
- Parent:
- 75:e2c825cdc511
- Child:
- 78:e36a7b844fb5
diff -r cd1b21936ef1 -r 780ce6556041 solaESKF.cpp --- a/solaESKF.cpp Tue Nov 30 07:11:03 2021 +0000 +++ b/solaESKF.cpp Tue Nov 30 11:26:56 2021 +0000 @@ -287,16 +287,15 @@ void solaESKF::updateGPS(Matrix posgps,float palt,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; Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); - Matrix z(6,1); - z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1)<< velgps(3,1)-vihat(3,1); + Matrix z(5,1); + z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1); errState = K * z; Phat = (MatrixMath::Eye(nState)-K*H)*Phat; fuseErr2Nominal();