File content as of revision 73:5770a0d470c0:

#ifndef __solaESKF_HPP__
#define __solaESKF_HPP__

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

#define M_PI 3.141592f

extern Serial twelite;

class solaESKF
{
private:

    int nState;
    Matrix pihat;
    Matrix vihat;
    Matrix qhat;
    Matrix accBias;
    Matrix gyroBias;
    Matrix gravity;
    Matrix magBias;
    float magRadius;


    Matrix errState;
    Matrix Phat;
    Matrix Q;

    void setDiag(Matrix& mat, float val);
    void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex);
    float wrap_pi(float x);

public:

    solaESKF();

    Matrix getPihat();
    Matrix getVihat();
    Matrix getQhat();
    Matrix getAccBias();
    Matrix getGyroBias();
    Matrix getGravity();
    Matrix getMagBias();
    float getMagRadius();
    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 setPhatMagBias(float val);
    void setPhatMagRadius(float val);
    void setQVelocity(float val);
    void setQAngleError(float val);
    void setQAccBias(float val);
    void setQGyroBias(float val);
    void setQMagBias(float val);
    void setQMagRadius(float val);
    
    void updateNominal(Matrix acc,Matrix gyro, float att_dt);
    void updateErrState(Matrix acc,Matrix gyro, float att_dt);
    void updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R);
    void updateImuConstraints(Matrix acc,Matrix mag,Matrix R);
    void updateOtherConstraints(Matrix mag,float palt,Matrix R);
    void updateAcc(Matrix acc,Matrix R); 
    void updateMag(Matrix mag,Matrix R);
    void updateHeading(float a,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);
    
    Matrix computeAngles();
    Matrix quatmultiply(Matrix p, Matrix q);
    
};

#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);
    //