trackrobot.hpp

#ifndef DDF_TEST_NODESTRACKROBOT_HPP
#define DDF_TEST_NODESTRACKROBOT_HPP

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

namespace jafar
{
  namespace ddfsimu
  {

    template<typename T_OBS>
    class TrackRobot : public SensorSimu<T_OBS>
    {
      RobotSimu      *m_target_robot;
      unsigned short m_rob_id;
      bool           m_use_only_vel;

      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:
         TrackRobot(unsigned short x_size, unsigned rob_id, bool use_only_vel, RobotSimu *other_robot):
           SensorSimu<T_OBS>(x_size, 3),
         m_target_robot(other_robot),
         m_rob_id(rob_id),
         m_use_only_vel(use_only_vel)
       
         {
           int i,j;

           JFR_PRECOND(rob_id != other_robot->GetRobID(), "wrong robot"); 
           this->m_type_name = SENSOR_TRACK_ROBOT;
           Hx.clear();

           for(i = 0,j = 0; i < 6; i = i+2,j++) {
             Hx(j,6*m_rob_id+i) = -1.0;
             Hx(j,6*other_robot->GetRobID()+i) = 1.0;
           }

         }

         virtual ~TrackRobot() {}

         // 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;
           VEC true_pos(3);
           VEC 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];
           }

           other_rob_pos = m_target_robot->ComputePositionAtTimeT(t.to_double());
           for(i = 0; i < 3; i++) measured_pos[i] = other_rob_pos[i] - true_pos[i];

           JFR_DEBUG( "Meas: " << measured_pos << "\n");
           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");
         }
    };

    class TrackRobotUncorr : public TrackRobot<Bayesian_filter::Linear_uncorrelated_observe_model>
    {
      public:
      TrackRobotUncorr(unsigned short x_size, unsigned short rob_id, bool use_only_vel, RobotSimu *other_robot) :
        TrackRobot<Bayesian_filter::Linear_uncorrelated_observe_model>(x_size, rob_id, use_only_vel, other_robot){}
   
      virtual ~TrackRobotUncorr() {}

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

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