$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Wim Meeussen */ 00036 /* Modified by Alex Noyvirt for SRS */ 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 // constructor 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 // create sys model 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 // create meas model 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 // destructor 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 // initialize prior density of filter 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 // tracker initialized 00122 tracker_initialized_ = true; 00123 quality_ = 1; 00124 filter_time_ = time; 00125 init_time_ = time; 00126 } 00127 00128 00129 00130 00131 // update filter prediction 00132 bool TrackerKalman::updatePrediction(const double time) 00133 { 00134 bool res = true; 00135 if (time > filter_time_){ 00136 // set dt in sys model 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 // scale system noise for dt 00142 sys_pdf_->AdditiveNoiseSigmaSet(sys_sigma_ * pow(time - filter_time_,2)); 00143 filter_time_ = time; 00144 00145 // update filter 00146 res = filter_->Update(sys_model_); 00147 if (!res) quality_ = 0; 00148 else quality_ = calculateQuality(); 00149 } 00150 return res; 00151 }; 00152 00153 00154 00155 // update filter correction 00156 bool TrackerKalman::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov) 00157 { 00158 assert(cov.columns() == 3); 00159 00160 // copy measurement 00161 ColumnVector meas_vec(3); 00162 for (unsigned int i=0; i<3; i++) 00163 meas_vec(i+1) = meas[i]; 00164 00165 // set covariance 00166 ((LinearAnalyticConditionalGaussian*)(meas_model_->MeasurementPdfGet()))->AdditiveNoiseSigmaSet(cov); 00167 00168 // update filter 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 }; // namespace 00230 00231