Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 70:d12e46fdc2f0
- Parent:
- 68:264a7e0e4a29
- Child:
- 71:56c32be982b8
--- a/solaESKF.cpp Thu Nov 18 10:10:07 2021 +0000 +++ b/solaESKF.cpp Fri Nov 19 08:21:44 2021 +0000 @@ -3,7 +3,7 @@ solaESKF::solaESKF() - :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),magBias(3,1),errState(20,1),Phat(20,20),Q(20,20) + :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),magBias(3,1),errState(16,1),Phat(16,16),Q(16,16) { pihat << 0.0f << 0.0f << 0.0f; vihat << 0.0f << 0.0f << 0.0f; @@ -30,14 +30,10 @@ setBlockDiag(Phat,0.1f,10,12);//acc bias setBlockDiag(Phat,0.1f,13,15);//gyro bias setBlockDiag(Phat,0.00000001f,16,16);//gravity - setBlockDiag(Phat,0.001f,17,19);//magBias - setBlockDiag(Phat,0.001f,20,20);//magRadius setBlockDiag(Q,0.00025f,4,6);//velocity setBlockDiag(Q,0.005f/57.0f,7,9);//angle error setBlockDiag(Q,0.001f,10,12);//acc bias - setBlockDiag(Q,0.001f,13,15);//gyro bias - setBlockDiag(Q,0.001f,17,19);//magRadius - setBlockDiag(Q,0.0001f,20,20);//magRadius //positionとgravityはQ項なし + setBlockDiag(Q,0.001f,13,15);//gyro bias//positionとgravityはQ項なし } @@ -217,19 +213,71 @@ fuseErr2Nominal(); } + +void solaESKF::updateHeading(Matrix mag,Matrix R) +{ + float q0 = qhat(1,1); + float q1 = qhat(2,1); + float q2 = qhat(3,1); + float q3 = qhat(4,1); + + float d0 = (-q3*q3-q2*q2+q1*q1+q0*q0); + float q0q3q1q2 = (q0*q3+q1*q2); + float h1lower = 2.0f*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f); + + float d1 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f); + float d2 = d0*d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f); + float d3 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f); + + + + Matrix Hh(2,4); + Hh(1,1) = -(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower; + Hh(1,2) = -(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower; + Hh(1,3) = -(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower; + Hh(1,4) = -(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower; + + Hh(2,1) = 2.0f*q3/d1-4.0f*q0*q0q3q1q2/d2-q0q3q1q2*(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3; + Hh(2,2) = 2.0f*q2/d1-4.0f*q1*q0q3q1q2/d2-q0q3q1q2*(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3; + Hh(2,3) = 2.0f*q1/d1-4.0f*q2*q0q3q1q2/d2-q0q3q1q2*(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3; + Hh(2,4) = 2.0f*q0/d1-4.0f*q3*q0q3q1q2/d2-q0q3q1q2*(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3; + + Matrix Hdq(4,3); + Hdq << -0.5f*q1 << -0.5f*q2 << -0.5f*q3 + << 0.5f*q0 << -0.5f*q3 << 0.5f*q2 + << 0.5f*q3 << 0.5f*q0 << -0.5f*q1 + << -0.5f*q2 << 0.5f*q1 << 0.5f*q0; + + Matrix Hpart = Hh*Hdq; + Matrix H(2,nState); + for(int j=1;j<4;j++){ + H(1,j+6) = Hpart(1,j); + H(2,j+6) = Hpart(2,j); + } + + Matrix z(2,1); + float a = 0.5f; + float gamma = 2.0f*q0q3q1q2/d0; + z << 1.0f/sqrt(a*a+1.0f)-1.0f/sqrt(gamma*gamma+1.0f) << a/sqrt(a*a+1.0f)-gamma/sqrt(gamma*gamma+1.0f); + + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + 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; 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 rotmag = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1)); - Matrix H(5,nState); - for (int i = 1; i < 4; i++){ + Matrix H(6,nState); + for (int i = 1; i < 3; i++){ for (int j = 1; j < 4; j++){ H(i,j+6) = rotgrav(i,j); } @@ -240,21 +288,43 @@ H(2,11) = -1.0f; H(3,12) = -1.0f; - Matrix magned = dcm*magm; + Matrix magned = dcm*mag; float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1)); + float hz = sqrt(magned(3,1)*magned(3,1)); 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(5,j+6) = rotmag(2,j); //H(5,j+16) = -dcm(2,j); + H(6,j+6) = rotmag(3,j)-(rotmag(3,j))/hz; + } + 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 Kmod(3,3); + for(int i = 1; i<4; i++){ + for(int j = 1;j<4;j++){ + Kmod(i,j) = K(i+6,j+3); + } + } + Kmod = kpart*Kmod; + */ + for(int i = 1; i<nState+1; i++){ + for(int j = 4;j<7;j++){ + if(i>6 && i<10){ + //K(i,j) = Kmod(i-6,j-3); + }else{ + K(i,j) = 0.0f; + } + } } - 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); + Matrix z(6,1); + z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(magned(1,1) - hx) << -magned(2,1) << -(magned(3,1) - hz); //twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx)); errState = K * z; Phat = (MatrixMath::Eye(nState)-K*H)*Phat; @@ -347,12 +417,7 @@ gravity(1,1) = 0.0f; gravity(2,1) = 0.0f; gravity(3,1) += errState(16,1); - - //Magnetic bias - magBias(1,1) += errState(17,1); - magBias(2,1) += errState(18,1); - magBias(3,1) += errState(19,1); - magRadius += errState(20,1); + for (int i = 1; i < nState+1; i++){