Change covariance model

diff_drive_controller/CMakeLists.txt

@@ -49,12 +49,16 @@ include_directories(
   include ${catkin_INCLUDE_DIRS}
   include ${Boost_INCLUDE_DIRS}
   include ${EIGEN_INCLUDE_DIRS}
-  include ${sophus_INCLUDE_DIRS}
   include ${CERES_INCLUDE_DIRS})
 
 roslint_cpp()
 
-add_library(${PROJECT_NAME} src/diff_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp)
+add_library(${PROJECT_NAME}
+  src/diff_drive_controller.cpp
+  src/linear_meas_covariance_model.cpp
+  src/quadratic_meas_covariance_model.cpp
+  src/odometry.cpp
+  src/speed_limiter.cpp)
 # Note that the entry for ${Ceres_LIBRARIES} was removed as we only used headers from that package
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)

diff_drive_controller/cfg/DiffDriveController.cfg

@@ -16,6 +16,8 @@ gen.add("right_wheel_radius_multiplier",  double_t, 0, "Right wheel radius multi
 gen.add("k_l",  double_t, 0, "Covariance model k_l multiplier.", 1.0, 0.0, 10.0)
 gen.add("k_r",  double_t, 0, "Covariance model k_r multiplier.", 1.0, 0.0, 10.0)
 
+gen.add("wheel_resolution",  double_t, 0, "Wheel position resolution [rad] (used in the Covariance model).", 0.0, 0.0, 10.0)
+
 gen.add("publish_cmd_vel_limited", bool_t, 0, "Publish limited velocity command.", False)
 gen.add("publish_state", bool_t, 0, "Publish joint trajectory controller state.", False)
 

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h

@@ -205,6 +205,8 @@ namespace diff_drive_controller
       double k_l;
       double k_r;
 
+      double wheel_resolution;
+
       bool publish_state;
       bool publish_cmd_vel_limited;
 
@@ -216,8 +218,9 @@ namespace diff_drive_controller
         , wheel_separation_multiplier(1.0)
         , left_wheel_radius_multiplier(1.0)
         , right_wheel_radius_multiplier(1.0)
-        , k_l(1.0)
-        , k_r(1.0)
+        , k_l(0.1)
+        , k_r(0.1)
+        , wheel_resolution(0.0001)
         , publish_state(false)
         , publish_cmd_vel_limited(false)
         , control_frequency_desired(0.0)
@@ -241,6 +244,8 @@ namespace diff_drive_controller
     double k_l_;
     double k_r_;
 
+    double wheel_resolution_;  // [rad]
+
     /// Timeout to consider cmd_vel commands old:
     double cmd_vel_timeout_;
 

diff_drive_controller/include/diff_drive_controller/linear_meas_covariance_model.h

@@ -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_ */

diff_drive_controller/include/diff_drive_controller/meas_covariance_model.h

@@ -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_ */

diff_drive_controller/include/diff_drive_controller/odometry.h

@@ -50,6 +50,8 @@
 #include <boost/accumulators/statistics/rolling_mean.hpp>
 #include <boost/function.hpp>
 
+#include <diff_drive_controller/meas_covariance_model.h>
+
 #include <diff_drive_controller/integrate_function.h>
 
 namespace diff_drive_controller
@@ -59,19 +61,6 @@ namespace diff_drive_controller
   /**
    * \brief The Odometry class handles odometry readings
    * (2D pose and velocity with related timestamp)
-   *
-   * 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.
    */
   class Odometry
   {
@@ -85,7 +74,7 @@ namespace diff_drive_controller
     typedef Covariance PoseCovariance;
     typedef Covariance TwistCovariance;
 
-    typedef Eigen::Matrix2d MeasCovariance;
+    typedef MeasCovarianceModel::MeasCovariance MeasCovariance;
 
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
@@ -259,8 +248,11 @@ namespace diff_drive_controller
      * \brief Sets the Measurement Covariance Model parameters: k_l and k_r
      * \param[in] k_l Left  wheel velocity multiplier
      * \param[in] k_r Right wheel velocity multiplier
+     * \param[in] wheel_resolution Wheel resolution [rad] (assumed the same for
+     *                             both wheels
      */
-    void setMeasCovarianceParams(const double k_l, const double k_r);
+    void setMeasCovarianceParams(const double k_l, const double k_r,
+        const double wheel_resolution);
 
     /**
      * \brief Velocity rolling window size setter
@@ -295,13 +287,6 @@ namespace diff_drive_controller
      */
     void updateIncrementalPose(const double dp_l, const double dp_r);
 
-    /**
-     * \brief Update the measurement covariance
-     * \param[in] dp_l Left  wheel position increment [rad]
-     * \param[in] dp_r Right wheel position increment [rad]
-     */
-    void updateMeasCovariance(const double dp_l, const double dp_r);
-
     /**
      * \brief Reset linear and angular accumulators
      */
@@ -339,18 +324,14 @@ namespace diff_drive_controller
     TwistCovariance twist_covariance_;
     TwistCovariance minimum_twist_covariance_;
 
-    /// Measurement covariance:
-    MeasCovariance meas_covariance_;
+    /// Meas(urement) Covariance Model:
+    boost::shared_ptr<MeasCovarianceModel> meas_covariance_model_;
 
     /// Wheel kinematic parameters [m]:
     double wheel_separation_;
     double left_wheel_radius_;
     double right_wheel_radius_;
 
-    /// Measurement Covariance Model parameters:
-    double k_l_;
-    double k_r_;
-
     /// Previous wheel position/state [rad]:
     double left_position_previous_;
     double right_position_previous_;