Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 64:f5b6256726f3
- Parent:
- 61:5e5c4fe12440
- Child:
- 67:0453896df0d6
--- a/solaESKF.cpp Tue Nov 16 13:56:44 2021 +0000 +++ b/solaESKF.cpp Wed Nov 17 05:11:57 2021 +0000 @@ -148,7 +148,7 @@ 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)); @@ -162,6 +162,7 @@ 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++){ @@ -169,28 +170,33 @@ 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,3); + Matrix Kmod(3,nH); for(int i = 1; i<4; i++){ - for(int j = 1;j<4;j++){ + 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<4;j++){ + for(int j = 1;j<nH+1;j++){ if(i>6 && i<10){ K(i,j) = Kmod(i-6,j); - }else{ + }else if(i<13 || i>15){ K(i,j) = 0.0f; } } } - Matrix z(3,1); + 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),z(3,1)); + //twelite.printf("%f %f %f\r\n",z(1,1),z(2,1)); errState = K * z; Phat = (MatrixMath::Eye(nState)-K*H)*Phat;