Takashi Koyama / HAPS_EKF

EKF class

Dependencies:   MatrixMath Vector3

HAPS_EKF.hpp

Committer:
cocorlow
Date:
2021-05-31
Revision:
2:771eed5f655a
Parent:
0:de56252b419e

File content as of revision 2:771eed5f655a:

#ifndef __HAPS_EKF_HPP__
#define __HAPS_EKF_HPP__

#include "Matrix.h"
#include "Vector3.hpp"


class HAPS_EKF
{
private:
    Matrix qhat;
    Matrix qhat_gyro;
    Matrix Phat;
    Matrix Qgyro;
    Matrix D;
    
public:  
    Matrix Racc;
    Matrix Rmag;
    
    HAPS_EKF();
    void updateBetweenMeasures(Vector3 gyro, float att_dt);
    void updateAcrossMeasures(Vector3 _v, Vector3 _u, Matrix& R);
    void computeAngles(Vector3& rpy, Vector3& rpy_g, Vector3 rpy_align);
    void triad(Vector3 fb, Vector3 fn, Vector3 mb, Vector3 mn);
    Vector3 calcMagRef(Vector3 m);
    Vector3 calcDynAcc(Vector3 LPacc, Vector3 accref);
};

#endif