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