Diff: ScErrStateEKF.cpp

Revision:

43:cbc2c2d65131

Parent:

40:119792aa6d3b

--- a/ScErrStateEKF.cpp	Mon Oct 18 12:16:57 2021 +0000
+++ b/ScErrStateEKF.cpp	Mon Oct 25 05:39:20 2021 +0000
@@ -8,7 +8,7 @@
 using namespace std;
 
 ScErrStateEKF::ScErrStateEKF()
-    :qhat(4,1),vihat(3,1), errState(12,1), Phat(12,12), Q(12,12), Ra(3,3), Rgps(3,3),Rsr(1,1), Rm(3,3), Qab(3,3),Rgsc(2,2),Rvsc(2,2),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f)
+    :qhat(4,1),vihat(3,1), errState(12,1),accned(3,1), Phat(12,12), Q(12,12),Ra(3,3), Rgps(3,3),Rsr(1,1), Rm(3,3), Qab(3,3),Rgsc(2,2),Rvsc(2,2),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f)
 { 
     nState = errState.getRows();
     qhat << 1.0f << 0.0f << 0.0f << 0.0f;
@@ -25,10 +25,13 @@
     Phat(7,7) = 1.0;
     Phat(8,8) = 1.0;
     Phat(9,9) = 1.0;
+    Phat(10,10) = 1.0;
+    Phat(11,11) = 1.0;
+    Phat(12,12) = 1.0;
     setQqerr(0.001f);  
     setQgbias(0.0001f);  
     setQabias(1.0f);
-    setQv(0.01f);
+    setQv(1.0f);
     
     //加速度の観測
     setDiag(Ra,0.1f);
@@ -41,7 +44,7 @@
     setDiag(Rm,5.0f);
     
     //GPS
-    setDiag(Rgps,0.001f);
+    setDiag(Rgps,10.0f);
     //降下速度
     setDiag(Rsr,0.001f);
     
@@ -57,7 +60,7 @@
 void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt)
 {
     gyro -= gyroBias;
-    acc -= accBias;
+    //acc -= accBias;
     Matrix A(4,4);
     A <<  0.0f        << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z
       <<  0.5f*gyro.x <<  0.0f        << 0.5f*gyro.z <<-0.5f*gyro.y
@@ -71,13 +74,14 @@
     
     Matrix dcm(3,3);
     computeDcm(dcm, qhat);
-    vihat += (MatrixMath::Transpose(dcm)*MatrixMath::Vector2mat(acc)-MatrixMath::Vector2mat(accref))*att_dt*9.8f;
+    accned = (MatrixMath::Transpose(dcm)*MatrixMath::Vector2mat(acc)-MatrixMath::Vector2mat(accref));
+    vihat += accned*att_dt*9.8f;
 }
 
 void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt)
 {
     gyro -= gyroBias;
-    acc -= accBias;
+    //acc -= accBias;
     Matrix A(nState,nState);
     A(1,2) =  gyro.z;
     A(1,3) = -gyro.y;
@@ -92,11 +96,11 @@
     Matrix dcm(3,3);
     computeDcm(dcm, qhat);
     Matrix qeterm = -2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z);
-    Matrix baterm = -9.8f*MatrixMath::Transpose(dcm);
+    //Matrix baterm = -9.8f*MatrixMath::Transpose(dcm);
     for (int i = 1; i < 4; i++){
         for (int j = 1; i < 4; i++){
             A(i+9,j) = qeterm(i,j);
-            A(i+9,j+6) = baterm(i,j);
+            //A(i+9,j+6) = baterm(i,j);
         }
     }
     
@@ -251,49 +255,20 @@
 
 void ScErrStateEKF::updateGPSVelocity(float vi_x, float vi_y, float sinkRate,Vector3 acc, Vector3 accref)
 {
-    acc -= accBias;
-    Matrix dcm(3,3);
-    computeDcm(dcm, qhat);
-    Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
     
-    Matrix H(6,nState);
-    H(1,2) = -2.0f*gvec.z;
-    H(1,3) =  2.0f*gvec.y;
-    H(2,1) =  2.0f*gvec.z;
-    H(2,3) = -2.0f*gvec.x;
-    H(3,1) = -2.0f*gvec.y;
-    H(3,2) =  2.0f*gvec.x;
-    H(1,7) = 1.0f;
-    H(2,8) = 1.0f;
-    H(3,9) = 1.0f;
-    H(4,10)  = 1.0f;
-    H(5,11)  = 1.0f;
-    H(6,12)  = 1.0f;
-    
-    Matrix R(6,6);
-    R(1,1) = Ra(1,1)+Qab(1,1);
-    R(2,2) = Ra(2,2)+Qab(2,2);
-    R(3,3) = Ra(3,3)+Qab(3,3);
-    R(4,4) = Rgps(1,1);
-    R(5,5) = Rgps(2,2);
-    R(6,6) = Rsr(1,1);
-    
-    /*
-    Matrix H(3, nState);
-    H(1, 10) = 1.0f;
-    H(2, 11) = 1.0f;
-    H(3, 12) = 1.0f;
-    
+    Matrix H(3,nState);
+    H(1,10)  = 1.0f;
+    H(2,11)  = 1.0f;
+    H(3,12)  = 1.0f;
+
     Matrix R(3,3);
     R(1,1) = Rgps(1,1);
     R(2,2) = Rgps(2,2);
-    R(3,3) = Rsr(1,1);
-    */
+    R(3,3) = Rgps(3,3);
     
     Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
-    Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
-    Matrix z(6,1);
-    z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);
+    Matrix z(3,1);
+    z << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);
     //z << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);
     Matrix corrVal =  K * (z-H*errState);
     errState = errState + corrVal;
@@ -481,6 +456,7 @@
 {
     Matrix dcm(3,3);
     computeDcm(dcm, qhat);
+    dcm = MatrixMath::Transpose(dcm);
     float _x, _y, _z;
     _x = acc.x-(dcm( 1, 1 )*accref.x+dcm( 1, 2 )*accref.y+dcm( 1, 3 )*accref.z);
     _y = acc.y-(dcm( 2, 1 )*accref.x+dcm( 2, 2 )*accref.y+dcm( 2, 3 )*accref.z);