[DKF]: updated the implementation to be more general, fixed some erro…

…rs, added an example

CMakeLists.txt

@@ -146,6 +146,13 @@ target_link_libraries(lkf_example
   ${Eigen_LIBRARIES}
   )
 
+add_executable(dkf_example src/dkf/example.cpp)
+target_link_libraries(dkf_example
+  MrsLib_Geometry
+  ${catkin_LIBRARIES}
+  ${Eigen_LIBRARIES}
+  )
+
 # # Slows the compilation of the library waaaay too much - just generate the conversions on-demand
 # # instead of pre-generating all combinations.
 # add_library(MrsLib_VectorConverter src/vector_converter/vector_converter.cpp)

include/mrs_lib/dkf.h

@@ -23,11 +23,12 @@ namespace mrs_lib
   * \tparam n_measurements   number of measurements of the system (length of the \f$ \mathbf{z} \f$ vector).
   *
   */
-  class DKF : public LKF<3, -1, 2>
+  template <int n_states, int n_inputs>
+  class DKF : public LKF<n_states, n_inputs, -1>
   {
   public:
     /* DKF definitions (typedefs, constants etc) //{ */
-    using Base_class = LKF<n, m, p>;            /*!< \brief Base class of this class. */
+    using Base_class = LKF<n_states, n_inputs, -1>;            /*!< \brief Base class of this class. */
 
     static constexpr int n = Base_class::n;              /*!< \brief Length of the state vector of the system. */
     static constexpr int m = Base_class::m;              /*!< \brief Length of the input vector of the system. */
@@ -49,7 +50,9 @@ namespace mrs_lib
     using mat2_t = Eigen::Matrix<double, 2, 2>;
     using mat3_t = Eigen::Matrix<double, 3, 3>;
     using pt3_t = mrs_lib::geometry::vec3_t;
+    using pt2_t = mrs_lib::geometry::vec2_t;
     using vec3_t = mrs_lib::geometry::vec3_t;
+    //}
 
   public:
   /*!
@@ -69,7 +72,7 @@ namespace mrs_lib
     */
     DKF(const A_t& A, const B_t& B) : Base_class(A, B, {}) {};
 
-    /* correct() method //{ */
+    /* correctLine() method //{ */
   /*!
     * \brief Applies the correction (update, measurement, data) step of the Kalman filter.
     *
@@ -83,21 +86,22 @@ namespace mrs_lib
     * \param R           The measurement noise covariance matrix to be used for correction.
     * \return            The state and covariance after the correction update.
     */
-    virtual statecov_t correctLine(const statecov_t& sc, const pt3_t& line_origin, const vec3_t& line_direction, const double line_variance) const
+    virtual std::enable_if_t<(n > 3), statecov_t> correctLine(const statecov_t& sc, const pt3_t& line_origin, const vec3_t& line_direction, const double line_variance) const
     {
       assert(line_direction.norm() > 0.0);
       const vec3_t zunit {0.0, 0.0, 1.0};
       // rot is a rotation matrix, transforming from F to F'
       const mat3_t rot = mrs_lib::geometry::rotationBetween(line_direction, zunit);
       /* const mat3_t rotT = rot.transpose(); */
 
-      const H_t Hprime = rot.block<2, 2>(0, 0);
+      H_t H = Eigen::Matrix<double, 2, n>::Zero();
+      H.block(2, 2, 0, 0) = rot.block<2, 2>(0, 0);
 
       const pt3_t oprime = rot*line_origin;
-      const z_t z = oprime.block<2, 1>(0, 0);
+      const pt2_t z = oprime.head<2>();
 
-      const R_t R = line_variance*R_t::Identity();
-      return correction_impl(sc, z, R, Hprime);
+      const mat2_t R = line_variance*mat2_t::Identity();
+      return this->correction_impl(sc, z, R, H);
     };
     //}
   };

src/dkf/example.cpp

