Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 32:321a756e12ad
- Parent:
- 31:e655d4d8e4d6
- Child:
- 34:dec4b37db3f1
- Child:
- 36:b1d107b00351
- Child:
- 37:7873fe37b425
diff -r e655d4d8e4d6 -r 321a756e12ad ScErrStateEKF.cpp --- 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);