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