From 5df5318a26e42cbd8cb19dc76dfdc5eafad64144 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Matou=C5=A1=20Vrba?= Date: Fri, 16 Feb 2024 11:39:07 +0100 Subject: [PATCH] [DKF]: updated the implementation to be more general, fixed some errors, added an example --- CMakeLists.txt | 7 ++ include/mrs_lib/dkf.h | 20 +++-- src/dkf/example.cpp | 171 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 190 insertions(+), 8 deletions(-) create mode 100644 src/dkf/example.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6e3e0237..92dd848c 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/include/mrs_lib/dkf.h b/include/mrs_lib/dkf.h index bf84aaa4..cfcb3325 100644 --- a/include/mrs_lib/dkf.h +++ b/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 + class DKF : public LKF { public: /* DKF definitions (typedefs, constants etc) //{ */ - using Base_class = LKF; /*!< \brief Base class of this class. */ + using Base_class = LKF; /*!< \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; using mat3_t = Eigen::Matrix; 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,7 +86,7 @@ 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}; @@ -91,13 +94,14 @@ namespace mrs_lib 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::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); }; //} }; diff --git a/src/dkf/example.cpp b/src/dkf/example.cpp new file mode 100644 index 00000000..f0e1466d --- /dev/null +++ b/src/dkf/example.cpp @@ -0,0 +1,171 @@ +// clang: MatousFormat +/** \file + \brief Example file for the DKF implementation + \author Matouš Vrba - vrbamato@fel.cvut.cz + + 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 +#include +#include + +// Define the KF we will be using +namespace mrs_lib +{ + const int n_states = 6; + const int n_inputs = 0; + + using dkf_t = DKF; +} + +// 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 +Eigen::Matrix normal_randmat(const Eigen::Matrix& cov) +{ + static std::random_device rd{}; + static std::mt19937 gen{rd()}; + static std::normal_distribution<> d{0,1}; + Eigen::Matrix 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 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; +} + +