sensorsimu.hpp

#ifndef DDF_SENSORSIMU_HPP
#define DDF_SENSORSIMU_HPP

#include "ddfsimu/sensor.hpp"
#include "jmath/random.hpp"
#include "ddf/definitions.hpp"

namespace jafar
{
  namespace ddfsimu
  {

    using jafar::jmath::MultiDimNormalDistribution;

    class SensorSimuBase : public SensorObs
    {
      VEC                        m_simu_var;          // the true measurement variance (for the simulation)
      long unsigned int          m_seed;              // for the gaussian random noise generator
      MultiDimNormalDistribution *m_norm_dist;        // used to add noise to the sensor data
      MultiDimNormalDistribution *m_norm_jitter_dist; // for the time stamps
      time                       m_offset;            // time offset for the simulation (used to modify the time origin)
      time                       m_stamp_jitter;      // used to add noise for the time stamp (both sensed and available)
   
    public:
      SensorSimuBase(unsigned short x_size, unsigned short z_size):
        SensorObs(x_size, z_size), m_simu_var(z_size), m_offset(0,0), m_stamp_jitter(0,0)
      {
        m_norm_dist = NULL;
        m_norm_jitter_dist = NULL;
        m_seed = GenSeed(GetID());
        m_simu_var.clear();
      }
   
      SensorSimuBase(SensorSimuBase const& other) :
          SensorObs(other.GetStateSize(),
                    other.GetMeasureSize()),
                    m_simu_var(other.GetMeasureSize()),
                    m_seed(other.m_seed),
                    m_offset(other.m_offset),
                    m_stamp_jitter(other.m_stamp_jitter)
      {
        JFR_PRECOND(GetMeasureSize() == other.GetMeasureSize(), "bad size");
        m_simu_var = other.m_simu_var;
    
        VEC  nul(GetMeasureSize());
        nul.clear();
  
        if (other.m_norm_dist != NULL)
          m_norm_dist = new MultiDimNormalDistribution(nul, m_simu_var, m_seed);
        else
          m_norm_dist = NULL;

        if (other.m_norm_jitter_dist != NULL)
          GenStampJitterDistrib();
        else
          m_norm_jitter_dist = NULL;
        }
      virtual ~SensorSimuBase() { if (m_norm_dist != NULL) delete m_norm_dist; }
   
      void SetTimeOffset(time const& offset)  { m_offset = offset; }
      time GetTimeOffset() const { return m_offset; }

      void SetStampJitterTwoSigma(time const& jitter) { m_stamp_jitter = jitter; GenStampJitterDistrib(); }
      time GetStampJitterTwoSigma() { return m_stamp_jitter; }
      time ComputeStampJitter() {
        if(m_stamp_jitter == time::null()) return time::null();
        VEC v(1); v = m_norm_jitter_dist->get();
        return time(fabs(v[0]));
      }

      void SetSeed(unsigned short sens_id) { m_seed = GenSeed(sens_id); }
      long unsigned int GetSeed() { return m_seed; }
      static long unsigned int GenSeed(unsigned short id) { return id * 3 + 55319; } // 17
      void SetSensorSimVariance(VEC const& var)
      {
        JFR_PRECOND(var.size() == GetMeasureSize(), "bad size");
        VEC  nul(GetMeasureSize());  nul.clear();
        m_simu_var = var;
        if (m_norm_dist != NULL) delete m_norm_dist;
        m_norm_dist = new MultiDimNormalDistribution(nul, m_simu_var, m_seed);
      }
   
      VEC GetSensorSimVariance() const { return m_simu_var; }

      void AddSensorNoise(VEC &measure)
      {
        JFR_PRECOND(measure.size() == GetMeasureSize(), "bad size");
        JFR_PRECOND(m_norm_dist != NULL, "ptr null");
        measure += m_norm_dist->get();
      }
   
      void AddMeasurementWithNoise(time sensed, time available, VEC &data)
      {
        VEC  tmp(data);
        AddSensorNoise(tmp);
        AddMeasurement(sensed, available, tmp);
      }

      virtual void Perceive(VEC const& proc_state, time t)=0;

      virtual void ComputeInformation(VEC const& z, VEC &i, MSYM &I)=0;

    private:
      void GenStampJitterDistrib()
      {
        VEC  v(1), v_var(1);
        v[0] = 0;
        v_var[0] = TWOSIGMA_TO_VAR(m_stamp_jitter.to_double());
        if (m_norm_jitter_dist != NULL) delete m_norm_jitter_dist;
        m_norm_jitter_dist = new MultiDimNormalDistribution(v, v_var, m_seed);
      }
    };

    template<typename T_OBS>
    class SensorSimu : public SensorSimuBase, public T_OBS
    {
   
    public:
   
      SensorSimu(unsigned short x_size, unsigned short z_size): SensorSimuBase(x_size,z_size), T_OBS(x_size, z_size) { }
      virtual ~SensorSimu() { }

      friend std::ostream& operator << (std::ostream &stream, SensorSimu<T_OBS> const& pd)
      {
        stream << static_cast<SensorBase> (pd) << " Simulation variance: " << pd.m_var;
        return stream;
      }
    };

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