Diff: ScErrStateEKF.cpp

Revision:

29:05e35ca2c9c4

Parent:

27:a74f101ba40a

Child:

30:ff884e9b2e30

--- 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);