Diff: solaESKF.cpp

Revision:

52:ab42c0497088

Parent:

51:a5af3b280d23

Child:

53:8b551358a7e3

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