Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 65:c25d7810de44
- Parent:
- 63:74a9565a0141
- Child:
- 66:6a093cb91728
diff -r 74a9565a0141 -r c25d7810de44 solaESKF.cpp --- a/solaESKF.cpp Wed Nov 17 01:40:33 2021 +0000 +++ b/solaESKF.cpp Thu Nov 18 00:50:38 2021 +0000 @@ -163,21 +163,22 @@ 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(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,j+16) = -dcm(3,j)+(dcm(3,j))/hz; } - //H(4,20) = 1.0f; - //H(4,17) = magm(1,1)/a; - //H(4,18) = magm(2,1)/a; - //H(4,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)<<-(magned(3,1) - hz); + z << -(magned(1,1) - hx) << -magned(2,1)<<-(magRadius-a); //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); + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + /* Matrix r3(3,1); r3 << tdcm(1,3)<< tdcm(2,3) << tdcm(3,3); Matrix kpart = r3*MatrixMath::Transpose(r3); @@ -210,7 +211,74 @@ } } } + */ + errState = K * z; + Phat = (MatrixMath::Eye(nState)-K*H)*Phat; + fuseErr2Nominal(); +} +void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R) +{ + Matrix accm = acc - accBias; + Matrix magm = mag - magBias; + Matrix dcm(3,3); + computeDcm(dcm, qhat); + Matrix tdcm = MatrixMath::Transpose(dcm); + Matrix tdcm_g = tdcm*gravity; + Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1)); + Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1)); + + Matrix H(5,nState); + for (int i = 1; i < 4; i++){ + for (int j = 1; j < 4; j++){ + H(i,j+6) = rotgrav(i,j); + } + H(i,16) = tdcm(i,3); + } + + H(1,10) = -1.0f; + H(2,11) = -1.0f; + H(3,12) = -1.0f; + + 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++){ + 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); + } + + 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); + //twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx)); + errState = K * z; + Phat = (MatrixMath::Eye(nState)-K*H)*Phat; + + fuseErr2Nominal(); +} + + +void solaESKF::updateOtherConstraints(Matrix mag,float palt,Matrix R) +{ + Matrix magm = mag - magBias; + float a = sqrt(magm(1,1)*magm(1,1)+magm(2,1)*magm(2,1)+magm(3,1)*magm(3,1)); + Matrix H(2,nState); + H(1,20) = -1.0f; + H(1,17) = -magm(1,1)/a; + H(1,18) = -magm(2,1)/a; + H(1,19) = -magm(3,1)/a; + H(2,3) = 1.0f; + + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + + Matrix z(2,1); + z << -(a-magRadius) << palt-pihat(3,1); + //twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx)); errState = K * z; Phat = (MatrixMath::Eye(nState)-K*H)*Phat;