Diff: ScErrStateEKF.cpp

Revision:

30:ff884e9b2e30

Parent:

29:05e35ca2c9c4

Child:

31:e655d4d8e4d6

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