Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 21:d6079def0473
- Parent:
- 20:37d3c3ee36e9
- Child:
- 22:7d84b8bc20b4
diff -r 37d3c3ee36e9 -r d6079def0473 ScErrStateEKF.cpp --- a/ScErrStateEKF.cpp Sat Aug 07 06:54:50 2021 +0000 +++ b/ScErrStateEKF.cpp Tue Aug 10 08:08:25 2021 +0000 @@ -8,57 +8,28 @@ using namespace std; ScErrStateEKF::ScErrStateEKF() - :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(4,1),vihat(3,1), errState(9,1), Phat(9,9), Q(9,9), Ra(3,3), Rm(3,3), Qab(3,3),Rgsc(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; - 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; - Phat(3,3) = 0.1f; - Phat(4,4) = 0.1f; - Phat(5,5) = 0.1f; - Phat(6,6) = 0.1f; - Phat(7,7) = 0.1f; - Phat(8,8) = 0.1f; - Phat(9,9) = 0.1f; - Phat(10,10) = 0.1f; - Phat(11,11) = 0.1f; - Phat(12,12) = 0.1f; - - 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; + setDiag(Phat,0.1f); + setQqerr(0.000005f); + setQgbias(0.00001f); + setQabias(0.006f); + //setQv(0.000005f); //加速度の観測 - 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; + setDiag(Ra,1.0f); + setDiag(Qab,120.0f); //ジャイロバイアスに関する制約 - Rgsc(1,1) = 263.980f; - Rgsc(2,2) = 263.980f; + setDiag(Rgsc,260.0f); - //速度に関する制約 - Rvsc(1,1) = 5000.0f; - Rvsc(2,2) = 5000.0f; - Rvsc(3,3) = 5000.0f; + //機体軸速度に関する制約 + //setDiag(Rvsc,100.0f); - Rm(1,1) = 5.0f; - Rm(2,2) = 5.0f; - Rm(3,3) = 5.0f; + setDiag(Rm,5.0f); for(int i = 0; i<10;i++){ histffunc[i] = 0.0f; @@ -69,9 +40,10 @@ } -void ScErrStateEKF::updateQhat(Vector3 gyro, float att_dt) +void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt) { gyro -= gyroBias; + acc -= accBias; Matrix A(4,4); A << 0.0f << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z << 0.5f*gyro.x << 0.0f << 0.5f*gyro.z <<-0.5f*gyro.y @@ -82,6 +54,10 @@ qhat = phi * qhat; float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); qhat *= (1.0f/ qnorm); + + Matrix dcm(3,3); + computeDcm(dcm, qhat); + vihat += (MatrixMath::Transpose(dcm)*MatrixMath::Vector2mat(acc)-MatrixMath::Vector2mat(accref))*att_dt*9.8f; } void ScErrStateEKF::setQqerr(float val){ @@ -101,26 +77,27 @@ Q(9,9) = val; } + void ScErrStateEKF::setQab(float val){ - Qab(1,1) = val; - Qab(2,2) = val; - Qab(3,3) = val; + setDiag(Qab,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; + setDiag(Rgsc,Vgsc); + setDiag(Rvsc,Vvsc); } - +void ScErrStateEKF::setDiag(Matrix& mat, float val){ + for (int i = 1; i < mat.getCols()+1; i++){ + mat(i,i) = val; + } +} -void ScErrStateEKF::updateErrState(Vector3 gyro, float att_dt) +void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt) { gyro -= gyroBias; - Matrix A(12,12); + acc -= accBias; + Matrix A(nState,nState); A(1,2) = gyro.z; A(1,3) = -gyro.y; A(2,1) = -gyro.z; @@ -130,11 +107,19 @@ A(1,4) = -0.5f; A(2,5) = -0.5f; A(3,6) = -0.5f; - A(10,7) = 1.0f; - 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; + Matrix dcm(3,3); + computeDcm(dcm, qhat); + Matrix qeterm = -2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z); + 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); + } + } + + Matrix phi = MatrixMath::Eye(nState) + 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; @@ -142,11 +127,11 @@ void ScErrStateEKF::updateAccMeasures(Vector3 acc, Vector3 accref) { - //acc -= accBias; + 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); + Matrix H(3,nState); H(1,2) = -2.0f*gvec.z; H(1,3) = 2.0f*gvec.y; H(2,1) = 2.0f*gvec.z; @@ -157,14 +142,10 @@ 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 z = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref); 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(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K); } /* @@ -209,42 +190,112 @@ void ScErrStateEKF::updateGyroBiasConstraints(Vector3 gyro) { + gyro -= gyroBias; + 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 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 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; + 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); + Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-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 dcm(3,3); + computeDcm(dcm, qhat); + Matrix H(2,nState); + + 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(1,j) = -qeterm(1,j); + H(2,j) = -qeterm(3,j); + //H(3,j) = -qeterm(3,j); + H(1,9+j) = -dcm(1,j); + H(2,9+j) = -dcm(3,j); + //H(3,9+j) = -dcm(3,j); + } + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc); - Matrix corrVal = K * (-H*errState); + Matrix z(2,1); + z << nomVb(1,1) << nomVb(3,1) ; + Matrix corrVal = K * (z-H*errState); errState = errState + corrVal; - Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K); + Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K); + } +void ScErrStateEKF::updateConstraints(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(4,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 nomVb = dcm*vihat; + qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1)); + for(int j = 1;j<4;j++){ + H(3,j) = -qeterm(1,j); + H(4,j) = -qeterm(3,j); + //H(3,j) = -qeterm(3,j); + H(3,9+j) = -dcm(1,j); + H(4,9+j) = -dcm(3,j); + //H(3,9+j) = -dcm(3,j); + } + + Matrix R(4,4); + R(1,1) = Rgsc(1,1); + R(2,2) = Rgsc(2,1); + R(3,3) = Rvsc(1,1); + R(4,4) = Rvsc(2,1); + //R(5,5) = Rvsc(3,1); + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + Matrix z(4,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 << 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); computeDcm(dcm, qhat); Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z); - Matrix H(3,12); + Matrix H(3,nState); H(1,2) = -2.0f*gvec.z; H(1,3) = 2.0f*gvec.y; H(2,1) = 2.0f*gvec.z; @@ -258,7 +309,7 @@ gref << 0.0f << 0.0f << accref.z; Matrix z = accmat-dcm*gref; errState = errState + K * (z-H*errState); - Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K); + Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K); } @@ -281,14 +332,14 @@ magrefmod(3,1) = magnedvec(3,1); Matrix mvec = dcm*magrefmod ; - Matrix H(3,9); + Matrix H(3,nState); H(1,2) = -2.0f*magvec(3,1); H(1,3) = 2.0f*magvec(2,1); H(2,1) = 2.0f*magvec(3,1); H(2,3) = -2.0f*magvec(1,1); H(3,1) = -2.0f*magvec(2,1); H(3,2) = 2.0f*magvec(1,1); - Matrix Pm(9,9); + Matrix Pm(nState,nState); for(int i = 1; i<4; i++){ for(int j = 1;j<4;j++){ Pm(i,j) = Phat(i,j); @@ -297,7 +348,7 @@ Matrix r3(3,1); r3 << dcm(1,3)<< dcm(2,3) << dcm(3,3); Matrix kpart = r3*MatrixMath::Transpose(r3); - Matrix Kmod(9,9); + Matrix Kmod(nState,nState); for(int i = 1; i<4; i++){ for(int j = 1;j<4;j++){ Kmod(i,j) = kpart(i,j); @@ -307,12 +358,11 @@ Matrix K = Kmod*(Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rm); Matrix z = magvec-mvec; errState = errState + K * (z-H*errState); - Phat = (MatrixMath::Eye(9)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(9)-K*H)+K*(Rm)*MatrixMath::Transpose(K); + Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rm)*MatrixMath::Transpose(K); } 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; @@ -325,7 +375,6 @@ errState(7,1) = 0.0f; errState(8,1) = 0.0f; errState(9,1) = 0.0f; - */ } void ScErrStateEKF::computeAngles(Vector3& rpy,Vector3 rpy_align) @@ -347,7 +396,18 @@ } -void ScErrStateEKF::fuseErr2Qhat() +void ScErrStateEKF::computeVb(Vector3& vb) +{ + Matrix dcm(3,3); + computeDcm(dcm, qhat); + Matrix vbmat = dcm*vihat; + vb.x = vbmat(1,1); + vb.y = vbmat(2,1); + vb.z = vbmat(3,1); + +} + +void ScErrStateEKF::fuseErr2Nominal() { Matrix qerr(4,1); qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1); @@ -359,6 +419,13 @@ errState(3,1) = 0.0f; 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; } Matrix ScErrStateEKF::quatmultiply(Matrix q, Matrix r) @@ -409,52 +476,6 @@ qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; } -void ScErrStateEKF::triad(Vector3 fb, Vector3 fn, Vector3 mb, Vector3 mn){ - Matrix W1(3,1); - W1 << fb.x << fb.y << fb.z; - Matrix W2(3,1); - W2 << mb.x << mb.y << mb.z; - - Matrix V1(3,1); - V1 << fn.x << fn.y << fn.z; - Matrix V2(3,1); - V2 << mn.x << mn.y << mn.z; - - - Matrix Ou2(3,1); - Ou2 << W1( 2, 1 )*W2( 3, 1 )-W1( 3, 1 )*W2( 2, 1 ) << W1( 3, 1 )*W2( 1, 1 )-W1( 1, 1 )*W2( 3, 1 ) << W1( 1, 1 )*W2( 2, 1 )-W1( 2, 1 )*W2( 1, 1 ); - Ou2 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2)); - Matrix Ou3(3,1); - Ou3 << W1( 2, 1 )*Ou2( 3, 1 )-W1( 3, 1 )*Ou2( 2, 1 ) << W1( 3, 1 )*Ou2( 1, 1 )-W1( 1, 1 )*Ou2( 3, 1 ) << W1( 1, 1 )*Ou2( 2, 1 )-W1( 2, 1 )*Ou2( 1, 1 ); - Ou3 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3)); - Matrix R2(3,1); - R2 << V1( 2, 1 )*V2( 3, 1 )-V1( 3, 1 )*V2( 2, 1 ) << V1( 3, 1 )*V2( 1, 1 )-V1( 1, 1 )*V2( 3, 1 ) << V1( 1, 1 )*V2( 2, 1 )-V1( 2, 1 )*V2( 1, 1 ); - R2 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2)); - Matrix R3(3,1); - R3 << V1( 2, 1 )*R2( 3, 1 )-V1( 3, 1 )*R2( 2, 1 ) << V1( 3, 1 )*R2( 1, 1 )-V1( 1, 1 )*R2( 3, 1 ) << V1( 1, 1 )*R2( 2, 1 )-V1( 2, 1 )*R2( 1, 1 ); - R3 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3)); - - Matrix Mou(3,3); - Mou << W1( 1, 1 ) << Ou2( 1, 1 ) << Ou3( 1, 1 ) - << W1( 2, 1 ) << Ou2( 2, 1 ) << Ou3( 2, 1 ) - << W1( 3, 1 ) << Ou2( 3, 1 ) << Ou3( 3, 1 ); - Matrix Mr(3,3); - Mr << V1( 1, 1 ) << R2( 1, 1 ) << R3( 1, 1 ) - << V1( 2, 1 ) << R2( 2, 1 ) << R3( 2, 1 ) - << V1( 3, 1 ) << R2( 3, 1 ) << R3( 3, 1 ); - - Matrix Cbn = Mr*MatrixMath::Transpose(Mou); - - float sqtrp1 = sqrt(1.0f+Cbn( 1, 1 )+Cbn( 2, 2 )+Cbn( 3, 3 )); - - qhat(1,1) = 0.5f*sqtrp1; - qhat(2,1) = -(Cbn( 2, 3 )-Cbn( 3, 2 ))/2.0f/sqtrp1; - qhat(3,1) = -(Cbn( 3, 1 )-Cbn( 1, 3 ))/2.0f/sqtrp1; - qhat(4,1) = -(Cbn( 1, 2 )-Cbn( 2, 1 ))/2.0f/sqtrp1; - - float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); - qhat *= (1.0f/ qnorm); -} Vector3 ScErrStateEKF::calcDynAcc(Vector3 acc, Vector3 accref)