gpssimu.hpp

#ifndef DDF_GPSSIMU_HPP
#define DDF_GPSSIMU_HPP

#include "ddfsimu/sensorsimu.hpp"
#include "ddfsimu/accelmotion.hpp"
#include "ddf/tools.hpp"

namespace jafar
{
  namespace ddfsimu
  {

    template<typename T_OBS>
    class GPSSimu : public SensorSimu<T_OBS>
    {
    public:
      using T_OBS::Hx;
      using SensorSimu<T_OBS>::GetProcTime;
      using SensorSimu<T_OBS>::GetMeasureSize;
      using SensorSimu<T_OBS>::GetID;
      using SensorSimu<T_OBS>::GetName;
   
      GPSSimu(unsigned short x_size, unsigned short z_size, bool use_speed_model): SensorSimu<T_OBS>(x_size, z_size)
      {
        this->m_type_name = SENSOR_GPS;
        //m_name = SENSOR_DESCRIPTION[m_type];
  
        JFR_PRECOND(z_size <= 3, ">3");
    
        // init H
        Hx.clear();
  
        if(use_speed_model){
          Hx(0, 0) = 1.;
          if(z_size > 1) Hx(1, 2) = 1.;
          if(z_size > 2) Hx(2, 4) = 1.;
        }
        else {
          Hx(0, AccelMotion::SV_X) = 1.;
          if(z_size > 1) Hx(1, AccelMotion::SV_Y) = 1.;
          if(z_size > 2) Hx(2, AccelMotion::SV_Z) = 1.;
        }
      }

      void Perceive(VEC const& proc_state, time t)
      {
        VEC measured_pos(GetMeasureSize());
        noalias(measured_pos) = prod(Hx, proc_state);
        AddMeasurementWithNoise(t, t+GetProcTime(), measured_pos);
      }
      void PerceiveRet(VEC const& proc_state, VEC & result) const { noalias(result) = prod(Hx, proc_state); }
      void PrintObservationModel(){
        JFR_DEBUG("Observation model of sensor " << GetID() << " (" << GetName() << ")\n");
        PrettyPrintMatrix(Hx, "Hx");
      }
    };

    class GPSSimuUncorr : public GPSSimu<Bayesian_filter::Linear_uncorrelated_observe_model>
    {
    public:
      GPSSimuUncorr(unsigned short x_size, unsigned short z_size, bool use_speed_model = false) :
        GPSSimu<Bayesian_filter::Linear_uncorrelated_observe_model>(x_size, z_size, use_speed_model){}
   
      virtual ~GPSSimuUncorr() {}

      void ComputeInformation(VEC const& z, VEC &i, MSYM &I) { compute_information_uncorr(*this, z, i, I); }
      void SetSensorModelVar(VEC const& var) { Zv = var; }
      void PrintObservationModel()
      {
        GPSSimu<Bayesian_filter::Linear_uncorrelated_observe_model>::PrintObservationModel();
        PrettyPrintVector(Zv, "Zv");
      }
    };

    class GPSSimuCorr : public GPSSimu<Bayesian_filter::Linear_correlated_observe_model>
    {
    public:
      GPSSimuCorr(unsigned short x_size, unsigned short z_size, bool use_speed_model = false) :
        GPSSimu<Bayesian_filter::Linear_correlated_observe_model>(x_size, z_size, use_speed_model){}
   
      virtual ~GPSSimuCorr() {}

      void ComputeInformation(VEC const& z, VEC &i, MSYM &I) { compute_information_corr(*this, z, m_sp, i, I); }
      void SetSensorModelCov(MSYM const& cov) { Z = cov; }
      void PrintObservationModel()
      {
        GPSSimu<Bayesian_filter::Linear_correlated_observe_model>::PrintObservationModel();
        PrettyPrintMatrix(Z, "R");
      }
    };

  } // namespace ddfsimu
} // namespace jafar
#endif