diff --git a/.clang-format b/.clang-format index ccfd92de..129df136 100644 --- a/.clang-format +++ b/.clang-format @@ -1,4 +1,5 @@ --- +# Documentation: https://releases.llvm.org/17.0.1/tools/clang/docs/ClangFormatStyleOptions.html Language: Cpp # BasedOnStyle: LLVM AccessModifierOffset: -2 @@ -132,5 +133,5 @@ StatementMacros: - QT_REQUIRE_VERSION TabWidth: 2 UseCRLF: false -UseTab: ForIndentation +UseTab: Never ... diff --git a/.github/workflows/clang-formatter.yml b/.github/workflows/clang-formatter.yml index 51e806d4..612029f3 100644 --- a/.github/workflows/clang-formatter.yml +++ b/.github/workflows/clang-formatter.yml @@ -13,15 +13,15 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 - - uses: DoozyX/clang-format-lint-action@v0.14 + - uses: actions/checkout@v3 + - uses: DoozyX/clang-format-lint-action@v0.17 with: source: '.' exclude: './lib' extensions: 'h,hpp,cpp,c' - clangFormatVersion: 14 + clangFormatVersion: 17 inplace: True - - uses: EndBug/add-and-commit@v4 + - uses: EndBug/add-and-commit@v9 with: author_name: Clang Robot author_email: robot@example.com diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 25922272..00000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,82 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(vortex-vkf) - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-local-typedefs -fopenmp) -endif() - -# Find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(Eigen3 REQUIRED) - - -# Add include -include_directories(include) - - - -add_executable(${PROJECT_NAME}_node - src/nodes/kf_node.cpp -) -ament_target_dependencies(${PROJECT_NAME}_node - rclcpp - std_msgs - geometry_msgs - Eigen3 -) - -# Let ros2 run find the executables -install( - DIRECTORY include/ - DESTINATION include -) - -# Install launch files. -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME}/ -) - -install(TARGETS - ${PROJECT_NAME}_node - DESTINATION lib/${PROJECT_NAME}) - - - -if(BUILD_TESTING) - # find_package(ament_lint_auto REQUIRED) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - # set(ament_cmake_cpplint_FOUND TRUE) - # ament_lint_auto_find_test_dependencies() - - - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(${PROJECT_NAME}_test - test/main.cpp - test/matrix_size_test.cpp - test/lti_model_test.cpp - # test/six_dof_model_test.cpp - test/ekf_test.cpp - test/ukf_test.cpp - test/erk_test.cpp - ) - target_include_directories(${PROJECT_NAME}_test PUBLIC - $ - $ - ) - ament_target_dependencies(${PROJECT_NAME}_test - std_msgs - ) - -endif() - -ament_package() diff --git a/README.md b/README.md index 6e8417cc..15e8d3c5 100644 --- a/README.md +++ b/README.md @@ -1,26 +1,8 @@ -Simple Kalman Filter for estimating objects in odom frame. Includes a general ekf implementation which can be configured for -use with generic process and sensor model. - -Kalman Filter: - -state vector: x = [x, y, z, phi, theta, psi] -measurement vector: y = [x, y, z, phi, theta, psi] - -process model: x_dot = 0 + v -measurement model: y = I_6x6 * x - - -Important Files in ekf_python2: - -dynamicmodels_py2.py: contains a parent class for dynamic models and as many different dynamic models as need to be defined -as subclasses - -measurementmodels_py2.py: contains a parent class for sensor models and as many different sensor models as need to be defined -as subclasses - -ekf_py2.py: contains a class implementing the ekf equations. In the case of linear model this will collapse to a standard Kalman Filter - -gaussparams_py2.py: contains a class which implements multivariate gaussians - -Pip3/pip requirement: -pip install dataclasses \ No newline at end of file +# Vortex VKF +## Introduction +This is a C++ library for state estimation. + +## Packages +### vortex-filtering +Currently, it has a pure C++ implementation of the Unscented Kalman Filter (UKF) and the Extended Kalman Filter (EKF) as well as model +[More info](vortex-filtering/README.md) diff --git a/docs/vkf_class_diagram.md b/docs/vkf_class_diagram.md new file mode 100644 index 00000000..60943286 --- /dev/null +++ b/docs/vkf_class_diagram.md @@ -0,0 +1,71 @@ + + +```mermaid +classDiagram + + DynamicModel <|-- DynamicModelLTV + SensorModel <|-- SensorModelLTV + DynamicModelLTV <|-- ConstantVelocity + DynamicModelLTV <|-- ConstantAcceleration + DynamicModelLTV <|-- CoordinatedTurn + + EKF -- DynamicModelLTV + EKF -- SensorModelLTV + + UKF -- DynamicModel + UKF -- SensorModel + + + + class EKF{ + +predict() + +update() + +step() + } + + class UKF{ + +predict() + +update() + +step() + -get_sigma_points() + -propagate_sigma_points_f() + -propagate_sigma_points_h() + -estimate_gaussian() + } + + class DynamicModel{ + +virtual f_d() Vec_x + +virtual Q_d() Mat_vv + +sample_f_d() Vec_x + } + + class SensorModel{ + +virtual h() Vec_z + +virtual R() Mat_ww + +sample_h() Vec_z + } + + class DynamicModelLTV { + +overide f_d() Vec_x + +virtual A_d() Mat_xx + +virtual Q_d() Mat_vv + +vurtual G_d() Mat_xv + +pred_from_est() Gauss_x + +pred_from_state() Gauss_x + } + + class SensorModelLTV { + +override h(x) Vec_z + +virtual R(x) Mat_ww + +virtual C(x) Mat_zx + +virtual H(x) Mat_zw + +pred_from_est(x_est) Gauss_z + +pred_from_state(x) Gauss_z + } + + class ConstantVelocity + class CoordinatedTurn + class ConstantAcceleration + +``` + \ No newline at end of file diff --git a/include/filters/EKF.hpp b/include/filters/EKF.hpp deleted file mode 100644 index 98639517..00000000 --- a/include/filters/EKF.hpp +++ /dev/null @@ -1,63 +0,0 @@ -#pragma once -#include -#include - -namespace Filters { -using namespace Models; - -template class EKF : public Kalman_filter_base { -public: - // These type definitions are needed because of the stupid two-phase lookup for dependent names in templates in C++ - using Base = Kalman_filter_base; - using Base::_n_x; using Base::_n_y; using Base::_n_u; using Base::_n_v; using Base::_n_w; // can be comma separated in C++17 - DEFINE_MODEL_TYPES(_n_x, _n_y, _n_u, _n_v, _n_w) - - EKF(std::shared_ptr ekf_model, State &x0, Mat_xx &P0) : Base(x0, P0), model{ekf_model} - { - } - ~EKF() {} - - State iterate(Time Ts, const Measurement &y, const Input &u = Input::Zero()) override final - { - // Calculate Jacobians F_x, F_v - Mat_xx F_x = model->F_x(Ts, this->_x, u); - Mat_xv F_v = model->F_v(Ts, this->_x, u); - Mat_vv Q = model->Q(Ts, this->_x); - // Predicted State Estimate x_k- - State x_pred = model->f(Ts, this->_x, u); - // Predicted State Covariance P_xx- - Mat_xx P_xx_pred = F_x * this->_P_xx * F_x.transpose() + F_v * Q * F_v.transpose(); - // Predicted Output y_pred - Measurement y_pred = model->h(Ts, x_pred); - - // Calculate Jacobians H_x, H_w - Mat_yx H_x = model->H_x(Ts, x_pred, u); - Mat_yw H_w = model->H_w(Ts, x_pred, u); - Mat_ww R = model->R(Ts, x_pred); - // Output Covariance P_yy - Mat_yy P_yy = H_x * P_xx_pred * H_x.transpose() + H_w * R * H_w.transpose(); - // Cross Covariance P_xy - Mat_xy P_xy = P_xx_pred * H_x.transpose(); - - // Kalman gain K - Mat_yy P_yy_inv = P_yy.llt().solve(Mat_yy::Identity()); // Use Cholesky decomposition for inverting P_yy - Mat_xy K = P_xy * P_yy_inv; - - // Corrected State Estimate x_next - State x_next = x_pred + K * (y - y_pred); - // Normalize quaternions if applicable - x_next = model->post_state_update(x_next); - // Corrected State Covariance P_xx_next - Mat_xx P_xx_next = (Mat_xx::Identity() - K * H_x) * P_xx_pred; - - // Update local state - this->_x = x_next; - this->_P_xx = P_xx_next; - - return x_next; - } - -private: - std::shared_ptr model; -}; -} // namespace Filters \ No newline at end of file diff --git a/include/filters/EKF.md b/include/filters/EKF.md deleted file mode 100644 index 654debb9..00000000 --- a/include/filters/EKF.md +++ /dev/null @@ -1,4 +0,0 @@ -# The EKF Algorithm -![Definitions](kf_definitions.png) -![Definitions](ekf_algorithm.png) - diff --git a/include/filters/Kalman_filter_base.hpp b/include/filters/Kalman_filter_base.hpp deleted file mode 100644 index 07bfb9cc..00000000 --- a/include/filters/Kalman_filter_base.hpp +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once -#include - -namespace Filters { -using namespace Models; - -template class Kalman_filter_base { -public: - static constexpr int _n_x = Model::_n_x, _n_y = Model::_n_y, _n_u = Model::_n_u, _n_v = Model::_n_v, _n_w = Model::_n_w; - DEFINE_MODEL_TYPES(_n_x, _n_y, _n_u, _n_v, _n_w) - - Kalman_filter_base(State x0, Mat_xx P0) : _x0{x0}, _P0_xx{P0}, _x{x0}, _P_xx{P0} {} - virtual ~Kalman_filter_base() {} - - virtual State iterate(Time Ts, const Measurement &y, const Input &u = Input::Zero()) = 0; - void set_state(State x_n) { _x = x_n; } - void set_covariance(Mat_xx P_n) { _P_xx = P_n; } - virtual void reset() - { - set_state(_x0); - set_covariance(_P0_xx); - } - State get_state() const { return _x; } - Mat_xx get_covariance() const { return _P_xx; } - - - -protected: - const State _x0; - const Mat_xx _P0_xx; - State _x; - Mat_xx _P_xx; -}; -} // namespace Filters \ No newline at end of file diff --git a/include/filters/UKF.hpp b/include/filters/UKF.hpp deleted file mode 100644 index 7ea8a719..00000000 --- a/include/filters/UKF.hpp +++ /dev/null @@ -1,157 +0,0 @@ -#pragma once -#include -#include -#include -#include - -namespace Filters { -using namespace Models; - -template class UKF : public Kalman_filter_base { -public: - // These type definitions are needed because of the stupid two-phase lookup for dependent names in templates in C++ - using Base = Kalman_filter_base; - using Base::_n_x; using Base::_n_y; using Base::_n_u; using Base::_n_v; using Base::_n_w; // can be comma separated in C++17 - DEFINE_MODEL_TYPES(_n_x, _n_y, _n_u, _n_v, _n_w) - static constexpr int _n_a = _n_x + _n_v + _n_w; // Size of augmented state - using Mat_aa = Matrix; - using State_a = Vector; - - UKF(std::shared_ptr model, State x0, Mat_xx P0) : Kalman_filter_base(x0, P0), model{model} {} - ~UKF() {} - -private: - // Parameters used for calculating scaling factor, _GAMMA and weights W_x0, W_c0 and W_xi - // lambda selected according to the scaled unscented transform. (van der Merwe (2004)) - static constexpr double _ALPHA_SQUARED = 1; - static constexpr double _BETA = 2; - static constexpr double _KAPPA = 0; - static constexpr double _LAMBDA = _ALPHA_SQUARED * (_n_a + _KAPPA) - _n_a; - static constexpr double _GAMMA = std::sqrt(_n_a + _LAMBDA); - - static constexpr double _W_x0 = _LAMBDA / (_n_a + _LAMBDA); - static constexpr double _W_c0 = _LAMBDA / (_n_a + _LAMBDA) + (1 - _ALPHA_SQUARED + _BETA); - static constexpr double _W_xi = 1 / (2 * (_n_a + _LAMBDA)); - static constexpr double _W_ci = 1 / (2 * (_n_a + _LAMBDA)); - - std::shared_ptr model; - - Matrix get_sigma_points(const State &x, const Mat_xx &P, const Mat_vv &Q, const Mat_ww &R) - { - // Make augmented covariance matrix - Mat_aa P_a; - // clang-format off - P_a << P , Mat_xv::Zero(), Mat_xw::Zero(), - Mat_vx::Zero(), Q , Mat_vw::Zero(), - Mat_wx::Zero(), Mat_wv::Zero(), R; - // clang-format on - Mat_aa sqrt_P_a = P_a.llt().matrixLLT(); - - // Make augmented state vector - State_a x_a; - x_a << x, Disturbance::Zero(), Noise::Zero(); - - // Calculate sigma points - Matrix sigma_points; - - // Use the symmetric sigma point set - sigma_points.col(0) = x_a; - for (size_t i = 1; i <= _n_a; i++) { - sigma_points.col(i) = x_a + _GAMMA * sqrt_P_a.col(i - 1); - sigma_points.col(i + _n_a) = x_a - _GAMMA * sqrt_P_a.col(i - 1); - } - return sigma_points; - } - -public: - State iterate(Time t, const Measurement &y, const Input &u = Input::Zero()) override final - { - Mat_vv Q = model->Q(t, this->_x); - Mat_ww R = model->R(t, this->_x); - - Matrix sigma_points = get_sigma_points(this->_x, this->_P_xx, Q, R); - - // Propagate sigma points through f - Matrix sigma_x_pred; - for (size_t i = 0; i < 2 * _n_a + 1; i++) { - auto x_i = sigma_points.template block<_n_x, 1>(0, i); - auto v_i = sigma_points.template block<_n_v, 1>(_n_x, i); - sigma_x_pred.col(i) = model->f(t, x_i, u, v_i); - } - - // Predicted State Estimate x_k- - State x_pred; - x_pred = _W_x0 * sigma_x_pred.col(0); - for (size_t i = 1; i < 2 * _n_a + 1; i++) { - x_pred += _W_xi * sigma_x_pred.col(i); - } - - // Predicted State Covariance P_xx- - Mat_xx P_xx_pred; - P_xx_pred = _W_c0 * (sigma_x_pred.col(0) - x_pred) * (sigma_x_pred.col(0) - x_pred).transpose(); - for (size_t i = 1; i < 2 * _n_a + 1; i++) { - _W_ci *(sigma_x_pred.col(i) - x_pred) * (sigma_x_pred.col(i) - x_pred).transpose(); - } - - // Propagate sigma points through h - Matrix sigma_y_pred; - for (size_t i = 0; i < 2 * _n_a + 1; i++) { - auto x_i = sigma_points.template block<_n_x, 1>(0, i); - auto w_i = sigma_points.template block<_n_w, 1>(_n_x + _n_v, i); - sigma_y_pred.col(i) = model->h(t, x_i, u, w_i); - } - - // Predicted Output y_pred - Measurement y_pred; - y_pred = _W_x0 * sigma_y_pred.col(0); - for (size_t i = 1; i < 2 * _n_a + 1; i++) { - y_pred += _W_xi * sigma_y_pred.col(i); - } - - // Output Covariance P_yy - Mat_yy P_yy; - P_yy = _W_c0 * (sigma_y_pred.col(0) - y_pred) * (sigma_y_pred.col(0) - y_pred).transpose(); - for (size_t i = 1; i < 2 * _n_a + 1; i++) { - P_yy += _W_ci * (sigma_y_pred.col(i) - y_pred) * (sigma_y_pred.col(i) - y_pred).transpose(); - } - - // Cross Covariance P_xy - Mat_xy P_xy; - P_xy = _W_c0 * (sigma_x_pred.col(0) - x_pred) * (sigma_y_pred.col(0) - y_pred).transpose(); - for (size_t i = 1; i < 2 * _n_a + 1; i++) { - P_xy += _W_ci * (sigma_x_pred.col(i) - x_pred) * (sigma_y_pred.col(i) - y_pred).transpose(); - } - - // Kalman gain K - Mat_yy P_yy_inv = P_yy.llt().solve(Mat_yy::Identity()); // Use Cholesky decomposition for inverting P_yy - Mat_xy K = P_xy * P_yy_inv; - - // Corrected State Estimate x_next - State x_next = x_pred + K * (y - y_pred); - // Normalize quaternions if applicable - x_next = model->post_state_update(x_next); - // Corrected State Covariance P_xx_next - Mat_xx P_xx_next = P_xx_pred - K * P_yy * K.transpose(); - - // Update local state - this->_x = x_next; - this->_P_xx = P_xx_next; - - return x_next; - } -}; - - -// required namespace-scope declarations to avoid linker errors -template constexpr int UKF::_n_a; -template constexpr double UKF::_ALPHA_SQUARED; -template constexpr double UKF::_BETA; -template constexpr double UKF::_KAPPA; -template constexpr double UKF::_LAMBDA; -template constexpr double UKF::_GAMMA; -template constexpr double UKF::_W_x0; -template constexpr double UKF::_W_c0; -template constexpr double UKF::_W_xi; -template constexpr double UKF::_W_ci; - -} // namespace Filters diff --git a/include/filters/ekf_algorithm.png b/include/filters/ekf_algorithm.png deleted file mode 100644 index c363fdc1..00000000 Binary files a/include/filters/ekf_algorithm.png and /dev/null differ diff --git a/include/filters/kf_definitions.png b/include/filters/kf_definitions.png deleted file mode 100644 index 97303ca8..00000000 Binary files a/include/filters/kf_definitions.png and /dev/null differ diff --git a/include/filters/ukf.png b/include/filters/ukf.png deleted file mode 100644 index ddfb2e1c..00000000 Binary files a/include/filters/ukf.png and /dev/null differ diff --git a/include/integration_methods/ERK_methods.hpp b/include/integration_methods/ERK_methods.hpp deleted file mode 100644 index ecbc00ae..00000000 --- a/include/integration_methods/ERK_methods.hpp +++ /dev/null @@ -1,225 +0,0 @@ -#pragma once -#include -#include -#include -#include - -#include - -namespace Integrator { -using namespace Models; - -// Base class RK_method for RK4 and forward_euler to derive from - - -template class None { -public: - /** - * @brief Does not integrate, just returns f(t_k, x_k). Use if f is a discrete model - */ - None() = default; - DEFINE_MODEL_TYPES(n_x, 0, 0, 0, 0) - static State integrate(State_dot f, Timestep dt, Time t_k, const State &x_k) { (void)dt; return f(t_k, x_k); } -}; -template using None_M = None; - -template class RK4 { -public: - DEFINE_MODEL_TYPES(n_x, 0, 0, 0, 0) - static State integrate(State_dot f, Timestep dt, Time t_k, const State &x_k) - { - State k1 = f(t_k, x_k); - State k2 = f(t_k + 0.5 * dt, x_k + 0.5 * dt / 1s * k1); - State k3 = f(t_k + 0.5 * dt, x_k + 0.5 * dt / 1s * k2); - State k4 = f(t_k + 1.0 * dt, x_k + 1.0 * dt / 1s * k3); - - return x_k + (dt / 1s / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4); - } -}; -template using RK4_M = RK4; - -template class Forward_Euler { -public: - DEFINE_MODEL_TYPES(n_x, 0, 0, 0, 0) - static State integrate(State_dot f, Timestep dt, Time t_k, const State &x_k) - { - return x_k + dt / 1s * f(t_k, x_k); - } -}; -template using Forward_Euler_M = Forward_Euler; - - - - -template class Butcher { -public: - DEFINE_MODEL_TYPES(n_x, 0, 0, 0, 0) - /** - * @brief Construct an ERK method from a Butcher table. Cannot be passed as a class to a template - * as it needs the non-static members A, b and c to be intitalized in an object. - * @param A - * @param b - * @param c - */ - Butcher(Eigen::Matrix A, Eigen::Vector b, Eigen::Vector c) : _A{A}, _b{b}, _c{c} - { - // Check if Butcher table is valid - if (A.isLowerTriangular() == false) { - throw std::invalid_argument("Butcher table: A is not lower triangular"); - } - - // Check if Butcher table is consistent - if (b.sum() != 1) { - throw std::invalid_argument("Butcher table: b does not sum to one"); - } - for (int i = 0; i < A.rows(); i++) { - if (std::abs(A.row(i).sum() - c(i)) > 1e-6) { - throw std::invalid_argument("Butcher table: row " + std::to_string(i) + " in A does not sum to c(" + std::to_string(i) + ")"); - } - } - } - - State integrate(State_dot f, Timestep dt, Time t_k, const State &x_k) - { - Eigen::Matrix k = Eigen::Matrix::Zero(); - for (size_t i = 0; i < n_stages; i++) { - k.col(i) = f(t_k + _c(i) * dt, x_k + dt / 1s * k * _A.row(i).transpose()); - } - return x_k + dt / 1s * k * _b; - } - -private: - Eigen::Matrix _A; - Eigen::Vector _b; - Eigen::Vector _c; -}; - -template class ODE45 { -public: - DEFINE_MODEL_TYPES(n_x, 0, 0, 0, 0) - /** - * @brief Variable step size Runge-Kutta method - * - * @param abs_error absolute error. Specify a value for all state variables - * @param rel_error relative error. Specify a value for all state variables - * @param max_iterations Maximum number of iterations before giving up - * @param min_step_size Minimum valid value for the step size in the integration - */ - ODE45(double abs_error = 1e-6, double rel_error = 1e-6, size_t max_iterations = 1000, Timestep min_step_size = 1ns) - : ODE45(State::Ones() * abs_error, State::Ones() * rel_error, max_iterations, min_step_size) - { - } - - /** - * @brief Variable step size Runge-Kutta method - * - * @param abs_error_vec absolute error vector. Specify a value for each state variable - * @param rel_error_vec relative error vector. Specify a value for each state variable - * @param max_iterations Maximum number of iterations before giving up - * @param min_step_size Minimum valid value for the step size in the integration - */ - ODE45(State abs_error_vec = State::Ones() * 1e-6, State rel_error_vec = State::Ones() * 1e-6, size_t max_iterations = 1000, Timestep min_step_size = 1ns) - : _Atol{abs_error_vec}, _Rtol{rel_error_vec}, _max_iter{max_iterations}, _min_step_size{min_step_size} - { - // Use the Dormand Prince (RKDP) method - // clang-format off - _A << 0 , 0 , 0 , 0 , 0 , 0 , 0, - 1/5.0 , 0 , 0 , 0 , 0 , 0 , 0, - 3/40.0 , 9/40.0 , 0 , 0 , 0 , 0 , 0, - 44/45.0 , -56/15.0 , 32/9.0 , 0 , 0 , 0 , 0, - 19372/6561.0, -25360/2187.0, 64448/6561.0, -212/729.0, 0 , 0 , 0, - 9017/3168.0 , -355/33.0 , 46732/5247.0, 49/176.0 , -5103/18656.0 , 0 , 0, - 35/384.0 , 0 , 500/1113.0 , 125/192.0, -2187/6784.0 , 11/84.0 , 0; - Eigen::Matrix b_T; - b_T <<35/384.0 , 0 , 500/1113.0 , 125/192.0, -2187/6784.0 , 11/84.0 , 0, // Error of order O(dt^5) - 5179/57600.0, 0 , 7571/16695.0, 393/640.0, -92097/339200.0, 187/2100.0, 1/40.0; // Error of order O(dt^4) - _b = b_T.transpose(); - _c << 0 , 1/5.0 , 3/10.0 , 4/5.0 , 8/9.0 , 1 , 1; - // clang-format on - } - - /** - * @brief Integrate the function f over one step dt - * - * @param f Function to integrate - * @param dt Time step size / Simulation time - * @param t_k Start time - * @param x_k Start state - * @return Integrated state - * @throws std::runtime_error if the maximum number of iterations is reached - * @throws std::runtime_error if the step size becomes too small - */ - State integrate(State_dot f, Timestep dt, Time t_k, const State &x_k) - { - // Copy t and x - const Time t_kp1 = t_k + dt; // Final time t_k+1 - Time t_i = t_k; // Intermediate times - State x_i = x_k; // Intermediate states - Timestep h = dt; // Variable step size - - for (size_t iter = 0; iter < _max_iter; iter++) { - // Compute k_i - Eigen::Matrix k = Eigen::Matrix::Zero(); - for (size_t i = 0; i < n_stages; i++) { - k.col(i) = f(t_i + _c(i) * h, x_i + h / 1s * k * _A.row(i).transpose()); - } - - State x_ip1_hat = x_i + h / 1s * k * _b.col(0); // Higher order solution - State x_ip1 = x_i + h / 1s * k * _b.col(1); // Lower order solution - - // From https://scicomp.stackexchange.com/questions/32563/dormand-prince-54-how-to-update-the-stepsize-and-make-accept-reject-decision - - // Compute max(|x_i|, |x_i+1|) - State abs_x_i = x_i.cwiseAbs(); - State abs_x_ip1 = x_ip1.cwiseAbs(); // Using lower order solution - State max_x = abs_x_i.cwiseMax(abs_x_ip1); - - // Scaling factor - State sc = _Atol + max_x.cwiseProduct(_Rtol); - - // Estimate error - State temp_error = (x_ip1 - x_ip1_hat).cwiseQuotient(sc); - double error = std::sqrt(temp_error.dot(temp_error) / n_x); - - // Accecpt step if error is within tolerance - if (error < 1) { - t_i += h; - x_i = x_ip1_hat; - - if (t_i >= t_kp1) { - return x_i; // Simulation completed successfully - } - } - - // Compute new step size - Timestep h_new; - static constexpr double f_ac = std::pow(0.25, 1.0 / (q + 1)); // safety factor - - h_new = f_ac * h * std::pow(1 / error, 1.0 / (q + 1)); // optimal step size - h_new = std::max(h_new / 1s, 0.2 * h / 1s) * 1s; // limit step size decrease - h_new = std::min(h_new / 1s, 5.0 * h / 1s) * 1s; // limit step size increase - h_new = std::min(h_new / 1s, (t_kp1 - t_i) / 1s) * 1s; // limit step size to not overshoot t_kp1 - h = h_new; - - if (h < _min_step_size) { - throw std::runtime_error("Could not reach tolerance within minimum step size" + std::to_string(_min_step_size / 1s)); - } - } - throw std::runtime_error("Could not reach tolerance within maximum number of iterations " + std::to_string(_max_iter)); - } - -private: - static constexpr int n_stages = 7; - static constexpr size_t q = 5; // Order of the method - - Eigen::Matrix _A; - Eigen::Matrix _b; - Eigen::Vector _c; - - State _Atol; - State _Rtol; - size_t _max_iter; - Timestep _min_step_size; -}; - -} // namespace Integrator \ No newline at end of file diff --git a/include/models/EKF_models.hpp b/include/models/EKF_models.hpp deleted file mode 100644 index 0560c353..00000000 --- a/include/models/EKF_models.hpp +++ /dev/null @@ -1,328 +0,0 @@ -#pragma once -#include -#include - -namespace Models { - -template -class EKF_model_base : public Model_base { -public: - // These type definitions are needed because of the stupid two-phase lookup for dependent names in templates in C++ - using Base = Model_base; - using Base::_n_x; using Base::_n_y; using Base::_n_u; using Base::_n_v; using Base::_n_w; - DEFINE_MODEL_TYPES(n_x, n_y, n_u, n_v, n_w) - - /** - * @brief Parent class for functions that need to be provided for the EKF filter. - * All children of this class will work in the EKF as well as in the UKK. - * For the latter case it is recommended to explicitly specify disturbance and noise as additive. (i.e. f(x)+v and h(x)+w) - */ - EKF_model_base() : Base() {} - virtual ~EKF_model_base() {} - - /** - * @brief Jacobian of f: - * Calculate the transition function jacobian for time \p t at \p x - * @param x State - * @param t Time-step - * @return Jacobian A - */ - virtual Mat_xx F_x(Time t, const State &x, const Input &u = Input::Zero(), const Disturbance &v = Disturbance::Zero()) const = 0; - /** - * @brief Jacobian of f with respect to v: - * Calculate the transition function jacobian for time \p t at \p x - * This has to be defined if the disturbance on the model is not additive. - * @param t Time - * @param x State - * @param u Input - * @param v Disturbance - * @return Jacobian G - */ - virtual Mat_xv F_v(Time t, const State &x, const Input &u = Input::Zero(), const Disturbance &v = Disturbance::Zero()) const - { - (void)t; - (void)x; - (void)u; - (void)v; - return Mat_xv::Identity(); - } - /** - * @brief Jacobian of h: - * Calculate the measurement function jacobian for time \p t at \p x - * @param t Time-step - * @param x State - * @return Jacobian C - */ - virtual Mat_yx H_x(Time t, const State &x, const Input &u = Input::Zero(), const Noise &w = Noise::Zero()) const = 0; - - /** - * @brief Jacobian of h with respect to w: - * Calculate the measurement function jacobian for time \p t at \p x - * This has to be defined if the noise on the measurement is not additive. - * @param t Time - * @param x State - * @param u Input - * @param w Noise - * @return Jacobian H - */ - virtual Mat_yw H_w(Time t, const State &x, const Input &u = Input::Zero(), const Noise &w = Noise::Zero()) const - { - (void)t; - (void)x; - (void)u; - (void)w; - return Mat_yw::Identity(); - } -}; - - - -template -class LTI_model : public EKF_model_base { -public: - // These type definitions are needed because of the stupid two-phase lookup for dependent names in templates in C++ - using Base = EKF_model_base; - using Base::_n_x; using Base::_n_y; using Base::_n_u; using Base::_n_v; using Base::_n_w; - DEFINE_MODEL_TYPES(n_x, n_y, n_u, n_v, n_w) - - /** - * @brief Construct a lti model object - * - * @param A System matrix - * @param B Input matrix - * @param C Measurement matrix - * @param D Feedthrough matrix - * @param Q Disturbance covariance matrix - * @param R Measurement covariance matrix - * @param G Disturbance input matrix - * @param H Noise input matrix - */ - LTI_model(Mat_xx A, Mat_xu B, Mat_yx C, Mat_yu D, Mat_vv Q, Mat_ww R, Mat_xv G, Mat_yw H) - : Base(), _A{A}, _B{B}, _C{C}, _D{D}, _Q{Q}, _R{R}, _G{G}, _H{H} - { - } - /** - * @brief Construct a lti model object with no feedthrough and no noise input - * - * @param A System matrix - * @param B Input matrix - * @param C Measurement matrix - * @param Q Disturbance covariance matrix - * @param R Measurement covariance matrix - */ - LTI_model(Mat_xx A, Mat_xu B, Mat_yx C, Mat_vv Q, Mat_ww R) : LTI_model(A, B, C, Mat_yu::Zero(), Q, R, Mat_xv::Identity(), Mat_yw::Identity()) {} - - ~LTI_model() {} - - /** - * @brief Prediction equation f(t,x,u,v) = x_dot. (Use intergator::None for discrete time) - * @param t Simulation time - * @param x State - * @param u Input - * @param v Disturbance - * @return The derivative x_dot = f(t,x,u,v) - */ - State f(Time t, const State &x, const Input &u = Input::Zero(), const Disturbance &v = Disturbance::Zero()) const override final - { - (void)t; // unused - return _A * x + _B * u + _G * v; - } - /** - * @brief Measurement equation h: - * Calculate the zero noise prediction at time \p t from \p x. - * @param t Simulation time - * @param x State - * @return The prediction y = h(t,x,u,w) - */ - Measurement h(Time t, const State &x, const Input &u = Input::Zero(), const Noise &w = Noise::Zero()) const override final - { - (void)t; // unused - return _C * x + _D * u + _H * w; - } - /** - * @brief Returns A: the jacobian of f with respect to x - * - * @param t Time - * @param x State - * @param u Input - * @param v Disturbance - * @return A - */ - Mat_xx F_x(Time t, const State &x, const Input &u, const Disturbance &v = Disturbance::Zero()) const override final - { - (void)t; // unused - (void)x; // unused - (void)u; // unused - (void)v; // unused - return _A; - } - - /** - * @brief Returns G: the jacobian of f with respect to v - * @param t Time - * @param x State - * @param u Input - * @param v Disturbance - * @return G - */ - Mat_xv F_v(Time t, const State &x, const Input &u, const Disturbance &v = Disturbance::Zero()) const override final - { - (void)t; // unused - (void)x; // unused - (void)u; // unused - (void)v; // unused - return _G; - } - /** - * @brief Returns C: the jacobian of h with respect to x - * - * @param t Time - * @param x State - * @param u Input - * @param w Noise - * @return Mat_yx - */ - Mat_yx H_x(Time t, const State &x, const Input &u = Input::Zero(), const Noise &w = Noise::Zero()) const override final - { - (void)t; - (void)x; - (void)u; - (void)w; - return _C; - } - /** - * @brief Returns H: the jacobian of h with respect to w - * - * @param t Time - * @param x State - * @param u Input - * @param w Noise - * @return Mat_yw - */ - Mat_yw H_w(Time t, const State &x, const Input &u = Input::Zero(), const Noise &w = Noise::Zero()) const override final - { - (void)x; - (void)u; - (void)t; - (void)w; - return _H; - } - /** - * @brief Covariance matrix of model disturbance: - * Calculate the transition covariance \p Q for time \p t - * @param t Simulation time - * @param x State - * @return System noise covariance matrix Q - */ - const Mat_vv &Q(Time t, const State &x) const override final - { - (void)t; - (void)x; - return _Q; - } - /** - * @brief Covariance matrix of measurement noise: - * Calculate the transition covariance \p Q for time \p t - * @param t Simulation time - * @param x State - * @return Measuerement noise covariance matrix R - */ - const Mat_ww &R(Time t, const State &x) const override final - { - (void)t; - (void)x; - return _R; - } - - Mat_xx _A; - Mat_xu _B; - Mat_yx _C; - Mat_yu _D; - Mat_vv _Q; - Mat_yy _R; - Mat_xv _G; - Mat_yw _H; -}; - -using Landmark_base = EKF_model_base, 7, 7, 0, 6, 6>; -class Landmark_model : public Landmark_base { -public: - // These type definitions are needed because of the stupid two-phase lookup for dependent names in templates in C++ - using Base = Landmark_base; - using Base::_n_x; using Base::_n_y; using Base::_n_u; using Base::_n_v; using Base::_n_w; - DEFINE_MODEL_TYPES(7, 7, 0, 6, 6) - - Landmark_model() : Base() {} - ~Landmark_model() {} - - /** - * @brief Prediction equation f(t,x,u,v) = x_dot - * - * @param t Simulation time - * @param x State [x, y, z, q_w, q_x, q_y, q_z] - * @param u Input (no input) - * @param v Disturbance [x, y, z, r_1, r_2, r_3] - * @return State - */ - State f(Time t, const State &x, const Input &u = Input::Zero(), const Disturbance &v = Disturbance::Zero()) const override final - { - (void)t; // unused - (void)u; // unused - - // Derivative of position - Eigen::Vector3d p_dot{v.head(3)}; - // Derivative of orientation - Eigen::Quaterniond q{x(3), x(4), x(5), x(6)}; - Eigen::Quaterniond q_dot = q * Eigen::Quaterniond(0, v(3), v(4), v(5)); - q_dot = q_dot.coeffs() * 0.5; - // Combine - State x_dot; - x_dot << p_dot, q_dot.coeffs(); - return x_dot; - } - - /** - * @brief Measurement equation h(t,x,u,w) = y - * - * @param t Simulation time - * @param x State [x, y, z, q_w, q_x, q_y, q_z] - * @param u Input (no input) - * @param w Noise [x, y, z, r_1, r_2, r_3] - * @return Measurement [x, y, z, q_w, q_x, q_y, q_z] - */ - Measurement h(Time t, const State &x, const Input &u = Input::Zero(), const Noise &w = Noise::Zero()) const override final - { - (void)t; - (void)u; - - Eigen::Vector3d p_meas{x.head(3)+w.head(3)}; - Eigen::Quaterniond q{x(3), x(4), x(5), x(6)}; - // Convert noise rotation vector to quaternion - double alpha = w.tail(3).norm(); - Eigen::Quaterniond q_noise{cos(alpha/2), sin(alpha/2)*w(3), sin(alpha/2)*w(4), sin(alpha/2)*w(5)}; - Eigen::Quaterniond q_meas = q * q_noise; - - Measurement y; - y << p_meas, q_meas.coeffs(); - return y; - } - Mat_xx F_x(Time t, const State &x, const Input &u, const Disturbance &v = Disturbance::Zero()) const override final - { - (void)t; - (void)x; - (void)u; - (void)v; - return Mat_xx::Identity(); - } - Mat_xv F_v(Time t, const State &x, const Input &u, const Disturbance &v = Disturbance::Zero()) const override final - { - (void)t; - (void)x; - (void)u; - (void)v; - return Mat_xv::Identity(); - } - -}; - -} // namespace Models \ No newline at end of file diff --git a/include/models/Model_base.hpp b/include/models/Model_base.hpp deleted file mode 100644 index 78430d55..00000000 --- a/include/models/Model_base.hpp +++ /dev/null @@ -1,102 +0,0 @@ -#pragma once -#include -#include -#include -#include - -#include -using std::placeholders::_1; -using std::placeholders::_2; -namespace Models { - -template -class Model_base { -public: - // Make static constexpr constants so that the model itself may be used to infer the sizes instead of individual parameters - static constexpr int _n_x = n_x, _n_y = n_y, _n_u = n_u, _n_v = n_v, _n_w = n_w; - - DEFINE_MODEL_TYPES(n_x, n_y, n_u, n_v, n_w) - /** - * @brief Parent class for modelling dynamics - */ - Model_base(Mat_vv Q = Mat_vv::Identity(), Mat_ww R = Mat_ww::Identity()) : _Q{Q}, _R{R} {} - - /** - * @brief Prediction equation f(t,x,u,v) = x_dot. (Use Intergator::None for discrete time) - * @param t Simulation time - * @param x State - * @param u Input - * @param v Disturbance - * @return The derivative x_dot = f(t,x,u,v) - */ - virtual State f(Time t, const State &x, const Input &u = Input::Zero(), const Disturbance &v = Disturbance::Zero()) const = 0; - - /** - * @brief Integrate the model from time \p t0 to \p t0 + \p dt - * @param dt Timestep - * @param t0 Start time - * @param x0 Initial state - * @param u Input - * @param v Disturbance - * @return The next state x_(k+1) = F(x_k,...) - */ - virtual State next_state(Timestep dt, Time t0, const State &x0, const Input &u = Input::Zero(), const Disturbance &v = Disturbance::Zero()) const - { - // Cast f to a lambda function to keep u and v constant - State_dot F = [this, &u, &v](Time t, const State &x) { return this->f(t, x, u, v); }; - return integrator::integrate(F, dt, t0, x0); - } - /** - * @brief Measurement equation h: - * Calculate the zero noise prediction at time \p t from \p x. - * @param t Simulation time - * @param x State - * @return The prediction y = h(t,x,u,w) - */ - virtual Measurement h(Time t, const State &x, const Input &u = Input::Zero(), const Noise &w = Noise::Zero()) const = 0; - - /** - * @brief Covariance matrix of model disturbance: - * Calculate the transition covariance \p Q for time \p t - * @param t Simulation time - * @param x State - * @return System noise covariance matrix Q - */ - virtual const Mat_vv &Q(Time t, const State &x) const - { - (void)t; - (void)x; - return _Q; - } - - /** - * @brief Covariance matrix of measurement noise: - * Calculate the transition covariance \p Q for time \p t - * @param t Simulation time - * @param x State - * @return Measuerement noise covariance matrix R - */ - virtual const Mat_ww &R(Time t, const State &x) const - { - (void)t; - (void)x; - return _R; - } - - /** - * @brief State update function: - * Use this for post-processing of the state after the correction step. - * This is needed for example in the case of quaternions, in order to normalize the state. - * @param x State - * @return The next state x_(k+1) = F(x_k,...) - */ - virtual const State post_state_update(const State &x) const - { - return x; - } - - Mat_vv _Q; - Mat_ww _R; -}; - -} // namespace Models \ No newline at end of file diff --git a/include/models/model_definitions.hpp b/include/models/model_definitions.hpp deleted file mode 100644 index 6a6af7b1..00000000 --- a/include/models/model_definitions.hpp +++ /dev/null @@ -1,61 +0,0 @@ -#pragma once -#include -#include - -namespace Models { -using Eigen::Matrix; -using Eigen::Vector; -using namespace std::chrono_literals; -using namespace std::chrono; -using Time = duration; -using Timestep = duration; - -/** - * @brief Define the types used in the models. - * THIS HAS TO BE USED INSIDE THE Models NAMESPACE - * @param n_x State size - * @param n_y Measurement size - * @param n_u Input size - * @param n_v Disturbance size - * @param n_w Noise size - */ -#define DEFINE_MODEL_TYPES(n_x, n_y, n_u, n_v, n_w) \ - using State = Eigen::Vector; \ - using Measurement = Eigen::Vector; \ - using Input = Eigen::Vector; \ - using Disturbance = Eigen::Vector; \ - using Noise = Eigen::Vector; \ - \ - using Mat_xx = Eigen::Matrix; \ - using Mat_xy = Eigen::Matrix; \ - using Mat_xu = Eigen::Matrix; \ - using Mat_xv = Eigen::Matrix; \ - using Mat_xw = Eigen::Matrix; \ - \ - using Mat_yx = Eigen::Matrix; \ - using Mat_yy = Eigen::Matrix; \ - using Mat_yu = Eigen::Matrix; \ - using Mat_yv = Eigen::Matrix; \ - using Mat_yw = Eigen::Matrix; \ - \ - using Mat_ux = Eigen::Matrix; \ - using Mat_uy = Eigen::Matrix; \ - using Mat_uu = Eigen::Matrix; \ - using Mat_uv = Eigen::Matrix; \ - using Mat_uw = Eigen::Matrix; \ - \ - using Mat_vx = Eigen::Matrix; \ - using Mat_vy = Eigen::Matrix; \ - using Mat_vu = Eigen::Matrix; \ - using Mat_vv = Eigen::Matrix; \ - using Mat_vw = Eigen::Matrix; \ - \ - using Mat_wx = Eigen::Matrix; \ - using Mat_wy = Eigen::Matrix; \ - using Mat_wu = Eigen::Matrix; \ - using Mat_wv = Eigen::Matrix; \ - using Mat_ww = Eigen::Matrix; \ - \ - using State_dot = std::function; - -} // namespace Models diff --git a/include/models/temp_gyro_model.hpp b/include/models/temp_gyro_model.hpp deleted file mode 100644 index 607dc330..00000000 --- a/include/models/temp_gyro_model.hpp +++ /dev/null @@ -1,41 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace Models { -constexpr int n_x = 7, n_y = 7, n_u = 1, n_v = 6, n_w = 6; -using Base = Model_base, n_x, n_y, n_u, n_v, n_w>; -class Temp_gyro_model : public Base { -public: - DEFINE_MODEL_TYPES(n_x, n_y, n_u, n_v, n_w) - using Quaternion = Eigen::Quaterniond; - - Temp_gyro_model() : Base(){}; - - State f(Time t, const State &x, const Input &u = Input::Zero(), const Disturbance &v = Disturbance::Zero()) const override final - { - (void)t; - (void)u; - Quaternion q(x(0), x(1), x(2), x(3)); - Quaternion q_dot = q * Quaternion(0, v(0), v(1), v(2)); - q_dot.coeffs() *= 0.5; - State x_dot; - x_dot << q_dot.w(), q_dot.x(), q_dot.y(), q_dot.z(), v(3), v(4), v(5); - - return x_dot; - } - - Measurement h(Time t, const State &x, const Input &u = Input::Zero(), const Noise &w = Noise::Zero()) const override final - { - (void)t; - (void)x; - (void)u; - - Measurement z; - z << 0, w; - return z; - } -}; -} // namespace Models \ No newline at end of file diff --git a/include/nodes/kf_node.hpp b/include/nodes/kf_node.hpp deleted file mode 100644 index 7f01ff2d..00000000 --- a/include/nodes/kf_node.hpp +++ /dev/null @@ -1,68 +0,0 @@ -#pragma once -#include -#include -#include -using std::placeholders::_1; - -#include -#include - -#include -#include -#include -#include -namespace Nodes { -using namespace Models; -using Period = Timestep; - -template class KF_node : public rclcpp::Node { -public: - static constexpr int _n_x = Model::_n_x, _n_y = Model::_n_y, _n_u = Model::_n_u, _n_v = Model::_n_v, _n_w = Model::_n_w; - DEFINE_MODEL_TYPES(_n_x, _n_y, _n_u, _n_v, _n_w) - KF_node(std::shared_ptr> filter, Period P = 0.1s) : Node("KF_node"), _filter{filter}, _P{P} - { - _timer = this->create_wall_timer(P, std::bind(&KF_node::timer_callback, this)); - _last_timestamp = this->now(); - _has_new_measurement = false; - - subscription_ = - this->create_subscription("measurement", 10, std::bind(&KF_node::meas_callback, this, _1)); - } - -private: - rclcpp::TimerBase::SharedPtr _timer; - std::shared_ptr> _filter; - const Period _P; - rclcpp::Time _last_timestamp; - Measurement _last_measurement; - bool _has_new_measurement; - rclcpp::Subscription::SharedPtr subscription_; - - void timer_callback() - { - // Get time since last call - rclcpp::Time now = this->now(); - Time Ts = (now - _last_timestamp).seconds() * 1s; - _last_timestamp = now; - // Reset filter if time goes backwards (e.g. when playing a rosbag on loop) - if (Ts < (Time)0) { - _filter->reset(); - RCLCPP_INFO_STREAM(this->get_logger(), "Reset filter due to negative time step"); - return; - } - // Calculate next iterate - State x_next = _filter->iterate(Ts, _last_measurement); - _has_new_measurement = false; - // Publish - RCLCPP_INFO_STREAM(this->get_logger(), '\n' << x_next); - } - - void meas_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) - { - RCLCPP_INFO_STREAM(this->get_logger(), "Measurement: " << msg->pose.position.x << ", " << msg->pose.position.y << ", " << msg->pose.position.z); - _last_measurement << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; - _has_new_measurement = true; - } -}; - -} // namespace Nodes \ No newline at end of file diff --git a/launch/vkf_node.launch.yaml b/launch/vkf_node.launch.yaml deleted file mode 100644 index 29be9186..00000000 --- a/launch/vkf_node.launch.yaml +++ /dev/null @@ -1,7 +0,0 @@ -launch: - -- node: - pkg: vortex-vkf - exec: vortex-vkf_node - name: vkf_node - diff --git a/scripts/ekf_python3/analysis_py2.py b/scripts/ekf_python3/analysis_py2.py deleted file mode 100644 index ced44beb..00000000 --- a/scripts/ekf_python3/analysis_py2.py +++ /dev/null @@ -1,105 +0,0 @@ -#!/usr/bin/env python -import numpy as np -from numpy import ndarray -import scipy.linalg as la -from ekf_python3.gaussparams_py2 import MultiVarGaussian -from config import DEBUG - - -def get_NIS(z_pred_gauss, z): - """Calculate the normalized innovation squared (NIS), this can be seen as - the normalized measurement prediction error squared. - See (4.66 in the Sensor Fusion book.) - Tip: use the mahalanobis_distance method of z_pred_gauss, (3.2) in the book - - Args: - z_pred_gauss (MultiVarGaussian): predigted measurement gaussian - z (ndarray): measurement - - Returns: - NIS (float): normalized innovation squared - """ - - z_pred, S = z_pred_gauss - - v = z - z_pred - - NIS = np.matmul(v.T, np.matmul(np.linalg.inv(S), v)) - - return NIS - - -def get_NEES(x_gauss, x_gt): - """Calculate the normalized estimation error squared (NEES) - See (4.65 in the Sensor Fusion book). - Tip: use the mahalanobis_distance method of x_gauss, (3.2) in the book - - Args: - x_gauss (MultiVarGaussian): state estimate gaussian - x_gt (ndarray): true state - - Returns: - NEES (float): normalized estimation error squared - """ - - x_hat, P_hat = x_gauss - - err = x_hat - x_gt - - NEES = np.matmul(err.T, np.matmul(np.linalg.inv(P_hat), err)) - - return NEES - - -def get_ANIS(z_pred_gauss_data, z_data): - """Calculate the average normalized innovation squared (ANIS) - Tip: use get_NIS - - Args: - z_pred_gauss_data (Sequence[MultiVarGaussian]): Sequence (List) of - predicted measurement gaussians - z_data (Sequence[ndarray]): Sequence (List) of true measurements - - Returns: - ANIS (float): average normalized innovation squared - """ - - NIS_arr = np.array([]) - - for i in range(len(z_data)): - - NIS = get_NIS(z_pred_gauss_data[i], z_data[i]) - np.append(NIS_arr, NIS) - - ANIS = np.average(NIS_arr) - - return ANIS - - -def get_ANEES(x_upd_gauss_data, x_gt_data): - """Calculate the average normalized estimation error squared (ANEES) - Tip: use get_NEES - - Args: - x_upd_gauss_data (Sequence[MultiVarGaussian]): Sequence (List) of - state estimate gaussians - x_gt_data (Sequence[ndarray]): Sequence (List) of true states - - Returns: - ANEES (float): average normalized estimation error squared - """ - NEES_arr = np.array([]) - - for i in range(len(x_gt_data)): - - NEES = get_NEES(x_upd_gauss_data[i], x_gt_data[i]) - np.append(NEES_arr, NEES) - - ANEES = np.average(NEES_arr) - - return ANEES - - -# def get_RMSE(x_upd_gauss_data: Sequence[MultiVarGaussian], -# x _gt_data: Sequence[ndarray]): -# #TODO diff --git a/scripts/ekf_python3/debugtools_py2.py b/scripts/ekf_python3/debugtools_py2.py deleted file mode 100644 index 86d9ee40..00000000 --- a/scripts/ekf_python3/debugtools_py2.py +++ /dev/null @@ -1,6 +0,0 @@ -#!/usr/bin/env python3 -import numpy as np - - -def isPSD(arr): - return np.allclose(arr, arr.T) and np.all(np.linalg.eigvals(arr) >= 0) diff --git a/scripts/ekf_python3/ekf_py2.py b/scripts/ekf_python3/ekf_py2.py deleted file mode 100644 index 20375657..00000000 --- a/scripts/ekf_python3/ekf_py2.py +++ /dev/null @@ -1,101 +0,0 @@ -#!/usr/bin/env python -## Addapted for use for Vortex NTNU from the course TTK4250. Credit for the underlying code goes to: -## @author: Lars-Christian Tokle, lars-christian.n.tokle@ntnu.no ## -""" -Notation: ----------- -x is generally used for either the state or the mean of a gaussian. It should be clear from context which it is. -P is used about the state covariance -z is a single measurement -Z are multiple measurements so that z = Z[k] at a given time step k -v is the innovation z - h(x) -S is the innovation covariance -""" - -## EKF Algorith notation: -# x_prev = mean of previous state posterior pdf -# P_prev = covariance of previous state posterior pdf - -# x_pred = kinematic prediction through dynamic model. Also called x_bar in literature -# P_pred = predicted prior covariance. Also called P_bar in the literature - -import numpy as np -import scipy.linalg as la - -from config import DEBUG -from ekf_python3.dynamicmodels_py2 import DynamicModel -from ekf_python3.measurementmodels_py2 import MeasurementModel -from ekf_python3.gaussparams_py2 import MultiVarGaussian - -# The EKF - - -class EKF: - - def __init__(self, DM, MM): - - self.dynamic_model = DM - self.sensor_model = MM - - def predict(self, state_upd_prev_gauss, Ts): - """Predict the EKF state Ts seconds ahead.""" - x_prev, P_prev = state_upd_prev_gauss - - Q = self.dynamic_model.Q(x_prev, Ts) - F = self.dynamic_model.F(x_prev, Ts) - - x_pred = self.dynamic_model.f(x_prev, Ts) - - P_pred = np.matmul(F, np.matmul(P_prev, F.T)) + Q - - state_pred_gauss = MultiVarGaussian(x_pred, P_pred) - - return state_pred_gauss - - def update(self, z, state_pred_gauss): - """Given the prediction and measurement, find innovation then - find the updated state estimate.""" - - x_pred, P = state_pred_gauss - - n = len(x_pred) - - H = self.sensor_model.H(x_pred) - R = self.sensor_model.R(x_pred) - - z_pred = self.sensor_model.h(x_pred) - S = np.matmul(H, np.matmul(P, H.T)) + R - - inov = z - z_pred - W = np.matmul(P, np.matmul(H.T, np.linalg.inv(S))) - - x_upd = x_pred + np.matmul(W, inov) - P_upd = np.matmul((np.eye(n) - np.matmul(W, H)), P) - - measure_pred_gauss = MultiVarGaussian(z_pred, S) - state_upd_gauss = MultiVarGaussian(x_upd, P_upd) - - return state_upd_gauss, measure_pred_gauss - - def step_with_info(self, state_upd_prev_gauss, z, Ts): - """ - Predict ekfstate Ts units ahead and then update this prediction with z. - - Returns: - state_pred_gauss: The state prediction - measurement_pred_gauss: - The measurement prediction after state prediction - state_upd_gauss: The predicted state updated with measurement - """ - - state_pred_gauss = self.predict(state_upd_prev_gauss, Ts) - - state_upd_gauss, measure_pred_gauss = self.update(z, state_pred_gauss) - - return state_pred_gauss, measure_pred_gauss, state_upd_gauss - - def step(self, state_upd_prev_gauss, z, Ts): - - _, _, state_upd_gauss = self.step_with_info(state_upd_prev_gauss, z, - Ts) - return state_upd_gauss diff --git a/scripts/ekf_python3/gaussparams_py2.py b/scripts/ekf_python3/gaussparams_py2.py deleted file mode 100644 index c28af20f..00000000 --- a/scripts/ekf_python3/gaussparams_py2.py +++ /dev/null @@ -1,72 +0,0 @@ -#!/usr/bin/env python -from statistics import mean -from turtle import shape -import numpy as np -from numpy import linalg as nla, ndarray - -from ekf_python3.debugtools_py2 import isPSD -from config import DEBUG - - -class MultiVarGaussian: - """A class for using Gaussians""" - - #mean: ndarray # shape=(n,) - #cov: ndarray # shape=(n, n) - - def __init__(self, m, P): - self.mean = m - self.cov = P - - def __post_init__(self): - # This is only for - if __debug__: - if DEBUG: - if not self.mean.shape * 2 == self.cov.shape: - - raise ValueError("mean and cov shape mismatch :" % - (self.mean.shape, self.cov.shape)) - - if not np.all(np.isfinite(self.mean)): - raise ValueError("Non finite mean = ", self.mean) - if not np.all(np.isfinite(self.cov)): - raise ValueError("Non finite cov =", self.cov) - if not isPSD(self.cov): - raise ValueError("Not PSD cov = ", self.cov) - else: - pass - - @property - def ndim(self): - return self.mean.shape[0] - - def mahalanobis_distance(self, x): - """Calculate the mahalanobis distance between self and x. - - This is also known as the quadratic form of the Gaussian. - See (3.2) in the book. - """ - # this method could be vectorized for efficient calls - error = x - self.mean - #mahalanobis_distance = error.T @ nla.solve(self.cov, error) - mahalanobis_distance = np.matmul(error.T, nla.solve(self.cov, error)) - return mahalanobis_distance - - def pdf(self, x): - raise NotImplementedError - - def logpdf(self, x): - raise NotImplementedError - - def gate(self, x, prob): - raise NotImplementedError - - def __iter__(self): # in order to use tuple unpacking - return iter((self.mean, self.cov)) - - def __eq__(self, o): - if not isinstance(o, MultiVarGaussian): - return False - else: - return (np.allclose(self.mean, o.mean) - and np.allclose(self.cov, o.cov)) diff --git a/scripts/vkf_node.py b/scripts/vkf_node.py deleted file mode 100755 index 1db34889..00000000 --- a/scripts/vkf_node.py +++ /dev/null @@ -1,377 +0,0 @@ -#!/usr/bin/env python3 - -#import debugpy -#print("Waiting for VSCode debugger...") -#debugpy.listen(5678) -#debugpy.wait_for_client() - -##EKF imports -#from logging import exception -from re import X - -from ekf_python3.gaussparams_py2 import MultiVarGaussian -from ekf_python3.dynamicmodels_py2 import landmark_gate, landmark_pose_world -from ekf_python3.measurementmodels_py2 import measurement_linear_landmark, LTV_full_measurement_model -from ekf_python3.ekf_py2 import EKF - -#Math imports -import numpy as np - -#ROS imports -import rospy -from std_msgs.msg import String -from vortex_msgs.msg import ObjectPosition -from geometry_msgs.msg import PoseStamped, TransformStamped -import tf.transformations as tft -import tf2_ros -import tf2_geometry_msgs.tf2_geometry_msgs - - -class VKFNode: - - def __init__(self): - ######################################## - ####Things you can change yourself#### - ######################################## - - #Name of the node - node_name = "ekf_vision" - - #Frame names, e.g. "odom" and "cam" - self.parent_frame = 'odom' - self.child_frame = 'odom' - self.object_frame = "" - - self.current_object = "" - - #Subscribe topic - object_topic_subscribe = "aruco_odom_obj" # TODO: put new topic name - mission_topic_subscribe = "/fsm/state" - - ################## - ####EKF stuff##### - ################## - - # Tuning parameters - self.sigma_a = 3 / 5 * np.array([0.05, 0.05, 0.05, 0.05, 0.05, 0.05]) - self.sigma_z = 2 * np.array([0.5, 0.5, 0.5, 0.5, 0.5, 0.5]) - - # Making gate model object - self.landmark_model = landmark_pose_world(self.sigma_a) - - #Gauss prev values - self.x_hat0 = np.array([0, 0, 0, 0, 0, 0]) - self.P_hat0 = np.diag(100 * self.sigma_z) - self.prev_gauss = MultiVarGaussian(self.x_hat0, self.P_hat0) - - ################ - ###ROS stuff#### - ################ - - # ROS node init - rospy.init_node(node_name) - self.last_time = rospy.get_time() - - now = rospy.get_rostime() - rospy.loginfo("Current time %i %i", now.secs, now.nsecs) - - # Subscribe to mission topic - #self.mission_topic = self.current_object + "/execute" - #self.mission_topic_old = self.mission_topic[:] - #self.mission_topic_sub = rospy.Subscriber(mission_topic_subscribe, - # String, self.update_mission) - - # LMS decision logic - self.Fnorm_threshold = 0.1 - self.is_detected = True - self.estimateConverged = False - self.estimateFucked = False - - # Subscriber to gate pose and orientation - self.object_pose_sub = rospy.Subscriber(object_topic_subscribe, - ObjectPosition, - self.obj_pose_callback, - queue_size=1) - - # Publisher to autonomous - self.gate_pose_pub = rospy.Publisher('object_positions_in', - ObjectPosition, - queue_size=1) - - #TF stuff - self.__tfBuffer = tf2_ros.Buffer() - self.__listener = tf2_ros.TransformListener(self.__tfBuffer) - self.__tfBroadcaster = tf2_ros.TransformBroadcaster() - - self.pose_transformer = tf2_geometry_msgs.tf2_geometry_msgs - - #The init will only continue if a transform between parent frame and child frame can be found - while self.__tfBuffer.can_transform(self.parent_frame, - self.child_frame, - rospy.Time()) == 0: - try: - rospy.loginfo("VKF_NODE: No transform between " + - str(self.parent_frame) + ' and ' + - str(self.child_frame)) - rospy.sleep(2) - except: #, tf2_ros.ExtrapolationException (tf2_ros.LookupException, tf2_ros.ConnectivityException) - rospy.sleep(2) - continue - - rospy.loginfo("VKF_NODE: Transform between " + str(self.parent_frame) + - ' and ' + str(self.child_frame) + ' found.') - - ############ - ##Init end## - ############ - - def update_mission(self, mission): - """ - Callback which updates the instance variable storing the fsm state information - - Input: - mission: ROS String message from fsm/state subscription - - """ - self.mission_topic = mission.data - - def get_Ts(self): - """ - Returns the time step since last Kalman update. The previous time is stored in the last_time instance - - Output: - Ts: The time step in seconds since last Kalman Update - """ - Ts = rospy.get_time() - self.last_time - return Ts - - def ekf_function(self, z, ekf): - """ - Performs a Kalman Update given the measurement and an ekf object - - Input: - z: A MultiVarGaussian object containing the result from the update step - - Output: - gauss_x_pred: A MultiVarGauss object resulting from the prediction of the state - Can be used in NEES and NIS analyses - gauss_z_pred: A MultiVarGauss object resulting from the prediction of the measurement - Can be used in NIS analysis of the filter performance - gauss_est: A MultiVarGauss object resulting from the update step - """ - - Ts = self.get_Ts() - - gauss_x_pred, gauss_z_pred, gauss_est = ekf.step_with_info( - self.prev_gauss, z, Ts) - - self.last_time = rospy.get_time() - self.prev_gauss = gauss_est - - return gauss_x_pred, gauss_z_pred, gauss_est - - def est_to_pose(self, gauss_est): - """ - Extracts the mean of the estimate from a MultiVarGaussian object and splits it into position and orientation - - Input: - gauss_est: A MultiVarGaussian object containing the result from the update step - - Output: - pos: A list of length 3 with the position of the estimate - euler_angs: A list of length 3 with the euler angles from the estimate - """ - x_hat = gauss_est.mean - pos = [x_hat[0], x_hat[1], x_hat[2]] - euler_angs = [x_hat[3], x_hat[4], x_hat[5]] - - return pos, euler_angs - - def process_measurement_message(self, msg): - """ - Takes in the msg which is the output of PCP, extracts the needed components and does some calculations/mappings - to get what we need in the different frames for the VKF. - Input: - msg: Output from PCP, a PoseStamped msg - - Output: - z : a 6 dim measurement of positions of object wrt camera in camera frame, and orientation between object and camera in euler angles - R_wc : rotation matrix between odom (world) and camera, needed for the LTV sensor model for the Kalman Filters - cam_pose_position_wc : position of camera in odom (world), needed for the LTV sensor model for the Kalman Filters - """ - # Gets the transform from odom to camera - self.tf_lookup_wc = self.__tfBuffer.lookup_transform( - self.parent_frame, self.child_frame, rospy.Time(), - rospy.Duration(5)) - - # Run the measurement back through tf tree to get the object in odom - msg_transformed_wg = self.pose_transformer.do_transform_pose( - msg.objectPose, self.tf_lookup_wc) - - #Broadcast to tf to make sure we get the correct transform. Uncomment for testing in rviz - self.transformbroadcast("odom", msg_transformed_wg) - - # Extract measurement of transformed and camera message - - # We need the position from the camera measurement - obj_pose_position_cg = np.array([ - msg.objectPose.pose.position.x, msg.objectPose.pose.position.y, - msg.objectPose.pose.position.z - ]) - - # We need the orientation from the world measruement - obj_pose_quat_wg = np.array([ - msg_transformed_wg.pose.orientation.x, - msg_transformed_wg.pose.orientation.y, - msg_transformed_wg.pose.orientation.z, - msg_transformed_wg.pose.orientation.w - ]) - - # We need to detection in world frame for creating new hypotheses - self.detection_position_odom = np.array([ - msg_transformed_wg.pose.position.x, - msg_transformed_wg.pose.position.y, - msg_transformed_wg.pose.position.z - ]) - - # Get time-varying transformation from world to camera - - # We need the position for the EKF - cam_pose_position_wc = np.array([ - self.tf_lookup_wc.transform.translation.x, - self.tf_lookup_wc.transform.translation.y, - self.tf_lookup_wc.transform.translation.z - ]) - - # We need the quaternion to make into a Rot matrix for the EKF - cam_pose_quat_wc = np.array([ - self.tf_lookup_wc.transform.rotation.x, - self.tf_lookup_wc.transform.rotation.y, - self.tf_lookup_wc.transform.rotation.z, - self.tf_lookup_wc.transform.rotation.w - ]) - - # Rotation from world to camera, needed for the LTV model - R_wc = tft.quaternion_matrix(cam_pose_quat_wc) - R_wc = R_wc[:3, :3] - - # Prepare measurement vector - z_phi, z_theta, z_psi = tft.euler_from_quaternion(obj_pose_quat_wg, - axes='sxyz') - - z = obj_pose_position_cg - z = np.append(z, [z_phi, z_theta, z_psi]) - - return z, R_wc, cam_pose_position_wc - - def check_filter_convergence(self, gauss_estimate): - """ - Sets the convergence boolean to True if Frobenius norm under a threshold - - Input: - gauss_estimate: A MultiVarGaussian containing the result from the Kalman Update - """ - cov = gauss_estimate.cov - Fnorm_P = np.linalg.norm(cov, "fro") - - if Fnorm_P < self.Fnorm_threshold: - self.estimateConverged = True - else: - pass - - def transformbroadcast(self, parent_frame, p): - """ - Publishes a transform representing the result from the Kalman Update given the name of the parent - frame and the objectPose message published for the landmark server. Used mainly for the purposes of - visualization and debugging. - - Input: - parent_frame: A string which is the parent frame - p : An objectPose which is the message published to landmark server - """ - - t = TransformStamped() - t.header.stamp = rospy.Time.now() - t.header.frame_id = parent_frame - t.child_frame_id = "object_" + self.current_object - t.transform.translation.x = p.pose.position.x - t.transform.translation.y = p.pose.position.y - t.transform.translation.z = p.pose.position.z - t.transform.rotation.x = p.pose.orientation.x - t.transform.rotation.y = p.pose.orientation.y - t.transform.rotation.z = p.pose.orientation.z - t.transform.rotation.w = p.pose.orientation.w - self.__tfBroadcaster.sendTransform(t) - - def publish_object(self, objectID, ekf_position, ekf_pose_quaterion): - p = ObjectPosition() - #p.pose.header[] - p.objectID = objectID - # p.objectPose.header = "object_" + str(objectID) - p.objectPose.pose.position.x = ekf_position[0] - p.objectPose.pose.position.y = ekf_position[1] - p.objectPose.pose.position.z = ekf_position[2] - p.objectPose.pose.orientation.x = ekf_pose_quaterion[0] - p.objectPose.pose.orientation.y = ekf_pose_quaterion[1] - p.objectPose.pose.orientation.z = ekf_pose_quaterion[2] - p.objectPose.pose.orientation.w = ekf_pose_quaterion[3] - - p.isDetected = self.is_detected - p.estimateConverged = self.estimateConverged - p.estimateFucked = self.estimateFucked - - self.gate_pose_pub.publish(p) - rospy.loginfo("Object published: %s", objectID) - self.transformbroadcast(self.parent_frame, p.objectPose) - - def obj_pose_callback(self, msg): - - #objID = self.mission_topic.split("/")[0] - objID = msg.objectID - # rospy.loginfo("Object data recieved for: %s", objID) - self.current_object = objID - - # Deprecated due to not using search/converge/execute this year #TODO: figure out a new way to do this logic - #if self.mission_topic == self.current_object + "/execute": - # rospy.loginfo("Mission status: %s", objID) - # self.prev_gauss = MultiVarGaussian(self.x_hat0, self.P_hat0) - # self.last_time = rospy.get_time() - # return None - - # Process msg of measurement - z, R_wc, cam_pose_position_wc = self.process_measurement_message(msg) - - # Initialization using the current measurement mapped to odom - if sorted(self.prev_gauss.mean) == sorted(self.x_hat0): - z = np.append(self.detection_position_odom[:3], z[3:]) - self.prev_gauss = MultiVarGaussian(z, self.P_hat0) - self.last_time = rospy.get_time() - return None - - # Create sensor model and ekf objects - full_measurement_model = LTV_full_measurement_model( - self.sigma_z, cam_pose_position_wc, R_wc) - full_ekf = EKF(self.landmark_model, full_measurement_model) - - # Call EKF step and format the data - _, _, gauss_est = self.ekf_function(z, full_ekf) - - # Format update Gaussian object - ekf_position, ekf_pose = self.est_to_pose(gauss_est) - ekf_pose_quaterion = tft.quaternion_from_euler(ekf_pose[0], - ekf_pose[1], - ekf_pose[2]) - - # Publish data, update mission topic - #self.mission_topic_old = self.mission_topic - self.check_filter_convergence(gauss_est) - self.publish_object(objID, ekf_position, ekf_pose_quaterion) - - -if __name__ == '__main__': - while not rospy.is_shutdown(): - try: - ekf_vision = VKFNode() - rospy.spin() - except rospy.ROSInterruptException: - pass diff --git a/src/nodes/kf_node.cpp b/src/nodes/kf_node.cpp deleted file mode 100644 index 30c5ed31..00000000 --- a/src/nodes/kf_node.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#include -#include -#include -#include - -#include -#include - -int main(int argc, char **argv) -{ - using namespace Models; - constexpr int n_x = Temp_gyro_model::_n_x; - constexpr int n_y = Temp_gyro_model::_n_y; - constexpr int n_u = Temp_gyro_model::_n_u; - constexpr int n_v = Temp_gyro_model::_n_v; - constexpr int n_w = Temp_gyro_model::_n_w; - DEFINE_MODEL_TYPES(n_x, n_y, n_u, n_v, n_w) - - rclcpp::init(argc, argv); - - // Create model - auto model = std::make_shared(); - // Create integrator - auto integrator = std::make_shared>(); - // Create filter - State x0 = State::Zero(); - Mat_xx P0 = Mat_xx::Identity(); - auto ukf = std::make_shared>(model, x0, P0); - - auto node = std::make_shared>(ukf, 0.1s); - RCLCPP_INFO(node->get_logger(), "KF node created"); - rclcpp::spin(node); - rclcpp::shutdown(); -} \ No newline at end of file diff --git a/test/ekf_test.cpp b/test/ekf_test.cpp deleted file mode 100644 index da8b26dc..00000000 --- a/test/ekf_test.cpp +++ /dev/null @@ -1,76 +0,0 @@ -#include -#include -#include - -#include -#include -#include - -using namespace Models; -using namespace Filters; -class EKFTest : public ::testing::Test { -protected: - static constexpr int n_x{3}, n_y{1}, n_u{2}, n_v{n_x}, n_w{n_y}; - using LTI_model = Models::LTI_model, n_x, n_y, n_u, n_v, n_w>; - static constexpr size_t num_iterations{1000}; - static constexpr double COV{1}; - - DEFINE_MODEL_TYPES(n_x, n_y, n_u, n_v, n_w) - std::shared_ptr model; - std::shared_ptr> filter; - - EKFTest() - { - Mat_xx A; - Mat_xu B; - Mat_yx C; - Mat_vv Q; - Mat_ww R; - State x0; - Mat_xx P0; - - A << .5, 0, 0, 0, .1, 0, 0, 0, 0; - B << 1, 0, 0, 0, 0, 0; - C << 1, 0, 0; - Q << 1e-4, 0, 0, 0, 1e-4, 0, 0, 0, 1e-4; - R << 1; - x0 << 1, 0, 0; - P0 = Mat_xx::Identity(); - - model = std::make_shared(A, B, C, Q, R); - filter = std::make_shared>(model, x0, P0); - - // Generate random measurements - std::random_device rd; - std::mt19937 gen(rd()); - std::normal_distribution<> d{0, COV}; - for (size_t i = 0; i < num_iterations; i++) { - Measurement y_i; - y_i << d(gen); - y.push_back(y_i); - } - // Generete vector of zero inputs - for (size_t i = 0; i < num_iterations; i++) { - Input u_i; - u_i << 0, 0; - u.push_back(u_i); - } - } - ~EKFTest() - { - } - // Vector for storing measurements - std::vector y; - // Vector for storing inputs - std::vector u; -}; - -TEST_F(EKFTest, filterConverges) -{ - // Vector for storing state estimates - std::vector x; - for (size_t i = 0; i < num_iterations; i++) { - x.push_back(filter->iterate(1ms, y[i], u[i])); - } - EXPECT_NEAR(0.0, (filter->get_state()).norm(), 1e-3) << "State estimate did not converge to zero."; -} diff --git a/test/erk_test.cpp b/test/erk_test.cpp deleted file mode 100644 index af90ac4e..00000000 --- a/test/erk_test.cpp +++ /dev/null @@ -1,231 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include - -using namespace Models; -using namespace Integrator; - -namespace sin_func_test { -constexpr int n_x = 1, n_y = 1, n_u = 1; -DEFINE_MODEL_TYPES(n_x, n_y, n_u, n_x, n_y) - -// Make functions to test the RK methods -State sin_func(Time t, const State &x) -{ - State x_dot; - x_dot << std::pow(std::sin(t / 1s), 2) * x(0); - return x_dot; -} -double sin_func_exact(double x_0, Time t) -{ - double t_s = t / 1s; - return x_0 * exp(0.5 * (t_s - sin(t_s) * cos(t_s))); -} - -class ERKTest : public ::testing::Test { -protected: - ERKTest() - { - dt = 1ms; - init(0s, State::Zero()); - u.setZero(); - v.setZero(); - } - Timestep dt; - - Input u; - Disturbance v; - - std::vector