Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include "srs_people_tracking_filter/sysmodel_pos_vel.h"
00040
00041
00042 using namespace std;
00043 using namespace BFL;
00044 using namespace tf;
00045
00046
00047 static const unsigned int NUM_SYS_POS_VEL_COND_ARGS = 1;
00048 static const unsigned int DIM_SYS_POS_VEL = 6;
00049
00050
00051
00052 SysPdfPosVel::SysPdfPosVel(const StatePosVel& sigma)
00053 : ConditionalPdf<StatePosVel, StatePosVel>(DIM_SYS_POS_VEL, NUM_SYS_POS_VEL_COND_ARGS),
00054 noise_(StatePosVel(Vector3(0,0,0), Vector3(0,0,0)), sigma)
00055 {}
00056
00057
00058
00059
00060 SysPdfPosVel::~SysPdfPosVel()
00061 {}
00062
00063
00064
00065 Probability
00066 SysPdfPosVel::ProbabilityGet(const StatePosVel& state) const
00067 {
00068 cerr << "SysPdfPosVel::ProbabilityGet Method not applicable" << endl;
00069 assert(0);
00070 return 0;
00071 }
00072
00073
00074 bool
00075 SysPdfPosVel::SampleFrom (Sample<StatePosVel>& one_sample, int method, void *args) const
00076 {
00077 StatePosVel& res = one_sample.ValueGet();
00078
00079
00080 res = this->ConditionalArgumentGet(0);
00081
00082
00083 res.pos_ += (res.vel_ * dt_);
00084
00085
00086 Sample<StatePosVel> noise_sample;
00087 noise_.SetDt(dt_);
00088 noise_.SampleFrom(noise_sample, method, args);
00089 res += noise_sample.ValueGet();
00090
00091 return true;
00092 }
00093
00094
00095 StatePosVel
00096 SysPdfPosVel::ExpectedValueGet() const
00097 {
00098 cerr << "SysPdfPosVel::ExpectedValueGet Method not applicable" << endl;
00099 assert(0);
00100 return StatePosVel();
00101
00102 }
00103
00104 SymmetricMatrix
00105 SysPdfPosVel::CovarianceGet() const
00106 {
00107 cerr << "SysPdfPosVel::CovarianceGet Method not applicable" << endl;
00108 SymmetricMatrix Covar(DIM_SYS_POS_VEL);
00109 assert(0);
00110 return Covar;
00111 }
00112