Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 23:1509648c2318
- Parent:
- 22:7d84b8bc20b4
- Child:
- 25:07ac5c6cd61c
diff -r 7d84b8bc20b4 -r 1509648c2318 ScErrStateEKF.cpp --- a/ScErrStateEKF.cpp Tue Aug 10 08:29:19 2021 +0000 +++ b/ScErrStateEKF.cpp Fri Aug 20 07:48:35 2021 +0000 @@ -8,7 +8,7 @@ using namespace std; ScErrStateEKF::ScErrStateEKF() - :qhat(4,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) + :qhat(4,1),vihat(3,1), errState(12,1), Phat(12,12), Q(12,12), Ra(3,3), 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; @@ -16,18 +16,18 @@ setDiag(Phat,0.1f); setQqerr(0.000005f); setQgbias(0.00001f); - setQabias(0.006f); - //setQv(0.000005f); + setQabias(1.0f); + setQv(0.000001f); //加速度の観測 - setDiag(Ra,1.0f); - setDiag(Qab,120.0f); + setDiag(Ra,0.005f); + setDiag(Qab,100.0f); //ジャイロバイアスに関する制約 setDiag(Rgsc,500.0f); //機体軸速度に関する制約 - //setDiag(Rvsc,100.0f); + setDiag(Rvsc,100000.0f); setDiag(Rm,5.0f); @@ -40,9 +40,10 @@ } -void ScErrStateEKF::updateNominal(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 @@ -53,8 +54,43 @@ 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::updateErrState(Vector3 gyro,Vector3 acc, float att_dt) +{ + gyro -= gyroBias; + acc -= accBias; + Matrix A(nState,nState); + A(1,2) = gyro.z; + A(1,3) = -gyro.y; + A(2,1) = -gyro.z; + 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); + 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; +} void ScErrStateEKF::setQqerr(float val){ Q(1,1) = val; Q(2,2) = val; @@ -71,15 +107,20 @@ 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); } + void ScErrStateEKF::setRsoftconst(float Vgsc,float Vvsc){ setDiag(Rgsc,Vgsc); - //setDiag(Rvsc,Vvsc); + setDiag(Rvsc,Vvsc); } void ScErrStateEKF::setDiag(Matrix& mat, float val){ @@ -88,25 +129,6 @@ } } -void ScErrStateEKF::updateErrState(Vector3 gyro, float att_dt) -{ - gyro -= gyroBias; - Matrix A(nState,nState); - A(1,2) = gyro.z; - A(1,3) = -gyro.y; - A(2,1) = -gyro.z; - 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 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; -} void ScErrStateEKF::updateAccMeasures(Vector3 acc, Vector3 accref) { @@ -137,8 +159,191 @@ Matrix dcm(3,3); computeDcm(dcm, qhat); - Matrix qeterm = 2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z); + 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::updateVelocityConstraints() +{ + + 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 z(2,1); + 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*(Rvsc)*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 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); +} + + +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::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); @@ -150,14 +355,32 @@ 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 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*(Rgsc)*MatrixMath::Transpose(K); -} - + 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) @@ -182,8 +405,6 @@ Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K); } - - void ScErrStateEKF::updateMagMeasures(Vector3 mag) { //mag = mag/mag.Norm(); @@ -279,6 +500,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) @@ -329,6 +557,17 @@ qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; } +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); + +} + Vector3 ScErrStateEKF::calcDynAcc(Vector3 acc, Vector3 accref)