Diff: solaESKF.cpp

Revision:

64:f5b6256726f3

Parent:

61:5e5c4fe12440

Child:

67:0453896df0d6

--- a/solaESKF.cpp	Tue Nov 16 13:56:44 2021 +0000
+++ b/solaESKF.cpp	Wed Nov 17 05:11:57 2021 +0000
@@ -148,7 +148,7 @@
     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));
@@ -162,6 +162,7 @@
     
     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++){
@@ -169,28 +170,33 @@
             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,3);
+    Matrix Kmod(3,nH);
     for(int i = 1; i<4; i++){
-        for(int j = 1;j<4;j++){
+        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<4;j++){
+        for(int j = 1;j<nH+1;j++){
             if(i>6 && i<10){
                 K(i,j) = Kmod(i-6,j);
-            }else{
+            }else if(i<13 || i>15){
                 K(i,j) = 0.0f;
             }
         }
     }
     
-    Matrix z(3,1);
+    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),z(3,1));
+    //twelite.printf("%f %f %f\r\n",z(1,1),z(2,1));
     errState =  K * z;
     Phat = (MatrixMath::Eye(nState)-K*H)*Phat;