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 #include "people_tracking_filter/tracker_kalman.h"
00038
00039 using namespace MatrixWrapper;
00040 using namespace BFL;
00041 using namespace tf;
00042 using namespace std;
00043 using namespace ros;
00044
00045
00046 const static double damping_velocity = 0.9;
00047
00048
00049 namespace estimation
00050 {
00051
00052 TrackerKalman::TrackerKalman(const string& name, const StatePosVel& sysnoise):
00053 Tracker(name),
00054 filter_(NULL),
00055 sys_pdf_(NULL),
00056 sys_model_(NULL),
00057 meas_pdf_(NULL),
00058 meas_model_(NULL),
00059 sys_matrix_(6, 6),
00060 tracker_initialized_(false)
00061 {
00062
00063 sys_matrix_ = 0;
00064 for (unsigned int i = 1; i <= 3; i++)
00065 {
00066 sys_matrix_(i, i) = 1;
00067 sys_matrix_(i + 3, i + 3) = damping_velocity;
00068 }
00069 ColumnVector sys_mu(6);
00070 sys_mu = 0;
00071 sys_sigma_ = SymmetricMatrix(6);
00072 sys_sigma_ = 0;
00073 for (unsigned int i = 0; i < 3; i++)
00074 {
00075 sys_sigma_(i + 1, i + 1) = pow(sysnoise.pos_[i], 2);
00076 sys_sigma_(i + 4, i + 4) = pow(sysnoise.vel_[i], 2);
00077 }
00078 Gaussian sys_noise(sys_mu, sys_sigma_);
00079 sys_pdf_ = new LinearAnalyticConditionalGaussian(sys_matrix_, sys_noise);
00080 sys_model_ = new LinearAnalyticSystemModelGaussianUncertainty(sys_pdf_);
00081
00082
00083
00084 Matrix meas_matrix(3, 6);
00085 meas_matrix = 0;
00086 for (unsigned int i = 1; i <= 3; i++)
00087 meas_matrix(i, i) = 1;
00088
00089 ColumnVector meas_mu(3);
00090 meas_mu = 0;
00091 SymmetricMatrix meas_sigma(3);
00092 meas_sigma = 0;
00093 for (unsigned int i = 0; i < 3; i++)
00094 meas_sigma(i + 1, i + 1) = 0;
00095 Gaussian meas_noise(meas_mu, meas_sigma);
00096 meas_pdf_ = new LinearAnalyticConditionalGaussian(meas_matrix, meas_noise);
00097 meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(meas_pdf_);
00098 };
00099
00100
00101
00102
00103 TrackerKalman::~TrackerKalman()
00104 {
00105 if (filter_) delete filter_;
00106 if (sys_pdf_) delete sys_pdf_;
00107 if (sys_model_) delete sys_model_;
00108 if (meas_pdf_) delete meas_pdf_;
00109 if (meas_model_) delete meas_model_;
00110 };
00111
00112
00113
00114
00115 void TrackerKalman::initialize(const StatePosVel& mu, const StatePosVel& sigma, const double time)
00116 {
00117 ColumnVector mu_vec(6);
00118 SymmetricMatrix sigma_vec(6);
00119 sigma_vec = 0;
00120 for (unsigned int i = 0; i < 3; i++)
00121 {
00122 mu_vec(i + 1) = mu.pos_[i];
00123 mu_vec(i + 4) = mu.vel_[i];
00124 sigma_vec(i + 1, i + 1) = pow(sigma.pos_[i], 2);
00125 sigma_vec(i + 4, i + 4) = pow(sigma.vel_[i], 2);
00126 }
00127 prior_ = Gaussian(mu_vec, sigma_vec);
00128 filter_ = new ExtendedKalmanFilter(&prior_);
00129
00130
00131 tracker_initialized_ = true;
00132 quality_ = 1;
00133 filter_time_ = time;
00134 init_time_ = time;
00135 }
00136
00137
00138
00139
00140
00141 bool TrackerKalman::updatePrediction(const double time)
00142 {
00143 bool res = true;
00144 if (time > filter_time_)
00145 {
00146
00147 for (unsigned int i = 1; i <= 3; i++)
00148 sys_matrix_(i, i + 3) = time - filter_time_;
00149 sys_pdf_->MatrixSet(0, sys_matrix_);
00150
00151
00152 sys_pdf_->AdditiveNoiseSigmaSet(sys_sigma_ * pow(time - filter_time_, 2));
00153 filter_time_ = time;
00154
00155
00156 res = filter_->Update(sys_model_);
00157 if (!res) quality_ = 0;
00158 else quality_ = calculateQuality();
00159 }
00160 return res;
00161 };
00162
00163
00164
00165
00166 bool TrackerKalman::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov)
00167 {
00168 assert(cov.columns() == 3);
00169
00170
00171 ColumnVector meas_vec(3);
00172 for (unsigned int i = 0; i < 3; i++)
00173 meas_vec(i + 1) = meas[i];
00174
00175
00176 ((LinearAnalyticConditionalGaussian*)(meas_model_->MeasurementPdfGet()))->AdditiveNoiseSigmaSet(cov);
00177
00178
00179 bool res = filter_->Update(meas_model_, meas_vec);
00180 if (!res) quality_ = 0;
00181 else quality_ = calculateQuality();
00182
00183 return res;
00184 };
00185
00186
00187 void TrackerKalman::getEstimate(StatePosVel& est) const
00188 {
00189 ColumnVector tmp = filter_->PostGet()->ExpectedValueGet();
00190 for (unsigned int i = 0; i < 3; i++)
00191 {
00192 est.pos_[i] = tmp(i + 1);
00193 est.vel_[i] = tmp(i + 4);
00194 }
00195 };
00196
00197
00198 void TrackerKalman::getEstimate(people_msgs::PositionMeasurement& est) const
00199 {
00200 ColumnVector tmp = filter_->PostGet()->ExpectedValueGet();
00201
00202 est.pos.x = tmp(1);
00203 est.pos.y = tmp(2);
00204 est.pos.z = tmp(3);
00205
00206 est.header.stamp.fromSec(filter_time_);
00207 est.object_id = getName();
00208 }
00209
00210
00211
00212
00213 double TrackerKalman::calculateQuality()
00214 {
00215 double sigma_max = 0;
00216 SymmetricMatrix cov = filter_->PostGet()->CovarianceGet();
00217 for (unsigned int i = 1; i <= 2; i++)
00218 sigma_max = max(sigma_max, sqrt(cov(i, i)));
00219
00220 return 1.0 - min(1.0, sigma_max / 1.5);
00221 }
00222
00223
00224 double TrackerKalman::getLifetime() const
00225 {
00226 if (tracker_initialized_)
00227 return filter_time_ - init_time_;
00228 else
00229 return 0;
00230 }
00231
00232 double TrackerKalman::getTime() const
00233 {
00234 if (tracker_initialized_)
00235 return filter_time_;
00236 else
00237 return 0;
00238 }
00239
00240 };
00241
00242