solaESKF.hpp

00001 #ifndef __solaESKF_HPP__
00002 #define __solaESKF_HPP__
00003 
00004 #include <Eigen/Dense.h>
00005 //#include <Eigen/Core.h>
00006 //#include <Eigen/LU.h>
00007 #include <cmath>
00008 
00009 using namespace Eigen;
00010 
00011 #ifndef M_PI_F
00012 #define M_PI_F 3.141592f
00013 #endif
00014 
00015 class solaESKF
00016 {
00017 private:
00018 
00019     int nState;
00020     Vector3f pihat;
00021     Vector3f vihat;
00022     Vector4f qhat;
00023     Vector3f accBias;
00024     Vector3f gyroBias;
00025     Vector3f gravity;
00026 
00027     VectorXf errState;
00028     MatrixXf Phat;
00029     MatrixXf Q;
00030 
00031     void setDiag(Matrix3f& mat, float val);
00032     void setDiag(MatrixXf& mat, float val);
00033     void setBlockDiag(MatrixXf& mat, float val,int startIndex, int endIndex);
00034 
00035 public:
00036 
00037     solaESKF();
00038 
00039     Vector3f getPihat();
00040     Vector3f getVihat();
00041     Vector4f getQhat();
00042     Vector3f getAccBias();
00043     Vector3f getGyroBias();
00044     Vector3f getGravity();
00045     VectorXf getErrState();
00046     VectorXf getState();
00047     VectorXf getVariance();
00048 
00049     void setPhatPosition(float valNE,float valD);
00050     void setPhatVelocity(float valNE,float valD);
00051     void setPhatAngleError(float val);
00052     void setPhatAccBias(float val);
00053     void setPhatGyroBias(float val);
00054     void setPhatGravity(float val);
00055     void setQVelocity(float valNE,float valD);
00056     void setQAngleError(float val);
00057     void setQAccBias(float val);
00058     void setQGyroBias(float val);
00059     
00060     void updateNominal(Vector3f acc, Vector3f gyro, float att_dt);
00061     void updateErrState(Vector3f acc, Vector3f gyro, float att_dt);
00062     void updateGPS(Vector3f posgps,float palt, Vector3f velgps, MatrixXf R);
00063     void updateGPSPosition(Vector3f posgps, float palt, Matrix3f R);
00064     void updateGPSVelocity(Vector3f velgps, Matrix3f R);
00065     void updateAcc(Vector3f acc, Matrix3f R); 
00066     void updateHeading(float a, Matrix<float, 1, 1> R);  
00067     void updateIMU(float palt,Vector3f acc,float heading, Matrix<float, 5, 5> R);
00068     void updateWhole(Vector3f posgps,float palt, Vector3f velgps, Vector3f acc,float heading, MatrixXf R);
00069     void fuseErr2Nominal();
00070     void fuseCenter2Nominal(VectorXf errVal);
00071     
00072     void computeDcm(Matrix3f& dcm, Vector4f quat);
00073     void setQhat(float ex,float ey,float ez);
00074     void setGravity(float gx,float gy,float gz);
00075     Vector3f calcDynAcc(Vector3f acc);
00076     Vector3f vector2Body(Vector3f veci);
00077     Vector3f vector2NED(Vector3f vecb);
00078     
00079     void setPihat(float pi_x, float pi_y,float pi_z);
00080     
00081     Vector3f computeAngles();
00082     Vector4f quatmultiply(Vector4f p, Vector4f q);
00083     
00084     static Matrix3f Matrixcross(Vector3f v);
00085 };
00086 
00087 #endif
00088 
00089     //void updateGPSVelocity(Matrix velgps,Matrix R);
00090     //void updateGPSPosition(Matrix posgps,Matrix R);
00091     //void updateGyroConstraints(Matrix gyro,Matrix R);
00092     //void updateMag(Matrix mag,Matrix R);
00093     //void updateAccConstraints(Matrix acc,float palt,Matrix R);
00094     //void updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R);
00095     //void updateGPSPosition(Matrix posgps,float palt,Matrix R);
00096     //