Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 59:03fe5e16a33c
- Parent:
- 58:93ba28cf5cb3
- Child:
- 60:f4b4231a8d3f
--- a/solaESKF.cpp Fri Nov 12 09:03:10 2021 +0000 +++ b/solaESKF.cpp Mon Nov 15 13:41:40 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),magField(3,1),errState(19,1),Phat(19,19),Q(19,19) + :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; @@ -11,7 +11,8 @@ accBias << 0.0f << 0.0f << 0.0f; gyroBias << 0.0f << 0.0f << 0.0f; gravity << 0.0f << 0.0f << 9.8f; - magField << -0.2f << -0.0f << 0.0f; + magBias << 0.0f << 0.0f << 0.0f; + magRadius = -0.5f; nState = errState.getRows(); @@ -23,21 +24,20 @@ } } - - - 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,18);//gravity - setBlockDiag(Phat,0.001f,19,19);//H0 + 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.0001f,19,19);//mag field //positionとgravityはQ項なし + setBlockDiag(Q,0.001f,17,19);//magRadius + setBlockDiag(Q,0.0001f,20,20);//magRadius //positionとgravityはQ項なし } @@ -85,9 +85,7 @@ } } - A(4,16) = 1.0f; - A(5,17) = 1.0f; - A(6,18) = 1.0f; + A(6,16) = 1.0f; //angulat error A(7,8) = gyrom(3,1); @@ -102,161 +100,82 @@ Matrix Fx = MatrixMath::Eye(nState) + A * att_dt;// + (0.5f*att_dt*att_dt) * A * A; - Matrix Qd = Q * att_dt + 0.5f*A*Q * att_dt * att_dt+ 0.5f*Q*MatrixMath::Transpose(A) * att_dt * att_dt; - + for (int i = 1; i < nState+1; i++){ + if(i>3 && i<10){ + 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; + } + } errState = Fx * errState; - Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Qd; + Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Q; } void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R) { Matrix accm = acc - accBias; + Matrix magm = mag - magBias; Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix tdcm = MatrixMath::Transpose(dcm); Matrix tdcm_g = tdcm*gravity; - Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1)); - Matrix a2v2 = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1)); + 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 H(5,nState); - for (int j = 1; j < 4; j++){ - for (int i = 1; i < 4; i++){ - H(i,j+6) = a2v(i,j); - H(i,j+15) = tdcm(i,j); + for (int i = 1; i < 4; i++){ + for (int j = 1; j < 4; j++){ + H(i,j+6) = rotgrav(i,j); } - H(4,j+6) = a2v2(1,j); - H(5,j+6) = a2v2(2,j); - } - for (int i = 1; i < 4; i++){ - + H(i,16) = tdcm(i,3); } H(1,10) = -1.0f; H(2,11) = -1.0f; H(3,12) = -1.0f; - H(4,19) = 1.0f; + + Matrix magned = dcm*magm; + float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1)); - //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; - //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); + for(int j = 1; j < 4; j++){ + H(4,j+6) = rotmag(1,j)+2.0f*(rotmag(1,j)+rotmag(2,j))/hx; + H(4,j+16) = -dcm(1,j)+2.0f*dcm(1,j)/hx; + H(5,j+6) = rotmag(2,j); + H(5,j+16) = -dcm(2,j)+2.0f*dcm(2,j)/hx; + } + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); Matrix zacc = -accm-tdcm*gravity; - Matrix zmag = -dcm*mag-magField; + Matrix zmag = dcm*magm; Matrix z(5,1); - z << zacc(1,1) << zacc(2,1) << zacc(3,1) << zmag(1,1) << zmag(2,1); + z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx) << zmag(2,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::updateMag(Matrix mag,float palt, Matrix R) -{ - Matrix dcm(3,3); - computeDcm(dcm, qhat); - Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1)); - - Matrix H(3,nState); - for (int i = 1; i < 3; i++){ - for (int j = 1; j < 4; j++){ - H(i,j+6) = a2v(i,j); - } - } - H(1,19) = 1.0f; - H(3,3) = 1.0f; - //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; - //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); - Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); - Matrix zmag = -dcm*mag-magField; - Matrix z(3,1); - z << zmag(1,1) << zmag(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::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix mag,Matrix R) +void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R) { - Matrix dcm(3,3); - computeDcm(dcm, qhat); - Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1)); - - Matrix H(6,nState); + Matrix H(5,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; - for (int j = 1; j < 4; j++){ - H(6,j+6) = a2v(2,j); - } - - Matrix zmag = -dcm*mag-magField; - - //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; - //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); - 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)<< zmag(2,1); + 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); 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::updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R) -{ - - Matrix dcm(3,3); - computeDcm(dcm, qhat); - Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1)); - - - Matrix H(3,nState); - H(1,4) = 1.0f; - H(2,5) = 1.0f; - - for (int j = 1; j < 4; j++){ - H(3,j+6) = a2v(2,j); - } - //H(3,19) = 1.0f; - - Matrix zmag = -dcm*mag-magField; - - //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; - //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); - Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); - Matrix z(3,1); - z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << zmag(2,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::updateGPSPosition(Matrix posgps,float palt,Matrix R) -{ - Matrix H(3,nState); - H(1,1) = 1.0f; - H(2,2) = 1.0f; - H(3,3) = 1.0f; - - //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 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(); -} Matrix solaESKF::computeAngles() { @@ -299,14 +218,15 @@ gyroBias(3,1) += errState(15,1); //gravity bias - gravity(1,1) += errState(16,1); - gravity(2,1) += errState(17,1); - gravity(3,1) += errState(18,1); + gravity(1,1) = 0.0f; + gravity(2,1) = 0.0f; + gravity(3,1) += errState(16,1); - //Magnetic Field - magField(1,1) += errState(19,1); - magField(2,1) = 0.0f; - magField(3,1) = 0.0f; + //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; @@ -391,9 +311,13 @@ { return gravity; } -Matrix solaESKF::getMagField() +Matrix solaESKF::getMagBias() { - return magField; + return magBias; +} +float solaESKF::getMagRadius() +{ + return magRadius; } Matrix solaESKF::getErrState() { @@ -422,11 +346,15 @@ } void solaESKF::setPhatGravity(float val) { - setBlockDiag(Phat,val,16,18); + setBlockDiag(Phat,val,16,16); } -void solaESKF::setPhatMagField(float val) +void solaESKF::setPhatMagBias(float val) { - setBlockDiag(Phat,val,19,19); + setBlockDiag(Phat,val,17,19); +} +void solaESKF::setPhatMagRadius(float val) +{ + setBlockDiag(Phat,val,20,20); } @@ -446,9 +374,13 @@ { setBlockDiag(Q,val,13,15); } -void solaESKF::setQMagField(float val) +void solaESKF::setQMagBias(float val) { - setBlockDiag(Q,val,19,19); + setBlockDiag(Q,val,17,19); +} +void solaESKF::setQMagRadius(float val) +{ + setBlockDiag(Q,val,20,20); } @@ -470,13 +402,6 @@ pihat(2,1) = pi_y; //pihat(3,1) = pi_z; } -void solaESKF::setMagField(float hx, float hy, float hz) -{ - magField(1,1) = -sqrt(hx*hx+hy*hy); - magField(2,1) = 0.0f; - magField(3,1) = 0.0f; -} - /* void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R) { @@ -566,6 +491,80 @@ //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K); fuseErr2Nominal(); } - +void solaESKF::updateMag(Matrix mag,float palt, Matrix R) +{ + Matrix dcm(3,3); + computeDcm(dcm, qhat); + Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1)); + + Matrix H(3,nState); + for (int i = 1; i < 3; i++){ + for (int j = 1; j < 4; j++){ + H(i,j+6) = a2v(i,j); + } + } + H(1,19) = 1.0f; + H(3,3) = 1.0f; + //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; + //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + Matrix zmag = -dcm*mag-magField; + Matrix z(3,1); + z << zmag(1,1) << zmag(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::updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R) +{ + + Matrix dcm(3,3); + computeDcm(dcm, qhat); + Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1)); + + + Matrix H(3,nState); + H(1,4) = 1.0f; + H(2,5) = 1.0f; + + for (int j = 1; j < 4; j++){ + H(3,j+6) = a2v(2,j); + } + //H(3,19) = 1.0f; + + Matrix zmag = -dcm*mag-magField; + + //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; + //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A)); + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + Matrix z(3,1); + z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << zmag(2,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::updateGPSPosition(Matrix posgps,float palt,Matrix R) +{ + Matrix H(3,nState); + H(1,1) = 1.0f; + H(2,2) = 1.0f; + H(3,3) = 1.0f; + + //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 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(); +} */ \ No newline at end of file