Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 39:6834e05d8a64
- Parent:
- 38:1f6532849c05
- Child:
- 40:119792aa6d3b
diff -r 1f6532849c05 -r 6834e05d8a64 ScErrStateEKF.cpp --- 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);