Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: ScErrStateEKF.cpp
- Revision:
- 19:3fae66745363
- Child:
- 20:37d3c3ee36e9
diff -r 03dd0eabe6b8 -r 3fae66745363 ScErrStateEKF.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ScErrStateEKF.cpp Fri Jul 23 05:55:28 2021 +0000 @@ -0,0 +1,392 @@ +#include "ScErrStateEKF.hpp" +#include "Matrix.h" +#include "MatrixMath.h" +#include <cmath> +#include "Vector3.hpp" + + +using namespace std; + +ScErrStateEKF::ScErrStateEKF() + :qhat(4,1), errState(12,1), Phat(12,12), Q(12,12), Ra(5,5), Rm(3,3), Qab(5,5),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f) +{ + qhat << 1.0f << 0.0f << 0.0f << 0.0f; + errState << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f; + + Phat(1,1) = 0.1f; + Phat(2,2) = 0.1f; + Phat(3,3) = 0.1f; + Phat(4,4) = 0.1f; + Phat(5,5) = 0.1f; + Phat(6,6) = 0.1f; + Phat(7,7) = 0.1f; + Phat(8,8) = 0.1f; + Phat(9,9) = 0.1f; + Phat(10,10) = 0.1f; + Phat(11,11) = 0.1f; + Phat(12,12) = 0.1f; + + Q(1,1) = 0.000005f*0.25f; + Q(2,2) = 0.000005f*0.25f; + Q(3,3) = 0.000005f*0.25f; + Q(4,4) = 0.0005f; + Q(5,5) = 0.0005f; + Q(6,6) = 0.0005f; + Q(7,7) = 100.0f; + Q(8,8) = 100.0f; + Q(9,9) = 100.0f; + Q(10,10) = 100.0f; + Q(11,11) = 100.0f; + Q(12,12) = 100.0f; + + Ra(1,1) = 0.000020f; + Ra(2,2) = 0.000020f; + Ra(3,3) = 0.000020f; + Ra(4,4) = 10.0f; + Ra(5,5) = 10.0f; + + Rm(1,1) = 5.0f; + Rm(2,2) = 5.0f; + Rm(3,3) = 5.0f; + + Qab(1,1) = 100.7f; + Qab(2,2) = 100.7f; + Qab(3,3) = 100.7f; + Qab(4,4) = 0.0f; + Qab(5,5) = 0.0f; + + for(int i = 0; i<10;i++){ + histffunc[i] = 0.0f; + } + histffuncindex = 0 ; + sigma2a = 0.000020f; + + +} + +void ScErrStateEKF::updateQhat(Vector3 gyro, float att_dt) +{ + gyro -= gyroBias; + 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 + (1.0f/6.0f*att_dt*att_dt*att_dt) * A * A * A; + qhat = phi * qhat; + float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); + qhat *= (1.0f/ qnorm); +} + +void ScErrStateEKF::updateErrState(Vector3 gyro, float att_dt) +{ + gyro -= gyroBias; + Matrix A(12,12); + 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; + A(10,7) = 1.0f; + A(11,8) = 1.0f; + A(12,9) = 1.0f; + + Matrix phi = MatrixMath::Eye(12) + A * att_dt + (0.5f*att_dt*att_dt) * A * A + (1.0f/6.0f*att_dt*att_dt*att_dt) * A * 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::updateAccMeasures(Vector3 acc, Vector3 accref) +{ + //acc -= accBias; + //acc = acc/acc.Norm(); + //accref = accref/accref.Norm(); + + 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(5,12); + 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,7) = 1.0f; + H(2,8) = 1.0f; + H(3,9) = 1.0f; + H(4,10) = 1.0f; + H(5,12) = 1.0f; + Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra+Qab); + Matrix accmat(3,1); + accmat << acc.x << acc.y << acc.z; + Matrix gref(3,1); + gref << 0.0f << 0.0f << accref.z; + Matrix zacc = accmat-dcm*gref; + Matrix z(5,1); + z << zacc(1,1) << zacc(2,1) << zacc(3,1) << 0.0f << 0.0f; + Matrix corrVal = K * (z-H*errState); + errState = errState + corrVal; + Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K); +} +void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref) +{ + //acc = acc/acc.Norm(); + //accref = accref/accref.Norm(); + + 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,9); + 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(9)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(9)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K); +} + + + +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,9); + 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(9,9); + 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(9,9); + 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(9)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(9)-K*H)+K*(Rm)*MatrixMath::Transpose(K); +} + +void ScErrStateEKF::resetBias() +{ + gyroBias.x = gyroBias.x + errState(4,1)*1.0f; + gyroBias.y = gyroBias.y + errState(5,1)*1.0f; + gyroBias.z = gyroBias.z + errState(6,1)*1.0f; + errState(4,1) = 0.0f; + errState(5,1) = 0.0f; + errState(6,1) = 0.0f; + //accBias.x = accBias.x + errState(7,1); + //accBias.y = accBias.y + errState(8,1); + //accBias.z = accBias.z + errState(9,1); + //errState(7,1) = 0.0f; + //errState(8,1) = 0.0f; + //errState(9,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::fuseErr2Qhat() +{ + 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); +} + +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::triad(Vector3 fb, Vector3 fn, Vector3 mb, Vector3 mn){ + Matrix W1(3,1); + W1 << fb.x << fb.y << fb.z; + Matrix W2(3,1); + W2 << mb.x << mb.y << mb.z; + + Matrix V1(3,1); + V1 << fn.x << fn.y << fn.z; + Matrix V2(3,1); + V2 << mn.x << mn.y << mn.z; + + + Matrix Ou2(3,1); + Ou2 << W1( 2, 1 )*W2( 3, 1 )-W1( 3, 1 )*W2( 2, 1 ) << W1( 3, 1 )*W2( 1, 1 )-W1( 1, 1 )*W2( 3, 1 ) << W1( 1, 1 )*W2( 2, 1 )-W1( 2, 1 )*W2( 1, 1 ); + Ou2 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2)); + Matrix Ou3(3,1); + Ou3 << W1( 2, 1 )*Ou2( 3, 1 )-W1( 3, 1 )*Ou2( 2, 1 ) << W1( 3, 1 )*Ou2( 1, 1 )-W1( 1, 1 )*Ou2( 3, 1 ) << W1( 1, 1 )*Ou2( 2, 1 )-W1( 2, 1 )*Ou2( 1, 1 ); + Ou3 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3)); + Matrix R2(3,1); + R2 << V1( 2, 1 )*V2( 3, 1 )-V1( 3, 1 )*V2( 2, 1 ) << V1( 3, 1 )*V2( 1, 1 )-V1( 1, 1 )*V2( 3, 1 ) << V1( 1, 1 )*V2( 2, 1 )-V1( 2, 1 )*V2( 1, 1 ); + R2 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2)); + Matrix R3(3,1); + R3 << V1( 2, 1 )*R2( 3, 1 )-V1( 3, 1 )*R2( 2, 1 ) << V1( 3, 1 )*R2( 1, 1 )-V1( 1, 1 )*R2( 3, 1 ) << V1( 1, 1 )*R2( 2, 1 )-V1( 2, 1 )*R2( 1, 1 ); + R3 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3)); + + Matrix Mou(3,3); + Mou << W1( 1, 1 ) << Ou2( 1, 1 ) << Ou3( 1, 1 ) + << W1( 2, 1 ) << Ou2( 2, 1 ) << Ou3( 2, 1 ) + << W1( 3, 1 ) << Ou2( 3, 1 ) << Ou3( 3, 1 ); + Matrix Mr(3,3); + Mr << V1( 1, 1 ) << R2( 1, 1 ) << R3( 1, 1 ) + << V1( 2, 1 ) << R2( 2, 1 ) << R3( 2, 1 ) + << V1( 3, 1 ) << R2( 3, 1 ) << R3( 3, 1 ); + + Matrix Cbn = Mr*MatrixMath::Transpose(Mou); + + float sqtrp1 = sqrt(1.0f+Cbn( 1, 1 )+Cbn( 2, 2 )+Cbn( 3, 3 )); + + qhat(1,1) = 0.5f*sqtrp1; + qhat(2,1) = -(Cbn( 2, 3 )-Cbn( 3, 2 ))/2.0f/sqtrp1; + qhat(3,1) = -(Cbn( 3, 1 )-Cbn( 1, 3 ))/2.0f/sqtrp1; + qhat(4,1) = -(Cbn( 1, 2 )-Cbn( 2, 1 ))/2.0f/sqtrp1; + + float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); + qhat *= (1.0f/ qnorm); +} + + +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; + +} + +