@@ -0,0 +1,171 @@
+// clang: MatousFormat
+/**  \file
+     \brief Example file for the DKF implementation
+     \author Matouš Vrba - [email protected]
+
+     This example may be run after building *mrs_lib* by executing `rosrun mrs_lib dkf_example`.
+
+     See \ref dkf/example.cpp.
+ */
+
+/**  \example "dkf/example.cpp"
+
+     This example may be run after building *mrs_lib* by executing `rosrun mrs_lib dkf_example`.
+ */
+
+// Include the DKF header
+#include <mrs_lib/dkf.h>
+#include <random>
+#include <ros/ros.h>
+
+// Define the KF we will be using
+namespace mrs_lib
+{
+  const int n_states = 6;
+  const int n_inputs = 0;
+
+  using dkf_t = DKF<n_states, n_inputs>;
+}
+
+// Some helpful aliases to make writing of types shorter
+using namespace mrs_lib;
+using A_t = dkf_t::A_t;
+using B_t = dkf_t::B_t;
+using H_t = dkf_t::H_t;
+using Q_t = dkf_t::Q_t;
+using x_t = dkf_t::x_t;
+using P_t = dkf_t::P_t;
+using u_t = dkf_t::u_t;
+using z_t = dkf_t::z_t;
+using R_t = dkf_t::R_t;
+using statecov_t = dkf_t::statecov_t;
+using pt3_t = dkf_t::pt3_t;
+using vec3_t = dkf_t::vec3_t;
+using mat3_t = Eigen::Matrix3d;
+
+// Some helper enums to make the code more readable
+enum x_pos
+{
+  x_x = 0,
+  x_y = 1,
+  x_z = 2,
+  x_dx = 3,
+  x_dy = 4,
+  x_dz = 5,
+};
+
+// Helper function to generate a random Eigen matrix with normal distribution
+template <int rows>
+Eigen::Matrix<double, rows, 1> normal_randmat(const Eigen::Matrix<double, rows, rows>& cov)
+{
+    static std::random_device rd{};
+    static std::mt19937 gen{rd()};
+    static std::normal_distribution<> d{0,1};
+    Eigen::Matrix<double, rows, 1> ret;
+    for (int row = 0; row < rows; row++)
+      ret(row, 0) = d(gen);
+    return cov*ret;
+}
+
+A_t A;
+
+// This function implements the state transition
+x_t tra_model_f(const x_t& x)
+{
+  return A*x;
+}
+
+struct obs_t
+{
+  pt3_t line_origin;
+  vec3_t line_direction;
+  double line_variance;
+};
+
+// This function implements the observation generation from a state
+obs_t observation(const x_t& x, const double meas_std)
+{
+  // the first three states are the x, y, z coordinates of the target's position
+  const pt3_t target = x.head<3>();
+  const vec3_t meas_noise = normal_randmat<3>(meas_std*mat3_t::Identity());
+
+  obs_t ret;
+  // generate a random observation point
+  ret.line_origin = 100.0*pt3_t::Random();
+  ret.line_direction = (target - ret.line_origin).normalized() + meas_noise;
+  ret.line_variance = meas_std;
+  return ret;
+}
+
+int main()
+{
+  // dt will be constant in this example
+  const double dt = 1.0;
+  const double sigma_a = 0.03;
+  const double sigma_R = 0.1;
+
+  // Initialize the state transition matrix
+  A = A_t::Identity();
+  A.block<3, 3>(3, 3) = dt*mat3_t::Identity();
+
+  const mat3_t Q_a = sigma_a*sigma_a*mat3_t::Identity();
+  Eigen::Matrix<double, 6, 3> B_Q;
+  B_Q.block<3, 3>(0, 0) = 0.5*dt*dt*mat3_t::Identity();
+  B_Q.block<3, 3>(3, 0) = dt*mat3_t::Identity();
+  // Initialize the process noise matrix
+  const Q_t Q = B_Q * Q_a * B_Q.transpose();
+
+  // Generate initial state and covariance
+  const x_t x0 = 100.0*x_t::Random();
+  const P_t P_tmp = P_t::Random();
+  const P_t P0 = 10.0*P_tmp*P_tmp.transpose();
+  const statecov_t sc0({x0, P0});
+
+  // Instantiate the KF itself
+  dkf_t kf(A, B_t::Zero());
+
+  const int n_its = 1e4;
+  // Prepare the ground-truth state and the estimated state and covariance
+  x_t x_gt = sc0.x;
+  statecov_t sc_est = sc0;
+
+  for (int it = 0; it < n_its; it++)
+  {
+    std::cout << "step: " << it << std::endl;
+
+    // Generate a new input vector
+    const u_t u = u_t::Random();
+
+    // Generate a new state according to the model and noise parameters
+    x_gt = tra_model_f(x_gt) + normal_randmat(Q);
+
+    // Generate a new observation according to the model and noise parameters
+    const obs_t obs = observation(x_gt, sigma_R);
+
+    // There should be a try-catch here to prevent program crashes
+    // in case of numerical instabilities (which are possible with UKF)
+    try
+    {
+      // Apply the prediction step
+      sc_est = kf.predict(sc_est, u, Q, dt);
+
+      // Apply the correction step
+      sc_est = kf.correctLine(sc_est, obs.line_origin, obs.line_direction, obs.line_variance);
+    }
+    catch (const std::exception& e)
+    {
+      // In case of error, alert the user
+      ROS_ERROR("KF failed: %s", e.what());
+    }
+
+    const auto error = (x_gt-sc_est.x).norm();
+    std::cout << "Current KF estimation error is: " << error << std::endl;
+    std::cout << "Current ground-truth state is:  " << x_gt.transpose() << std::endl;
+    std::cout << "Current KF estimated state is: " << sc_est.x.transpose() << std::endl;
+    std::cout << "Current KF state covariance is:" << std::endl << sc_est.P << std::endl;
+  }
+
+  return 0;
+}
+
+