Diff: ScErrStateEKF.cpp

Revision:

39:6834e05d8a64

Parent:

38:1f6532849c05

Child:

40:119792aa6d3b

--- a/ScErrStateEKF.cpp	Sun Oct 10 09:15:29 2021 +0000
+++ b/ScErrStateEKF.cpp	Thu Oct 14 11:39:38 2021 +0000
@@ -13,29 +13,32 @@
     nState = errState.getRows();
     qhat << 1.0f << 0.0f << 0.0f << 0.0f;
     
-    setDiag(Phat,0.1f);  //0.1
-    Phat(4,4) = 0.00001;
-    Phat(5,5) = 0.00001;
-    Phat(6,6) = 0.00001;
-    setQqerr(0.000005f);  //0.000005
-    setQgbias(0.0001f);
-    setQabias(0.0001f);
-    setQv(0.0001f);
+    setDiag(Phat,0.1f); 
+    Phat(4,4) = 0.001;
+    Phat(5,5) = 0.001;
+    Phat(6,6) = 0.001;
+    Phat(7,7) = 1.0;
+    Phat(8,8) = 1.0;
+    Phat(9,9) = 1.0;
+    setQqerr(0.001f);  
+    setQgbias(0.0001f);  
+    setQabias(1.0f);
+    setQv(0.01f);
     
     //加速度の観測
-    setDiag(Ra,0.005f);
-    setDiag(Qab,10.0f);
+    setDiag(Ra,0.1f);
+    setDiag(Qab,1.5f);  
     
     //ジャイロバイアスに関する制約
-    setDiag(Rgsc,500.0f);
+    setDiag(Rgsc,500.0f);  
     
     //地磁気（未使用）
     setDiag(Rm,5.0f);
     
     //GPS
-    setDiag(Rgps,5.5f);
+    setDiag(Rgps,0.001f);
     //降下速度
-    setDiag(Rsr,5.5f);
+    setDiag(Rsr,0.001f);
     
     for(int i = 0; i<10;i++){
         histffunc[i] = 0.0f;
@@ -272,7 +275,8 @@
     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);;
+    z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);
+    //z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << vi_x - vihat(1,1) << vi_y-vihat(2,1);
     Matrix corrVal =  K * (z-H*errState);
     errState = errState + corrVal;
     Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);