Diff: solaESKF.cpp

Revision:

75:e2c825cdc511

Parent:

74:f5fe7fecbd3c

Child:

77:780ce6556041

--- a/solaESKF.cpp	Mon Nov 22 09:51:36 2021 +0000
+++ b/solaESKF.cpp	Mon Nov 29 09:45:20 2021 +0000
@@ -59,51 +59,82 @@
 {
     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);
-            A(i+3,j+15) =  1.0f;
+            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;
+    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);
         }
     }
-    
-    
+    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;
 
-    Matrix Fx = MatrixMath::Eye(nState) + A * att_dt;// + (0.5f*att_dt*att_dt) * A * A;
+    //acc bias
+    Fx(10,10) =  1.0f;
+    Fx(11,11) =  1.0f;
+    Fx(12,12) =  1.0f;
+
+    //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);
     for (int i = 1; i < nState+1; i++){
         if(i>3 && i<10){
-            Q(i,i) *=att_dt;
+            Phat(i,i)  += Q(i,i)*att_dt;
         }else if(i>9 && i<16){
-            Q(i,i) *= att_dt*att_dt;
-        }else{
-            Q(i,i) = 0.0f;
+            Phat(i,i)  += Q(i,i)* att_dt*att_dt;
         }      
     }
-    errState = Fx * errState;
-    Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Q;
 }
 
 void solaESKF::updateAcc(Matrix acc,Matrix R)
@@ -235,6 +266,24 @@
     //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
     fuseErr2Nominal();
 }
+void solaESKF::updateGPSVelocity(Matrix velgps,Matrix R)
+{
+    Matrix H(3,nState);
+    H(1,4)  = 1.0f;
+    H(2,5)  = 1.0f;
+    H(3,6)  = 1.0f;
+    
+    //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
+    //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
+    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+    Matrix z(3,1);
+    z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1)<< velgps(3,1)-vihat(3,1);
+    errState =  K * z;
+    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
+    //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
+    //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
+    fuseErr2Nominal();
+}
 
 void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R)
 {
@@ -352,6 +401,16 @@
         qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;    
 }
 
+Matrix solaESKF::calcDynAcc(Matrix acc)
+{
+    Matrix accm = acc - accBias;
+    Matrix tdcm(3,3);
+    computeDcm(tdcm, qhat);
+    tdcm = MatrixMath::Transpose(tdcm);
+
+    Matrix dynAcc = accm+tdcm*gravity;
+    return dynAcc;
+}
 
 
 void solaESKF::setGravity(float gx,float gy,float gz)