Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 26:73c3f58b9d70
- Parent:
- 25:07ac5c6cd61c
- Child:
- 27:a74f101ba40a
diff -r 07ac5c6cd61c -r 73c3f58b9d70 ScErrStateEKF.cpp --- a/ScErrStateEKF.cpp Wed Aug 25 07:01:05 2021 +0000 +++ b/ScErrStateEKF.cpp Tue Sep 07 06:19:29 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), 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(2,2), 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,6 +31,8 @@ setDiag(Rm,5.0f); + setDiag(Rgps,0.5f); + for(int i = 0; i<10;i++){ histffunc[i] = 0.0f; } @@ -226,32 +228,6 @@ 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) { @@ -372,53 +348,6 @@ 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); - } - } - 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); -} @@ -444,51 +373,17 @@ 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) +void ScErrStateEKF::updateGPSVelocity(float speed, float direction) { - //mag = mag/mag.Norm(); - Matrix dcm(3,3); - computeDcm(dcm, qhat); - - Matrix magvec(3,1); - magvec(1,1) = mag.x; - magvec(2,1) = mag.y; - magvec(3,1) = mag.z; - - Matrix magnedvec = MatrixMath::Transpose(dcm)*magvec; - Matrix magrefmod(3,1); - magrefmod(1,1) = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1)); - magrefmod(2,1) = 0.0f; - magrefmod(3,1) = magnedvec(3,1); - - Matrix mvec = dcm*magrefmod ; 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(nState,nState); - for(int i = 1; i<4; i++){ - for(int j = 1;j<4;j++){ - Pm(i,j) = Phat(i,j); - } - } - Matrix r3(3,1); - r3 << dcm(1,3)<< dcm(2,3) << dcm(3,3); - Matrix kpart = r3*MatrixMath::Transpose(r3); - Matrix Kmod(nState,nState); - for(int i = 1; i<4; i++){ - for(int j = 1;j<4;j++){ - Kmod(i,j) = kpart(i,j); - } - } - - 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(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rm)*MatrixMath::Transpose(K); + H(1,9) = 1.0f; + H(2,10) = 1.0f; + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rgps); + Matrix z(2,1); + z << speed * cos(direction / 180.0f * M_PI) - vihat(1,1) << speed * sin(direction / 180.0f * M_PI)-vihat(2,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::updateCenteredGyroBiasCorrection(float (&cBg)[3]) @@ -843,6 +738,80 @@ vb.z = vbmat(3,1); } + +void ScErrStateEKF::updateMagMeasures(Vector3 mag) +{ + //mag = mag/mag.Norm(); + Matrix dcm(3,3); + computeDcm(dcm, qhat); + + Matrix magvec(3,1); + magvec(1,1) = mag.x; + magvec(2,1) = mag.y; + magvec(3,1) = mag.z; + + Matrix magnedvec = MatrixMath::Transpose(dcm)*magvec; + Matrix magrefmod(3,1); + magrefmod(1,1) = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1)); + magrefmod(2,1) = 0.0f; + magrefmod(3,1) = magnedvec(3,1); + + Matrix mvec = dcm*magrefmod ; + 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(nState,nState); + for(int i = 1; i<4; i++){ + for(int j = 1;j<4;j++){ + Pm(i,j) = Phat(i,j); + } + } + Matrix r3(3,1); + r3 << dcm(1,3)<< dcm(2,3) << dcm(3,3); + Matrix kpart = r3*MatrixMath::Transpose(r3); + Matrix Kmod(nState,nState); + for(int i = 1; i<4; i++){ + for(int j = 1;j<4;j++){ + Kmod(i,j) = kpart(i,j); + } + } + + 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(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rm)*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); + +} */