Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
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);