velocitysimu.hpp

#ifndef DDF_VELOCITYSIMU_HPP
#define DDF_VELOCITYSIMU_HPP

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

namespace jafar
{
  namespace ddfsimu
  {

    template<typename T_OBS>
    class VelocitySimu : 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;
      
      VelocitySimu(unsigned short x_size, unsigned short z_size, bool use_speed_model)
        : SensorSimu<T_OBS>(x_size, z_size)
      {
        this->m_type_name = SENSOR_VEL;
        JFR_PRECOND(z_size <= 3, "> 3");
    
        // init H
        Hx.clear();
  
        if(use_speed_model){
          Hx(0, 1) = 1.;
          if(z_size > 1) Hx(1, 3) = 1.;
          if(z_size > 2) Hx(2, 5) = 1.;
        }
        else{
          Hx(0, AccelMotion::SV_VX) = 1.;
          if(z_size > 1) { Hx(1, AccelMotion::SV_VY) = 1.; }
          if(z_size > 2) { Hx(2, AccelMotion::SV_VZ) = 1.; }
        }
      }

      virtual ~VelocitySimu() {}

      void Perceive(VEC const& proc_state, time t)
      {
        VEC  measured_vel(GetMeasureSize());
  
        noalias(measured_vel) = prod(Hx, proc_state);
        AddMeasurementWithNoise(t,t+GetProcTime(),measured_vel);
      }

      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 VelocitySimuUncorr : public VelocitySimu<Bayesian_filter::Linear_uncorrelated_observe_model>
    {
    public:
      VelocitySimuUncorr(unsigned short x_size, unsigned short z_size, bool use_speed_model = false):
        VelocitySimu<Bayesian_filter::Linear_uncorrelated_observe_model>(x_size, z_size, use_speed_model){}
      virtual ~VelocitySimuUncorr() {}

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

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