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