File content as of revision 88:06d5b2cc5e20:

#ifndef __solaESKF_HPP__
#define __solaESKF_HPP__

#include <Eigen/Dense.h>
//#include <Eigen/Core.h>
//#include <Eigen/LU.h>
#include <cmath>

using namespace Eigen;

#ifndef M_PI_F
#define M_PI_F 3.141592f
#endif

class solaESKF
{
private:

    int nState;
    Vector3f pihat;
    Vector3f vihat;
    Vector4f qhat;
    Vector3f accBias;
    Vector3f gyroBias;
    Vector3f gravity;

    VectorXf errState;
    MatrixXf Phat;
    MatrixXf Q;

    void setDiag(Matrix3f& mat, float val);
    void setDiag(MatrixXf& mat, float val);
    void setBlockDiag(MatrixXf& mat, float val,int startIndex, int endIndex);

public:

    solaESKF();

    Vector3f getPihat();
    Vector3f getVihat();
    Vector4f getQhat();
    Vector3f getAccBias();
    Vector3f getGyroBias();
    Vector3f getGravity();
    VectorXf getErrState();
    VectorXf getState();
    VectorXf getVariance();

    void setPhatPosition(float valNE,float valD);
    void setPhatVelocity(float valNE,float valD);
    void setPhatAngleError(float val);
    void setPhatAccBias(float val);
    void setPhatGyroBias(float val);
    void setPhatGravity(float val);
    void setQVelocity(float valNE,float valD);
    void setQAngleError(float val);
    void setQAccBias(float val);
    void setQGyroBias(float val);
    
    void updateNominal(Vector3f acc, Vector3f gyro, float att_dt);
    void updateErrState(Vector3f acc, Vector3f gyro, float att_dt);
    void updateGPS(Vector3f posgps,float palt, Vector3f velgps, MatrixXf R);
    void updateGPSPosition(Vector3f posgps, float palt, Matrix3f R);
    void updateGPSVelocity(Vector3f velgps, Matrix3f R);
    void updateAcc(Vector3f acc, Matrix3f R); 
    void updateHeading(float a, Matrix<float, 1, 1> R);  
    void updateIMU(float palt,Vector3f acc,float heading, Matrix<float, 5, 5> R);
    void updateWhole(Vector3f posgps,float palt, Vector3f velgps, Vector3f acc,float heading, MatrixXf R);
    void fuseErr2Nominal();
    void fuseCenter2Nominal(VectorXf errVal);
    
    void computeDcm(Matrix3f& dcm, Vector4f quat);
    void setQhat(float ex,float ey,float ez);
    void setGravity(float gx,float gy,float gz);
    Vector3f calcDynAcc(Vector3f acc);
    Vector3f vector2Body(Vector3f veci);
    Vector3f vector2NED(Vector3f vecb);
    
    void setPihat(float pi_x, float pi_y,float pi_z);
    
    Vector3f computeAngles();
    Vector4f quatmultiply(Vector4f p, Vector4f q);
    
    static Matrix3f Matrixcross(Vector3f v);
};

#endif

    //void updateGPSVelocity(Matrix velgps,Matrix R);
    //void updateGPSPosition(Matrix posgps,Matrix R);
    //void updateGyroConstraints(Matrix gyro,Matrix R);
    //void updateMag(Matrix mag,Matrix R);
    //void updateAccConstraints(Matrix acc,float palt,Matrix R);
    //void updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R);
    //void updateGPSPosition(Matrix posgps,float palt,Matrix R);
    //