odom_estimation.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Wim Meeussen */
36 
38 
39 using namespace MatrixWrapper;
40 using namespace BFL;
41 using namespace tf;
42 using namespace std;
43 using namespace ros;
44 
45 
46 namespace estimation
47 {
48  // constructor
49  OdomEstimation::OdomEstimation():
50  prior_(NULL),
51  filter_(NULL),
52  filter_initialized_(false),
53  odom_initialized_(false),
54  imu_initialized_(false),
55  vo_initialized_(false),
56  gps_initialized_(false),
57  output_frame_(std::string("odom_combined")),
58  base_footprint_frame_(std::string("base_footprint"))
59  {
60  // create SYSTEM MODEL
61  ColumnVector sysNoise_Mu(6); sysNoise_Mu = 0;
62  SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;
63  for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000.0,2);
64  Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
65  sys_pdf_ = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty);
67 
68  // create MEASUREMENT MODEL ODOM
69  ColumnVector measNoiseOdom_Mu(6); measNoiseOdom_Mu = 0;
70  SymmetricMatrix measNoiseOdom_Cov(6); measNoiseOdom_Cov = 0;
71  for (unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1;
72  Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);
73  Matrix Hodom(6,6); Hodom = 0;
74  Hodom(1,1) = 1; Hodom(2,2) = 1; Hodom(6,6) = 1;
75  odom_meas_pdf_ = new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom);
77 
78  // create MEASUREMENT MODEL IMU
79  ColumnVector measNoiseImu_Mu(3); measNoiseImu_Mu = 0;
80  SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
81  for (unsigned int i=1; i<=3; i++) measNoiseImu_Cov(i,i) = 1;
82  Gaussian measurement_Uncertainty_Imu(measNoiseImu_Mu, measNoiseImu_Cov);
83  Matrix Himu(3,6); Himu = 0;
84  Himu(1,4) = 1; Himu(2,5) = 1; Himu(3,6) = 1;
85  imu_meas_pdf_ = new LinearAnalyticConditionalGaussian(Himu, measurement_Uncertainty_Imu);
87 
88  // create MEASUREMENT MODEL VO
89  ColumnVector measNoiseVo_Mu(6); measNoiseVo_Mu = 0;
90  SymmetricMatrix measNoiseVo_Cov(6); measNoiseVo_Cov = 0;
91  for (unsigned int i=1; i<=6; i++) measNoiseVo_Cov(i,i) = 1;
92  Gaussian measurement_Uncertainty_Vo(measNoiseVo_Mu, measNoiseVo_Cov);
93  Matrix Hvo(6,6); Hvo = 0;
94  Hvo(1,1) = 1; Hvo(2,2) = 1; Hvo(3,3) = 1; Hvo(4,4) = 1; Hvo(5,5) = 1; Hvo(6,6) = 1;
95  vo_meas_pdf_ = new LinearAnalyticConditionalGaussian(Hvo, measurement_Uncertainty_Vo);
97 
98  // create MEASUREMENT MODEL GPS
99  ColumnVector measNoiseGps_Mu(3); measNoiseGps_Mu = 0;
100  SymmetricMatrix measNoiseGps_Cov(3); measNoiseGps_Cov = 0;
101  for (unsigned int i=1; i<=3; i++) measNoiseGps_Cov(i,i) = 1;
102  Gaussian measurement_Uncertainty_GPS(measNoiseGps_Mu, measNoiseGps_Cov);
103  Matrix Hgps(3,6); Hgps = 0;
104  Hgps(1,1) = 1; Hgps(2,2) = 1; Hgps(3,3) = 1;
105  gps_meas_pdf_ = new LinearAnalyticConditionalGaussian(Hgps, measurement_Uncertainty_GPS);
107  };
108 
109 
110 
111  // destructor
113  if (filter_) delete filter_;
114  if (prior_) delete prior_;
115  delete odom_meas_model_;
116  delete odom_meas_pdf_;
117  delete imu_meas_model_;
118  delete imu_meas_pdf_;
119  delete vo_meas_model_;
120  delete vo_meas_pdf_;
121  delete gps_meas_model_;
122  delete gps_meas_pdf_;
123  delete sys_pdf_;
124  delete sys_model_;
125  };
126 
127 
128  // initialize prior density of filter
129  void OdomEstimation::initialize(const Transform& prior, const Time& time)
130  {
131  // set prior of filter
132  ColumnVector prior_Mu(6);
133  decomposeTransform(prior, prior_Mu(1), prior_Mu(2), prior_Mu(3), prior_Mu(4), prior_Mu(5), prior_Mu(6));
134  SymmetricMatrix prior_Cov(6);
135  for (unsigned int i=1; i<=6; i++) {
136  for (unsigned int j=1; j<=6; j++){
137  if (i==j) prior_Cov(i,j) = pow(0.001,2);
138  else prior_Cov(i,j) = 0;
139  }
140  }
141  prior_ = new Gaussian(prior_Mu,prior_Cov);
143 
144  // remember prior
146  filter_estimate_old_vec_ = prior_Mu;
147  filter_estimate_old_ = prior;
148  filter_time_old_ = time;
149 
150  // filter initialized
151  filter_initialized_ = true;
152  }
153 
154 
155 
156 
157 
158  // update filter
159  bool OdomEstimation::update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const Time& filter_time, bool& diagnostics_res)
160  {
161  // only update filter when it is initialized
162  if (!filter_initialized_){
163  ROS_INFO("Cannot update filter when filter was not initialized first.");
164  return false;
165  }
166 
167  // only update filter for time later than current filter time
168  double dt = (filter_time - filter_time_old_).toSec();
169  if (dt == 0) return false;
170  if (dt < 0){
171  ROS_INFO("Will not update robot pose with time %f sec in the past.", dt);
172  return false;
173  }
174  ROS_DEBUG("Update filter at time %f with dt %f", filter_time.toSec(), dt);
175 
176 
177  // system update filter
178  // --------------------
179  // for now only add system noise
180  ColumnVector vel_desi(2); vel_desi = 0;
181  filter_->Update(sys_model_, vel_desi);
182 
183 
184  // process odom measurement
185  // ------------------------
186  ROS_DEBUG("Process odom meas");
187  if (odom_active){
188  if (!transformer_.canTransform(base_footprint_frame_,"wheelodom", filter_time)){
189  ROS_ERROR("filter time older than odom message buffer");
190  return false;
191  }
192  transformer_.lookupTransform("wheelodom", base_footprint_frame_, filter_time, odom_meas_);
193  if (odom_initialized_){
194  // convert absolute odom measurements to relative odom measurements in horizontal plane
197  ColumnVector odom_rel(6);
198  decomposeTransform(odom_rel_frame, odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
200  // update filter
202 
203  ROS_DEBUG("Update filter with odom measurement %f %f %f %f %f %f",
204  odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
205  filter_->Update(odom_meas_model_, odom_rel);
206  diagnostics_odom_rot_rel_ = odom_rel(6);
207  }
208  else{
209  odom_initialized_ = true;
211  }
213  }
214  // sensor not active
215  else odom_initialized_ = false;
216 
217 
218  // process imu measurement
219  // -----------------------
220  if (imu_active){
221  if (!transformer_.canTransform(base_footprint_frame_,"imu", filter_time)){
222  ROS_ERROR("filter time older than imu message buffer");
223  return false;
224  }
226  if (imu_initialized_){
227  // convert absolute imu yaw measurement to relative imu yaw measurement
229  ColumnVector imu_rel(3); double tmp;
230  decomposeTransform(imu_rel_frame, tmp, tmp, tmp, tmp, tmp, imu_rel(3));
231  decomposeTransform(imu_meas_, tmp, tmp, tmp, imu_rel(1), imu_rel(2), tmp);
233  diagnostics_imu_rot_rel_ = imu_rel(3);
234  // update filter
236  filter_->Update(imu_meas_model_, imu_rel);
237  }
238  else{
239  imu_initialized_ = true;
241  }
243  }
244  // sensor not active
245  else imu_initialized_ = false;
246 
247 
248 
249  // process vo measurement
250  // ----------------------
251  if (vo_active){
252  if (!transformer_.canTransform(base_footprint_frame_,"vo", filter_time)){
253  ROS_ERROR("filter time older than vo message buffer");
254  return false;
255  }
257  if (vo_initialized_){
258  // convert absolute vo measurements to relative vo measurements
260  ColumnVector vo_rel(6);
261  decomposeTransform(vo_rel_frame, vo_rel(1), vo_rel(2), vo_rel(3), vo_rel(4), vo_rel(5), vo_rel(6));
263  // update filter
265  filter_->Update(vo_meas_model_, vo_rel);
266  }
267  else vo_initialized_ = true;
269  }
270  // sensor not active
271  else vo_initialized_ = false;
272 
273 
274 
275  // process gps measurement
276  // ----------------------
277  if (gps_active){
278  if (!transformer_.canTransform(base_footprint_frame_,"gps", filter_time)){
279  ROS_ERROR("filter time older than gps message buffer");
280  return false;
281  }
283  if (gps_initialized_){
285  ColumnVector gps_vec(3);
286  double tmp;
287  //Take gps as an absolute measurement, do not convert to relative measurement
288  decomposeTransform(gps_meas_, gps_vec(1), gps_vec(2), gps_vec(3), tmp, tmp, tmp);
289  filter_->Update(gps_meas_model_, gps_vec);
290  }
291  else {
292  gps_initialized_ = true;
294  }
295  }
296  // sensor not active
297  else gps_initialized_ = false;
298 
299 
300 
301  // remember last estimate
303  tf::Quaternion q;
307  filter_time_old_ = filter_time;
309 
310  // diagnostics
311  diagnostics_res = true;
312  if (odom_active && imu_active){
313  double diagnostics = fabs(diagnostics_odom_rot_rel_ - diagnostics_imu_rot_rel_)/dt;
314  if (diagnostics > 0.3 && dt > 0.01){
315  diagnostics_res = false;
316  }
317  }
318 
319  return true;
320  };
321 
323  {
324  ROS_DEBUG("AddMeasurement from %s to %s: (%f, %f, %f) (%f, %f, %f, %f)",
325  meas.frame_id_.c_str(), meas.child_frame_id_.c_str(),
326  meas.getOrigin().x(), meas.getOrigin().y(), meas.getOrigin().z(),
327  meas.getRotation().x(), meas.getRotation().y(),
328  meas.getRotation().z(), meas.getRotation().w());
329  transformer_.setTransform( meas );
330  }
331 
332  void OdomEstimation::addMeasurement(const StampedTransform& meas, const MatrixWrapper::SymmetricMatrix& covar)
333  {
334  // check covariance
335  for (unsigned int i=0; i<covar.rows(); i++){
336  if (covar(i+1,i+1) == 0){
337  ROS_ERROR("Covariance specified for measurement on topic %s is zero", meas.child_frame_id_.c_str());
338  return;
339  }
340  }
341  // add measurements
342  addMeasurement(meas);
343  if (meas.child_frame_id_ == "wheelodom") odom_covariance_ = covar;
344  else if (meas.child_frame_id_ == "imu") imu_covariance_ = covar;
345  else if (meas.child_frame_id_ == "vo") vo_covariance_ = covar;
346  else if (meas.child_frame_id_ == "gps") gps_covariance_ = covar;
347  else ROS_ERROR("Adding a measurement for an unknown sensor %s", meas.child_frame_id_.c_str());
348  };
349 
350 
351  // get latest filter posterior as vector
352  void OdomEstimation::getEstimate(MatrixWrapper::ColumnVector& estimate)
353  {
354  estimate = filter_estimate_old_vec_;
355  };
356 
357  // get filter posterior at time 'time' as Transform
359  {
360  StampedTransform tmp;
362  ROS_ERROR("Cannot get transform at time %f", time.toSec());
363  return;
364  }
366  estimate = tmp;
367  };
368 
369  // get filter posterior at time 'time' as Stamped Transform
371  {
373  ROS_ERROR("Cannot get transform at time %f", time.toSec());
374  return;
375  }
377  };
378 
379  // get most recent filter posterior as PoseWithCovarianceStamped
380  void OdomEstimation::getEstimate(geometry_msgs::PoseWithCovarianceStamped& estimate)
381  {
382  // pose
383  StampedTransform tmp;
385  ROS_ERROR("Cannot get transform at time %f", 0.0);
386  return;
387  }
389  poseTFToMsg(tmp, estimate.pose.pose);
390 
391  // header
392  estimate.header.stamp = tmp.stamp_;
393  estimate.header.frame_id = output_frame_;
394 
395  // covariance
396  SymmetricMatrix covar = filter_->PostGet()->CovarianceGet();
397  for (unsigned int i=0; i<6; i++)
398  for (unsigned int j=0; j<6; j++)
399  estimate.pose.covariance[6*i+j] = covar(i+1,j+1);
400  };
401 
402  // correct for angle overflow
403  void OdomEstimation::angleOverflowCorrect(double& a, double ref)
404  {
405  while ((a-ref) > M_PI) a -= 2*M_PI;
406  while ((a-ref) < -M_PI) a += 2*M_PI;
407  };
408 
409  // decompose Transform into x,y,z,Rx,Ry,Rz
411  double& x, double& y, double&z, double&Rx, double& Ry, double& Rz){
412  x = trans.getOrigin().x();
413  y = trans.getOrigin().y();
414  z = trans.getOrigin().z();
415  trans.getBasis().getEulerYPR(Rz, Ry, Rx);
416  };
417 
418  // decompose Transform into x,y,z,Rx,Ry,Rz
420  double& x, double& y, double&z, double&Rx, double& Ry, double& Rz){
421  x = trans.getOrigin().x();
422  y = trans.getOrigin().y();
423  z = trans.getOrigin().z();
424  trans.getBasis().getEulerYPR(Rz, Ry, Rx);
425  };
426 
427  void OdomEstimation::setOutputFrame(const std::string& output_frame){
428  output_frame_ = output_frame;
429  };
430 
431  void OdomEstimation::setBaseFootprintFrame(const std::string& base_frame){
432  base_footprint_frame_ = base_frame;
433  };
434 
435 }; // namespace
MatrixWrapper::ColumnVector filter_estimate_old_vec_
virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const
tf::StampedTransform odom_meas_old_
virtual ~OdomEstimation()
destructor
virtual MatrixWrapper::ColumnVector ExpectedValueGet() const
tf::StampedTransform imu_meas_old_
BFL::LinearAnalyticMeasurementModelGaussianUncertainty * odom_meas_model_
void angleOverflowCorrect(double &a, double ref)
correct for angle overflow
tf::StampedTransform gps_meas_old_
void getEulerYPR(tfScalar &yaw, tfScalar &pitch, tfScalar &roll, unsigned int solution_number=1) const
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
tf::StampedTransform vo_meas_old_
void initialize(const tf::Transform &prior, const ros::Time &time)
bool setTransform(const StampedTransform &transform, const std::string &authority="default_authority")
BFL::LinearAnalyticMeasurementModelGaussianUncertainty * vo_meas_model_
MatrixWrapper::SymmetricMatrix odom_covariance_
BFL::LinearAnalyticConditionalGaussian * vo_meas_pdf_
tf::StampedTransform vo_meas_
void decomposeTransform(const tf::StampedTransform &trans, double &x, double &y, double &z, double &Rx, double &Ry, double &Rz)
BFL::AnalyticSystemModelGaussianUncertainty * sys_model_
bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const
BFL::LinearAnalyticConditionalGaussian * gps_meas_pdf_
BFL::LinearAnalyticMeasurementModelGaussianUncertainty * gps_meas_model_
BFL::LinearAnalyticMeasurementModelGaussianUncertainty * imu_meas_model_
TFSIMD_FORCE_INLINE const tfScalar & x() const
MatrixWrapper::SymmetricMatrix vo_covariance_
std::string child_frame_id_
TFSIMD_FORCE_INLINE const tfScalar & z() const
static Quaternion createQuaternionFromYaw(double yaw)
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
#define ROS_INFO(...)
#define M_PI
TFSIMD_FORCE_INLINE const tfScalar & y() const
MatrixWrapper::SymmetricMatrix imu_covariance_
bool update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const ros::Time &filter_time, bool &diagnostics_res)
Transform inverse() const
tf::StampedTransform odom_meas_
virtual Gaussian * PostGet()
void addMeasurement(const tf::StampedTransform &meas)
tf::Transform filter_estimate_old_
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
BFL::LinearAnalyticConditionalGaussian * imu_meas_pdf_
BFL::ExtendedKalmanFilter * filter_
tf::StampedTransform gps_meas_
Quaternion getRotation() const
BFL::LinearAnalyticConditionalGaussian * odom_meas_pdf_
void setBaseFootprintFrame(const std::string &base_frame)
virtual bool Update(SystemModel< StateVar > *const sysmodel, const StateVar &u, MeasurementModel< MeasVar, StateVar > *const measmodel, const MeasVar &z, const StateVar &s)
MatrixWrapper::SymmetricMatrix gps_covariance_
BFL::NonLinearAnalyticConditionalGaussianOdo * sys_pdf_
void setOutputFrame(const std::string &output_frame)
#define ROS_ERROR(...)
void AdditiveNoiseSigmaSet(const MatrixWrapper::SymmetricMatrix &sigma)
void getEstimate(MatrixWrapper::ColumnVector &estimate)
tf::StampedTransform imu_meas_
std::string frame_id_
#define ROS_DEBUG(...)


robot_pose_ekf
Author(s): Wim Meeussen, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:38