Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 20:37d3c3ee36e9
- Parent:
- 19:3fae66745363
- Child:
- 21:d6079def0473
diff -r 3fae66745363 -r 37d3c3ee36e9 ScErrStateEKF.cpp --- a/ScErrStateEKF.cpp Fri Jul 23 05:55:28 2021 +0000 +++ b/ScErrStateEKF.cpp Sat Aug 07 06:54:50 2021 +0000 @@ -8,10 +8,10 @@ using namespace std; ScErrStateEKF::ScErrStateEKF() - :qhat(4,1), errState(12,1), Phat(12,12), Q(12,12), Ra(5,5), Rm(3,3), Qab(5,5),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f) + :qhat(4,1), errState(12,1), Phat(12,12), Q(12,12), Ra(3,3), Rm(3,3), Qab(3,3),Rgsc(2,2),Rvsc(3,3),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f) { qhat << 1.0f << 0.0f << 0.0f << 0.0f; - errState << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f; + errState << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f; Phat(1,1) = 0.1f; Phat(2,2) = 0.1f; @@ -26,35 +26,40 @@ Phat(11,11) = 0.1f; Phat(12,12) = 0.1f; - Q(1,1) = 0.000005f*0.25f; - Q(2,2) = 0.000005f*0.25f; - Q(3,3) = 0.000005f*0.25f; - Q(4,4) = 0.0005f; - Q(5,5) = 0.0005f; - Q(6,6) = 0.0005f; - Q(7,7) = 100.0f; - Q(8,8) = 100.0f; - Q(9,9) = 100.0f; - Q(10,10) = 100.0f; - Q(11,11) = 100.0f; - Q(12,12) = 100.0f; + Q(1,1) = 0.0000046f; + Q(2,2) = 0.0000046f; + Q(3,3) = 0.0000046f; + Q(4,4) = 0.000013f; + Q(5,5) = 0.000013f; + Q(6,6) = 0.000013f; + Q(7,7) = 0.0057f; + Q(8,8) = 0.0057f; + Q(9,9) = 0.0057f; + Q(10,10) = 0.01f; + Q(11,11) = 0.01f; + Q(12,12) = 0.01f; - Ra(1,1) = 0.000020f; - Ra(2,2) = 0.000020f; - Ra(3,3) = 0.000020f; - Ra(4,4) = 10.0f; - Ra(5,5) = 10.0f; + //加速度の観測 + Ra(1,1) = 1.020f; + Ra(2,2) = 1.020f; + Ra(3,3) = 1.020f; + Qab(1,1) = 124.810f; + Qab(2,2) = 124.810f; + Qab(3,3) = 124.810f; + + //ジャイロバイアスに関する制約 + Rgsc(1,1) = 263.980f; + Rgsc(2,2) = 263.980f; + + //速度に関する制約 + Rvsc(1,1) = 5000.0f; + Rvsc(2,2) = 5000.0f; + Rvsc(3,3) = 5000.0f; Rm(1,1) = 5.0f; Rm(2,2) = 5.0f; Rm(3,3) = 5.0f; - Qab(1,1) = 100.7f; - Qab(2,2) = 100.7f; - Qab(3,3) = 100.7f; - Qab(4,4) = 0.0f; - Qab(5,5) = 0.0f; - for(int i = 0; i<10;i++){ histffunc[i] = 0.0f; } @@ -73,12 +78,45 @@ << 0.5f*gyro.y << -0.5f*gyro.z << 0.0f << 0.5f*gyro.x << 0.5f*gyro.z << 0.5f*gyro.y <<-0.5f*gyro.x << 0.0f ; - Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A + (1.0f/6.0f*att_dt*att_dt*att_dt) * A * A * A; + Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A; qhat = phi * qhat; float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); qhat *= (1.0f/ qnorm); } +void ScErrStateEKF::setQqerr(float val){ + Q(1,1) = val; + Q(2,2) = val; + Q(3,3) = val; +} + +void ScErrStateEKF::setQgbias(float val){ + Q(4,4) = val; + Q(5,5) = val; + Q(6,6) = val; +} +void ScErrStateEKF::setQabias(float val){ + Q(7,7) = val; + Q(8,8) = val; + Q(9,9) = val; +} + +void ScErrStateEKF::setQab(float val){ + Qab(1,1) = val; + Qab(2,2) = val; + Qab(3,3) = val; +} + +void ScErrStateEKF::setRsoftconst(float Vgsc,float Vvsc){ + Rgsc(1,1) = Vgsc; + Rgsc(2,2) = Vgsc; + Rvsc(1,1) = Vvsc; + Rvsc(2,2) = Vvsc; + Rvsc(3,3) = Vvsc; +} + + + void ScErrStateEKF::updateErrState(Vector3 gyro, float att_dt) { gyro -= gyroBias; @@ -96,7 +134,7 @@ A(11,8) = 1.0f; A(12,9) = 1.0f; - Matrix phi = MatrixMath::Eye(12) + A * att_dt + (0.5f*att_dt*att_dt) * A * A + (1.0f/6.0f*att_dt*att_dt*att_dt) * A * A * A; + Matrix phi = MatrixMath::Eye(12) + A * att_dt + (0.5f*att_dt*att_dt) * A * A; Matrix Qd = Q * att_dt + 0.5f*A*Q * att_dt * att_dt+ 0.5f*Q*MatrixMath::Transpose(A) * att_dt * att_dt; errState = phi * errState; Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd; @@ -105,6 +143,34 @@ void ScErrStateEKF::updateAccMeasures(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(3,12); + 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; + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra+Qab); + Matrix accmat(3,1); + accmat << acc.x << acc.y << acc.z; + Matrix gref(3,1); + gref << 0.0f << 0.0f << accref.z; + Matrix z = accmat-dcm*gref; + Matrix corrVal = K * (z-H*errState); + errState = errState + corrVal; + Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K); +} + +/* +void ScErrStateEKF::updateScAccMeasures(Vector3 acc,Vector3 gyro, Vector3 accref) +{ + acc -= accBias; //acc = acc/acc.Norm(); //accref = accref/accref.Norm(); @@ -121,29 +187,64 @@ H(1,7) = 1.0f; H(2,8) = 1.0f; H(3,9) = 1.0f; - H(4,10) = 1.0f; - H(5,12) = 1.0f; - Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra+Qab); + H(4,4) = -1.0f*(dcm(1,1)); + H(4,5) = -1.0f*(dcm(2,1)); + H(4,6) = -1.0f*(dcm(3,1)); + H(5,4) = -1.0f*(dcm(1,2)); + H(5,5) = -1.0f*(dcm(2,2)); + H(5,6) = -1.0f*(dcm(3,2)); + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rasc+Qab); Matrix accmat(3,1); accmat << acc.x << acc.y << acc.z; Matrix gref(3,1); gref << 0.0f << 0.0f << accref.z; Matrix zacc = accmat-dcm*gref; Matrix z(5,1); - z << zacc(1,1) << zacc(2,1) << zacc(3,1) << 0.0f << 0.0f; + z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -dcm(1,1)*gyro.x-dcm(2,1)*gyro.y-dcm(3,1)*gyro.z<< -dcm(1,2)*gyro.x-dcm(2,2)*gyro.y-dcm(3,2)*gyro.z; Matrix corrVal = K * (z-H*errState); errState = errState + corrVal; - Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K); + Phat = (MatrixMath::Eye(9)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(9)-K*H)+K*(Rasc+Qab)*MatrixMath::Transpose(K); } +*/ + +void ScErrStateEKF::updateGyroBiasConstraints(Vector3 gyro) +{ + Matrix dcm(3,3); + computeDcm(dcm, qhat); + Matrix H(2,12); + H(1,4) = -1.0f*(dcm(1,1)); + H(1,5) = -1.0f*(dcm(2,1)); + H(1,6) = -1.0f*(dcm(3,1)); + H(2,4) = -1.0f*(dcm(1,2)); + H(2,5) = -1.0f*(dcm(2,2)); + H(2,6) = -1.0f*(dcm(3,2)); + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rgsc); + Matrix z(2,1); + z <<-dcm(1,1)*gyro.x-dcm(2,1)*gyro.y-dcm(3,1)*gyro.z<< -dcm(1,2)*gyro.x-dcm(2,2)*gyro.y-dcm(3,2)*gyro.z; + Matrix corrVal = K * (z-H*errState); + errState = errState + corrVal; + Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K); +} + + +void ScErrStateEKF::updateVelocityConstraints() +{ + Matrix H(3,12); + H(1,10) = 1.0f; + H(2,11) = 1.0f; + H(3,12) = 1.0f; + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc); + Matrix corrVal = K * (-H*errState); + errState = errState + corrVal; + Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K); +} + void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref) { - //acc = acc/acc.Norm(); - //accref = accref/accref.Norm(); - 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(3,9); + Matrix H(3,12); H(1,2) = -2.0f*gvec.z; H(1,3) = 2.0f*gvec.y; H(2,1) = 2.0f*gvec.z; @@ -157,7 +258,7 @@ gref << 0.0f << 0.0f << accref.z; Matrix z = accmat-dcm*gref; errState = errState + K * (z-H*errState); - Phat = (MatrixMath::Eye(9)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(9)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K); + Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K); } @@ -211,18 +312,20 @@ void ScErrStateEKF::resetBias() { + /* gyroBias.x = gyroBias.x + errState(4,1)*1.0f; gyroBias.y = gyroBias.y + errState(5,1)*1.0f; gyroBias.z = gyroBias.z + errState(6,1)*1.0f; errState(4,1) = 0.0f; errState(5,1) = 0.0f; errState(6,1) = 0.0f; - //accBias.x = accBias.x + errState(7,1); - //accBias.y = accBias.y + errState(8,1); - //accBias.z = accBias.z + errState(9,1); - //errState(7,1) = 0.0f; - //errState(8,1) = 0.0f; - //errState(9,1) = 0.0f; + accBias.x = accBias.x + errState(7,1); + accBias.y = accBias.y + errState(8,1); + accBias.z = accBias.z + errState(9,1); + errState(7,1) = 0.0f; + errState(8,1) = 0.0f; + errState(9,1) = 0.0f; + */ } void ScErrStateEKF::computeAngles(Vector3& rpy,Vector3 rpy_align)