Diff: solaESKF.cpp

Revision:

48:06ed39e3376e

Parent:

47:2467de40951f

Child:

49:0a1976eb5420

diff -r 2467de40951f -r 06ed39e3376e solaESKF.cpp
--- a/solaESKF.cpp	Fri Oct 29 13:30:03 2021 +0000
+++ b/solaESKF.cpp	Fri Oct 29 13:33:25 2021 +0000
@@ -63,46 +63,90 @@
 {
     Matrix gyrom = gyro  - gyroBias;
     Matrix accm = acc - accBias;
-    
+
     Matrix dcm(3,3);
     computeDcm(dcm, qhat);
-    Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1));
-    
-    Matrix A(nState,nState);
+    Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1))*att_dt;
+    Matrix a2v2 = 0.5f*a2v*att_dt;
+
+    Matrix Fx(nState,nState);
     //position
-    A(1,4) =  1.0f;
-    A(2,5) =  1.0f;
-    A(3,6) =  1.0f;
-    
-    //velocity
+    Fx(1,1) =  1.0f;
+    Fx(2,2) =  1.0f;
+    Fx(3,3) =  1.0f;
+    Fx(1,4) =  1.0f*att_dt;
+    Fx(2,5) =  1.0f*att_dt;
+    Fx(3,6) =  1.0f*att_dt;
     for (int i = 1; i < 4; i++){
         for (int j = 1; j < 4; j++){
-            A(i+3,j+6) = a2v(i,j);
-            A(i+3,j+9) = -dcm(i,j);
+            Fx(i,j+6) = a2v2(i,j);
+            Fx(i,j+9) = -0.5f*dcm(i,j)*att_dt*att_dt;
+        }
+        Fx(i,i+15) = 0.5f*att_dt*att_dt;
+    }
+
 
+    //velocity
+    Fx(4,4) =  1.0f;
+    Fx(5,5) =  1.0f;
+    Fx(6,6) =  1.0f;
+    Matrix a2v = -dcm*MatrixMath::Matrixcross(acc(1,1),acc(2,1),acc(3,1))*att_dt;
+    for (int i = 1; i < 4; i++){
+        for (int j = 1; j < 4; j++){
+            Fx(i+3,j+6) = a2v(i,j);
+            Fx(i+3,j+9) = -dcm(i,j)*att_dt;
+            Fx(i+3,j+12) = -a2v2(i,j);
         }
     }
-    A(4,16) =  1.0f;
-    A(5,17) =  1.0f;
-    A(6,18) =  1.0f;
-    
+    Fx(4,16) =  1.0f*att_dt;
+    Fx(5,17) =  1.0f*att_dt;
+    Fx(6,18) =  1.0f*att_dt;
+
     //angulat error
-    A(7,8) =  gyrom(3,1);
-    A(7,9) = -gyrom(2,1);
-    A(8,7) = -gyrom(3,1);
-    A(8,9) =  gyrom(1,1);
-    A(9,7) =  gyrom(2,1);
-    A(9,8) = -gyrom(1,1);
-    A(7,13) =  -1.0f;
-    A(8,14) =  -1.0f;
-    A(9,15) =  -1.0f;
-    
+    Fx(7,7) =  1.0f;
+    Fx(8,8) =  1.0f;
+    Fx(9,9) =  1.0f;
+    Fx(7,8) =  gyrom(3,1)*att_dt;
+    Fx(7,9) = -gyrom(2,1)*att_dt;
+    Fx(8,7) = -gyrom(3,1)*att_dt;
+    Fx(8,9) =  gyrom(1,1)*att_dt;
+    Fx(9,7) =  gyrom(2,1)*att_dt;
+    Fx(9,8) = -gyrom(1,1)*att_dt;
+    Fx(7,13) =  -1.0f*att_dt;
+    Fx(8,14) =  -1.0f*att_dt;
+    Fx(9,15) =  -1.0f*att_dt;
+
+    //acc bias
+    Fx(10,10) =  1.0f;
+    Fx(11,11) =  1.0f;
+    Fx(12,12) =  1.0f;
 
-    Matrix Fx = MatrixMath::Eye(nState) + A * att_dt;// + (0.5f*att_dt*att_dt) * A * A;
-    Matrix Qd = Q * att_dt + 0.5f*A*Q * att_dt * att_dt+ 0.5f*Q*MatrixMath::Transpose(A) * att_dt * att_dt;
-    
+    //gyro bias
+    Fx(13,13) =  1.0f;
+    Fx(14,14) =  1.0f;
+    Fx(15,15) =  1.0f;
+
+    //gravity bias
+    Fx(16,16) =  1.0f;
+    Fx(17,17) =  1.0f;
+    Fx(18,18) =  1.0f;
+
     errState = Fx * errState;
-    Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Qd;
+    Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Q*att_dt*att_dt;
+    Matrix Qi(nState,nState);
+    for (int i = 1; i < 4; i++){
+        Qi(i,i) = 0.0f;
+    }
+    for (int i = 4; i < 10; i++){
+        Qi(i,i) = Q(i,i)*att_dt*att_dt;
+    }
+    for (int i = 10; i < 16; i++){
+        Qi(i,i) = Q(i,i)*att_dt;
+    }
+    for (int i = 16; i < 19; i++){
+        Qi(i,i) = 0.0f;
+    }
+    Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Qi;
 }
 
 void solaESKF::updateAccConstraints(Matrix acc,Matrix R)