Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 74:f5fe7fecbd3c
- Parent:
- 73:5770a0d470c0
- Child:
- 75:e2c825cdc511
diff -r 5770a0d470c0 -r f5fe7fecbd3c solaESKF.cpp --- a/solaESKF.cpp Sun Nov 21 08:24:40 2021 +0000 +++ b/solaESKF.cpp Mon Nov 22 09:51:36 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(16,1),Phat(16,16),Q(16,16) + :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),errState(18,1),Phat(18,18),Q(18,18) { pihat << 0.0f << 0.0f << 0.0f; vihat << 0.0f << 0.0f << 0.0f; @@ -11,8 +11,6 @@ accBias << 0.0f << 0.0f << 0.0f; gyroBias << 0.0f << 0.0f << 0.0f; gravity << 0.0f << 0.0f << 9.8f; - magBias << 0.0f << 0.0f << 0.0f; - magRadius = 0.0f; nState = errState.getRows(); @@ -29,7 +27,7 @@ setBlockDiag(Phat,0.1f,7,9);//angle error 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.00000001f,16,18);//gravity 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 @@ -77,10 +75,10 @@ for (int j = 1; j < 4; j++){ A(i+3,j+6) = a2v(i,j); A(i+3,j+9) = -dcm(i,j); - + A(i+3,j+15) = 1.0f; } } - A(6,16) = 1.0f; + //angulat error A(7,8) = gyrom(3,1); @@ -100,8 +98,6 @@ Q(i,i) *=att_dt; }else if(i>9 && i<16){ Q(i,i) *= att_dt*att_dt; - }else if(i>16 && i<nState+1){ - Q(i,i) *=att_dt*att_dt; }else{ Q(i,i) = 0.0f; } @@ -123,8 +119,8 @@ for (int i = 1; i < 4; i++){ for (int j = 1; j < 4; j++){ H(i,j+6) = rotgrav(i,j); + H(i,j+15) = tdcm(i,j); } - H(i,16) = tdcm(i,3); } H(1,10) = -1.0f; @@ -140,79 +136,6 @@ fuseErr2Nominal(); } -void solaESKF::updateMag(Matrix mag,Matrix R) -{ - Matrix magm = mag-magBias; - Matrix dcm(3,3); - computeDcm(dcm, qhat); - Matrix tdcm = MatrixMath::Transpose(dcm); - Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1)); - - Matrix H(2,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 a = sqrt(magm(1,1)*magm(1,1)+magm(2,1)*magm(2,1)+magm(3,1)*magm(3,1)); - - H(1,3+6) = rotmag(1,3)-(rotmag(1,3)+rotmag(2,3))/hx; - H(2,3+6) = rotmag(2,3); - for(int j = 1; j < 4; 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(2,j+16) = -dcm(2,j); - //H(3,j+16) = -dcm(3,j)+(dcm(3,j))/hz; - } - //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); - //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); - } - } - for(int i = 17; i<nState+1; i++){ - for(int j = 17;j<nState+1;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;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; - - fuseErr2Nominal(); -} void solaESKF::updateHeading(float a,Matrix R) { @@ -285,115 +208,46 @@ H(1,j+6) = Hpart(1,j); } - float psi = atan2f(qhat(2,1)*qhat(3,1) + qhat(1,1)*qhat(4,1), 0.5f - qhat(3,1)*qhat(3,1) - qhat(4,1)*qhat(4,1)); + const float psi = atan2f(qhat(2,1)*qhat(3,1) + qhat(1,1)*qhat(4,1), 0.5f - qhat(3,1)*qhat(3,1) - qhat(4,1)*qhat(4,1)); Matrix z(1,1); - z << atan2(sin(a-psi),cos(a-psi)); + z << atan2f(sin(a-psi),cos(a-psi)); 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) +void solaESKF::updateGPSPosition(Matrix posgps,float palt,Matrix R) { - Matrix accm = acc - accBias; - 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(mag(1,1),mag(2,1),mag(3,1)); - - 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); - } - H(i,16) = tdcm(i,3); - } - - H(1,10) = -1.0f; - H(2,11) = -1.0f; - H(3,12) = -1.0f; - - 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)); + Matrix H(3,nState); + H(1,1) = 1.0f; + H(2,2) = 1.0f; + H(3,3) = 1.0f; - 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 A = H*Phat*MatrixMath::Transpose(H)+R; + //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(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 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 zacc = -accm-tdcm*gravity; - 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); + Matrix z(3,1); + z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1); errState = K * z; Phat = (MatrixMath::Eye(nState)-K*H)*Phat; - + //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K); + //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); 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; - - fuseErr2Nominal(); -} - - void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R) { - Matrix H(5,nState); + Matrix H(6,nState); H(1,1) = 1.0f; H(2,2) = 1.0f; H(3,3) = 1.0f; H(4,4) = 1.0f; H(5,5) = 1.0f; + H(6,6) = 1.0f; Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); - Matrix z(5,1); - z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1); + Matrix z(6,1); + z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1)<< velgps(3,1)-vihat(3,1); errState = K * z; Phat = (MatrixMath::Eye(nState)-K*H)*Phat; fuseErr2Nominal(); @@ -441,9 +295,9 @@ gyroBias(3,1) += errState(15,1); //gravity bias - gravity(1,1) = 0.0f; - gravity(2,1) = 0.0f; - gravity(3,1) += errState(16,1); + gravity(1,1) += errState(16,1); + gravity(2,1) += errState(17,1); + gravity(3,1) += errState(18,1); @@ -531,14 +385,7 @@ { return gravity; } -Matrix solaESKF::getMagBias() -{ - return magBias; -} -float solaESKF::getMagRadius() -{ - return magRadius; -} + Matrix solaESKF::getErrState() { return errState; @@ -566,15 +413,7 @@ } void solaESKF::setPhatGravity(float val) { - setBlockDiag(Phat,val,16,16); -} -void solaESKF::setPhatMagBias(float val) -{ - setBlockDiag(Phat,val,17,19); -} -void solaESKF::setPhatMagRadius(float val) -{ - setBlockDiag(Phat,val,20,20); + setBlockDiag(Phat,val,16,18); } @@ -594,14 +433,6 @@ { setBlockDiag(Q,val,13,15); } -void solaESKF::setQMagBias(float val) -{ - setBlockDiag(Q,val,17,19); -} -void solaESKF::setQMagRadius(float val) -{ - setBlockDiag(Q,val,20,20); -} void solaESKF::setDiag(Matrix& mat, float val){ @@ -623,18 +454,6 @@ //pihat(3,1) = pi_z; } -float solaESKF::wrap_pi(float x) { - const float pi = 3.141592f; - const float range = pi + pi; - twelite.printf("1 : %f ",x); - - if (x < -pi) { - x += range * ((-pi - x) / range + 1); - } - - twelite.printf("2 : %f\r\n",-pi + fmodf( (x + pi),range)); - return -pi + fmodf((x + pi),range); -} /* void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R) {