-
Notifications
You must be signed in to change notification settings - Fork 525
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #33 from clearpathrobotics/change_covariance_model
Create linear/quadratic meas cov model classes
- Loading branch information
Showing
12 changed files
with
576 additions
and
61 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
93 changes: 93 additions & 0 deletions
93
diff_drive_controller/include/diff_drive_controller/linear_meas_covariance_model.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,93 @@ | ||
| /********************************************************************* | ||
| * Software License Agreement (BSD License) | ||
| * | ||
| * Copyright (c) 2016, Clearpath Robotics, Inc. | ||
| * All rights reserved. | ||
| * | ||
| * Redistribution and use in source and binary forms, with or without | ||
| * modification, are permitted provided that the following conditions | ||
| * are met: | ||
| * | ||
| * * Redistributions of source code must retain the above copyright | ||
| * notice, this list of conditions and the following disclaimer. | ||
| * * Redistributions in binary form must reproduce the above | ||
| * copyright notice, this list of conditions and the following | ||
| * disclaimer in the documentation and/or other materials provided | ||
| * with the distribution. | ||
| * * Neither the name of the PAL Robotics nor the names of its | ||
| * contributors may be used to endorse or promote products derived | ||
| * from this software without specific prior written permission. | ||
| * | ||
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| * POSSIBILITY OF SUCH DAMAGE. | ||
| *********************************************************************/ | ||
|
|
||
| /* | ||
| * Author: Enrique Fernández | ||
| */ | ||
|
|
||
| #ifndef LINEAR_MEAS_COVARIANCE_MODEL_H_ | ||
| #define LINEAR_MEAS_COVARIANCE_MODEL_H_ | ||
|
|
||
| #include <diff_drive_controller/meas_covariance_model.h> | ||
|
|
||
| namespace diff_drive_controller | ||
| { | ||
|
|
||
| /** | ||
| * \brief Linear Meas(urement) Covariance Model | ||
| * | ||
| * The odometry covariance is computed according with the model presented in: | ||
| * | ||
| * [Siegwart, 2004]: | ||
| * Roland Siegwart, Illah R. Nourbakhsh | ||
| * Introduction to Autonomous Mobile Robots | ||
| * 1st Edition, 2004 | ||
| * | ||
| * Section: | ||
| * 5.2.4 'An error model for odometric position estimation' (pp. 186-191) | ||
| * | ||
| * Although the twist covariance doesn't appear explicitly, the implementation | ||
| * here is based on the same covariance model used for the pose covariance. | ||
| * | ||
| * The model also includes the wheel resolution, as a constant additive | ||
| * diagonal covariance, that can be easily disabled by using a zero | ||
| * (ideal/perfect) wheel resolution. | ||
| */ | ||
| class LinearMeasCovarianceModel : public MeasCovarianceModel | ||
| { | ||
| public: | ||
| /** | ||
| * \brief Constructor | ||
| * \param[in] wheel_resolution Wheel resolution [rad] | ||
| */ | ||
| LinearMeasCovarianceModel(const double wheel_resolution = 0.0); | ||
|
|
||
| /** | ||
| * \brief Destructor | ||
| */ | ||
| virtual ~LinearMeasCovarianceModel() | ||
| {} | ||
|
|
||
| /** | ||
| * \brief Integrates w/o computing the jacobians | ||
| * \param [in] dp_l Left wheel position increment [rad] | ||
| * \param [in] dp_r Right wheel position increment [rad] | ||
| * \return Meas(urement) covariance | ||
| */ | ||
| const MeasCovariance& compute(const double dp_l, const double dp_r); | ||
| }; | ||
|
|
||
| } // namespace diff_drive_controller | ||
|
|
||
| #endif /* LINEAR_MEAS_COVARIANCE_MODEL_H_ */ |
143 changes: 143 additions & 0 deletions
143
diff_drive_controller/include/diff_drive_controller/meas_covariance_model.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,143 @@ | ||
| /********************************************************************* | ||
| * Software License Agreement (BSD License) | ||
| * | ||
| * Copyright (c) 2016, Clearpath Robotics, Inc. | ||
| * All rights reserved. | ||
| * | ||
| * Redistribution and use in source and binary forms, with or without | ||
| * modification, are permitted provided that the following conditions | ||
| * are met: | ||
| * | ||
| * * Redistributions of source code must retain the above copyright | ||
| * notice, this list of conditions and the following disclaimer. | ||
| * * Redistributions in binary form must reproduce the above | ||
| * copyright notice, this list of conditions and the following | ||
| * disclaimer in the documentation and/or other materials provided | ||
| * with the distribution. | ||
| * * Neither the name of the PAL Robotics nor the names of its | ||
| * contributors may be used to endorse or promote products derived | ||
| * from this software without specific prior written permission. | ||
| * | ||
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| * POSSIBILITY OF SUCH DAMAGE. | ||
| *********************************************************************/ | ||
|
|
||
| /* | ||
| * Author: Enrique Fernández | ||
| */ | ||
|
|
||
| #ifndef MEAS_COVARIANCE_MODEL_H_ | ||
| #define MEAS_COVARIANCE_MODEL_H_ | ||
|
|
||
| #include <Eigen/Core> | ||
|
|
||
| namespace diff_drive_controller | ||
| { | ||
|
|
||
| /** | ||
| * \brief Meas(urement) Covariance Model | ||
| */ | ||
| class MeasCovarianceModel | ||
| { | ||
| public: | ||
| /// Meas(urement) covariance type: | ||
| typedef Eigen::Matrix2d MeasCovariance; | ||
|
|
||
| /** | ||
| * \brief Constructor | ||
| * \param[in] wheel_resolution Wheel resolution [rad] | ||
| */ | ||
| MeasCovarianceModel(const double wheel_resolution = 0.0) | ||
| : wheel_resolution_(wheel_resolution) | ||
| {} | ||
|
|
||
| virtual ~MeasCovarianceModel() | ||
| {} | ||
|
|
||
| /** | ||
| * \brief Integrates w/o computing the jacobians | ||
| * \param [in] dp_l Left wheel position increment [rad] | ||
| * \param [in] dp_r Right wheel position increment [rad] | ||
| * \return Meas(urement) covariance | ||
| */ | ||
| virtual const MeasCovariance& compute(const double dp_l, const double dp_r) = 0; | ||
|
|
||
| /** | ||
| * \brief Left wheel covariance gain getter | ||
| * \return Left wheel covariance gain | ||
| */ | ||
| double getKl() const | ||
| { | ||
| return k_l_; | ||
| } | ||
|
|
||
| /** | ||
| * \brief Right wheel covariance gain getter | ||
| * \return Right wheel covariance gain | ||
| */ | ||
| double getKr() const | ||
| { | ||
| return k_r_; | ||
| } | ||
|
|
||
| /** | ||
| * \brief Wheel resolution getter | ||
| * \return Wheel resolution [rad] | ||
| */ | ||
| double getWheelResolution() const | ||
| { | ||
| return wheel_resolution_; | ||
| } | ||
|
|
||
| /** | ||
| * \brief Left wheel covariance gain setter | ||
| * \param[in] k_l Left wheel covariance gain | ||
| */ | ||
| void setKl(const double k_l) | ||
| { | ||
| k_l_ = k_l; | ||
| } | ||
|
|
||
| /** | ||
| * \brief Right wheel covariance gain setter | ||
| * \param[in] k_r Right wheel covariance gain | ||
| */ | ||
| void setKr(const double k_r) | ||
| { | ||
| k_r_ = k_r; | ||
| } | ||
|
|
||
| /** | ||
| * \brief Wheel resolution setter | ||
| * \param[in] wheel_resolution Wheel resolution [rad] | ||
| */ | ||
| void setWheelResolution(const double wheel_resolution) | ||
| { | ||
| wheel_resolution_ = wheel_resolution; | ||
| } | ||
|
|
||
| protected: | ||
| /// Meas(urement) covariance: | ||
| MeasCovariance meas_covariance_; | ||
|
|
||
| /// Meas(urement) Covariance Model gains: | ||
| double k_l_; | ||
| double k_r_; | ||
|
|
||
| /// Wheel resolution [rad]: | ||
| double wheel_resolution_; | ||
| }; | ||
|
|
||
| } // namespace diff_drive_controller | ||
|
|
||
| #endif /* MEAS_COVARIANCE_MODEL_H_ */ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.