trackersimu.hpp

#ifndef DDF_TRACKERSIMU_HPP
#define DDF_TRACKERSIMU_HPP

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

namespace jafar
{
  namespace ddfsimu
  {

    template<typename T_OBS>
    class TrackerSimu : public SensorSimu<T_OBS>
    {
      // Perceive will use the true current state proc_state (containing the position of the robot)
      // and the list of observable targets to produce a measurement DX, DY DZ with noise
      typedef std::vector<VEC>         TargetsArray;
      typedef std::vector<RobotSimu *> TargetsRobotsArray;

      TargetsArray         m_targets;
      TargetsRobotsArray   m_tracked_robots;
      unsigned short       m_num_targets;
      unsigned short       m_rob_id;
      bool                 m_use_only_vel;
      unsigned short       m_num_rob_to_track;

    protected:
      using T_OBS::Hx;
      using SensorSimu<T_OBS>::GetProcTime;
      using SensorSimu<T_OBS>::GetMeasureSize;
      using SensorSimu<T_OBS>::GetID;
      using SensorSimu<T_OBS>::GetName;
   
    public:
      TrackerSimu(unsigned short x_size, unsigned short rob_id, unsigned short num_targets, bool use_only_vel, unsigned short num_rob_to_track):
        SensorSimu<T_OBS>(x_size,
                          (num_targets+num_rob_to_track) * 3),
                          m_num_targets(num_targets),
                          m_rob_id(rob_id),
                          m_use_only_vel(use_only_vel),
                          m_num_rob_to_track(num_rob_to_track)
      {
        unsigned short num_robots = (x_size - num_targets * 3)/6;
        int i,j;
        this->m_type_name = SENSOR_TRACK;
  
        Hx.clear();
  
        for(i = 0; i < num_targets; i++) {
          for(j = 0; j < 3; j++) {
            Hx(3*i+j,6*m_rob_id+2*j)     = -1.;
            Hx(3*i+j,6*num_robots+3*i+j) = 1.;
          }
        }
      }

      void AddTarget(VEC const& target) { JFR_PRECOND(m_targets.size() < m_num_targets, "too many targets"); m_targets.push_back(target); }
      unsigned short GetNumTargets() { return m_num_targets; }

      void AddRobotToTracker(RobotSimu *other_robot) {
        int i,j;
  
        JFR_PRECOND(other_robot->GetRobID() != m_rob_id, "wrong robot");
        std::cout << m_num_rob_to_track << " " << m_tracked_robots.size();
          
        m_tracked_robots.push_back(other_robot);

        JFR_PRECOND(m_num_rob_to_track <= m_tracked_robots.size(), "bad sizes");
          
        for(i = 0,j = 3*m_num_targets; i < 6; i = i+2,j++) {
          Hx(j,6*m_rob_id+i) = -1.0;
          Hx(j,6*other_robot->GetRobID()+i) = 1.0;
        }
      }

      // proc_state is the true state [x vx ax y vy ay z vz az]
      void Perceive(VEC const& proc_state, time t)
      {
        int  i,j,k=0;
        VEC  true_pos(3),other_rob_pos(3);
        VEC  measured_pos(GetMeasureSize());
        if(m_use_only_vel){
          for(i = 0, j = 0; i < 3; i++, j = j+2) true_pos[i] = proc_state[j];
        }
        else {
          true_pos[0] = proc_state[AccelMotion::SV_X];
          true_pos[1] = proc_state[AccelMotion::SV_Y];
          true_pos[2] = proc_state[AccelMotion::SV_Z];
        }
         
        for(i = 0; i < m_num_targets; i++)
          {
            for(j = 0; j < 3; j++) measured_pos[k++] = m_targets[i][j] - true_pos[j];
          }

        if (!m_tracked_robots.empty())
          {
            for(i = 0; i < m_num_rob_to_track; i++)
            {
              other_rob_pos = m_tracked_robots[i]->ComputePositionAtTimeT(t.to_double());
              for(j = 0; j < 3; j++) measured_pos[k++] = other_rob_pos[j] - true_pos[j];
             }

          }
        // Add a noise wich is proportional to the dist^2 !
        AddMeasurementWithNoise(t, t+GetProcTime(), measured_pos);
      }
      void PerceiveRet(VEC const& proc_state, VEC & result) const {   }
      void PrintObservationModel(){
        JFR_DEBUG("Observation model of sensor " << GetID() << " (" << GetName() << ")\n");
        PrettyPrintMatrix(Hx, "Hx");
      }
      void PrintTargets()
      {
        std::ostringstream stream;
        
        stream << "\nNum targets: " << GetNumTargets() << "\n";
        
        for(int i = 0; i < m_num_targets; i++)
          stream << "Target " << i << " " << m_targets[i] << "\n";
        
        JFR_DEBUG(stream.str());
      }
    };

    class TrackerSimuUncorr : public TrackerSimu<Bayesian_filter::Linear_uncorrelated_observe_model>
    {
    public:
      TrackerSimuUncorr(unsigned short x_size, unsigned short rob_id, unsigned short num_targets, \
                        bool use_only_vel, unsigned short num_rob_to_track) :
        TrackerSimu<Bayesian_filter::Linear_uncorrelated_observe_model>(x_size, rob_id, num_targets, use_only_vel, num_rob_to_track){}
   
      virtual ~TrackerSimuUncorr() {}

      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()
      {
        TrackerSimu<Bayesian_filter::Linear_uncorrelated_observe_model>::PrintObservationModel();
        PrettyPrintVector(Zv, "Zv");
      }
    };

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