Diff: solaESKF.cpp

Revision:

65:c25d7810de44

Parent:

63:74a9565a0141

Child:

66:6a093cb91728

--- a/solaESKF.cpp	Wed Nov 17 01:40:33 2021 +0000
+++ b/solaESKF.cpp	Thu Nov 18 00:50:38 2021 +0000
@@ -163,21 +163,22 @@
     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(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,j+16) =  -dcm(3,j)+(dcm(3,j))/hz;
     }
-    //H(4,20) = 1.0f;
-    //H(4,17) = magm(1,1)/a;
-    //H(4,18) = magm(2,1)/a;
-    //H(4,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)<<-(magned(3,1) - hz);
+    z << -(magned(1,1) - hx) << -magned(2,1)<<-(magRadius-a);
     //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);
+    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
     
+    /*
     Matrix r3(3,1);
     r3 <<  tdcm(1,3)<<  tdcm(2,3) <<  tdcm(3,3);
     Matrix kpart  = r3*MatrixMath::Transpose(r3);
@@ -210,7 +211,74 @@
             }
         }
     }
+    */
+    errState =  K * z;
+    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
     
+    fuseErr2Nominal();
+}
+void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
+{
+    Matrix accm = acc - accBias;
+    Matrix magm = mag - magBias;
+    Matrix dcm(3,3);
+    computeDcm(dcm, qhat);
+    Matrix tdcm = MatrixMath::Transpose(dcm);
+    Matrix tdcm_g = tdcm*gravity;
+    Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
+    Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
+    
+    Matrix H(5,nState);
+    for (int i = 1; i < 4; i++){
+        for (int j = 1; j < 4; j++){
+            H(i,j+6) =  rotgrav(i,j);
+        }
+        H(i,16) = tdcm(i,3);
+    }
+
+    H(1,10) =  -1.0f;
+    H(2,11) =  -1.0f;
+    H(3,12) =  -1.0f;
+    
+    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++){
+        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);
+    }
+    
+    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);
+    //twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
+    errState =  K * z;
+    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
+    
+    fuseErr2Nominal();
+}
+
+
+void solaESKF::updateOtherConstraints(Matrix mag,float palt,Matrix R)
+{
+    Matrix magm = mag - magBias;
+    float a = sqrt(magm(1,1)*magm(1,1)+magm(2,1)*magm(2,1)+magm(3,1)*magm(3,1));
+    Matrix H(2,nState);
+    H(1,20) = -1.0f;
+    H(1,17) = -magm(1,1)/a;
+    H(1,18) = -magm(2,1)/a;
+    H(1,19) = -magm(3,1)/a;
+    H(2,3) = 1.0f;
+
+    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+    
+    Matrix z(2,1);
+    z << -(a-magRadius) << palt-pihat(3,1);
+    //twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
     errState =  K * z;
     Phat = (MatrixMath::Eye(nState)-K*H)*Phat;