Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 66:6a093cb91728
- Parent:
- 65:c25d7810de44
- Child:
- 68:264a7e0e4a29
diff -r c25d7810de44 -r 6a093cb91728 solaESKF.cpp --- a/solaESKF.cpp Thu Nov 18 00:50:38 2021 +0000 +++ b/solaESKF.cpp Thu Nov 18 08:40:15 2021 +0000 @@ -220,7 +220,7 @@ void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R) { Matrix accm = acc - accBias; - Matrix magm = mag - magBias; + Matrix magm = mag; Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix tdcm = MatrixMath::Transpose(dcm); @@ -243,11 +243,11 @@ Matrix magned = dcm*magm; float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1)); - for(int j = 1; j < 4; j++){ + for(int j = 3; j < 4; j++){ 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(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); + //H(5,j+16) = -dcm(2,j); } Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);