Diff: solaESKF.cpp

Revision:

63:74a9565a0141

Parent:

62:5519d34eb6e8

Child:

65:c25d7810de44

--- a/solaESKF.cpp	Tue Nov 16 14:17:40 2021 +0000
+++ b/solaESKF.cpp	Wed Nov 17 01:40:33 2021 +0000
@@ -153,27 +153,64 @@
     Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
     
     Matrix H(3,nState);
+    int nH = H.getRows();
     
     Matrix magned = dcm*magm;
     float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
-    //float hz = sqrt(magned(3,1)*magned(3,1));
+    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));
 
     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(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;
+    
+    Matrix z(nH,1);
+    z << -(magned(1,1) - hx) << -magned(2,1)<<-(magned(3,1) - hz);
+    //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 r3(3,1);
+    r3 <<  tdcm(1,3)<<  tdcm(2,3) <<  tdcm(3,3);
+    Matrix kpart  = r3*MatrixMath::Transpose(r3);
+    Matrix Pm(nState,nState);
+    for(int i = 7; i<10; i++){
+        for(int j = 7;j<10;j++){
+            Pm(i,j) = Phat(i,j);
+        }
     }
-    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;
+    for(int i = 17; i<nState+1; i++){
+        for(int j = 17;j<nState+1;j++){
+            Pm(i,j) = Phat(i,j);
+        }
+    }
     
-    Matrix z(3,1);
-    z << -(magned(1,1) - hx) << -magned(2,1) <<-(magRadius-a) ;
-    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+    Matrix K = (Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+R);
+    Matrix Kmod(3,nH);
+    for(int i = 1; i<4; i++){
+        for(int j = 1;j<nH+1;j++){
+            Kmod(i,j) = K(i+6,j);
+        }
+    }
+    Kmod = kpart*Kmod;
+    for(int i = 1; i<nState+1; i++){
+        for(int j = 1;j<nH;j++){
+            if(i>6 && i<10){
+                K(i,j) = Kmod(i-6,j);
+            }else if(i<17){
+                K(i,j) = 0.0f;
+            }
+        }
+    }
+    
     errState =  K * z;
     Phat = (MatrixMath::Eye(nState)-K*H)*Phat;