EKF class
Dependencies: MatrixMath Vector3
HAPS_EKF.cpp
- Committer:
- cocorlow
- Date:
- 2021-05-31
- Revision:
- 0:de56252b419e
- Child:
- 2:771eed5f655a
File content as of revision 0:de56252b419e:
#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); }