Filter.h

Go to the documentation of this file.

#include <vrecko/Ability.h>
#include <vreckoDP/base/PHANToM.h>

//newfilter
#define BASEK1 (0.9)
#define BASEK2 (0.1)
//oldfilter
//#define DEFAULTBASEK1 (0.154882 * 2000.0)
//#define DEFAULTBASEK2 (0.141881 * 2000.0)


using namespace vrecko;
using namespace vreckoDP_base;
namespace VirtualFixture{



class Filter: public PHANToMFilter {
    public:
        Filter();
        ~Filter() {};
        
        void Filter_Initialize(void); //inicializacni funkce
        void Filter_Update(void); //pri zmene frekvence
        void Filter_SetX_hat1(osg::Vec3 nx); //nastavuje inicialni hodnotu 1. vnitrni promenne
        void Filter_SetX_hat2(osg::Vec3 nx); //nastavuje inicialni hodnotu 2. vnitrni promenne
        void Filter_Mesurement(osg::Vec3 z); //mereni 
        void Filter_Prediction(void); //predikcni krok
        osg::Vec3 Filter_GetPosition(void); //vraci hodnotu pozice (vystup filtru)
        osg::Vec3 Filter_GetVelocity(void); //vraci hodnotu rychlosti 
        void apply(PHANToM *pPHANToM, osg::Vec3 &force);
        void processEvent(const std::string &input_name, void *value);

        void preInitialize(void);
        void postInitialize(void);
        void update(void);

        bool loadXMLParameters(XERCES_CPP_NAMESPACE_QUALIFIER DOMNode *pParametersNode);
        XERCES_CPP_NAMESPACE_QUALIFIER DOMNode *getXMLParameters(XERCES_CPP_NAMESPACE_QUALIFIER DOMDocument *);
        
    protected:
        PHANToM *PHANToMDevice;
        bool all_on;
        bool active;        //if true, filter will be active
        unsigned int freq;  //frequncy of phantom thread
        float k1;       //kalmanuv zisk (1.radek)
        float k2;       //kalmanuv zisk (2.radek)
        osg::Vec3 x_hat1;   //odhad x po mereni
        osg::Vec3 x_hat2;   //minuly odhad (pro urceni rychlosti)
        osg::Vec3 x_hat_prev1;   //odhad x pred merenim 
        osg::Vec3 x_hat_prev2;  //odhad x pred merenim 
        osg::Vec3 inovation;   //rozdil odhadu a mereni
};

};