Diff: solaESKF.cpp

Revision:

50:dadad0567349

Parent:

47:2467de40951f

Child:

51:a5af3b280d23

--- a/solaESKF.cpp	Fri Oct 29 13:30:03 2021 +0000
+++ b/solaESKF.cpp	Mon Nov 01 09:17:52 2021 +0000
@@ -105,7 +105,7 @@
     Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Qd;
 }
 
-void solaESKF::updateAccConstraints(Matrix acc,Matrix R)
+void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R)
 {
     Matrix accm = acc - accBias;
     Matrix tdcm(3,3);
@@ -114,7 +114,7 @@
     Matrix tdcm_g = tdcm*gravity;
     Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
     
-    Matrix H(3,nState);
+    Matrix H(4,nState);
     for (int i = 1; i < 4; i++){
         for (int j = 1; j < 4; j++){
             H(i,j+6) =  a2v(i,j);
@@ -124,15 +124,18 @@
     H(1,10) =  -1.0f;
     H(2,11) =  -1.0f;
     H(3,12) =  -1.0f;
+    H(4,3) = 1.0f;
     
     //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
     //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
     Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
-    Matrix z = -accm-tdcm*gravity;
+    Matrix zacc = -accm-tdcm*gravity;
+    Matrix z(4,1);
+    z << zacc(1,1) << zacc(2,1) << zacc(3,1) << palt- vihat(3,1);
     errState =  K * z;
-    //Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
+    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
     //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
-    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
+    //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
 }
 
 void solaESKF::updateGyroConstraints(Matrix gyro,Matrix R)
@@ -178,9 +181,9 @@
     Matrix z(3,1);
     z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << velgps(3,1) - vihat(3,1);
     errState =  K * z;
-    //Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
+    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
     //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
-    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
+    //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
 }
 
 void solaESKF::updateGPSPosition(Matrix posgps,Matrix R)
@@ -196,9 +199,9 @@
     Matrix z(3,1);
     z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << posgps(3,1) - pihat(3,1);
     errState =  K * z;
-    //Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
+    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
     //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
-    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
+    //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
 }
 
 void solaESKF::updateGPS(Matrix posgps,Matrix velgps,Matrix R)