identifier/ekfIdentifier.h

#ifndef INCLUDE_NAVLIB_EKFIDENTIFIER
#define INCLUDE_NAVLIB_EKFIDENTIFIER

#include "identifier.h"

namespace navlib
{
        class EkfIdentifier : public Identifier
        {
        public:
                typedef ublas::matrix<double> matrix;
                typedef ublas::vector<double> vector;

        public:
                EkfIdentifier(boost::shared_ptr<DynamicsModel> dynamics, const double& eps=1.0e-9);
                EkfIdentifier(boost::shared_ptr<DynamicsModel> dynamics, matrix Px, matrix Pp);
                boost::shared_ptr<DynamicsModel> getModel() const;
            void predict(vector u, const double& dt, matrix Qx, matrix Qp);
                void correct(ExtendedKalmanFilter<double>::ObserveFun h, vector z, matrix R);
        
        private:
                boost::shared_ptr<DynamicsModel> _dynamics;
                ExtendedKalmanFilter<double> _ekf;
                unsigned int _nx;
                unsigned int _np;

        private:
                void init(boost::shared_ptr<DynamicsModel> dynamics, matrix Px, matrix Pp);
                vector ppredict(vector x, vector u, const double& dt);
        };
}

#endif

---