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;