Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
ScErrStateEKF.cpp
- Committer:
- NaotoMorita
- Date:
- 2021-09-29
- Revision:
- 32:321a756e12ad
- Parent:
- 31:e655d4d8e4d6
- Child:
- 34:dec4b37db3f1
- Child:
- 36:b1d107b00351
- Child:
- 37:7873fe37b425
File content as of revision 32:321a756e12ad:
#include "ScErrStateEKF.hpp" #include "Matrix.h" #include "MatrixMath.h" #include <cmath> #include "Vector3.hpp" using namespace std; ScErrStateEKF::ScErrStateEKF() :qhat(4,1),vihat(3,1), errState(9,1), bgState(3,1), Phat(9,9), Q(9,9),Phatbg(3,3),Qbg(3,3), Ra(3,3), Rgps(3,3),Rsr(1,1), Rm(3,3), Qab(3,3),Rgsc(2,2),Rvsc(2,2),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f) { nState = errState.getRows(); qhat << 1.0f << 0.0f << 0.0f << 0.0f; setDiag(Phat,0.1f); setDiag(Phatbg,0.1f); setQqerr(0.000005f); setQgbias(0.001f); setQabias(0.00001f); setQv(0.0001f); //加速度の観測 setDiag(Ra,0.005f); setDiag(Qab,10.0f); //ジャイロバイアスに関する制約 setDiag(Rgsc,500.0f); setDiag(Rm,5.0f); setDiag(Rgps,5.5f); setDiag(Rsr,5.5f); for(int i = 0; i<10;i++){ histffunc[i] = 0.0f; } histffuncindex = 0 ; sigma2a = 0.000020f; } void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt) { gyro -= gyroBias; acc -= accBias; Matrix A(4,4); A << 0.0f << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z << 0.5f*gyro.x << 0.0f << 0.5f*gyro.z <<-0.5f*gyro.y << 0.5f*gyro.y << -0.5f*gyro.z << 0.0f << 0.5f*gyro.x << 0.5f*gyro.z << 0.5f*gyro.y <<-0.5f*gyro.x << 0.0f ; Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A; qhat = phi * qhat; float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); qhat *= (1.0f/ qnorm); Matrix dcm(3,3); computeDcm(dcm, qhat); vihat += (MatrixMath::Transpose(dcm)*MatrixMath::Vector2mat(acc)-MatrixMath::Vector2mat(accref))*att_dt*9.8f; } void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt) { gyro -= gyroBias; acc -= accBias; Matrix A(nState,nState); A(1,2) = gyro.z; A(1,3) = -gyro.y; A(2,1) = -gyro.z; A(2,3) = gyro.x; A(3,1) = gyro.y; A(3,2) = -gyro.x; Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix qeterm = -2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z); Matrix baterm = -9.8f*MatrixMath::Transpose(dcm); for (int i = 1; i < 4; i++){ for (int j = 1; i < 4; i++){ A(i+6,j) = qeterm(i,j); A(i+6,j+3) = baterm(i,j); } } Matrix phi = 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; errState = phi * errState; Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd; } void ScErrStateEKF::setQqerr(float val){ Q(1,1) = val; Q(2,2) = val; Q(3,3) = val; } void ScErrStateEKF::setQgbias(float val){ Qbg(1,1) = val; Qbg(2,2) = val; Qbg(2,2) = val; } void ScErrStateEKF::setQabias(float val){ Q(4,4) = val; Q(5,5) = val; Q(6,6) = val; } void ScErrStateEKF::setQv(float val){ Q(7,7) = val; Q(8,8) = val; Q(9,9) = val; } void ScErrStateEKF::setQab(float val){ setDiag(Qab,val); } void ScErrStateEKF::setRsoftconst(float Vgsc,float Vvsc){ setDiag(Rgsc,Vgsc); setDiag(Rvsc,Vvsc); } void ScErrStateEKF::setDiag(Matrix& mat, float val){ for (int i = 1; i < mat.getCols()+1; i++){ mat(i,i) = val; } } void ScErrStateEKF::getQhat(float (&res)[4]){ for (int i = 0; i < 4; i++){ res[i] = qhat(i+1,1); } } void ScErrStateEKF::getVihat(float (&res)[3]){ for (int i = 0; i < 3; i++){ res[i] = vihat(i+1,1); } } void ScErrStateEKF::getGyroBias(float (&resVal)[3], float (&resCov)[6]){ resVal[0] = gyroBias.x; resVal[1] = gyroBias.y; resVal[2] = gyroBias.z; resCov[0] = Phatbg(1,1); resCov[1] = Phatbg(1,2); resCov[2] = Phatbg(1,3); resCov[3] = Phatbg(2,2); resCov[4] = Phatbg(2,3); resCov[5] = Phatbg(3,3); } void ScErrStateEKF::getAccBias(float (&resVal)[3], float (&resCov)[6]){ resVal[0] = accBias.x; resVal[1] = accBias.y; resVal[2] = accBias.z; resCov[0] = Phat(4,4); resCov[1] = Phat(4,5); resCov[2] = Phat(4,6); resCov[3] = Phat(5,5); resCov[4] = Phat(5,6); resCov[5] = Phat(6,6); } void ScErrStateEKF::updateAccMeasures(Vector3 acc, Vector3 accref) { acc -= accBias; Matrix dcm(3,3); computeDcm(dcm, qhat); Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z); Matrix H(3,nState); H(1,2) = -2.0f*gvec.z; H(1,3) = 2.0f*gvec.y; H(2,1) = 2.0f*gvec.z; H(2,3) = -2.0f*gvec.x; H(3,1) = -2.0f*gvec.y; H(3,2) = 2.0f*gvec.x; H(1,4) = 1.0f; H(2,5) = 1.0f; H(3,6) = 1.0f; Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra+Qab); Matrix z = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref); Matrix corrVal = K * (z-H*errState); errState = errState + corrVal; Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K); } void ScErrStateEKF::updateGyroBiasConstraints(Vector3 gyro) { gyro -= gyroBias; Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix H(2,3); H(1,1) = 1.0f*(dcm(1,1)); H(1,2) = 1.0f*(dcm(2,1)); H(1,3) = 1.0f*(dcm(3,1)); H(2,1) = 1.0f*(dcm(1,2)); H(2,2) = 1.0f*(dcm(2,2)); H(2,3) = 1.0f*(dcm(3,2)); Matrix K = (Phatbg*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phatbg*MatrixMath::Transpose(H)+Rgsc); Matrix z(2,1); z <<dcm(1,1)*gyro.x+dcm(2,1)*gyro.y+dcm(3,1)*gyro.z<< dcm(1,2)*gyro.x+dcm(2,2)*gyro.y+dcm(3,2)*gyro.z; Matrix corrVal = K * (z-H*bgState); bgState = bgState + corrVal; Phatbg = (MatrixMath::Eye(3)-K*H)*Phatbg*MatrixMath::Transpose(MatrixMath::Eye(3)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K); } void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref) { Matrix dcm(3,3); computeDcm(dcm, qhat); Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z); Matrix H(3,nState); H(1,2) = -2.0f*gvec.z; H(1,3) = 2.0f*gvec.y; H(2,1) = 2.0f*gvec.z; H(2,3) = -2.0f*gvec.x; H(3,1) = -2.0f*gvec.y; H(3,2) = 2.0f*gvec.x; Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra*100.0f); Matrix accmat(3,1); accmat << acc.x << acc.y << acc.z; Matrix gref(3,1); gref << 0.0f << 0.0f << accref.z; Matrix z = accmat-dcm*gref; errState = errState + K * (z-H*errState); Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K); } void ScErrStateEKF::updateGPSVelocity(float vi_x, float vi_y, float sinkRate,Vector3 acc, Vector3 accref) { acc -= accBias; Matrix dcm(3,3); computeDcm(dcm, qhat); Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z); Matrix H(6,nState); H(1,2) = -2.0f*gvec.z; H(1,3) = 2.0f*gvec.y; H(2,1) = 2.0f*gvec.z; H(2,3) = -2.0f*gvec.x; H(3,1) = -2.0f*gvec.y; H(3,2) = 2.0f*gvec.x; H(1,4) = 1.0f; H(2,5) = 1.0f; H(3,6) = 1.0f; H(4,7) = 1.0f; H(5,8) = 1.0f; H(6,9) = 1.0f; Matrix R(6,6); R(1,1) = Ra(1,1)+Qab(1,1); R(2,2) = Ra(2,2)+Qab(2,2); R(3,3) = Ra(3,3)+Qab(3,3); R(4,4) = Rgps(1,1); R(5,5) = Rgps(2,2); R(6,6) = Rsr(1,1); Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref); Matrix z(6,1); z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);; Matrix corrVal = K * (z-H*errState); errState = errState + corrVal; Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K); } void ScErrStateEKF::updateSinkRate(float sinkRate,Vector3 acc, Vector3 accref) { acc -= accBias; Matrix dcm(3,3); computeDcm(dcm, qhat); Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z); Matrix H(4,nState); H(1,2) = -2.0f*gvec.z; H(1,3) = 2.0f*gvec.y; H(2,1) = 2.0f*gvec.z; H(2,3) = -2.0f*gvec.x; H(3,1) = -2.0f*gvec.y; H(3,2) = 2.0f*gvec.x; H(1,4) = 1.0f; H(2,5) = 1.0f; H(3,6) = 1.0f; H(4,9) = 1.0f; Matrix R(4,4); R(1,1) = Ra(1,1)+Qab(1,1); R(2,2) = Ra(2,2)+Qab(2,2); R(3,3) = Ra(3,3)+Qab(3,3); R(4,4) = Rsr(1,1); Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref); Matrix z(4,1); z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << sinkRate - vihat(3,1); Matrix corrVal = K * (z-H*errState); errState = errState + corrVal; Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K); } void ScErrStateEKF::updateCenteredGyroBiasCorrection(float (&cBg)[3]) { Matrix RgyroBias(3,3); for(int i = 1;i<4;i++){ RgyroBias(i,i) = 0.1f; } Matrix H(3,3); H(1,1) = 1.0f; H(2,2) = 1.0f; H(3,3) = 1.0f; Matrix K = (Phatbg*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phatbg*MatrixMath::Transpose(H)+RgyroBias); Matrix z(3,1); z <<cBg[0]-gyroBias.x <<cBg[1]-gyroBias.y <<cBg[2]-gyroBias.z; Matrix corrVal = K * (z-H*bgState); bgState = bgState + corrVal; Phatbg = (MatrixMath::Eye(3)-K*H)*Phatbg*MatrixMath::Transpose(MatrixMath::Eye(3)-K*H)+K*(RgyroBias)*MatrixMath::Transpose(K); } void ScErrStateEKF::resetBias() { gyroBias.x = gyroBias.x + bgState(1,1)*1.0f; gyroBias.y = gyroBias.y + bgState(2,1)*1.0f; gyroBias.z = gyroBias.z + bgState(3,1)*1.0f; bgState(1,1) = 0.0f; bgState(2,1) = 0.0f; bgState(3,1) = 0.0f; accBias.x = accBias.x + errState(4,1); accBias.y = accBias.y + errState(5,1); accBias.z = accBias.z + errState(6,1); errState(4,1) = 0.0f; errState(5,1) = 0.0f; errState(6,1) = 0.0f; } void ScErrStateEKF::computeAngles(Vector3& rpy,Vector3 rpy_align) { Matrix qerr(4,1); qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1); //float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qerr),qerr)); //qerr *= (1.0f/ qnorm); Matrix qest = quatmultiply(qhat, qerr); float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qest),qest)); qest *= (1.0f/ qnorm); float q0 = qest( 1, 1 ); float q1 = qest( 2, 1 ); float q2 = qest( 3, 1 ); float q3 = qest( 4, 1 ); rpy.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x; rpy.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y; rpy.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); } void ScErrStateEKF::fuseErr2Nominal() { Matrix qerr(4,1); qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1); //float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qerr),qerr)); //qerr *= (1.0f/ qnorm); qhat = quatmultiply(qhat, qerr); errState(1,1) = 0.0f; errState(2,1) = 0.0f; errState(3,1) = 0.0f; float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); qhat *= (1.0f/ qnorm); vihat(1,1) += errState(7,1); vihat(2,1) += errState(8,1); vihat(3,1) += errState(9,1); errState(7,1) = 0.0f; errState(8,1) = 0.0f; errState(9,1) = 0.0f; } Matrix ScErrStateEKF::quatmultiply(Matrix q, Matrix r) { Matrix qout(4,1); qout(1,1) = q(1,1)*r(1,1) - q(2,1)*r(2,1) - q(3,1)*r(3,1) - q(4,1)*r(4,1); qout(2,1) = q(1,1)*r(2,1) + r(1,1)*q(2,1) + q(3,1)*r(4,1)-q(4,1)*r(3,1); qout(3,1) = q(1,1)*r(3,1) + r(1,1)*q(3,1) + q(4,1)*r(2,1)-q(2,1)*r(4,1); qout(4,1) = q(1,1)*r(4,1) + r(1,1)*q(4,1) + q(2,1)*r(3,1)-q(3,1)*r(2,1); float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout)); qout *= (1.0f/ qnorm); return qout; } void ScErrStateEKF::computeDcm(Matrix& dcm, Matrix quat) { float q0 = quat( 1, 1 ); float q1 = quat( 2, 1 ); float q2 = quat( 3, 1 ); float q3 = quat( 4, 1 ); dcm(1,1) = q0*q0 + q1*q1 - q2*q2 - q3*q3; dcm(1,2) = 2*(q1*q2 + q0*q3); dcm(1,3) = 2*(q1*q3 - q0*q2); dcm(2,1) = 2*(q1*q2 - q0*q3); dcm(2,2) = q0*q0 - q1*q1 + q2*q2 - q3*q3; dcm(2,3) = 2*(q2*q3 + q0*q1); dcm(3,1) = 2*(q1*q3 + q0*q2); dcm(3,2) = 2*(q2*q3 - q0*q1); dcm(3,3) = q0*q0 - q1*q1 - q2*q2 + q3*q3; } void ScErrStateEKF::defineQhat(Vector3 align){ float cos_z_2 = cosf(0.5f*align.z); float cos_y_2 = cosf(0.5f*align.y); float cos_x_2 = cosf(0.5f*align.x); float sin_z_2 = sinf(0.5f*align.z); float sin_y_2 = sinf(0.5f*align.y); float sin_x_2 = sinf(0.5f*align.x); // 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 ScErrStateEKF::computeVb(Vector3& vb) { Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix vbmat = dcm*vihat; vb.x = vbmat(1,1); vb.y = vbmat(2,1); vb.z = vbmat(3,1); } Vector3 ScErrStateEKF::calcDynAcc(Vector3 acc, Vector3 accref) { Matrix dcm(3,3); computeDcm(dcm, qhat); float _x, _y, _z; _x = acc.x-(dcm( 1, 1 )*accref.x+dcm( 1, 2 )*accref.y+dcm( 1, 3 )*accref.z); _y = acc.y-(dcm( 2, 1 )*accref.x+dcm( 2, 2 )*accref.y+dcm( 2, 3 )*accref.z); _z = acc.z-(dcm( 3, 1 )*accref.x+dcm( 3, 2 )*accref.y+dcm( 3, 3 )*accref.z); return Vector3(_x, _y, _z); } bool ScErrStateEKF::determinDynStatus(Vector3 acc, Vector3 accref) { histffunc[histffuncindex] = acc.Norm()*acc.Norm()-accref.Norm()*accref.Norm()-3.0f*sigma2a; if(histffuncindex<9){ histffuncindex += 1; }else{ histffuncindex =0; } aveffunc = 0; for(int i = 1;i<10;i++){ aveffunc += 1.0f/10.0f*histffunc[i]; } sigma2f = 1.0f/10.0f*(6.0f*sigma2a*sigma2a+4.0f*accref.Norm()*accref.Norm()*sigma2a); dynacc = calcDynAcc(acc,accref); bool dynCase = true; if((dynacc.Norm()<0.005f) && (abs(acc.Norm()-accref.Norm())<0.005f)){dynCase = false;} if(aveffunc<sqrt(sigma2f/0.99f) && abs(acc.Norm()-accref.Norm())<0.001f){dynCase = false;} return dynCase; } /* void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt) { gyro -= gyroBias; acc -= accBias; Matrix A(4,4); A << 0.0f << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z << 0.5f*gyro.x << 0.0f << 0.5f*gyro.z <<-0.5f*gyro.y << 0.5f*gyro.y << -0.5f*gyro.z << 0.0f << 0.5f*gyro.x << 0.5f*gyro.z << 0.5f*gyro.y <<-0.5f*gyro.x << 0.0f ; Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A; qhat = phi * qhat; float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); qhat *= (1.0f/ qnorm); Matrix dcm(3,3); computeDcm(dcm, qhat); vihat += (MatrixMath::Transpose(dcm)*MatrixMath::Vector2mat(acc)-MatrixMath::Vector2mat(accref))*att_dt*9.8f; } void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt) { gyro -= gyroBias; acc -= accBias; Matrix A(nState,nState); A(1,2) = gyro.z; A(1,3) = -gyro.y; A(2,1) = -gyro.z; A(2,3) = gyro.x; A(3,1) = gyro.y; A(3,2) = -gyro.x; A(1,4) = -0.5f; A(2,5) = -0.5f; A(3,6) = -0.5f; Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix qeterm = -2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z); Matrix baterm = -9.8f*MatrixMath::Transpose(dcm); for (int i = 1; i < 4; i++){ for (int j = 1; i < 4; i++){ A(i+9,j) = qeterm(i,j); A(i+9,j+6) = baterm(i,j); } } Matrix phi = 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; errState = phi * errState; Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd; } void ScErrStateEKF::updateVelocityConstraints() { Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix H(2,nState); Matrix nomVb = dcm*vihat; Matrix qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1)); for(int j = 1;j<4;j++){ H(1,j) = -qeterm(1,j); H(2,j) = -qeterm(3,j); //H(3,j) = -qeterm(3,j); H(1,9+j) = -dcm(1,j); H(2,9+j) = -dcm(3,j); //H(3,9+j) = -dcm(3,j); } Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc); Matrix z(2,1); z << nomVb(1,1) << nomVb(3,1) ; Matrix corrVal = K * (z-H*errState); errState = errState + corrVal; Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K); } void ScErrStateEKF::updateConstraints(Vector3 gyro) { gyro -=gyroBias; Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z); Matrix H(4,nState); for(int i = 1; i<3; i++){ for(int j = 1;j<4;j++){ H(i,j) = qeterm(i,j); } } H(1,4) = 1.0f*(dcm(1,1)); H(1,5) = 1.0f*(dcm(2,1)); H(1,6) = 1.0f*(dcm(3,1)); H(2,4) = 1.0f*(dcm(1,2)); H(2,5) = 1.0f*(dcm(2,2)); H(2,6) = 1.0f*(dcm(3,2)); Matrix nomVb = dcm*vihat; qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1)); for(int j = 1;j<4;j++){ H(3,j) = -qeterm(1,j); H(4,j) = -qeterm(3,j); //H(3,j) = -qeterm(3,j); H(3,9+j) = -dcm(1,j); H(4,9+j) = -dcm(3,j); //H(3,9+j) = -dcm(3,j); } Matrix R(4,4); R(1,1) = Rgsc(1,1); R(2,2) = Rgsc(2,1); R(3,3) = Rvsc(1,1); R(4,4) = Rvsc(2,1); //R(5,5) = Rvsc(3,1); Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); Matrix z(4,1); z <<dcm(1,1)*gyro.x+dcm(2,1)*gyro.y+dcm(3,1)*gyro.z<< dcm(1,2)*gyro.x+dcm(2,2)*gyro.y+dcm(3,2)*gyro.z << nomVb(1,1) << nomVb(3,1) ; Matrix corrVal = K * (z-H*errState); errState = errState + corrVal; Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K); } void ScErrStateEKF::computeVb(Vector3& vb) { Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix vbmat = dcm*vihat; vb.x = vbmat(1,1); vb.y = vbmat(2,1); vb.z = vbmat(3,1); } void ScErrStateEKF::updateMagMeasures(Vector3 mag) { //mag = mag/mag.Norm(); Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix magvec(3,1); magvec(1,1) = mag.x; magvec(2,1) = mag.y; magvec(3,1) = mag.z; Matrix magnedvec = MatrixMath::Transpose(dcm)*magvec; Matrix magrefmod(3,1); magrefmod(1,1) = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1)); magrefmod(2,1) = 0.0f; magrefmod(3,1) = magnedvec(3,1); Matrix mvec = dcm*magrefmod ; Matrix H(3,nState); H(1,2) = -2.0f*magvec(3,1); H(1,3) = 2.0f*magvec(2,1); H(2,1) = 2.0f*magvec(3,1); H(2,3) = -2.0f*magvec(1,1); H(3,1) = -2.0f*magvec(2,1); H(3,2) = 2.0f*magvec(1,1); Matrix Pm(nState,nState); for(int i = 1; i<4; i++){ for(int j = 1;j<4;j++){ Pm(i,j) = Phat(i,j); } } Matrix r3(3,1); r3 << dcm(1,3)<< dcm(2,3) << dcm(3,3); Matrix kpart = r3*MatrixMath::Transpose(r3); Matrix Kmod(nState,nState); for(int i = 1; i<4; i++){ for(int j = 1;j<4;j++){ Kmod(i,j) = kpart(i,j); } } Matrix K = Kmod*(Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rm); Matrix z = magvec-mvec; errState = errState + K * (z-H*errState); Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rm)*MatrixMath::Transpose(K); } void ScErrStateEKF::updateVelocityConstraints() { Matrix dcm(3,3); computeDcm(dcm, qhat); Matrix H(2,nState); Matrix nomVb = dcm*vihat; Matrix qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1)); for(int j = 1;j<4;j++){ H(1,j) = -qeterm(1,j); H(2,j) = -qeterm(3,j); //H(3,j) = -qeterm(3,j); H(1,9+j) = -dcm(1,j); H(2,9+j) = -dcm(3,j); //H(3,9+j) = -dcm(3,j); } Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc); Matrix z(2,1); z << nomVb(1,1) << nomVb(3,1) ; Matrix corrVal = K * (z-H*errState); errState = errState + corrVal; Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K); } */