HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

Diff: solaESKF.cpp

Revision:
71:56c32be982b8
Parent:
70:d12e46fdc2f0
Child:
72:8ebae608ae12
--- a/solaESKF.cpp	Fri Nov 19 08:21:44 2021 +0000
+++ b/solaESKF.cpp	Fri Nov 19 09:21:07 2021 +0000
@@ -256,10 +256,10 @@
     }
     
     Matrix z(2,1);
-    float a = 0.5f;
+    float a = tan(180.0f*M_PI/180.0f);
     float gamma = 2.0f*q0q3q1q2/d0;
-    z << 1.0f/sqrt(a*a+1.0f)-1.0f/sqrt(gamma*gamma+1.0f) << a/sqrt(a*a+1.0f)-gamma/sqrt(gamma*gamma+1.0f);
-    
+    z << -1.0f-1.0f/sqrt(gamma*gamma+1.0f) << 0.0f-gamma/sqrt(gamma*gamma+1.0f);
+    twelite.printf("%f %f \r\n",z(1,1),z(2,1));
     Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
     errState =  K * z;
     Phat = (MatrixMath::Eye(nState)-K*H)*Phat;