Diff: solaESKF.cpp

Revision:

83:8f6ae61d47ac

Parent:

82:662b810155e9

Child:

84:fb14acabefd3

diff -r 662b810155e9 -r 8f6ae61d47ac solaESKF.cpp
--- a/solaESKF.cpp	Wed Jun 29 01:39:26 2022 +0000
+++ b/solaESKF.cpp	Wed Jun 29 07:56:42 2022 +0000
@@ -243,8 +243,12 @@
     fuseErr2Nominal();
 }
 
-void solaESKF::updateIMU(Vector3f acc,float heading, Matrix<float, 4, 4> R)
+void solaESKF::updateIMU(float palt,Vector3f acc,float heading, Matrix<float, 5, 5> R)
 {
+    MatrixXf H = MatrixXf::Zero(5,nState);
+    
+    
+    H(0,2) = 1.0f;
     
     Vector3f accm = acc - accBias;
     Matrix3f dcm;
@@ -253,17 +257,16 @@
     Vector3f tdcm_g = tdcm*gravity;
     Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g);
     
-    MatrixXf H = MatrixXf::Zero(4,nState);
     for (int i = 0; i < 3; i++){
         for (int j = 0; j < 3; j++){
-            H(i,j+6) =  rotgrav(i,j);
-            H(i,j+15) = tdcm(i,j);
+            H(i+1,j+6) =  rotgrav(i,j);
+            H(i+1,j+15) = tdcm(i,j);
         }
     }
 
-    H(0,9) =   -1.0f;
-    H(1,10) =  -1.0f;
-    H(2,11) =  -1.0f;
+    H(1,9) =   -1.0f;
+    H(2,10) =  -1.0f;
+    H(3,11) =  -1.0f;
 
     float q0 = qhat(0);
     float q1 = qhat(1);
@@ -330,13 +333,15 @@
     
     MatrixXf  Hpart = Hh*Hdq;
     for(int j=0; j<3; j++){
-        H(3,j+6) = Hpart(0,j);
+        H(4,j+6) = Hpart(0,j);
     }
     
+
+    
     const float psi = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3));
     Vector3f zacc = -accm-tdcm*gravity;
-    VectorXf z = VectorXf::Zero(4);
-    z <<zacc(0),zacc(1),zacc(2),std::atan2(std::sin(heading-psi), std::cos(heading-psi));
+    VectorXf z = VectorXf::Zero(5);
+    z << palt-pihat(2),zacc(0),zacc(1),zacc(2),std::atan2(std::sin(heading-psi), std::cos(heading-psi));
     MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
     errState =  K * z;
     Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
@@ -613,15 +618,14 @@
     return dynAcc;
 }
 
-Vector3f solaESKF::calcDynAcc(Vector3f acc)
+Vector3f solaESKF::calcVb()
 {
-    Vector3f accm = acc - accBias;
     Matrix3f dcm;
     computeDcm(dcm, qhat);
     Matrix3f tdcm = dcm.transpose();
 
     Vector3f Vb = tdcm*vihat;
-    return dynAcc;
+    return Vb;
 }
 
 
@@ -774,7 +778,7 @@
 
 Matrix3f solaESKF::Matrixcross(Vector3f v)
 {
-    Matrix3f m;
+    Matrix3f m = Matrix3f::Zero();
     m << 0.0f, -v(2),  v(1),
          v(2),  0.0f, -v(0),
         -v(1),  v(0),  0.0f;