Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 29:05e35ca2c9c4
- Parent:
- 27:a74f101ba40a
- Child:
- 30:ff884e9b2e30
diff -r a74f101ba40a -r 05e35ca2c9c4 ScErrStateEKF.cpp --- a/ScErrStateEKF.cpp Tue Sep 07 08:20:42 2021 +0000 +++ b/ScErrStateEKF.cpp Thu Sep 16 09:30:14 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(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) + :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) { nState = errState.getRows(); qhat << 1.0f << 0.0f << 0.0f << 0.0f; @@ -31,7 +31,7 @@ setDiag(Rm,5.0f); - setDiag(Rgps,5.0f); + setDiag(Rgps,50.0f); for(int i = 0; i<10;i++){ histffunc[i] = 0.0f; @@ -373,14 +373,15 @@ Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K); } -void ScErrStateEKF::updateGPSVelocity(float speed, float direction) +void ScErrStateEKF::updateVelocity(float vix, float viy,float sinkRate) { Matrix H(3,nState); - H(1,9) = 1.0f; - H(2,10) = 1.0f; + H(1,10) = 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(2,1); - z << speed * cos(direction / 180.0f * M_PI) - vihat(1,1) << speed * sin(direction / 180.0f * M_PI)-vihat(2,1); + Matrix z(3,1); + z << vix - vihat(1,1) << viy-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);