Diff: solaESKF.cpp

Revision:

67:0453896df0d6

Parent:

64:f5b6256726f3

--- a/solaESKF.cpp	Wed Nov 17 05:11:57 2021 +0000
+++ b/solaESKF.cpp	Thu Nov 18 08:48:14 2021 +0000
@@ -154,46 +154,13 @@
     float hz = sqrt(magned(3,1)*magned(3,1));
    
 
-    for(int j = 1; j < 4; j++){
+    for(int j = 3; 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;
     }
     
-    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);
-        }
-    }
-    for(int i = 13; i<16; i++){
-        for(int j = 13;j<16;j++){
-            Pm(i,j) = Phat(i,j);
-        }
-    }
-    
-    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+1;j++){
-            if(i>6 && i<10){
-                K(i,j) = Kmod(i-6,j);
-            }else if(i<13 || i>15){
-                K(i,j) = 0.0f;
-            }
-        }
-    }
-    
+    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
     Matrix z(nH,1);
     z << -(magned(1,1) - hx) << -magned(2,1) <<-(magned(3,1) - hz) ;
     //twelite.printf("%f %f %f\r\n",z(1,1),z(2,1));