Diff: ScErrStateEKF.cpp

Revision:

40:119792aa6d3b

Parent:

39:6834e05d8a64

Child:

42:b90c348a49c0

Child:

43:cbc2c2d65131

--- a/ScErrStateEKF.cpp	Thu Oct 14 11:39:38 2021 +0000
+++ b/ScErrStateEKF.cpp	Mon Oct 18 12:16:57 2021 +0000
@@ -12,6 +12,11 @@
 { 
     nState = errState.getRows();
     qhat << 1.0f << 0.0f << 0.0f << 0.0f;
+    vihat << 0.0f << 0.0f << 0.0f;
+    for (int i = 1; i <= nState; i++)
+    {
+        errState(i, 1) = 0.0f;
+    }
     
     setDiag(Phat,0.1f); 
     Phat(4,4) = 0.001;
@@ -250,6 +255,7 @@
     Matrix dcm(3,3);
     computeDcm(dcm, qhat);
     Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
+    
     Matrix H(6,nState);
     H(1,2) = -2.0f*gvec.z;
     H(1,3) =  2.0f*gvec.y;
@@ -272,11 +278,23 @@
     R(5,5) = Rgps(2,2);
     R(6,6) = Rsr(1,1);
     
+    /*
+    Matrix H(3, nState);
+    H(1, 10) = 1.0f;
+    H(2, 11) = 1.0f;
+    H(3, 12) = 1.0f;
+    
+    Matrix R(3,3);
+    R(1,1) = Rgps(1,1);
+    R(2,2) = Rgps(2,2);
+    R(3,3) = Rsr(1,1);
+    */
+    
     Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
     Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
     Matrix z(6,1);
     z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);
-    //z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << vi_x - vihat(1,1) << vi_y-vihat(2,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*(R)*MatrixMath::Transpose(K);
@@ -305,7 +323,11 @@
     R(2,2) = Ra(2,2)+Qab(2,2);
     R(3,3) = Ra(3,3)+Qab(3,3);
     R(4,4) = Rsr(1,1);
-    
+    /*
+    R(1,1) = 0.0001f;
+    R(2,2) = 0.0001f;
+    R(3,3) = 0.0001f;
+    */
     Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
     Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
     Matrix z(4,1);