Diff: ScErrStateEKF.cpp

Revision:

32:321a756e12ad

Parent:

31:e655d4d8e4d6

Child:

34:dec4b37db3f1

Child:

36:b1d107b00351

Child:

37:7873fe37b425

--- a/ScErrStateEKF.cpp	Tue Sep 28 08:17:42 2021 +0000
+++ b/ScErrStateEKF.cpp	Wed Sep 29 04:50:42 2021 +0000
@@ -16,20 +16,17 @@
     setDiag(Phat,0.1f);
     setDiag(Phatbg,0.1f);
     setQqerr(0.000005f);
-    setQgbias(0.00001f);
+    setQgbias(0.001f);
     setQabias(0.00001f);
-    setQv(0.000001f);
+    setQv(0.0001f);
     
     //加速度の観測
     setDiag(Ra,0.005f);
-    setDiag(Qab,10000.0f);
+    setDiag(Qab,10.0f);
     
     //ジャイロバイアスに関する制約
     setDiag(Rgsc,500.0f);
     
-    //機体軸速度に関する制約
-    setDiag(Rvsc,100000.0f);
-    
     setDiag(Rm,5.0f);
     
     setDiag(Rgps,5.5f);
@@ -236,13 +233,13 @@
     Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K);
 }
 
-void ScErrStateEKF::updateGPSVelocity(float vi_x, float vi_y,Vector3 acc, Vector3 accref)
+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(5,nState);
+    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;
@@ -254,18 +251,20 @@
     H(3,6) = 1.0f;
     H(4,7)  = 1.0f;
     H(5,8)  = 1.0f;
+    H(6,9)  = 1.0f;
     
-    Matrix R(5,5);
+    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 K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
     Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
-    Matrix z(5,1);
-    z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << vi_x - vihat(1,1) << vi_y-vihat(2,1);
+    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 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);