Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 42:b90c348a49c0
- Parent:
- 40:119792aa6d3b
diff -r ab7dbfa22f71 -r b90c348a49c0 ScErrStateEKF.cpp --- a/ScErrStateEKF.cpp Wed Oct 20 01:50:36 2021 +0000 +++ b/ScErrStateEKF.cpp Thu Oct 21 06:40:38 2021 +0000 @@ -8,7 +8,7 @@ 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), Phat(9,9), Q(9,9), 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; @@ -22,12 +22,8 @@ Phat(4,4) = 0.001; Phat(5,5) = 0.001; Phat(6,6) = 0.001; - Phat(7,7) = 1.0; - Phat(8,8) = 1.0; - Phat(9,9) = 1.0; setQqerr(0.001f); setQgbias(0.0001f); - setQabias(1.0f); setQv(0.01f); //加速度の観測 @@ -57,7 +53,7 @@ void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt) { gyro -= gyroBias; - acc -= accBias; + //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 @@ -77,7 +73,7 @@ void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt) { gyro -= gyroBias; - acc -= accBias; + //acc -= accBias; Matrix A(nState,nState); A(1,2) = gyro.z; A(1,3) = -gyro.y; @@ -91,12 +87,12 @@ 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); + 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); + A(i+6,j) = qeterm(i,j); + //A(i+9,j+6) = baterm(i,j); } } @@ -116,16 +112,12 @@ 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); @@ -168,23 +160,8 @@ } -void ScErrStateEKF::getAccBias(float (&resVal)[3], float (&resCov)[6]){ - 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); - -} - - 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); @@ -195,9 +172,6 @@ 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 z = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref); Matrix corrVal = K * (z-H*errState); @@ -251,7 +225,6 @@ void ScErrStateEKF::updateGPSVelocity(float vi_x, float vi_y, float sinkRate,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); @@ -263,12 +236,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(4,10) = 1.0f; - H(5,11) = 1.0f; - H(6,12) = 1.0f; + H(4,7) = 1.0f; + H(5,8) = 1.0f; + H(6,9) = 1.0f; Matrix R(6,6); R(1,1) = Ra(1,1)+Qab(1,1); @@ -278,23 +248,10 @@ R(5,5) = Rgps(2,2); R(6,6) = Rsr(1,1); - /* - Matrix H(3, nState); - H(1, 10) = 1.0f; - H(2, 11) = 1.0f; - H(3, 12) = 1.0f; - - Matrix R(3,3); - R(1,1) = Rgps(1,1); - R(2,2) = Rgps(2,2); - R(3,3) = 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(6,1); - z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1); - //z << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1); + z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << vi_x - vihat(1,1) << vi_y-vihat(2,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*(R)*MatrixMath::Transpose(K); @@ -302,7 +259,6 @@ void ScErrStateEKF::updateSinkRate(float sinkRate,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); @@ -313,10 +269,7 @@ 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(4,12) = 1.0f; + H(4,9) = 1.0f; Matrix R(4,4); R(1,1) = Ra(1,1)+Qab(1,1); @@ -367,12 +320,6 @@ 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) @@ -408,12 +355,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)