Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 60:f4b4231a8d3f
- Parent:
- 59:03fe5e16a33c
- Child:
- 61:5e5c4fe12440
diff -r 03fe5e16a33c -r f4b4231a8d3f solaESKF.cpp --- a/solaESKF.cpp Mon Nov 15 13:41:40 2021 +0000 +++ b/solaESKF.cpp Tue Nov 16 01:20:50 2021 +0000 @@ -142,17 +142,18 @@ float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1)); for(int j = 1; j < 4; j++){ - H(4,j+6) = rotmag(1,j)+2.0f*(rotmag(1,j)+rotmag(2,j))/hx; - H(4,j+16) = -dcm(1,j)+2.0f*dcm(1,j)/hx; + H(4,j+6) = rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx; + H(4,j+16) = -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx; H(5,j+6) = rotmag(2,j); - H(5,j+16) = -dcm(2,j)+2.0f*dcm(2,j)/hx; + H(5,j+16) = -dcm(2,j); } Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); Matrix zacc = -accm-tdcm*gravity; Matrix zmag = dcm*magm; Matrix z(5,1); - z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx) << zmag(2,1); + z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx) << -zmag(2,1); + twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx)); errState = K * z; Phat = (MatrixMath::Eye(nState)-K*H)*Phat;