Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 62:5519d34eb6e8
- Parent:
- 61:5e5c4fe12440
- Child:
- 63:74a9565a0141
diff -r 5e5c4fe12440 -r 5519d34eb6e8 solaESKF.cpp --- a/solaESKF.cpp Tue Nov 16 13:56:44 2021 +0000 +++ b/solaESKF.cpp Tue Nov 16 14:17:40 2021 +0000 @@ -3,17 +3,19 @@ solaESKF::solaESKF() - :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(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),magBias(3,1),errState(20,1),Phat(20,20),Q(20,20) +{ pihat << 0.0f << 0.0f << 0.0f; vihat << 0.0f << 0.0f << 0.0f; qhat << 1.0f << 0.0f << 0.0f << 0.0f; 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(); - + nState = errState.getRows(); + for (int i = 1; i < nState+1; i++){ errState(i,1) = 0.0f; for (int j = 1; j < nState+1; j++){ @@ -21,18 +23,21 @@ Q(i,j) = 0.0f; } } - + setBlockDiag(Phat,0.1f,1,3);//position setBlockDiag(Phat,0.1f,4,6);//velocity 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,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//positionとgravityはQ項なし - + 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項なし } @@ -141,7 +146,7 @@ } void solaESKF::updateMag(Matrix mag,Matrix R) { - Matrix magm = mag; + Matrix magm = mag-magBias; Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix tdcm = MatrixMath::Transpose(dcm); @@ -151,46 +156,24 @@ 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; - } - - 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,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); } - - Matrix K = (Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+R); - 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); - } - } - Kmod = kpart*Kmod; - for(int i = 1; i<nState+1; i++){ - for(int j = 1;j<4;j++){ - if(i>6 && i<10){ - K(i,j) = Kmod(i-6,j); - }else{ - K(i,j) = 0.0f; - } - } - } + 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(3,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)); + z << -(magned(1,1) - hx) << -magned(2,1) <<-(magRadius-a) ; + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); errState = K * z; Phat = (MatrixMath::Eye(nState)-K*H)*Phat; @@ -260,6 +243,12 @@ 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++){ errState(i,1) = 0.0f; @@ -320,6 +309,7 @@ gravity(2,1) = gy; gravity(3,1) = gz; } + Matrix solaESKF::getPihat() { return pihat; @@ -344,6 +334,14 @@ { return gravity; } +Matrix solaESKF::getMagBias() +{ + return magBias; +} +float solaESKF::getMagRadius() +{ + return magRadius; +} Matrix solaESKF::getErrState() { return errState; @@ -373,6 +371,14 @@ { 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); +} void solaESKF::setQVelocity(float val) @@ -391,6 +397,14 @@ { 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){