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 <robot_pose_ekf/odom_estimation.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 namespace estimation
00047 {
00048
00049 OdomEstimation::OdomEstimation():
00050 prior_(NULL),
00051 filter_(NULL),
00052 filter_initialized_(false),
00053 odom_initialized_(false),
00054 imu_initialized_(false),
00055 vo_initialized_(false),
00056 gps_initialized_(false)
00057 {
00058
00059 ColumnVector sysNoise_Mu(6); sysNoise_Mu = 0;
00060 SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;
00061 for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000.0,2);
00062 Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
00063 sys_pdf_ = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty);
00064 sys_model_ = new AnalyticSystemModelGaussianUncertainty(sys_pdf_);
00065
00066
00067 ColumnVector measNoiseOdom_Mu(6); measNoiseOdom_Mu = 0;
00068 SymmetricMatrix measNoiseOdom_Cov(6); measNoiseOdom_Cov = 0;
00069 for (unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1;
00070 Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);
00071 Matrix Hodom(6,6); Hodom = 0;
00072 Hodom(1,1) = 1; Hodom(2,2) = 1; Hodom(6,6) = 1;
00073 odom_meas_pdf_ = new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom);
00074 odom_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(odom_meas_pdf_);
00075
00076
00077 ColumnVector measNoiseImu_Mu(3); measNoiseImu_Mu = 0;
00078 SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
00079 for (unsigned int i=1; i<=3; i++) measNoiseImu_Cov(i,i) = 1;
00080 Gaussian measurement_Uncertainty_Imu(measNoiseImu_Mu, measNoiseImu_Cov);
00081 Matrix Himu(3,6); Himu = 0;
00082 Himu(1,4) = 1; Himu(2,5) = 1; Himu(3,6) = 1;
00083 imu_meas_pdf_ = new LinearAnalyticConditionalGaussian(Himu, measurement_Uncertainty_Imu);
00084 imu_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(imu_meas_pdf_);
00085
00086
00087 ColumnVector measNoiseVo_Mu(6); measNoiseVo_Mu = 0;
00088 SymmetricMatrix measNoiseVo_Cov(6); measNoiseVo_Cov = 0;
00089 for (unsigned int i=1; i<=6; i++) measNoiseVo_Cov(i,i) = 1;
00090 Gaussian measurement_Uncertainty_Vo(measNoiseVo_Mu, measNoiseVo_Cov);
00091 Matrix Hvo(6,6); Hvo = 0;
00092 Hvo(1,1) = 1; Hvo(2,2) = 1; Hvo(3,3) = 1; Hvo(4,4) = 1; Hvo(5,5) = 1; Hvo(6,6) = 1;
00093 vo_meas_pdf_ = new LinearAnalyticConditionalGaussian(Hvo, measurement_Uncertainty_Vo);
00094 vo_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(vo_meas_pdf_);
00095
00096
00097
00098
00099 ColumnVector measNoiseGps_Mu(3); measNoiseGps_Mu = 0;
00100 SymmetricMatrix measNoiseGps_Cov(3); measNoiseGps_Cov = 0;
00101 for (unsigned int i=1; i<=3; i++) measNoiseGps_Cov(i,i) = 1;
00102 Gaussian measurement_Uncertainty_GPS(measNoiseGps_Mu, measNoiseGps_Cov);
00103 Matrix Hgps(3,6); Hgps = 0;
00104 Hgps(1,1) = 1; Hgps(2,2) = 1; Hgps(3,3) = 1;
00105 gps_meas_pdf_ = new LinearAnalyticConditionalGaussian(Hgps, measurement_Uncertainty_GPS);
00106 gps_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(gps_meas_pdf_);
00107 };
00108
00109
00110
00111
00112 OdomEstimation::~OdomEstimation(){
00113 if (filter_) delete filter_;
00114 if (prior_) delete prior_;
00115 delete odom_meas_model_;
00116 delete odom_meas_pdf_;
00117 delete imu_meas_model_;
00118 delete imu_meas_pdf_;
00119 delete vo_meas_model_;
00120 delete vo_meas_pdf_;
00121 delete gps_meas_model_;
00122 delete gps_meas_pdf_;
00123 delete sys_pdf_;
00124 delete sys_model_;
00125 };
00126
00127
00128
00129 void OdomEstimation::initialize(const Transform& prior, const Time& time)
00130 {
00131
00132 ColumnVector prior_Mu(6);
00133 decomposeTransform(prior, prior_Mu(1), prior_Mu(2), prior_Mu(3), prior_Mu(4), prior_Mu(5), prior_Mu(6));
00134 SymmetricMatrix prior_Cov(6);
00135 for (unsigned int i=1; i<=6; i++) {
00136 for (unsigned int j=1; j<=6; j++){
00137 if (i==j) prior_Cov(i,j) = pow(0.001,2);
00138 else prior_Cov(i,j) = 0;
00139 }
00140 }
00141 prior_ = new Gaussian(prior_Mu,prior_Cov);
00142 filter_ = new ExtendedKalmanFilter(prior_);
00143
00144
00145 addMeasurement(StampedTransform(prior, time, "odom_combined", "base_footprint"));
00146 filter_estimate_old_vec_ = prior_Mu;
00147 filter_estimate_old_ = prior;
00148 filter_time_old_ = time;
00149
00150
00151 filter_initialized_ = true;
00152 }
00153
00154
00155
00156
00157
00158
00159 bool OdomEstimation::update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const Time& filter_time, bool& diagnostics_res)
00160 {
00161
00162 if (!filter_initialized_){
00163 ROS_INFO("Cannot update filter when filter was not initialized first.");
00164 return false;
00165 }
00166
00167
00168 double dt = (filter_time - filter_time_old_).toSec();
00169 if (dt == 0) return false;
00170 if (dt < 0){
00171 ROS_INFO("Will not update robot pose with time %f sec in the past.", dt);
00172 return false;
00173 }
00174 ROS_DEBUG("Update filter at time %f with dt %f", filter_time.toSec(), dt);
00175
00176
00177
00178
00179
00180 ColumnVector vel_desi(2); vel_desi = 0;
00181 filter_->Update(sys_model_, vel_desi);
00182
00183
00184
00185
00186 ROS_DEBUG("Process odom meas");
00187 if (odom_active){
00188 if (!transformer_.canTransform("base_footprint","wheelodom", filter_time)){
00189 ROS_ERROR("filter time older than odom message buffer");
00190 return false;
00191 }
00192 transformer_.lookupTransform("wheelodom", "base_footprint", filter_time, odom_meas_);
00193 if (odom_initialized_){
00194
00195 Transform odom_rel_frame = Transform(tf::createQuaternionFromYaw(filter_estimate_old_vec_(6)),
00196 filter_estimate_old_.getOrigin()) * odom_meas_old_.inverse() * odom_meas_;
00197 ColumnVector odom_rel(6);
00198 decomposeTransform(odom_rel_frame, odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
00199 angleOverflowCorrect(odom_rel(6), filter_estimate_old_vec_(6));
00200
00201 odom_meas_pdf_->AdditiveNoiseSigmaSet(odom_covariance_ * pow(dt,2));
00202
00203 ROS_DEBUG("Update filter with odom measurement %f %f %f %f %f %f",
00204 odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
00205 filter_->Update(odom_meas_model_, odom_rel);
00206 diagnostics_odom_rot_rel_ = odom_rel(6);
00207 }
00208 else{
00209 odom_initialized_ = true;
00210 diagnostics_odom_rot_rel_ = 0;
00211 }
00212 odom_meas_old_ = odom_meas_;
00213 }
00214
00215 else odom_initialized_ = false;
00216
00217
00218
00219
00220 if (imu_active){
00221 if (!transformer_.canTransform("base_footprint","imu", filter_time)){
00222 ROS_ERROR("filter time older than imu message buffer");
00223 return false;
00224 }
00225 transformer_.lookupTransform("imu", "base_footprint", filter_time, imu_meas_);
00226 if (imu_initialized_){
00227
00228 Transform imu_rel_frame;
00229 if (gps_active)
00230 imu_rel_frame = imu_meas_;
00231 else
00232 imu_rel_frame = filter_estimate_old_ * imu_meas_old_.inverse() * imu_meas_;
00233
00234
00235 ColumnVector imu_rel(3); double tmp;
00236 decomposeTransform(imu_rel_frame, tmp, tmp, tmp, tmp, tmp, imu_rel(3));
00237 decomposeTransform(imu_meas_, tmp, tmp, tmp, imu_rel(1), imu_rel(2), tmp);
00238 angleOverflowCorrect(imu_rel(3), filter_estimate_old_vec_(6));
00239 diagnostics_imu_rot_rel_ = imu_rel(3);
00240
00241 imu_meas_pdf_->AdditiveNoiseSigmaSet(imu_covariance_ * pow(dt,2));
00242 filter_->Update(imu_meas_model_, imu_rel);
00243 }
00244 else{
00245 imu_initialized_ = true;
00246 diagnostics_imu_rot_rel_ = 0;
00247 }
00248 imu_meas_old_ = imu_meas_;
00249 }
00250
00251 else imu_initialized_ = false;
00252
00253
00254
00255
00256
00257 if (vo_active){
00258 if (!transformer_.canTransform("base_footprint","vo", filter_time)){
00259 ROS_ERROR("filter time older than vo message buffer");
00260 return false;
00261 }
00262 transformer_.lookupTransform("vo", "base_footprint", filter_time, vo_meas_);
00263 if (vo_initialized_){
00264
00265 Transform vo_rel_frame = filter_estimate_old_ * vo_meas_old_.inverse() * vo_meas_;
00266 ColumnVector vo_rel(6);
00267 decomposeTransform(vo_rel_frame, vo_rel(1), vo_rel(2), vo_rel(3), vo_rel(4), vo_rel(5), vo_rel(6));
00268 angleOverflowCorrect(vo_rel(6), filter_estimate_old_vec_(6));
00269
00270 vo_meas_pdf_->AdditiveNoiseSigmaSet(vo_covariance_ * pow(dt,2));
00271 filter_->Update(vo_meas_model_, vo_rel);
00272 }
00273 else vo_initialized_ = true;
00274 vo_meas_old_ = vo_meas_;
00275 }
00276
00277 else vo_initialized_ = false;
00278
00279
00280
00281
00282
00283 if (gps_active){
00284 if (!transformer_.canTransform("base_footprint","gps", filter_time)){
00285 ROS_ERROR("filter time older than gps message buffer");
00286 return false;
00287 }
00288 transformer_.lookupTransform("gps", "base_footprint", filter_time, gps_meas_);
00289 if (gps_initialized_){
00290 gps_meas_pdf_->AdditiveNoiseSigmaSet(gps_covariance_ * pow(dt,2));
00291 ColumnVector gps_vec(3);
00292 double tmp;
00293
00294 decomposeTransform(gps_meas_, gps_vec(1), gps_vec(2), gps_vec(3), tmp, tmp, tmp);
00295 filter_->Update(gps_meas_model_, gps_vec);
00296 }
00297 else {
00298 gps_initialized_ = true;
00299 gps_meas_old_ = gps_meas_;
00300 }
00301 }
00302
00303 else gps_initialized_ = false;
00304
00305
00306
00307
00308 filter_estimate_old_vec_ = filter_->PostGet()->ExpectedValueGet();
00309 tf::Quaternion q;
00310 q.setRPY(filter_estimate_old_vec_(4), filter_estimate_old_vec_(5), filter_estimate_old_vec_(6));
00311 filter_estimate_old_ = Transform(q,
00312 Vector3(filter_estimate_old_vec_(1), filter_estimate_old_vec_(2), filter_estimate_old_vec_(3)));
00313 filter_time_old_ = filter_time;
00314 addMeasurement(StampedTransform(filter_estimate_old_, filter_time, "odom_combined", "base_footprint"));
00315
00316
00317 diagnostics_res = true;
00318 if (odom_active && imu_active){
00319 double diagnostics = fabs(diagnostics_odom_rot_rel_ - diagnostics_imu_rot_rel_)/dt;
00320 if (diagnostics > 0.3 && dt > 0.01){
00321 diagnostics_res = false;
00322 }
00323 }
00324
00325 return true;
00326 };
00327
00328 void OdomEstimation::addMeasurement(const StampedTransform& meas)
00329 {
00330 ROS_DEBUG("AddMeasurement from %s to %s: (%f, %f, %f) (%f, %f, %f, %f)",
00331 meas.frame_id_.c_str(), meas.child_frame_id_.c_str(),
00332 meas.getOrigin().x(), meas.getOrigin().y(), meas.getOrigin().z(),
00333 meas.getRotation().x(), meas.getRotation().y(),
00334 meas.getRotation().z(), meas.getRotation().w());
00335 transformer_.setTransform( meas );
00336 }
00337
00338 void OdomEstimation::addMeasurement(const StampedTransform& meas, const MatrixWrapper::SymmetricMatrix& covar)
00339 {
00340
00341 for (unsigned int i=0; i<covar.rows(); i++){
00342 if (covar(i+1,i+1) == 0){
00343 ROS_ERROR("Covariance specified for measurement on topic %s is zero", meas.child_frame_id_.c_str());
00344 return;
00345 }
00346 }
00347
00348 addMeasurement(meas);
00349 if (meas.child_frame_id_ == "wheelodom") odom_covariance_ = covar;
00350 else if (meas.child_frame_id_ == "imu") imu_covariance_ = covar;
00351 else if (meas.child_frame_id_ == "vo") vo_covariance_ = covar;
00352 else if (meas.child_frame_id_ == "gps") gps_covariance_ = covar;
00353 else ROS_ERROR("Adding a measurement for an unknown sensor %s", meas.child_frame_id_.c_str());
00354 };
00355
00356
00357
00358 void OdomEstimation::getEstimate(ColumnVector& estimate)
00359 {
00360 estimate = filter_estimate_old_vec_;
00361 };
00362
00363
00364 void OdomEstimation::getEstimate(Time time, Transform& estimate)
00365 {
00366 StampedTransform tmp;
00367 if (!transformer_.canTransform("base_footprint","odom_combined", time)){
00368 ROS_ERROR("Cannot get transform at time %f", time.toSec());
00369 return;
00370 }
00371 transformer_.lookupTransform("odom_combined", "base_footprint", time, tmp);
00372 estimate = tmp;
00373 };
00374
00375
00376 void OdomEstimation::getEstimate(Time time, StampedTransform& estimate)
00377 {
00378 if (!transformer_.canTransform("odom_combined", "base_footprint", time)){
00379 ROS_ERROR("Cannot get transform at time %f", time.toSec());
00380 return;
00381 }
00382 transformer_.lookupTransform("odom_combined", "base_footprint", time, estimate);
00383 };
00384
00385
00386 void OdomEstimation::getEstimate(geometry_msgs::PoseWithCovarianceStamped& estimate)
00387 {
00388
00389 StampedTransform tmp;
00390 if (!transformer_.canTransform("odom_combined", "base_footprint", ros::Time())){
00391 ROS_ERROR("Cannot get transform at time %f", 0.0);
00392 return;
00393 }
00394 transformer_.lookupTransform("odom_combined", "base_footprint", ros::Time(), tmp);
00395 poseTFToMsg(tmp, estimate.pose.pose);
00396
00397
00398 estimate.header.stamp = tmp.stamp_;
00399 estimate.header.frame_id = "odom";
00400
00401
00402 SymmetricMatrix covar = filter_->PostGet()->CovarianceGet();
00403 for (unsigned int i=0; i<6; i++)
00404 for (unsigned int j=0; j<6; j++)
00405 estimate.pose.covariance[6*i+j] = covar(i+1,j+1);
00406 };
00407
00408
00409 void OdomEstimation::angleOverflowCorrect(double& a, double ref)
00410 {
00411 while ((a-ref) > M_PI) a -= 2*M_PI;
00412 while ((a-ref) < -M_PI) a += 2*M_PI;
00413 };
00414
00415
00416 void OdomEstimation::decomposeTransform(const StampedTransform& trans,
00417 double& x, double& y, double&z, double&Rx, double& Ry, double& Rz){
00418 x = trans.getOrigin().x();
00419 y = trans.getOrigin().y();
00420 z = trans.getOrigin().z();
00421 trans.getBasis().getEulerYPR(Rz, Ry, Rx);
00422 };
00423
00424
00425 void OdomEstimation::decomposeTransform(const Transform& trans,
00426 double& x, double& y, double&z, double&Rx, double& Ry, double& Rz){
00427 x = trans.getOrigin().x();
00428 y = trans.getOrigin().y();
00429 z = trans.getOrigin().z();
00430 trans.getBasis().getEulerYPR(Rz, Ry, Rx);
00431 };
00432
00433 };