EKF class
Dependencies: MatrixMath Vector3
Diff: HAPS_EKF.cpp
- Revision:
- 0:de56252b419e
- Child:
- 2:771eed5f655a
diff -r 000000000000 -r de56252b419e HAPS_EKF.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HAPS_EKF.cpp Mon May 31 07:10:39 2021 +0000 @@ -0,0 +1,254 @@ +#include "HAPS_EKF.hpp" +#include "Matrix.h" +#include "MatrixMath.h" +#include <cmath> +#include "Vector3.hpp" + + +using namespace std; + +HAPS_EKF::HAPS_EKF() + :qhat(4,1), qhat_gyro(4,1), Phat(4,4), Qgyro(3,3), Racc(3,3), Rmag(3,3), D(3,3) +{ + qhat << 1 << 0 << 0 << 0; + + Phat.add(1,1,0.05); + Phat.add(2,2,0.05); + Phat.add(3,3,0.05); + Phat.add(4,4,0.05); + + D.add(1,1,1.0); + D.add(2,2,1.0); + D.add(3,3,1.0); + + Qgyro.add(1,1,0.0224); + Qgyro.add(2,2,0.0224); + Qgyro.add(3,3,0.0224); + + Racc.add(1,1,0.0330*200); + Racc.add(2,2,0.0330*200); + Racc.add(3,3,0.0330*200); + + Rmag.add(1,1,1.0); + Rmag.add(2,2,1.0); + Rmag.add(3,3,1.0); +} + +void HAPS_EKF::updateBetweenMeasures(Vector3 gyro, float att_dt) +{ + float q0 = qhat.getNumber( 1, 1 ); + float q1 = qhat.getNumber( 2, 1 ); + float q2 = qhat.getNumber( 3, 1 ); + float q3 = qhat.getNumber( 4, 1 ); + + Matrix B(4,3); + B << q1 << q2 << q3 + <<-q0 << q3 <<-q2 + <<-q3 <<-q0 << q1 + << q2 <<-q1 <<-q0; + B *= 0.5f; + + Matrix phi(4,4); + phi << 1.0 << -gyro.x*0.5*att_dt <<-gyro.y*0.5*att_dt <<-gyro.z*0.5*att_dt + << gyro.x*0.5*att_dt << 1.0 << gyro.z*0.5*att_dt <<-gyro.y*0.5*att_dt + << gyro.y*0.5*att_dt << -gyro.z*0.5*att_dt << 1.0 << gyro.x*0.5*att_dt + << gyro.z*0.5*att_dt << gyro.y*0.5*att_dt <<-gyro.x*0.5*att_dt << 1.0; + + qhat = phi*qhat; + float qnorm; + qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); + qhat *= (1.0f/ qnorm); + + qhat_gyro = phi*qhat_gyro; + qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat_gyro),qhat_gyro)); + qhat_gyro *= (1.0f/ qnorm); + + Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B); + + q0 = qhat.getNumber( 1, 1 ); + q1 = qhat.getNumber( 2, 1 ); + q2 = qhat.getNumber( 3, 1 ); + q3 = qhat.getNumber( 4, 1 ); + + D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3); + D.add(1,2,2*(q1*q2 + q0*q3)); + D.add(1,3,2*(q1*q3 - q0*q2)); + D.add(2,1,2*(q1*q2 - q0*q3)); + D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3); + D.add(2,3,2*(q2*q3 + q0*q1)); + D.add(3,1,2*(q1*q3 + q0*q2)); + D.add(3,2,2*(q2*q3 - q0*q1)); + D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3); +} + +void HAPS_EKF::updateAcrossMeasures(Vector3 _v, Vector3 _u, Matrix& R) +{ + Matrix u(3,1); + Matrix v(3,1); + + u << _u.x << _u.y << _u.z; + v << _v.x << _v.y << _v.z; + + float q0 = qhat.getNumber( 1, 1 ); + float q1 = qhat.getNumber( 2, 1 ); + float q2 = qhat.getNumber( 3, 1 ); + float q3 = qhat.getNumber( 4, 1 ); + + Matrix A1(3,3); + A1 << q0 << q3 << -q2 + <<-q3 << q0 << q1 + <<q2 <<-q1 <<q0; + A1 *= 2.0f; + + Matrix A2(3,3); + A2 << q1 << q2 << q3 + << q2 <<-q1 << q0 + << q3 <<-q0 <<-q1; + A2 *= 2.0f; + + Matrix A3(3,3); + A3 <<-q2 << q1 <<-q0 + << q1 << q2 << q3 + << q0 << q3 <<-q2; + A3 *= 2.0f; + + Matrix A4(3,3); + A4 <<-q3 << q0 << q1 + <<-q0 <<-q3 << q2 + << q1 << q2 << q3; + A4 *= 2.0f; + + Matrix H(3,4); + + Matrix ab1(A1*u); + Matrix ab2(A2*u); + Matrix ab3(A3*u); + Matrix ab4(A4*u); + + H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 ) + << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 ) + << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 ); + + + Matrix K(4,3); + K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + + Matrix dq(4,1); + dq = K*(v-D*u); + qhat = qhat+dq; + + float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); + qhat *= (1.0f/ qnorm); + + Matrix eye4(4,4); + eye4 << 1 << 0 << 0 << 0 + << 0 << 1 << 0 << 0 + << 0 << 0 << 1 << 0 + << 0 << 0 << 0 << 1; + Phat = (eye4-K*H)*Phat*MatrixMath::Transpose(eye4-K*H)+K*R*MatrixMath::Transpose(K); +} + +void HAPS_EKF::computeAngles(Vector3& rot, Vector3& rot_g, Vector3 rot_align) +{ + float q0 = qhat.getNumber( 1, 1 ); + float q1 = qhat.getNumber( 2, 1 ); + float q2 = qhat.getNumber( 3, 1 ); + float q3 = qhat.getNumber( 4, 1 ); + rot.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rot_align.x; + rot.y = asinf(-2.0f * (q1*q3 - q0*q2))-rot_align.y; + rot.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); + + q0 = qhat_gyro.getNumber( 1, 1 ); + q1 = qhat_gyro.getNumber( 2, 1 ); + q2 = qhat_gyro.getNumber( 3, 1 ); + q3 = qhat_gyro.getNumber( 4, 1 ); + rot_g.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rot_align.x; + rot_g.y = asinf(-2.0f * (q1*q3 - q0*q2))-rot_align.y; + rot_g.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); + +} + +void HAPS_EKF::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.getNumber( 2, 1 )*W2.getNumber( 3, 1 )-W1.getNumber( 3, 1 )*W2.getNumber( 2, 1 ) << W1.getNumber( 3, 1 )*W2.getNumber( 1, 1 )-W1.getNumber( 1, 1 )*W2.getNumber( 3, 1 ) << W1.getNumber( 1, 1 )*W2.getNumber( 2, 1 )-W1.getNumber( 2, 1 )*W2.getNumber( 1, 1 ); + Ou2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2)); + Matrix Ou3(3,1); + Ou3 << W1.getNumber( 2, 1 )*Ou2.getNumber( 3, 1 )-W1.getNumber( 3, 1 )*Ou2.getNumber( 2, 1 ) << W1.getNumber( 3, 1 )*Ou2.getNumber( 1, 1 )-W1.getNumber( 1, 1 )*Ou2.getNumber( 3, 1 ) << W1.getNumber( 1, 1 )*Ou2.getNumber( 2, 1 )-W1.getNumber( 2, 1 )*Ou2.getNumber( 1, 1 ); + Ou3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3)); + Matrix R2(3,1); + R2 << V1.getNumber( 2, 1 )*V2.getNumber( 3, 1 )-V1.getNumber( 3, 1 )*V2.getNumber( 2, 1 ) << V1.getNumber( 3, 1 )*V2.getNumber( 1, 1 )-V1.getNumber( 1, 1 )*V2.getNumber( 3, 1 ) << V1.getNumber( 1, 1 )*V2.getNumber( 2, 1 )-V1.getNumber( 2, 1 )*V2.getNumber( 1, 1 ); + R2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2)); + Matrix R3(3,1); + R3 << V1.getNumber( 2, 1 )*R2.getNumber( 3, 1 )-V1.getNumber( 3, 1 )*R2.getNumber( 2, 1 ) << V1.getNumber( 3, 1 )*R2.getNumber( 1, 1 )-V1.getNumber( 1, 1 )*R2.getNumber( 3, 1 ) << V1.getNumber( 1, 1 )*R2.getNumber( 2, 1 )-V1.getNumber( 2, 1 )*R2.getNumber( 1, 1 ); + R3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3)); + + Matrix Mou(3,3); + Mou << W1.getNumber( 1, 1 ) << Ou2.getNumber( 1, 1 ) << Ou3.getNumber( 1, 1 ) + << W1.getNumber( 2, 1 ) << Ou2.getNumber( 2, 1 ) << Ou3.getNumber( 2, 1 ) + << W1.getNumber( 3, 1 ) << Ou2.getNumber( 3, 1 ) << Ou3.getNumber( 3, 1 ); + Matrix Mr(3,3); + Mr << V1.getNumber( 1, 1 ) << R2.getNumber( 1, 1 ) << R3.getNumber( 1, 1 ) + << V1.getNumber( 2, 1 ) << R2.getNumber( 2, 1 ) << R3.getNumber( 2, 1 ) + << V1.getNumber( 3, 1 ) << R2.getNumber( 3, 1 ) << R3.getNumber( 3, 1 ); + + Matrix Cbn = Mr*MatrixMath::Transpose(Mou); + + float sqtrp1 = sqrt(1.0+Cbn.getNumber( 1, 1 )+Cbn.getNumber( 2, 2 )+Cbn.getNumber( 3, 3 )); + + qhat.add(1,1,0.5*sqtrp1); + qhat.add(2,1,-(Cbn.getNumber( 2, 3 )-Cbn.getNumber( 3, 2 ))/2.0/sqtrp1); + qhat.add(3,1,-(Cbn.getNumber( 3, 1 )-Cbn.getNumber( 1, 3 ))/2.0/sqtrp1); + qhat.add(4,1,-(Cbn.getNumber( 1, 2 )-Cbn.getNumber( 2, 1 ))/2.0/sqtrp1); + + float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); + qhat *= (1.0f/ qnorm); + + qhat_gyro = qhat; + + float q0 = qhat.getNumber( 1, 1 ); + float q1 = qhat.getNumber( 2, 1 ); + float q2 = qhat.getNumber( 3, 1 ); + float q3 = qhat.getNumber( 4, 1 ); + + D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3); + D.add(1,2,2*(q1*q2 + q0*q3)); + D.add(1,3,2*(q1*q3 - q0*q2)); + D.add(2,1,2*(q1*q2 - q0*q3)); + D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3); + D.add(2,3,2*(q2*q3 + q0*q1)); + D.add(3,1,2*(q1*q3 + q0*q2)); + D.add(3,2,2*(q2*q3 - q0*q1)); + D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3); +} + +Vector3 HAPS_EKF::calcMagRef(Vector3 m) +{ + float _x, _y, _z; + Matrix magvec(3,1); + magvec << m.x << m.y << m.z; + Matrix magnedvec = MatrixMath::Transpose(D)*magvec; + _x = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1)); + _y = 0.0f; + _z = magnedvec(3,1); + return Vector3(_x, _y, _z); +} + +Vector3 HAPS_EKF::calcDynAcc(Vector3 LPacc, Vector3 accref) +{ + float _x, _y, _z; + _x = LPacc.x-(D.getNumber( 1, 1 )*accref.x+D.getNumber( 1, 2 )*accref.y+D.getNumber( 1, 3 )*accref.z); + _y = LPacc.y-(D.getNumber( 2, 1 )*accref.x+D.getNumber( 2, 2 )*accref.y+D.getNumber( 2, 3 )*accref.z); + _z = LPacc.z-(D.getNumber( 3, 1 )*accref.x+D.getNumber( 3, 2 )*accref.y+D.getNumber( 3, 3 )*accref.z); + return Vector3(_x, _y, _z); +}