Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 25:07ac5c6cd61c
- Parent:
- 23:1509648c2318
- Child:
- 26:73c3f58b9d70
diff -r 1653b9547008 -r 07ac5c6cd61c ScErrStateEKF.cpp --- a/ScErrStateEKF.cpp Fri Aug 20 07:51:01 2021 +0000 +++ b/ScErrStateEKF.cpp Wed Aug 25 07:01:05 2021 +0000 @@ -129,6 +129,44 @@ } } +void ScErrStateEKF::getQhat(float (&res)[4]){ + for (int i = 0; i < 4; i++){ + res[i] = qhat(i+1,1); + } +} + +void ScErrStateEKF::getVihat(float (&res)[3]){ + for (int i = 0; i < 3; i++){ + res[i] = vihat(i+1,1); + } +} + +void ScErrStateEKF::getGyroBias(float (&resVal)[3], float (&resCov)[6]){ + 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); + +} + +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) { @@ -380,7 +418,8 @@ 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) @@ -452,6 +491,68 @@ Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rm)*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; + } + + Matrix H(3,nState); + 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)+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); + + //gyroBias.x = cBg[0]; + //gyroBias.y = cBg[1]; + //gyroBias.z = cBg[2]; +} + +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;