Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 67:0453896df0d6
- Parent:
- 64:f5b6256726f3
diff -r f5b6256726f3 -r 0453896df0d6 solaESKF.cpp --- a/solaESKF.cpp Wed Nov 17 05:11:57 2021 +0000 +++ b/solaESKF.cpp Thu Nov 18 08:48:14 2021 +0000 @@ -154,46 +154,13 @@ float hz = sqrt(magned(3,1)*magned(3,1)); - for(int j = 1; j < 4; j++){ + for(int j = 3; 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; } - 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); - } - } - for(int i = 13; i<16; i++){ - for(int j = 13;j<16;j++){ - Pm(i,j) = Phat(i,j); - } - } - - 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+1;j++){ - if(i>6 && i<10){ - K(i,j) = Kmod(i-6,j); - }else if(i<13 || i>15){ - K(i,j) = 0.0f; - } - } - } - + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); Matrix z(nH,1); z << -(magned(1,1) - hx) << -magned(2,1) <<-(magned(3,1) - hz) ; //twelite.printf("%f %f %f\r\n",z(1,1),z(2,1));