ImuError.hpp

Go to the documentation of this file.

#ifndef IMUMODEL_IMUMODEL_HPP__
#define IMUMODEL_IMUMODEL_HPP__

#include <base/Eigen.hpp>
#include <base/samples/IMUSensors.hpp>
#include <boost/random.hpp>

namespace imumodel
{

struct Configuration
{
    int seed;
    double dt;

    base::Matrix3d Dacc;
    base::Matrix3d Dgyro;

    base::Vector3d accrw;
    base::Vector3d accrrw;
    base::Vector3d accbias;
    base::Vector3d abeta;

    base::Vector3d gyrorw;
    base::Vector3d gyrorrw;
    base::Vector3d gyrobias;
    base::Vector3d gbeta;

    Configuration() :
        seed(-1),
        dt(0.01),
        Dacc( base::Matrix3d::Zero() ),
        Dgyro( base::Matrix3d::Zero() ),
        accrw( base::Vector3d::Zero() ),
        accrrw( base::Vector3d::Zero() ),
        accbias( base::Vector3d::Zero() ),
        abeta( base::Vector3d::Zero() ),
        gyrorw( base::Vector3d::Zero() ),
        gyrorrw( base::Vector3d::Zero() ),
        gyrobias( base::Vector3d::Zero() ),
        gbeta( base::Vector3d::Zero() ) 
    {}
};

class ImuError
{
public:
    static const int NUMAXIS = 3;

    void init();
    void reset();
    void step();
    void addNoise( base::samples::IMUSensors &imu_sample );
    void setConfiguration( const Configuration& config );
    const Configuration& getConfiguration() const;
    Configuration& getConfiguration();

    base::Vector3d getGyroError() const;

protected:
    Configuration config;

    base::Vector2d xax; 
    base::Vector2d xay; 
    base::Vector2d xaz; 
    base::Matrix2d Aax; 
    base::Vector2d Gax; 
    base::Matrix2d Aay; 
    base::Vector2d Gay; 
    base::Matrix2d Aaz; 
    base::Vector2d Gaz; 
    base::Vector2d Ha; 
    base::Vector2d xgx; 
    base::Vector2d xgy; 
    base::Vector2d xgz; 
    base::Matrix2d Agx; 
    base::Vector2d Ggx; 
    base::Matrix2d Agy; 
    base::Vector2d Ggy; 
    base::Matrix2d Agz; 
    base::Vector2d Ggz; 
    base::Vector2d Hg; 
    Eigen::Matrix <double,NUMAXIS,1> acc, gyros; 
    Eigen::Matrix <double,NUMAXIS,NUMAXIS> Dacc, Dgyro; 
    typedef boost::mt19937 RandomGenerator;
    RandomGenerator rng; 
    double GetNormalDistri(double mean, double sigma);
};

class AhrsError
{
    base::Transform3d orientationError;
    ImuError model;

public:
    AhrsError()
    {
        reset();
    }

    void setZGyroProperties( double bias, double rw, double rrw )
    {
        Configuration &config( model.getConfiguration() );
        config.gyrobias = base::Vector3d::UnitZ() * bias;
        config.gyrorw = base::Vector3d::UnitZ() * rw;
        config.gyrorrw = base::Vector3d::UnitZ() * rrw;
        model.init();
    };

    void reset( int seed = -1 )
    {
        model.getConfiguration().seed = seed;
        model.init();
        model.reset();
        orientationError = base::Transform3d::Identity();
    }

    void step()
    {
        model.step();
        base::Vector3d error = model.getGyroError();
        orientationError = Eigen::AngleAxisd( error.z(), Eigen::Vector3d::UnitZ() ) * 
            orientationError;
    }

    base::Transform3d getErrorTransform()
    {
        return orientationError;
    }
};

}

#endif