Diff: solaESKF.cpp

Revision:

72:8ebae608ae12

Parent:

71:56c32be982b8

Child:

73:5770a0d470c0

--- a/solaESKF.cpp	Fri Nov 19 09:21:07 2021 +0000
+++ b/solaESKF.cpp	Fri Nov 19 14:24:09 2021 +0000
@@ -239,14 +239,14 @@
     
     Hh(2,1) = 2.0f*q3/d1-4.0f*q0*q0q3q1q2/d2-q0q3q1q2*(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
     Hh(2,2) = 2.0f*q2/d1-4.0f*q1*q0q3q1q2/d2-q0q3q1q2*(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
-    Hh(2,3) = 2.0f*q1/d1-4.0f*q2*q0q3q1q2/d2-q0q3q1q2*(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
-    Hh(2,4) = 2.0f*q0/d1-4.0f*q3*q0q3q1q2/d2-q0q3q1q2*(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
+    Hh(2,3) = 2.0f*q1/d1+4.0f*q2*q0q3q1q2/d2-q0q3q1q2*(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
+    Hh(2,4) = 2.0f*q0/d1+4.0f*q3*q0q3q1q2/d2-q0q3q1q2*(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
     
     Matrix Hdq(4,3);
     Hdq  << -0.5f*q1 << -0.5f*q2 << -0.5f*q3
-         << 0.5f*q0 << -0.5f*q3 << 0.5f*q2
-         << 0.5f*q3 << 0.5f*q0 << -0.5f*q1
-         << -0.5f*q2 << 0.5f*q1 << 0.5f*q0; 
+         <<  0.5f*q0 << -0.5f*q3 <<  0.5f*q2
+         <<  0.5f*q3 <<  0.5f*q0 << -0.5f*q1
+         << -0.5f*q2 <<  0.5f*q1 <<  0.5f*q0; 
     
     Matrix Hpart = Hh*Hdq;
     Matrix H(2,nState);