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 "cob_people_tracking_filter/tracker_kalman.h"
00038 #include <cob_perception_msgs/PositionMeasurement.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
00090 Gaussian meas_noise(meas_mu, meas_sigma);
00091 meas_pdf_ = new LinearAnalyticConditionalGaussian(meas_matrix, meas_noise);
00092 meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(meas_pdf_);
00093 };
00094
00095
00096
00097
00098 TrackerKalman::~TrackerKalman(){
00099 if (filter_) delete filter_;
00100 if (sys_pdf_) delete sys_pdf_;
00101 if (sys_model_) delete sys_model_;
00102 if (meas_pdf_) delete meas_pdf_;
00103 if (meas_model_) delete meas_model_;
00104 };
00105
00106
00107
00108
00109 void TrackerKalman::initialize(const StatePosVel& mu, const StatePosVel& sigma, const double time)
00110 {
00111 ColumnVector mu_vec(6);
00112 SymmetricMatrix sigma_vec(6); sigma_vec = 0;
00113 for (unsigned int i=0; i<3; i++){
00114 mu_vec(i+1) = mu.pos_[i];
00115 mu_vec(i+4) = mu.vel_[i];
00116 sigma_vec(i+1,i+1) = pow(sigma.pos_[i],2);
00117 sigma_vec(i+4,i+4) = pow(sigma.vel_[i],2);
00118 }
00119 prior_ = Gaussian(mu_vec, sigma_vec);
00120 filter_ = new ExtendedKalmanFilter(&prior_);
00121
00122
00123 tracker_initialized_ = true;
00124 quality_ = 1;
00125 filter_time_ = time;
00126 init_time_ = time;
00127 }
00128
00129
00130
00131
00132
00133 bool TrackerKalman::updatePrediction(const double time)
00134 {
00135 bool res = true;
00136 if (time > filter_time_){
00137
00138 for (unsigned int i=1; i<=3; i++)
00139 sys_matrix_(i, i+3) = time - filter_time_;
00140 sys_pdf_->MatrixSet(0, sys_matrix_);
00141
00142
00143 sys_pdf_->AdditiveNoiseSigmaSet(sys_sigma_ * pow(time - filter_time_,2));
00144 filter_time_ = time;
00145
00146
00147 res = filter_->Update(sys_model_);
00148 if(!res){ quality_ = 0;}
00149 else {quality_ = calculateQuality();ROS_INFO("quality by prediction is %f", quality_);}
00150 }
00151 return res;
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(); ROS_INFO("quality by correction is %f", quality_);}
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(cob_perception_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.vel.x = tmp(4);
00195 est.vel.y = tmp(5);
00196 est.vel.z = tmp(6);
00197
00198 est.header.stamp.fromSec( filter_time_ );
00199 est.object_id = getName();
00200 }
00201
00202
00203
00204
00205 double TrackerKalman::calculateQuality()
00206 {
00207 double sigma_max = 0;
00208 SymmetricMatrix cov = filter_->PostGet()->CovarianceGet();
00209 for (unsigned int i=1; i<=2; i++)
00210 sigma_max = max(sigma_max, sqrt(cov(i,i)));
00211
00212 return 1.0 - min(1.0, sigma_max / 1.5);
00213 }
00214
00215
00216 double TrackerKalman::getLifetime() const
00217 {
00218 if (tracker_initialized_)
00219 return filter_time_ - init_time_;
00220 else
00221 return 0;
00222 }
00223
00224 double TrackerKalman::getTime() const
00225 {
00226 if (tracker_initialized_)
00227 return filter_time_;
00228 else
00229 return 0;
00230 }
00231
00232 };
00233
00234