Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 63:74a9565a0141
- Parent:
- 62:5519d34eb6e8
- Child:
- 65:c25d7810de44
diff -r 5519d34eb6e8 -r 74a9565a0141 solaESKF.cpp --- a/solaESKF.cpp Tue Nov 16 14:17:40 2021 +0000 +++ b/solaESKF.cpp Wed Nov 17 01:40:33 2021 +0000 @@ -153,27 +153,64 @@ Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1)); Matrix H(3,nState); + int nH = H.getRows(); Matrix magned = dcm*magm; float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1)); - //float hz = sqrt(magned(3,1)*magned(3,1)); + 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)); 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(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; + + Matrix z(nH,1); + z << -(magned(1,1) - hx) << -magned(2,1)<<-(magned(3,1) - hz); + //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 r3(3,1); + r3 << tdcm(1,3)<< tdcm(2,3) << tdcm(3,3); + Matrix kpart = r3*MatrixMath::Transpose(r3); + Matrix Pm(nState,nState); + for(int i = 7; i<10; i++){ + for(int j = 7;j<10;j++){ + Pm(i,j) = Phat(i,j); + } } - 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; + for(int i = 17; i<nState+1; i++){ + for(int j = 17;j<nState+1;j++){ + Pm(i,j) = Phat(i,j); + } + } - Matrix z(3,1); - z << -(magned(1,1) - hx) << -magned(2,1) <<-(magRadius-a) ; - Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + Matrix K = (Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+R); + Matrix Kmod(3,nH); + for(int i = 1; i<4; i++){ + for(int j = 1;j<nH+1;j++){ + Kmod(i,j) = K(i+6,j); + } + } + Kmod = kpart*Kmod; + for(int i = 1; i<nState+1; i++){ + for(int j = 1;j<nH;j++){ + if(i>6 && i<10){ + K(i,j) = Kmod(i-6,j); + }else if(i<17){ + K(i,j) = 0.0f; + } + } + } + errState = K * z; Phat = (MatrixMath::Eye(nState)-K*H)*Phat;