Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
solaESKF.cpp
- Committer:
- NaotoMorita
- Date:
- 2021-11-19
- Revision:
- 71:56c32be982b8
- Parent:
- 70:d12e46fdc2f0
- Child:
- 72:8ebae608ae12
File content as of revision 71:56c32be982b8:
#include "solaESKF.hpp" 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 << 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(); for (int i = 1; i < nState+1; i++){ errState(i,1) = 0.0f; for (int j = 1; j < nState+1; j++){ Phat(i,j) = 0.0f; 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(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項なし } void solaESKF::updateNominal(Matrix acc, Matrix gyro,float att_dt) { Matrix gyrom = gyro - gyroBias; Matrix accm = acc - accBias; Matrix qint(4,1); qint << 1.0f << 0.5f*gyrom(1,1)*att_dt << 0.5f*gyrom(2,1)*att_dt << 0.5f*gyrom(3,1)*att_dt; qhat = quatmultiply(qhat,qint); float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); qhat *= (1.0f/ qnorm); Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix accned = dcm*accm+gravity; vihat += accned*att_dt; pihat += vihat*att_dt+0.5f*accned*att_dt*att_dt; } void solaESKF::updateErrState(Matrix acc,Matrix gyro,float att_dt) { Matrix gyrom = gyro - gyroBias; Matrix accm = acc - accBias; Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1)); Matrix A(nState,nState); //position A(1,4) = 1.0f; A(2,5) = 1.0f; A(3,6) = 1.0f; //velocity for (int i = 1; i < 4; i++){ for (int j = 1; j < 4; j++){ A(i+3,j+6) = a2v(i,j); A(i+3,j+9) = -dcm(i,j); } } A(6,16) = 1.0f; //angulat error A(7,8) = gyrom(3,1); A(7,9) = -gyrom(2,1); A(8,7) = -gyrom(3,1); A(8,9) = gyrom(1,1); A(9,7) = gyrom(2,1); A(9,8) = -gyrom(1,1); A(7,13) = -1.0f; A(8,14) = -1.0f; A(9,15) = -1.0f; Matrix Fx = MatrixMath::Eye(nState) + A * att_dt;// + (0.5f*att_dt*att_dt) * A * A; 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)+Q; } void solaESKF::updateAcc(Matrix acc,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 H(3,nState); for (int i = 1; i < 4; 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 K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); Matrix zacc = -accm-tdcm*gravity; Matrix z(3,1); z << zacc(1,1) << zacc(2,1) << zacc(3,1); errState = K * z; Phat = (MatrixMath::Eye(nState)-K*H)*Phat; 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(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 = tan(180.0f*M_PI/180.0f); float gamma = 2.0f*q0q3q1q2/d0; z << -1.0f-1.0f/sqrt(gamma*gamma+1.0f) << 0.0f-gamma/sqrt(gamma*gamma+1.0f); twelite.printf("%f %f \r\n",z(1,1),z(2,1)); 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 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)); 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 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); //twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx)); errState = K * z; Phat = (MatrixMath::Eye(nState)-K*H)*Phat; 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); 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; 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); errState = K * z; Phat = (MatrixMath::Eye(nState)-K*H)*Phat; fuseErr2Nominal(); } Matrix solaESKF::computeAngles() { Matrix euler(3,1); euler(1,1) = atan2f(qhat(1,1)*qhat(2,1) + qhat(3,1)*qhat(4,1), 0.5f - qhat(2,1)*qhat(2,1) - qhat(3,1)*qhat(3,1)); euler(2,1) = asinf(-2.0f * (qhat(2,1)*qhat(4,1) - qhat(1,1)*qhat(3,1))); euler(3,1) = 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)); return euler; } void solaESKF::fuseErr2Nominal() { //position pihat(1,1) += errState(1,1); pihat(2,1) += errState(2,1); pihat(3,1) += errState(3,1); //velocity vihat(1,1) += errState(4,1); vihat(2,1) += errState(5,1); vihat(3,1) += errState(6,1); //angle error Matrix qerr(4,1); qerr << 1.0f << 0.5f*errState(7,1) << 0.5f*errState(8,1) << 0.5f*errState(9,1); qhat = quatmultiply(qhat, qerr); float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); qhat *= (1.0f/ qnorm); //acc bias accBias(1,1) += errState(10,1); accBias(2,1) += errState(11,1); accBias(3,1) += errState(12,1); //gyro bias gyroBias(1,1) += errState(13,1); gyroBias(2,1) += errState(14,1); gyroBias(3,1) += errState(15,1); //gravity bias gravity(1,1) = 0.0f; gravity(2,1) = 0.0f; gravity(3,1) += errState(16,1); for (int i = 1; i < nState+1; i++){ errState(i,1) = 0.0f; } } Matrix solaESKF::quatmultiply(Matrix p, Matrix q) { Matrix qout(4,1); qout(1,1) = p(1,1)*q(1,1) - p(2,1)*q(2,1) - p(3,1)*q(3,1) - p(4,1)*q(4,1); qout(2,1) = p(1,1)*q(2,1) + p(2,1)*q(1,1) + p(3,1)*q(4,1) - p(4,1)*q(3,1); qout(3,1) = p(1,1)*q(3,1) - p(2,1)*q(4,1) + p(3,1)*q(1,1) + p(4,1)*q(2,1); qout(4,1) = p(1,1)*q(4,1) + p(2,1)*q(3,1) - p(3,1)*q(2,1) + p(4,1)*q(1,1); float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout)); qout *= (1.0f/ qnorm); return qout; } void solaESKF::computeDcm(Matrix& dcm, Matrix quat) { dcm(1,1) = quat(1,1)*quat(1,1) + quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1); dcm(1,2) = 2.0f*(quat(2,1)*quat(3,1) - quat(1,1)*quat(4,1)); dcm(1,3) = 2.0f*(quat(2,1)*quat(4,1) + quat(1,1)*quat(3,1)); dcm(2,1) = 2.0f*(quat(2,1)*quat(3,1) + quat(1,1)*quat(4,1)); dcm(2,2) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) + quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1); dcm(2,3) = 2.0f*(quat(3,1)*quat(4,1) - quat(1,1)*quat(2,1)); dcm(3,1) = 2.0f*(quat(2,1)*quat(4,1) - quat(1,1)*quat(3,1)); dcm(3,2) = 2.0f*(quat(3,1)*quat(4,1) + quat(1,1)*quat(2,1)); dcm(3,3) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) + quat(4,1)*quat(4,1); } void solaESKF::setQhat(float ex,float ey,float ez) { float cos_z_2 = cosf(0.5f*ez); float cos_y_2 = cosf(0.5f*ey); float cos_x_2 = cosf(0.5f*ex); float sin_z_2 = sinf(0.5f*ez); float sin_y_2 = sinf(0.5f*ey); float sin_x_2 = sinf(0.5f*ex); // and now compute quaternion qhat(1,1) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2; qhat(2,1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2; qhat(3,1) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2; qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; } void solaESKF::setGravity(float gx,float gy,float gz) { gravity(1,1) = gx; gravity(2,1) = gy; gravity(3,1) = gz; } Matrix solaESKF::getPihat() { return pihat; } Matrix solaESKF::getVihat() { return vihat; } Matrix solaESKF::getQhat() { return qhat; } Matrix solaESKF::getAccBias() { return accBias; } Matrix solaESKF::getGyroBias() { return gyroBias; } Matrix solaESKF::getGravity() { return gravity; } Matrix solaESKF::getMagBias() { return magBias; } float solaESKF::getMagRadius() { return magRadius; } Matrix solaESKF::getErrState() { return errState; } void solaESKF::setPhatPosition(float val) { setBlockDiag(Phat,val,1,3); } void solaESKF::setPhatVelocity(float val) { setBlockDiag(Phat,val,4,6); } void solaESKF::setPhatAngleError(float val) { setBlockDiag(Phat,val,7,9); } void solaESKF::setPhatAccBias(float val) { setBlockDiag(Phat,val,10,12); } void solaESKF::setPhatGyroBias(float val) { setBlockDiag(Phat,val,13,15); } 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); } void solaESKF::setQVelocity(float val) { setBlockDiag(Q,val,4,6); } void solaESKF::setQAngleError(float val) { setBlockDiag(Q,val,7,9); } void solaESKF::setQAccBias(float val) { setBlockDiag(Q,val,10,12); } void solaESKF::setQGyroBias(float val) { 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){ for (int i = 1; i < mat.getCols()+1; i++){ mat(i,i) = val; } } void solaESKF::setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex){ for (int i = startIndex; i < endIndex+1; i++){ mat(i,i) = val; } } void solaESKF::setPihat(float pi_x, float pi_y) { pihat(1,1) = pi_x; pihat(2,1) = pi_y; //pihat(3,1) = pi_z; } /* void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R) { Matrix accm = acc - accBias; Matrix tdcm(3,3); computeDcm(tdcm, qhat); tdcm = MatrixMath::Transpose(tdcm); Matrix tdcm_g = tdcm*gravity; Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1)); Matrix H(4,nState); for (int i = 1; i < 4; i++){ for (int j = 1; j < 4; j++){ H(i,j+6) = a2v(i,j); H(i,j+15) = tdcm(i,j); } } H(1,10) = -1.0f; H(2,11) = -1.0f; H(3,12) = -1.0f; H(4,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 zacc = -accm-tdcm*gravity; Matrix z(4,1); z << zacc(1,1) << zacc(2,1) << zacc(3,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::updateGyroConstraints(Matrix gyro,Matrix R) { Matrix gyrom = gyro - gyroBias; Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix a2v = dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1)); Matrix H(2,nState); for (int i = 1; i < 3; i++){ for (int j = 1; j < 4; j++){ H(i,j+6) = a2v(i,j); H(i,j+12) = -dcm(i,j); } } //Matrix A = H*Phat*MatrixMath::Transpose(H)+R; //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A)); Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); Matrix z3 = -dcm*gyrom; Matrix z(2,1); z << z3(1,1) << z3(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, 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(2,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,20) = 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(2,1); z << zmag(1,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::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(); } 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 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 i = 1; i < 4; 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*magm; float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1)); for(int j = 1; 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); } 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); twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx)); errState = K * z; Phat = (MatrixMath::Eye(nState)-K*H)*Phat; fuseErr2Nominal(); } */