Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 68:264a7e0e4a29
- Parent:
- 66:6a093cb91728
- Child:
- 69:2b2815603e5a
- Child:
- 70:d12e46fdc2f0
--- a/solaESKF.cpp Thu Nov 18 08:40:15 2021 +0000 +++ b/solaESKF.cpp Thu Nov 18 10:10:07 2021 +0000 @@ -152,7 +152,7 @@ Matrix tdcm = MatrixMath::Transpose(dcm); Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1)); - Matrix H(3,nState); + Matrix H(2,nState); int nH = H.getRows(); Matrix magned = dcm*magm; @@ -160,21 +160,21 @@ float hz = sqrt(magned(3,1)*magned(3,1)); float a = sqrt(magm(1,1)*magm(1,1)+magm(2,1)*magm(2,1)+magm(3,1)*magm(3,1)); + H(1,3+6) = rotmag(1,3)-(rotmag(1,3)+rotmag(2,3))/hx; + H(2,3+6) = rotmag(2,3); for(int j = 1; j < 4; j++){ - H(1,j+6) = rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx; - H(2,j+6) = rotmag(2,j); //H(3,j+6) = rotmag(3,j)-(rotmag(3,j))/hz; H(1,j+16) = -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx; H(2,j+16) = -dcm(2,j); //H(3,j+16) = -dcm(3,j)+(dcm(3,j))/hz; } - H(3,20) = 1.0f; - H(3,17) = magm(1,1)/a; - H(3,18) = magm(2,1)/a; - H(3,19) = magm(3,1)/a; + //H(3,20) = 1.0f; + //H(3,17) = magm(1,1)/a; + //H(3,18) = magm(2,1)/a; + //H(3,19) = magm(3,1)/a; Matrix z(nH,1); - z << -(magned(1,1) - hx) << -magned(2,1)<<-(magRadius-a); + z << -(magned(1,1) - hx) << -magned(2,1); //twelite.printf("%f %f %f %f\r\n",-(magned(1,1) - hx),-magned(2,1),-(magned(3,1) - hz),-(magRadius-a)); Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);