Added GaussMarkovProcess

examples/CMakeLists.txt

@@ -7,6 +7,9 @@ find_package(ignition-math${IGN_MATH_VER} REQUIRED)
 add_executable(angle_example angle_example.cc)
 target_link_libraries(angle_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
 
+add_executable(gauss_markov_process gauss_markov_process_example.cc)
+target_link_libraries(gauss_markov_process ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
+
 add_executable(vector2_example vector2_example.cc)
 target_link_libraries(vector2_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
 

examples/gauss_markov_process_example.cc

@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+//! [complete]
+#include <iostream>
+#include <ignition/math/GaussMarkovProcess.hh>
+
+// You can plot the data generated by this program by following these
+// steps.
+//
+// 1. Run this program and save the output to a file:
+//     ./gauss_markov_process > plot.data
+//
+// 2. Use gnuplot to create a plot:
+//     gnuplot -e 'set terminal jpeg; plot "plot.data" with lines' > out.jpg
+int main(int argc, char **argv)
+{
+  // Create the process with:
+  //   * Start value of 20.2
+  //   * Theta (rate at which the process should approach the mean) of 0.1
+  //   * Mu (mean value) 0.
+  //   * Sigma (volatility) of 0.5.
+  ignition::math::GaussMarkovProcess gmp(20.2, 0.1, 0, 0.5);
+
+  std::chrono::steady_clock::duration dt = std::chrono::milliseconds(100);
+
+  // This process should decrease toward the mean value of 0.
+  // With noise of 0.5, the process will walk a bit.
+  for (int i = 0; i < 1000; ++i)
+  {
+    double value = gmp.Update(dt);
+    std::cout << value << std::endl;
+  }
+
+  return 0;
+}
+//! [complete]

include/ignition/math/GaussMarkovProcess.hh

@@ -0,0 +1,146 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef IGNITION_MATH_GAUSSMARKOVPROCESS_HH_
+#define IGNITION_MATH_GAUSSMARKOVPROCESS_HH_
+
+#include <chrono>
+#include <memory>
+#include <ignition/math/Export.hh>
+#include <ignition/math/config.hh>
+
+namespace ignition
+{
+  namespace math
+  {
+    // Use a steady clock
+    using clock = std::chrono::steady_clock;
+
+    // Inline bracket to help doxygen filtering.
+    inline namespace IGNITION_MATH_VERSION_NAMESPACE {
+    //
+    // Forward declarations.
+    class GaussMarkovProcessPrivate;
+
+    /** \class GaussMarkovProcess GaussMarkovProcess.hh\
+     * ignition/math/GaussMarkovProcess.hh
+     **/
+    /// \brief Implementation of a stationary gauss-markov process, also
+    /// known as a Ornstein–Uhlenbeck process.
+    ///
+    /// See the Update(const clock::duration &) for details on the forumla
+    /// used to update the process.
+    ///
+    /// ## Example usage
+    ///
+    /// \snippet examples/gauss_markov_process_example.cc complete
+    class IGNITION_MATH_VISIBLE GaussMarkovProcess
+    {
+      // Default constructor. This sets all the parameters to zero.
+      public: GaussMarkovProcess();
+
+      /// \brief Create a process with the provided process parameters.
+      /// This will also call Set(), and in turn Reset().
+      /// \param[in] _start The start value of the process.
+      /// \param[in] _theta The theta (\f$\theta\f$) parameter.
+      /// \param[in] _mu The mu (\f$\mu\f$) parameter.
+      /// \param[in] _sigma The sigma (\f$\sigma\f$) parameter.
+      /// \sa Update(const clock::duration &)
+      public: GaussMarkovProcess(double _start, double _theta, double _mu,
+                  double _sigma);
+
+      /// \brief Destructor.
+      public: ~GaussMarkovProcess();
+
+      /// \brief Set the process parameters. This will also call Reset().
+      /// \param[in] _start The start value of the process.
+      /// \param[in] _theta The theta (\f$\theta\f$) parameter.
+      /// \param[in] _mu The mu (\f$\mu\f$) parameter.
+      /// \param[in] _sigma The sigma (\f$\sigma\f$) parameter.
+      /// \sa Update(const clock::duration &)
+      public: void Set(double _start, double _theta, double _mu, double _sigma);
+
+      /// \brief Get the start value.
+      /// \return The start value.
+      /// \sa Set(double, double, double, double)
+      public: double Start() const;
+
+      /// \brief Get the current process value.
+      /// \return The value of the process.
+      public: double Value() const;
+
+      /// \brief Get the theta (\f$\theta\f$) value.
+      /// \return The theta value.
+      /// \sa Set(double, double, double, double)
+      public: double Theta() const;
+
+      /// \brief Get the mu (\f$\mu\f$) value.
+      /// \return The mu value.
+      /// \sa Set(double, double, double, double)
+      public: double Mu() const;
+
+      /// \brief Get the sigma (\f$\sigma\f$) value.
+      /// \return The sigma value.
+      /// \sa Set(double, double, double, double)
+      public: double Sigma() const;
+
+      /// \brief Reset the process. This will set the current process value
+      /// to the start value.
+      public: void Reset();
+
+      /// \brief Update the process and get the new value.
+      ///
+      /// The following equation is computed:
+      ///
+      /// \f$x_{t+1} += \theta * (\mu - x_t) * dt + \sigma * dW_t\f$
+      ///
+      /// where
+      ///
+      ///   * \f$\theta, \mu, \sigma\f$ are parameters specified by the
+      ///     user. In order, the parameters are theta, mu, and sigma. Theta
+      ///     and sigma must be greater than or equal to zero. You can think
+      ///     of mu as representing the mean or equilibrium value, sigma as the
+      ///     degree of volatility, and theta as the rate by which changes
+      ///     dissipate and revert towards the mean.
+      ///   * \f$dt\f$ is the time step in seconds.
+      ///   * \f$dW_t\f$ is a random number drawm from a normal distribution
+      ///     with mean of zero and variance of 1.
+      ///   * \f$x_t\f$ is the current value of the Gauss-Markov process
+      ///   * \f$x_{t+1}\f$ is the new value of the Gauss-Markvov process
+      ///
+      /// See also: https://en.wikipedia.org/wiki/Ornstein%E2%80%93Uhlenbeck_process
+      /// \param[in] _time The next sample time.
+      /// \return The new value of this process.
+      public: double Update(const clock::duration &_dt);
+
+      public: double Update(double _dt);
+
+#ifdef _WIN32
+// Disable warning C4251 which is triggered by
+// std::unique_ptr
+#pragma warning(push)
+#pragma warning(disable: 4251)
+#endif
+      /// \brief Private data pointer.
+      private: std::unique_ptr<GaussMarkovProcessPrivate> dataPtr;
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
+    };
+    }
+  }
+}
+#endif

src/CMakeLists.txt

@@ -23,6 +23,7 @@ if (SWIG_FOUND)
   set(swig_files
     ruby
     Angle
+    GaussMarkovProcess
     Vector2
     Vector3)
 endif()

src/GaussMarkovProcess.cc

@@ -0,0 +1,126 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <ignition/math/GaussMarkovProcess.hh>
+#include <ignition/math/Rand.hh>
+
+using namespace ignition::math;
+
+//////////////////////////////////////////////////
+class ignition::math::GaussMarkovProcessPrivate
+{
+  /// \brief Current process value.
+  public: double value{0};
+
+  /// \brief Process start value.
+  public: double start{0};
+
+  /// \brief Process theta value.
+  public: double theta{0};
+
+  /// \brief Process mu value.
+  public: double mu{0};
+
+  /// \brief Process sigma value.
+  public: double sigma{0};
+};
+
+//////////////////////////////////////////////////
+GaussMarkovProcess::GaussMarkovProcess()
+  : dataPtr(new GaussMarkovProcessPrivate)
+{
+}
+
+//////////////////////////////////////////////////
+GaussMarkovProcess::GaussMarkovProcess(double _start, double _theta, double _mu,
+    double _sigma)
+  : dataPtr(new GaussMarkovProcessPrivate)
+{
+  this->Set(_start, _theta, _mu, _sigma);
+}
+
+//////////////////////////////////////////////////
+GaussMarkovProcess::~GaussMarkovProcess()
+{
+}
+
+//////////////////////////////////////////////////
+void GaussMarkovProcess::Set(double _start, double _theta, double _mu,
+    double _sigma)
+{
+  this->dataPtr->start = _start;
+  this->dataPtr->theta = std::max(0.0, _theta);
+  this->dataPtr->mu = _mu;
+  this->dataPtr->sigma = std::max(0.0, _sigma);
+  this->Reset();
+}
+
+//////////////////////////////////////////////////
+void GaussMarkovProcess::Reset()
+{
+  this->dataPtr->value = this->dataPtr->start;
+}
+
+//////////////////////////////////////////////////
+double GaussMarkovProcess::Start() const
+{
+  return this->dataPtr->start;
+}
+
+//////////////////////////////////////////////////
+double GaussMarkovProcess::Value() const
+{
+  return this->dataPtr->value;
+}
+
+//////////////////////////////////////////////////
+double GaussMarkovProcess::Theta() const
+{
+  return this->dataPtr->theta;
+}
+
+//////////////////////////////////////////////////
+double GaussMarkovProcess::Mu() const
+{
+  return this->dataPtr->mu;
+}
+
+//////////////////////////////////////////////////
+double GaussMarkovProcess::Sigma() const
+{
+  return this->dataPtr->sigma;
+}
+
+//////////////////////////////////////////////////
+double GaussMarkovProcess::Update(const clock::duration &_dt)
+{
+  // Time difference in seconds
+  return this->Update(std::chrono::duration<double>(_dt).count());
+}
+
+//////////////////////////////////////////////////
+double GaussMarkovProcess::Update(double _dt)
+{
+  // This equation comes from:
+  // https://en.wikipedia.org/wiki/Ornstein%E2%80%93Uhlenbeck_process
+  this->dataPtr->value += this->dataPtr->theta *
+    (this->dataPtr->mu - this->dataPtr->value) * _dt +
+    this->dataPtr->sigma * Rand::DblNormal(0, 1);
+
+  // Output the new value.
+  return this->dataPtr->value;
+}

src/GaussMarkovProcess.i

@@ -0,0 +1,43 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+%module gaussMarkovProcess
+%{
+#include <ignition/math/GaussMarkovProcess.hh>
+%}
+
+namespace ignition
+{
+  namespace math
+  {
+    class GaussMarkovProcess
+    {
+      public: GaussMarkovProcess();
+      public: GaussMarkovProcess(double _start, double _theta, double _mu,
+                  double _sigma);
+      public: ~GaussMarkovProcess();
+      public: void Set(double _start, double _theta, double _mu, double _sigma);
+      public: double Start() const;
+      public: double Value() const;
+      public: double Theta() const;
+      public: double Mu() const;
+      public: double Sigma() const;
+      public: void Reset();
+      public: double Update(double _dt);
+    };
+  }
+}