Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 38:1f6532849c05
- Parent:
- 34:dec4b37db3f1
- Child:
- 39:6834e05d8a64
diff -r dec4b37db3f1 -r 1f6532849c05 ScErrStateEKF.cpp --- a/ScErrStateEKF.cpp Fri Oct 08 08:11:17 2021 +0000 +++ b/ScErrStateEKF.cpp Sun Oct 10 09:15:29 2021 +0000 @@ -8,16 +8,18 @@ using namespace std; ScErrStateEKF::ScErrStateEKF() - :qhat(4,1),vihat(3,1), errState(9,1), bgState(3,1), Phat(9,9), Q(9,9),Phatbg(3,3),Qbg(3,3), Ra(3,3), Rgps(3,3),Rsr(1,1), Rm(3,3), Qab(3,3),Rgsc(2,2),Rvsc(2,2),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f) + :qhat(4,1),vihat(3,1), errState(12,1), Phat(12,12), Q(12,12), Ra(3,3), Rgps(3,3),Rsr(1,1), Rm(3,3), Qab(3,3),Rgsc(2,2),Rvsc(2,2),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f) { nState = errState.getRows(); qhat << 1.0f << 0.0f << 0.0f << 0.0f; setDiag(Phat,0.1f); //0.1 - setDiag(Phatbg,0.0001f); - setQqerr(0.0000005f); //0.000005 - setQgbias(0.001f); - setQabias(0.00001f); + 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); //加速度の観測 @@ -75,6 +77,9 @@ A(2,3) = gyro.x; A(3,1) = gyro.y; A(3,2) = -gyro.x; + A(1,4) = -0.5f; + A(2,5) = -0.5f; + A(3,6) = -0.5f; Matrix dcm(3,3); computeDcm(dcm, qhat); @@ -82,8 +87,8 @@ Matrix baterm = -9.8f*MatrixMath::Transpose(dcm); for (int i = 1; i < 4; i++){ for (int j = 1; i < 4; i++){ - A(i+6,j) = qeterm(i,j); - A(i+6,j+3) = baterm(i,j); + A(i+9,j) = qeterm(i,j); + A(i+9,j+6) = baterm(i,j); } } @@ -99,20 +104,20 @@ } void ScErrStateEKF::setQgbias(float val){ - Qbg(1,1) = val; - Qbg(2,2) = val; - Qbg(2,2) = val; -} -void ScErrStateEKF::setQabias(float val){ Q(4,4) = val; Q(5,5) = val; Q(6,6) = val; } -void ScErrStateEKF::setQv(float val){ +void ScErrStateEKF::setQabias(float val){ Q(7,7) = val; Q(8,8) = val; Q(9,9) = val; } +void ScErrStateEKF::setQv(float val){ + Q(10,10) = val; + Q(11,11) = val; + Q(12,12) = val; +} void ScErrStateEKF::setQab(float val){ setDiag(Qab,val); @@ -146,12 +151,12 @@ resVal[0] = gyroBias.x; resVal[1] = gyroBias.y; resVal[2] = gyroBias.z; - resCov[0] = Phatbg(1,1); - resCov[1] = Phatbg(1,2); - resCov[2] = Phatbg(1,3); - resCov[3] = Phatbg(2,2); - resCov[4] = Phatbg(2,3); - resCov[5] = Phatbg(3,3); + resCov[0] = Phat(4,4); + resCov[1] = Phat(4,5); + resCov[2] = Phat(4,6); + resCov[3] = Phat(5,5); + resCov[4] = Phat(5,6); + resCov[5] = Phat(6,6); } @@ -159,12 +164,12 @@ resVal[0] = accBias.x; resVal[1] = accBias.y; resVal[2] = accBias.z; - resCov[0] = Phat(4,4); - resCov[1] = Phat(4,5); - resCov[2] = Phat(4,6); - resCov[3] = Phat(5,5); - resCov[4] = Phat(5,6); - resCov[5] = Phat(6,6); + resCov[0] = Phat(7,7); + resCov[1] = Phat(7,8); + resCov[2] = Phat(7,9); + resCov[3] = Phat(8,8); + resCov[4] = Phat(8,9); + resCov[5] = Phat(9,9); } @@ -182,9 +187,9 @@ H(2,3) = -2.0f*gvec.x; H(3,1) = -2.0f*gvec.y; H(3,2) = 2.0f*gvec.x; - H(1,4) = 1.0f; - H(2,5) = 1.0f; - H(3,6) = 1.0f; + 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 z = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref); Matrix corrVal = K * (z-H*errState); @@ -197,20 +202,20 @@ gyro -= gyroBias; Matrix dcm(3,3); computeDcm(dcm, qhat); - Matrix H(2,3); - H(1,1) = 1.0f*(dcm(1,1)); - H(1,2) = 1.0f*(dcm(2,1)); - H(1,3) = 1.0f*(dcm(3,1)); - H(2,1) = 1.0f*(dcm(1,2)); - H(2,2) = 1.0f*(dcm(2,2)); - H(2,3) = 1.0f*(dcm(3,2)); + Matrix H(2,nState); + 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 = (Phatbg*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phatbg*MatrixMath::Transpose(H)+Rgsc); + 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*bgState); - bgState = bgState + corrVal; - Phatbg = (MatrixMath::Eye(3)-K*H)*Phatbg*MatrixMath::Transpose(MatrixMath::Eye(3)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K); + Matrix corrVal = K * (z-H*errState); + errState = errState + corrVal; + Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K); } @@ -249,12 +254,12 @@ H(2,3) = -2.0f*gvec.x; H(3,1) = -2.0f*gvec.y; H(3,2) = 2.0f*gvec.x; - H(1,4) = 1.0f; - H(2,5) = 1.0f; - H(3,6) = 1.0f; - H(4,7) = 1.0f; - H(5,8) = 1.0f; - H(6,9) = 1.0f; + H(1,7) = 1.0f; + H(2,8) = 1.0f; + H(3,9) = 1.0f; + H(4,10) = 1.0f; + H(5,11) = 1.0f; + H(6,12) = 1.0f; Matrix R(6,6); R(1,1) = Ra(1,1)+Qab(1,1); @@ -286,10 +291,10 @@ H(2,3) = -2.0f*gvec.x; H(3,1) = -2.0f*gvec.y; H(3,2) = 2.0f*gvec.x; - H(1,4) = 1.0f; - H(2,5) = 1.0f; - H(3,6) = 1.0f; - H(4,9) = 1.0f; + H(1,7) = 1.0f; + H(2,8) = 1.0f; + H(3,9) = 1.0f; + H(4,12) = 1.0f; Matrix R(4,4); R(1,1) = Ra(1,1)+Qab(1,1); @@ -314,34 +319,34 @@ RgyroBias(i,i) = 0.1f; } - Matrix H(3,3); - H(1,1) = 1.0f; - H(2,2) = 1.0f; - H(3,3) = 1.0f; + Matrix H(3,nState); + H(1,4) = 1.0f; + H(2,5) = 1.0f; + H(3,6) = 1.0f; - Matrix K = (Phatbg*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phatbg*MatrixMath::Transpose(H)+RgyroBias); + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+RgyroBias); Matrix z(3,1); z <<cBg[0]-gyroBias.x <<cBg[1]-gyroBias.y <<cBg[2]-gyroBias.z; - Matrix corrVal = K * (z-H*bgState); - bgState = bgState + corrVal; - Phatbg = (MatrixMath::Eye(3)-K*H)*Phatbg*MatrixMath::Transpose(MatrixMath::Eye(3)-K*H)+K*(RgyroBias)*MatrixMath::Transpose(K); + Matrix corrVal = K * (z-H*errState); + errState = errState + corrVal; + Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(RgyroBias)*MatrixMath::Transpose(K); } void ScErrStateEKF::resetBias() { - gyroBias.x = gyroBias.x + bgState(1,1)*1.0f; - gyroBias.y = gyroBias.y + bgState(2,1)*1.0f; - gyroBias.z = gyroBias.z + bgState(3,1)*1.0f; - bgState(1,1) = 0.0f; - bgState(2,1) = 0.0f; - bgState(3,1) = 0.0f; - accBias.x = accBias.x + errState(4,1); - accBias.y = accBias.y + errState(5,1); - accBias.z = accBias.z + errState(6,1); + 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; } void ScErrStateEKF::computeAngles(Vector3& rpy,Vector3 rpy_align) @@ -377,12 +382,12 @@ float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); qhat *= (1.0f/ qnorm); - vihat(1,1) += errState(7,1); - vihat(2,1) += errState(8,1); - vihat(3,1) += errState(9,1); - errState(7,1) = 0.0f; - errState(8,1) = 0.0f; - errState(9,1) = 0.0f; + vihat(1,1) += errState(10,1); + vihat(2,1) += errState(11,1); + vihat(3,1) += errState(12,1); + errState(10,1) = 0.0f; + errState(11,1) = 0.0f; + errState(12,1) = 0.0f; } Matrix ScErrStateEKF::quatmultiply(Matrix q, Matrix r)