Diff: solaESKF.cpp

Revision:

77:780ce6556041

Parent:

75:e2c825cdc511

Child:

78:e36a7b844fb5

--- 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();