Diff: solaESKF.cpp

Revision:

68:264a7e0e4a29

Parent:

66:6a093cb91728

Child:

69:2b2815603e5a

Child:

70:d12e46fdc2f0

--- a/solaESKF.cpp	Thu Nov 18 08:40:15 2021 +0000
+++ b/solaESKF.cpp	Thu Nov 18 10:10:07 2021 +0000
@@ -152,7 +152,7 @@
     Matrix tdcm = MatrixMath::Transpose(dcm);
     Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
     
-    Matrix H(3,nState);
+    Matrix H(2,nState);
     int nH = H.getRows();
     
     Matrix magned = dcm*magm;
@@ -160,21 +160,21 @@
     float hz = sqrt(magned(3,1)*magned(3,1));
     float a = sqrt(magm(1,1)*magm(1,1)+magm(2,1)*magm(2,1)+magm(3,1)*magm(3,1));
 
+    H(1,3+6) =  rotmag(1,3)-(rotmag(1,3)+rotmag(2,3))/hx;
+    H(2,3+6) =  rotmag(2,3);
     for(int j = 1; j < 4; j++){
-        H(1,j+6) =  rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx;
-        H(2,j+6) =  rotmag(2,j);
         //H(3,j+6) =  rotmag(3,j)-(rotmag(3,j))/hz;
         H(1,j+16) =  -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
         H(2,j+16) =  -dcm(2,j);
         //H(3,j+16) =  -dcm(3,j)+(dcm(3,j))/hz;
     }
-    H(3,20) = 1.0f;
-    H(3,17) = magm(1,1)/a;
-    H(3,18) = magm(2,1)/a;
-    H(3,19) = magm(3,1)/a;
+    //H(3,20) = 1.0f;
+    //H(3,17) = magm(1,1)/a;
+    //H(3,18) = magm(2,1)/a;
+    //H(3,19) = magm(3,1)/a;
     
     Matrix z(nH,1);
-    z << -(magned(1,1) - hx) << -magned(2,1)<<-(magRadius-a);
+    z << -(magned(1,1) - hx) << -magned(2,1);
     //twelite.printf("%f %f %f %f\r\n",-(magned(1,1) - hx),-magned(2,1),-(magned(3,1) - hz),-(magRadius-a));
     Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);