Diff: solaESKF.cpp

Revision:

60:f4b4231a8d3f

Parent:

59:03fe5e16a33c

Child:

61:5e5c4fe12440

--- a/solaESKF.cpp	Mon Nov 15 13:41:40 2021 +0000
+++ b/solaESKF.cpp	Tue Nov 16 01:20:50 2021 +0000
@@ -142,17 +142,18 @@
     float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
     
     for(int j = 1; j < 4; j++){
-        H(4,j+6) =  rotmag(1,j)+2.0f*(rotmag(1,j)+rotmag(2,j))/hx;
-        H(4,j+16) =  -dcm(1,j)+2.0f*dcm(1,j)/hx;
+        H(4,j+6) =  rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx;
+        H(4,j+16) =  -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
         H(5,j+6) =  rotmag(2,j);
-        H(5,j+16) =  -dcm(2,j)+2.0f*dcm(2,j)/hx;
+        H(5,j+16) =  -dcm(2,j);
     }
     
     Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
     Matrix zacc = -accm-tdcm*gravity;
     Matrix zmag = dcm*magm;
     Matrix z(5,1);
-    z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx) << zmag(2,1);
+    z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx) << -zmag(2,1);
+    twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
     errState =  K * z;
     Phat = (MatrixMath::Eye(nState)-K*H)*Phat;