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 #include "people_tracking_filter/sysmodel_pos_vel.h"
00039
00040
00041 using namespace std;
00042 using namespace BFL;
00043 using namespace tf;
00044
00045
00046 static const unsigned int NUM_SYS_POS_VEL_COND_ARGS = 1;
00047 static const unsigned int DIM_SYS_POS_VEL = 6;
00048
00049
00050
00051 SysPdfPosVel::SysPdfPosVel(const StatePosVel& sigma)
00052 : ConditionalPdf<StatePosVel, StatePosVel>(DIM_SYS_POS_VEL, NUM_SYS_POS_VEL_COND_ARGS),
00053 noise_(StatePosVel(Vector3(0, 0, 0), Vector3(0, 0, 0)), sigma)
00054 {}
00055
00056
00057
00058
00059 SysPdfPosVel::~SysPdfPosVel()
00060 {}
00061
00062
00063
00064 Probability
00065 SysPdfPosVel::ProbabilityGet(const StatePosVel& state) const
00066 {
00067 cerr << "SysPdfPosVel::ProbabilityGet Method not applicable" << endl;
00068 assert(0);
00069 return 0;
00070 }
00071
00072
00073 bool
00074 SysPdfPosVel::SampleFrom(Sample<StatePosVel>& one_sample, int method, void *args) const
00075 {
00076 StatePosVel& res = one_sample.ValueGet();
00077
00078
00079 res = this->ConditionalArgumentGet(0);
00080
00081
00082 res.pos_ += (res.vel_ * dt_);
00083
00084
00085 Sample<StatePosVel> noise_sample;
00086 noise_.SetDt(dt_);
00087 noise_.SampleFrom(noise_sample, method, args);
00088 res += noise_sample.ValueGet();
00089
00090 return true;
00091 }
00092
00093
00094 StatePosVel
00095 SysPdfPosVel::ExpectedValueGet() const
00096 {
00097 cerr << "SysPdfPosVel::ExpectedValueGet Method not applicable" << endl;
00098 assert(0);
00099 return StatePosVel();
00100
00101 }
00102
00103 SymmetricMatrix
00104 SysPdfPosVel::CovarianceGet() const
00105 {
00106 cerr << "SysPdfPosVel::CovarianceGet Method not applicable" << endl;
00107 SymmetricMatrix Covar(DIM_SYS_POS_VEL);
00108 assert(0);
00109 return Covar;
00110 }
00111