Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 30:ff884e9b2e30
- Parent:
- 29:05e35ca2c9c4
- Child:
- 31:e655d4d8e4d6
diff -r 05e35ca2c9c4 -r ff884e9b2e30 ScErrStateEKF.cpp --- a/ScErrStateEKF.cpp Thu Sep 16 09:30:14 2021 +0000 +++ b/ScErrStateEKF.cpp Wed Sep 22 09:30:37 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), 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; @@ -31,7 +31,8 @@ setDiag(Rm,5.0f); - setDiag(Rgps,50.0f); + setDiag(Rgps,0.01f); + setDiag(Rsr,10.0f); for(int i = 0; i<10;i++){ histffunc[i] = 0.0f; @@ -373,35 +374,36 @@ Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K); } -void ScErrStateEKF::updateVelocity(float vix, float viy,float sinkRate) +void ScErrStateEKF::updateGPSVelocity(float vi_x, float vi_y, float sinkRate) { Matrix H(3,nState); H(1,10) = 1.0f; - H(2,11) = 1.0f; - H(3,12) = 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 << vix - vihat(1,1) << viy-vihat(2,1) << sinkRate-vihat(3,1); + z << 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*(Rgps)*MatrixMath::Transpose(K); } +void ScErrStateEKF::updateSinkRate(float sinkRate) +{ + 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); + 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); +} + void ScErrStateEKF::updateCenteredGyroBiasCorrection(float (&cBg)[3]) { Matrix RgyroBias(3,3); - /* - RgyroBias(1,1) = cBgCov[0]; - RgyroBias(1,2) = cBgCov[1]; - RgyroBias(2,1) = cBgCov[1]; - RgyroBias(1,3) = cBgCov[2]; - RgyroBias(3,1) = cBgCov[2]; - RgyroBias(2,2) = cBgCov[3]; - RgyroBias(2,3) = cBgCov[4]; - RgyroBias(3,2) = cBgCov[4]; - RgyroBias(3,3) = cBgCov[5]; - */ for(int i = 1;i<4;i++){ RgyroBias(i,i) = 0.1f; } @@ -418,9 +420,6 @@ errState = errState + corrVal; Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(RgyroBias)*MatrixMath::Transpose(K); - //gyroBias.x = cBg[0]; - //gyroBias.y = cBg[1]; - //gyroBias.z = cBg[2]; } void ScErrStateEKF::updateCenteredAccBiasCorrection(float (&cBa)[3])