Diff: ScErrStateEKF.cpp

Revision:

25:07ac5c6cd61c

Parent:

23:1509648c2318

Child:

26:73c3f58b9d70

--- a/ScErrStateEKF.cpp	Fri Aug 20 07:51:01 2021 +0000
+++ b/ScErrStateEKF.cpp	Wed Aug 25 07:01:05 2021 +0000
@@ -129,6 +129,44 @@
     }
 }
 
+void ScErrStateEKF::getQhat(float (&res)[4]){
+    for (int i = 0; i < 4; i++){
+        res[i] = qhat(i+1,1);
+    }
+}
+
+void ScErrStateEKF::getVihat(float (&res)[3]){
+    for (int i = 0; i < 3; i++){
+        res[i] = vihat(i+1,1);
+    }
+}
+
+void ScErrStateEKF::getGyroBias(float (&resVal)[3], float (&resCov)[6]){
+    resVal[0] = gyroBias.x;
+    resVal[1] = gyroBias.y;
+    resVal[2] = gyroBias.z;
+    resCov[0] = Phat(4,4);
+    resCov[1] = Phat(4,5);
+    resCov[2] = Phat(4,6);
+    resCov[3] = Phat(5,5);
+    resCov[4] = Phat(5,6);
+    resCov[5] = Phat(6,6);
+    
+}
+
+void ScErrStateEKF::getAccBias(float (&resVal)[3], float (&resCov)[6]){
+    resVal[0] = accBias.x;
+    resVal[1] = accBias.y;
+    resVal[2] = accBias.z;
+    resCov[0] = Phat(7,7);
+    resCov[1] = Phat(7,8);
+    resCov[2] = Phat(7,9);
+    resCov[3] = Phat(8,8);
+    resCov[4] = Phat(8,9);
+    resCov[5] = Phat(9,9);
+    
+}
+
 
 void ScErrStateEKF::updateAccMeasures(Vector3 acc, Vector3 accref)
 {
@@ -380,7 +418,8 @@
     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);
-}  
+}
+
 
 
 void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref)
@@ -452,6 +491,68 @@
     Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rm)*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;
+    }
+    
+    Matrix H(3,nState);
+    H(1,4) =  1.0f;
+    H(2,5) =  1.0f;
+    H(3,6) =  1.0f;
+    
+    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+RgyroBias);
+    Matrix z(3,1);
+    z <<cBg[0]-gyroBias.x <<cBg[1]-gyroBias.y <<cBg[2]-gyroBias.z;
+    Matrix corrVal =  K * (z-H*errState);
+    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])
+{
+    
+    Matrix RaccBias(3,3);
+    for(int i = 1;i<4;i++){
+        RaccBias(i,i) = 5.0f;
+    }
+    
+    Matrix H(3,nState);
+    H(1,7) =  1.0f;
+    H(2,8) =  1.0f;
+    H(3,9) =  1.0f;
+    
+    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+RaccBias);
+    Matrix z(3,1);
+    z <<cBa[0]-accBias.x <<cBa[1]-accBias.y <<cBa[2]-accBias.z;
+    Matrix corrVal =  K * (z-H*errState);
+    errState = errState + corrVal;
+    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(RaccBias)*MatrixMath::Transpose(K);
+    
+    //gyroBias.x = cBg[0];
+    //gyroBias.y = cBg[1];
+    //gyroBias.z = cBg[2];
+}
+
+
 void ScErrStateEKF::resetBias()
 {
     gyroBias.x = gyroBias.x + errState(4,1)*1.0f;