00001
00002 #ifndef DDF_TEST_NODESTRACKROBOT_HPP
00003 #define DDF_TEST_NODESTRACKROBOT_HPP
00004
00005 #include "sensorsimu.hpp"
00006 #include "robotsimu.hpp"
00007 #include "ddf/tools.hpp"
00008
00009 namespace jafar
00010 {
00011 namespace ddfsimu
00012 {
00013
00014 template<typename T_OBS>
00015 class TrackRobot : public SensorSimu<T_OBS>
00016 {
00017 RobotSimu *m_target_robot;
00018 unsigned short m_rob_id;
00019 bool m_use_only_vel;
00020
00021 protected:
00022 using T_OBS::Hx;
00023 using SensorSimu<T_OBS>::GetProcTime;
00024 using SensorSimu<T_OBS>::GetMeasureSize;
00025 using SensorSimu<T_OBS>::GetID;
00026 using SensorSimu<T_OBS>::GetName;
00027
00028 public:
00029 TrackRobot(unsigned short x_size, unsigned rob_id, bool use_only_vel, RobotSimu *other_robot):
00030 SensorSimu<T_OBS>(x_size, 3),
00031 m_target_robot(other_robot),
00032 m_rob_id(rob_id),
00033 m_use_only_vel(use_only_vel)
00034
00035 {
00036 int i,j;
00037
00038 JFR_PRECOND(rob_id != other_robot->GetRobID(), "wrong robot");
00039 this->m_type_name = SENSOR_TRACK_ROBOT;
00040 Hx.clear();
00041
00042 for(i = 0,j = 0; i < 6; i = i+2,j++) {
00043 Hx(j,6*m_rob_id+i) = -1.0;
00044 Hx(j,6*other_robot->GetRobID()+i) = 1.0;
00045 }
00046
00047 }
00048
00049 virtual ~TrackRobot() {}
00050
00051
00052 void Perceive(VEC const& proc_state, time t)
00053 {
00054 int i,j;
00055 VEC true_pos(3);
00056 VEC other_rob_pos(3);
00057 VEC measured_pos(GetMeasureSize());
00058
00059 if(m_use_only_vel){
00060 for(i = 0, j = 0; i < 3; i++, j = j+2) true_pos[i] = proc_state[j];
00061 }
00062 else {
00063 true_pos[0] = proc_state[AccelMotion::SV_X];
00064 true_pos[1] = proc_state[AccelMotion::SV_Y];
00065 true_pos[2] = proc_state[AccelMotion::SV_Z];
00066 }
00067
00068 other_rob_pos = m_target_robot->ComputePositionAtTimeT(t.to_double());
00069 for(i = 0; i < 3; i++) measured_pos[i] = other_rob_pos[i] - true_pos[i];
00070
00071 JFR_DEBUG( "Meas: " << measured_pos << "\n");
00072 AddMeasurementWithNoise(t, t+GetProcTime(), measured_pos);
00073 }
00074 void PerceiveRet(VEC const& proc_state, VEC & result) const { }
00075 void PrintObservationModel(){
00076 JFR_DEBUG( "Observation model of sensor " << GetID() << " (" << GetName() << ")" << "\n");
00077 PrettyPrintMatrix(Hx, "Hx");
00078 }
00079 };
00080
00081 class TrackRobotUncorr : public TrackRobot<Bayesian_filter::Linear_uncorrelated_observe_model>
00082 {
00083 public:
00084 TrackRobotUncorr(unsigned short x_size, unsigned short rob_id, bool use_only_vel, RobotSimu *other_robot) :
00085 TrackRobot<Bayesian_filter::Linear_uncorrelated_observe_model>(x_size, rob_id, use_only_vel, other_robot){}
00086
00087 virtual ~TrackRobotUncorr() {}
00088
00089 void ComputeInformation(VEC const& z, VEC &i, MSYM &I) { compute_information_uncorr(*this, z, i, I); }
00090 void SetSensorModelVar(VEC const& var) { Zv = var; }
00091 void PrintObservationModel()
00092 {
00093 TrackRobot<Bayesian_filter::Linear_uncorrelated_observe_model>::PrintObservationModel();
00094 PrettyPrintVector(Zv, "Zv");
00095 }
00096 };
00097
00098 }
00099 }
00100 #endif