File content as of revision 46:15988dc41923:

#ifndef __solaESKF_HPP__
#define __solaESKF_HPP__

#include "Matrix.h"

#define M_PI 3.141592f


class solaESKF
{
private:

    int nState;
        
public:
    
    Matrix pihat;
    Matrix vihat;
    Matrix qhat;
    Matrix accBias;
    Matrix gyroBias;
    Matrix gravity;
    
    Matrix errState;
    Matrix Phat;
    Matrix Q;
    
    solaESKF();
    
    void setDiag(Matrix& mat, float val);
    void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex);
    void updateNominal(Matrix acc,Matrix gyro, float att_dt);
    void updateErrState(Matrix acc,Matrix gyro, float att_dt);
    void updateGPSVelocity(Matrix velgps,Matrix R);
    void updateGPSPosition(Matrix posgps,Matrix R);
    void updateGPS(Matrix posgps,Matrix velgps,Matrix R);
    void updateAccConstraints(Matrix acc,Matrix R);
    void updateGyroConstraints(Matrix gyro,Matrix R);
    void fuseErr2Nominal();
    
    void computeDcm(Matrix& dcm, Matrix quat);
    void setQhat(float ex,float ey,float ez);
    void setGravity(float gx,float gy,float gz);
    
    Matrix computeAngles();
    Matrix quatmultiply(Matrix p, Matrix q);
    
};

#endif