File content as of revision 54:cd514d9d4b19:

#ifndef __solaESKF_HPP__
#define __solaESKF_HPP__

#include "Matrix.h"
#include "MatrixMath.h"
#include <cmath>

#define M_PI 3.141592f


class solaESKF
{
private:

    int nState;
    Matrix pihat;
    Matrix vihat;
    Matrix qhat;
    Matrix accBias;
    Matrix gyroBias;
    Matrix gravity;
    
    Matrix errState;
    Matrix Phat;
    Matrix Q;
    
    void setDiag(Matrix& mat, float val);
    void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex);
        
public:
    
    solaESKF();
    
    Matrix getPihat();
    Matrix getVihat();
    Matrix getQhat();
    Matrix getAccBias();
    Matrix getGyroBias();
    Matrix getGravity();
    Matrix getErrState();
    
    void setPhatPosition(float val);
    void setPhatVelocity(float val);
    void setPhatAngleError(float val);
    void setPhatAccBias(float val);
    void setPhatGyroBias(float val);
    void setPhatGravity(float val);
    void setQVelocity(float val);
    void setQAngleError(float val);
    void setQAccBias(float val);
    void setQGyroBias(float val);
    
    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,float palt,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);
    
    void setPihat(float pi_x, float pi_y, float pi_z);
    
    Matrix computeAngles();
    Matrix quatmultiply(Matrix p, Matrix q);
    
};

#endif