Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 31:e655d4d8e4d6
- Parent:
- 30:ff884e9b2e30
- Child:
- 32:321a756e12ad
diff -r ff884e9b2e30 -r e655d4d8e4d6 ScErrStateEKF.cpp --- a/ScErrStateEKF.cpp Wed Sep 22 09:30:37 2021 +0000 +++ b/ScErrStateEKF.cpp Tue Sep 28 08:17:42 2021 +0000 @@ -8,20 +8,21 @@ using namespace std; ScErrStateEKF::ScErrStateEKF() - :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) + :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) { nState = errState.getRows(); qhat << 1.0f << 0.0f << 0.0f << 0.0f; setDiag(Phat,0.1f); + setDiag(Phatbg,0.1f); setQqerr(0.000005f); setQgbias(0.00001f); - setQabias(1.0f); + setQabias(0.00001f); setQv(0.000001f); //加速度の観測 setDiag(Ra,0.005f); - setDiag(Qab,100.0f); + setDiag(Qab,10000.0f); //ジャイロバイアスに関する制約 setDiag(Rgsc,500.0f); @@ -31,8 +32,8 @@ setDiag(Rm,5.0f); - setDiag(Rgps,0.01f); - setDiag(Rsr,10.0f); + setDiag(Rgps,5.5f); + setDiag(Rsr,5.5f); for(int i = 0; i<10;i++){ histffunc[i] = 0.0f; @@ -74,9 +75,6 @@ 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); @@ -84,8 +82,8 @@ Matrix baterm = -9.8f*MatrixMath::Transpose(dcm); for (int i = 1; i < 4; i++){ for (int j = 1; i < 4; i++){ - A(i+9,j) = qeterm(i,j); - A(i+9,j+6) = baterm(i,j); + A(i+6,j) = qeterm(i,j); + A(i+6,j+3) = baterm(i,j); } } @@ -101,20 +99,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::setQabias(float val){ +void ScErrStateEKF::setQv(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); @@ -148,12 +146,12 @@ resVal[0] = gyroBias.x; resVal[1] = gyroBias.y; resVal[2] = gyroBias.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] = 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); } @@ -161,12 +159,12 @@ resVal[0] = accBias.x; resVal[1] = accBias.y; resVal[2] = accBias.z; - 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); + 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); } @@ -184,9 +182,9 @@ 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; + H(1,4) = 1.0f; + H(2,5) = 1.0f; + H(3,6) = 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,161 +195,25 @@ void ScErrStateEKF::updateGyroBiasConstraints(Vector3 gyro) { gyro -= gyroBias; - - Matrix dcm(3,3); - computeDcm(dcm, qhat); - Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z); - Matrix H(2,nState); - for(int i = 1; i<3; i++){ - for(int j = 1;j<4;j++){ - H(i,j) = qeterm(i,j); - } - } - 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 Pm(nState,nState); - for(int i = 4; i<7; i++){ - for(int j = 4;j<7;j++){ - Pm(i,j) = Phat(i,j); - } - } - - Matrix K = (Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*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(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K); -} - - -void ScErrStateEKF::updateMeasures(Vector3 gyro, Vector3 acc, Vector3 accref) -{ - acc -= accBias; - gyro -= gyroBias; 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); - 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 qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z); - for(int j = 1;j<4;j++){ - H(4,j) = qeterm(1,j); - H(5,j) = qeterm(2,j); - } - 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 nomVb = dcm*vihat; - Matrix qeterm = 2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1)); - for(int j = 1;j<4;j++){ - H(4,j) = -qeterm(1,j); - H(5,j) = -qeterm(3,j); - //H(3,j) = -qeterm(3,j); - H(4,9+j) = -dcm(1,j); - H(5,9+j) = -dcm(3,j); - //H(3,9+j) = -dcm(3,j); - } + 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 R(5,5); - 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) = Rgsc(1,1); - //R(5,5) = Rgsc(2,2); - R(4,4) = Rvsc(1,1); - R(5,5) = Rvsc(2,2); - - 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) << nomVb(1,1) << nomVb(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); + Matrix K = (Phatbg*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phatbg*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); } -void ScErrStateEKF::updateStaticMeasures(Vector3 gyro,Vector3 acc, Vector3 accref) -{ - acc -= accBias; - gyro -= gyroBias; - 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); - 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 qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z); - for(int j = 1;j<4;j++){ - H(4,j) = qeterm(1,j); - H(5,j) = qeterm(2,j); - } - 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 nomVb = dcm*vihat; - Matrix qeterm = 2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1)); - for(int j = 1;j<4;j++){ - H(4,j) = -qeterm(1,j); - H(5,j) = -qeterm(3,j); - //H(3,j) = -qeterm(3,j); - H(4,9+j) = -dcm(1,j); - H(5,9+j) = -dcm(3,j); - //H(3,9+j) = -dcm(3,j); - } - - Matrix R(5,5); - R(1,1) = Ra(1,1)*100.0f; - R(2,2) = Ra(2,2)*100.0f; - R(3,3) = Ra(3,3)*100.0f; - //R(4,4) = Rgsc(1,1); - //R(5,5) = Rgsc(2,2); - R(4,4) = Rvsc(1,1); - R(5,5) = Rvsc(2,2); - - 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) << nomVb(1,1) << nomVb(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); -} - - - - void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref) { Matrix dcm(3,3); @@ -374,30 +236,72 @@ 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, float sinkRate) +void ScErrStateEKF::updateGPSVelocity(float vi_x, float vi_y,Vector3 acc, Vector3 accref) { - Matrix H(3,nState); - 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)+Rgps); - Matrix z(3,1); - z << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1); + 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); + 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,4) = 1.0f; + H(2,5) = 1.0f; + H(3,6) = 1.0f; + H(4,7) = 1.0f; + H(5,8) = 1.0f; + + Matrix R(5,5); + 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); + + 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 corrVal = K * (z-H*errState); errState = errState + corrVal; - Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rgps)*MatrixMath::Transpose(K); + Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K); } -void ScErrStateEKF::updateSinkRate(float sinkRate) +void ScErrStateEKF::updateSinkRate(float sinkRate,Vector3 acc, Vector3 accref) { - Matrix H(1,nState); - H(1,12) = 1.0f; - Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rsr); - Matrix z(1,1); - z << sinkRate - vihat(3,1); + 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(4,nState); + 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,4) = 1.0f; + H(2,5) = 1.0f; + H(3,6) = 1.0f; + H(4,9) = 1.0f; + + Matrix R(4,4); + 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) = 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(4,1); + z << zacc(1,1)<< zacc(2,1)<< zacc(3,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*(Rsr)*MatrixMath::Transpose(K); + Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K); } void ScErrStateEKF::updateCenteredGyroBiasCorrection(float (&cBg)[3]) @@ -408,60 +312,34 @@ RgyroBias(i,i) = 0.1f; } - Matrix H(3,nState); - H(1,4) = 1.0f; - H(2,5) = 1.0f; - H(3,6) = 1.0f; + Matrix H(3,3); + H(1,1) = 1.0f; + H(2,2) = 1.0f; + H(3,3) = 1.0f; - Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+RgyroBias); + Matrix K = (Phatbg*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phatbg*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*errState); - errState = errState + corrVal; - Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(RgyroBias)*MatrixMath::Transpose(K); + 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); } -void ScErrStateEKF::updateCenteredAccBiasCorrection(float (&cBa)[3]) -{ - - Matrix RaccBias(3,3); - for(int i = 1;i<4;i++){ - RaccBias(i,i) = 5.0f; - } - - Matrix H(3,nState); - 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)+RaccBias); - Matrix z(3,1); - z <<cBa[0]-accBias.x <<cBa[1]-accBias.y <<cBa[2]-accBias.z; - Matrix corrVal = K * (z-H*errState); - errState = errState + corrVal; - Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(RaccBias)*MatrixMath::Transpose(K); - - //gyroBias.x = cBg[0]; - //gyroBias.y = cBg[1]; - //gyroBias.z = cBg[2]; -} - - 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; + 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); 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) @@ -497,12 +375,12 @@ float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); qhat *= (1.0f/ qnorm); - 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; + 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; } Matrix ScErrStateEKF::quatmultiply(Matrix q, Matrix r)