Diff: solaESKF.cpp

Revision:

66:6a093cb91728

Parent:

65:c25d7810de44

Child:

68:264a7e0e4a29

--- a/solaESKF.cpp	Thu Nov 18 00:50:38 2021 +0000
+++ b/solaESKF.cpp	Thu Nov 18 08:40:15 2021 +0000
@@ -220,7 +220,7 @@
 void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
 {
     Matrix accm = acc - accBias;
-    Matrix magm = mag - magBias;
+    Matrix magm = mag;
     Matrix dcm(3,3);
     computeDcm(dcm, qhat);
     Matrix tdcm = MatrixMath::Transpose(dcm);
@@ -243,11 +243,11 @@
     Matrix magned = dcm*magm;
     float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
     
-    for(int j = 1; j < 4; j++){
+    for(int j = 3; j < 4; j++){
         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(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);
+        //H(5,j+16) =  -dcm(2,j);
     }
     
     Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);