perturbation.hpp

Go to the documentation of this file.

#ifndef PERTURBATION_HPP_
#define PERTURBATION_HPP_

#include "jmath/jblas.hpp"
#include "jmath/ublasExtra.hpp"

#include "rtslam/gaussian.hpp"

namespace jafar {
  namespace rtslam {
    using namespace jblas;
    using namespace jmath::ublasExtra;

    class Perturbation: public Gaussian {
      private:
        vec x_ct; 
        sym_mat P_ct; 
        jblas::ind_array ia_deterministic;

      public:
        Perturbation(): x_ct(0), P_ct(0) {}

        Perturbation(const size_t _size) :
          Gaussian(_size), x_ct(_size), P_ct(_size) {
          x_ct.clear();
          P_ct.clear();
        }
        virtual void resize(size_t _size, bool preserve = true)
        {
          Gaussian::resize(_size, preserve);
          x_ct.resize(_size, preserve);
          P_ct.resize(_size, _size, preserve);
        }

        Perturbation(const vec & p, const sym_mat & P) :
          Gaussian(p, P), x_ct(p.size()), P_ct(p.size()) {
          x_ct.clear();
          P_ct.clear();
        }
        Perturbation(const vec & p, const vec & _std) :
          Gaussian(p.size()), x_ct(p.size()), P_ct(p.size()) {
          x_ct.clear();
          P_ct.clear();
          this->x(p);
          this->std(_std);
        }

        Perturbation(const vec & p, const sym_mat & P, double dt) ;
        Perturbation(const vec & p, const vec & _std, double dt) ;

        Perturbation(const Gaussian & p) :
          Gaussian(p), x_ct(p.size()), P_ct(p.size()) {
          x_ct.clear();
          P_ct.clear();
        }
        
        void clear()
        {
          Gaussian::clear();
          x_ct.clear();
          P_ct.clear();
        }
        
        template<class SymMat>
        void set_P_continuous(SymMat & _P_ct) {
          JFR_ASSERT(_P_ct.size1() == size(), "Matrix sizes mismatch.");
          P_ct = _P_ct;
        }
        template<class V>
        void set_x_continuous(V & _x_ct) {
          JFR_ASSERT(_x_ct.size() == size(), "Vector sizes mismatch.");
          x_ct = _x_ct;
        }
        void set_std_continuous(const vec & _std) {
          JFR_ASSERT(size() == _std.size(), "Sizes mismatch");
          P_ct.clear();
          size_t i;
          for (i = 0 ; i< _std.size() ; i++)
            P_ct(i,i) = _std(i)*_std(i);
//cout << "setting P_ct: " << P_ct << endl;
        }
        void set_continuous(const vec & _x, const vec & _std){
          set_x_continuous(_x);
          set_std_continuous(_std);
        }
        void set_continuous(const vec & _x, const sym_mat & _P){
          set_x_continuous(_x);
          set_P_continuous(_P);
        }

        void set_deterministic(jblas::ind_array ia) { ia_deterministic = ia; }
        void set_deterministic(ublas::range ia) { ia_deterministic = jmath::ublasExtra::ia_set(ia); }


        void set_P_from_continuous(double _dt) {
          JFR_ASSERT(P_ct.size1() == size(), "Continuous-time covariance not yet initialized.");
          P(P_ct * _dt); // perturbation is random => variance is linear with time
          ublas::project(P(), ia_deterministic, ia_deterministic) *= _dt; // if it is not random => variance linear with time^2
        }

        void set_P_from_continuous(sym_mat & _P_ct, double _dt) {
          JFR_ASSERT(_P_ct.size1() == size(), "Matrix sizes mismatch.");
          set_P_continuous(_P_ct);
          P(P_ct * _dt); // perturbation is random => variance is linear with time
          ublas::project(P(), ia_deterministic, ia_deterministic) *= _dt;
        }
        void set_from_continuous(double _dt) {
          JFR_ASSERT(x_ct.size() == size(), "Continuous-time values not yet initialized.");
          double sqdt = sqrt(_dt);
          x(x_ct * sqdt); // perturbation is random => mean is linear with square root of time
          ublas::project(x(), ia_deterministic) *= sqdt;
          P(P_ct * _dt); // perturbation is random => variance is linear with time
          ublas::project(P(), ia_deterministic, ia_deterministic) *= _dt;
//JFR_DEBUG("setting P: " << P() << " using P_ct: " << P_ct << " and dt " << _dt);
        }
        void set_from_continuous(Gaussian & Pct, double _dt) {
          JFR_ASSERT(Pct.size() == size(), "Sizes mismatch");
          set_x_continuous(Pct.x());
          set_P_continuous(Pct.P());
          set_from_continuous(_dt);
        }

    };


  }
}

#endif /* PERTURBATION_HPP_ */