From ecc87edc0f0ac4d369925dddccc288d26787a206 Mon Sep 17 00:00:00 2001 From: FrancoisCarouge Date: Fri, 27 Oct 2023 23:33:18 -0700 Subject: [PATCH] [filter] declarative paradigm --- README.md | 3 + include/CMakeLists.txt | 7 +- include/fcarouge/internal/factory.hpp | 235 +++++++++++++++++ include/fcarouge/internal/format.hpp | 40 +-- include/fcarouge/internal/kalman.tpp | 242 +++++++----------- include/fcarouge/internal/type.hpp | 193 ++++++++++++++ include/fcarouge/internal/utility.hpp | 41 ++- include/fcarouge/internal/x_z.hpp | 84 ++++++ include/fcarouge/internal/x_z_p_q_r_us_ps.hpp | 162 ++++++++++++ .../{kalman.hpp => x_z_u_p_q_r_f_g_ps.hpp} | 157 ++---------- include/fcarouge/internal/x_z_u_q_r_us_ps.hpp | 184 +++++++++++++ include/fcarouge/kalman.hpp | 139 ++++++---- include/fcarouge/utility.hpp | 7 + linalg/naive/fcarouge/linalg.hpp | 15 ++ sample/CMakeLists.txt | 3 +- sample/ekf_4x1x0_soaring.cpp | 59 ++--- sample/kf_1x1x0_building_height.cpp | 38 ++- sample/kf_1x1x0_liquid_temperature.cpp | 51 ++-- sample/kf_1x1x1_dog_position.cpp | 54 ++-- sample/kf_2x1x1_rocket_altitude.cpp | 173 +++++++------ sample/kf_6x2x0_vehicle_location.cpp | 114 ++++----- sample/kf_8x4x0_deep_sort_bounding_box.cpp | 112 ++++---- test/CMakeLists.txt | 2 +- test/kalman_assign_move_5x4x3.cpp | 5 +- test/kalman_constructor_default.cpp | 2 - test/kalman_constructor_default_1x1x3.cpp | 3 +- test/kalman_constructor_default_1x4x1.cpp | 3 +- test/kalman_constructor_default_1x4x3.cpp | 3 +- test/kalman_constructor_default_5x1x1.cpp | 4 +- test/kalman_constructor_default_5x1x3.cpp | 4 +- test/kalman_constructor_default_5x4x0.cpp | 6 +- test/kalman_constructor_default_5x4x1.cpp | 4 +- test/kalman_constructor_default_5x4x3.cpp | 4 +- ...kalman_constructor_default_float_1x1x1.cpp | 3 +- test/kalman_constructor_move_5x4x3.cpp | 4 +- test/kalman_f.cpp | 19 -- test/kalman_f_5x4x3.cpp | 10 +- test/kalman_format.cpp | 5 +- test/kalman_format_arguments.cpp | 11 +- test/kalman_format_float_1x1x1.cpp | 2 +- test/kalman_h.cpp | 2 +- test/kalman_h_5x4x3.cpp | 9 +- 42 files changed, 1477 insertions(+), 741 deletions(-) create mode 100644 include/fcarouge/internal/factory.hpp create mode 100644 include/fcarouge/internal/type.hpp create mode 100644 include/fcarouge/internal/x_z.hpp create mode 100644 include/fcarouge/internal/x_z_p_q_r_us_ps.hpp rename include/fcarouge/internal/{kalman.hpp => x_z_u_p_q_r_f_g_ps.hpp} (59%) create mode 100644 include/fcarouge/internal/x_z_u_q_r_us_ps.hpp diff --git a/README.md b/README.md index 84b187213..dedcaa5ca 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,7 @@ Designing a filter is as much art as science, with the following recipe. Model t This library supports various simple and extended filters. The implementation is independent from linear algebra backends. Arbitrary parameters can be added to the prediction and update stages to participate in gain-scheduling or linear parameter varying (LPV) systems. The default filter type is a generalized, customizable, and extended filter. The default type parameters implement a one-state, one-output, and double-precision floating-point type filter. The default update equation uses the Joseph form. Examples illustrate various usages and implementation tradeoffs. A standard formatter specialization is included for representation of the filter states. Filters with `state x output x input` dimensions as 1x1x1 and 1x1x0 (no input) are supported through vanilla C++. Higher dimension filters require a linear algebra backend. Customization points and type injections allow for implementation tradeoffs. +- [Kalman Filter](#kalman-filter) - [Examples](#examples) - [1x1 Constant System Dynamic Model Filter](#1x1-constant-system-dynamic-model-filter) - [6x2 Constant Acceleration Dynamic Model Filter](#6x2-constant-acceleration-dynamic-model-filter) @@ -382,6 +383,8 @@ Thanks everyone!
[![License Scan](https://app.fossa.com/api/projects/git%2Bgithub.com%2FFrancoisCarouge%2FKalman.svg?type=shield)](https://app.fossa.com/projects/git%2Bgithub.com%2FFrancoisCarouge%2FKalman?ref=badge_shield)
+[![OpenSSF Best Practices](https://www.bestpractices.dev/projects/8933/badge)](https://www.bestpractices.dev/projects/8933) +

[![Deploy Doxygen](https://github.com/FrancoisCarouge/Kalman/actions/workflows/deploy_doxygen.yml/badge.svg)](https://github.com/FrancoisCarouge/Kalman/actions/workflows/deploy_doxygen.yml)
diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt index 874634d98..c0b7b6589 100644 --- a/include/CMakeLists.txt +++ b/include/CMakeLists.txt @@ -45,11 +45,16 @@ target_sources( "HEADERS" FILES "fcarouge/algorithm.hpp" + "fcarouge/internal/factory.hpp" "fcarouge/internal/format.hpp" "fcarouge/internal/function.hpp" - "fcarouge/internal/kalman.hpp" "fcarouge/internal/kalman.tpp" + "fcarouge/internal/type.hpp" "fcarouge/internal/utility.hpp" + "fcarouge/internal/x_z_p_q_r_us_ps.hpp" + "fcarouge/internal/x_z_u_p_q_r_f_g_ps.hpp" + "fcarouge/internal/x_z_u_q_r_us_ps.hpp" + "fcarouge/internal/x_z.hpp" "fcarouge/kalman.hpp" "fcarouge/utility.hpp") install( diff --git a/include/fcarouge/internal/factory.hpp b/include/fcarouge/internal/factory.hpp new file mode 100644 index 000000000..d64be6025 --- /dev/null +++ b/include/fcarouge/internal/factory.hpp @@ -0,0 +1,235 @@ +/* __ _ __ __ _ _ +| |/ / /\ | | | \/ | /\ | \ | | +| ' / / \ | | | \ / | / \ | \| | +| < / /\ \ | | | |\/| | / /\ \ | . ` | +| . \ / ____ \| |____| | | |/ ____ \| |\ | +|_|\_\/_/ \_\______|_| |_/_/ \_\_| \_| + +Kalman Filter +Version 0.3.0 +https://github.com/FrancoisCarouge/Kalman + +SPDX-License-Identifier: Unlicense + +This is free and unencumbered software released into the public domain. + +Anyone is free to copy, modify, publish, use, compile, sell, or +distribute this software, either in source code form or as a compiled +binary, for any purpose, commercial or non-commercial, and by any +means. + +In jurisdictions that recognize copyright laws, the author or authors +of this software dedicate any and all copyright interest in the +software to the public domain. We make this dedication for the benefit +of the public at large and to the detriment of our heirs and +successors. We intend this dedication to be an overt act of +relinquishment in perpetuity of all present and future rights to this +software under copyright law. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR +OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +OTHER DEALINGS IN THE SOFTWARE. + +For more information, please refer to */ + +#ifndef FCAROUGE_INTERNAL_FACTORY_HPP +#define FCAROUGE_INTERNAL_FACTORY_HPP + +#include "fcarouge/internal/type.hpp" +#include "fcarouge/internal/x_z.hpp" +#include "fcarouge/internal/x_z_p_q_r_us_ps.hpp" +#include "fcarouge/internal/x_z_u_p_q_r_f_g_ps.hpp" +#include "fcarouge/internal/x_z_u_q_r_us_ps.hpp" + +#include +#include + +namespace fcarouge::internal { +//! @todo Support arbritary order of configuration parameters? +//! @todo Support user defined types by type name reflection? Case, naming +//! convention insensitive? +//! @todo Some of these overload should probably be removed to guide the user +//! towards better initialization practices? +template struct make_filter { + template + inline constexpr void + operator()([[maybe_unused]] Arguments... arguments) const + requires(std::same_as) + { + static_assert(false, + "This requested filter configuration is not yet supported. " + "Please, submit a pull request or feature request."); + } + + //! @todo Rename, clean the concet: undeduced? + inline constexpr auto operator()() const -> x_z + requires(std::same_as); + + inline constexpr auto operator()() const { return Filter{}; } + + template + inline constexpr auto operator()(state x, + [[maybe_unused]] output_t z) const { + return x_z{x.value}; + } + + template + inline constexpr auto operator()(state x, + [[maybe_unused]] output_t z, + [[maybe_unused]] input_t u) const { + using kt = x_z_u_q_r_us_ps; + return kt{typename kt::state{x.value}}; + } + + template + //! @todo Simplify the require clause? + requires requires(ProcessUncertainty q) { + requires std::invocable; + } + inline constexpr auto + operator()(state x, [[maybe_unused]] output_t z, + [[maybe_unused]] input_t u, + estimate_uncertainty p, + [[maybe_unused]] process_uncertainty q, + [[maybe_unused]] output_uncertainty r, + [[maybe_unused]] state_transition f, + [[maybe_unused]] input_control g, + [[maybe_unused]] prediction_types_t pts) const { + using kt = x_z_u_p_q_r_f_g_ps>>; + return kt{typename kt::state{x.value}, + typename kt::estimate_uncertainty{p.value}, + typename kt::noise_process_function{q.value}, + typename kt::output_uncertainty{r.value}, + typename kt::transition_state_function{f.value}, + typename kt::transition_control_function{g.value}}; + } + + template + inline constexpr auto + operator()(state x, [[maybe_unused]] output_t z, + [[maybe_unused]] input_t u, + [[maybe_unused]] update_types_t uts, + [[maybe_unused]] prediction_types_t pts) const { + using kt = + x_z_u_q_r_us_ps>, + repack_t>>; + return kt{typename kt::state{x.value}}; + } + + template + inline constexpr auto + operator()(state x, [[maybe_unused]] output_t z, + estimate_uncertainty p, + process_uncertainty q, + output_uncertainty r, + [[maybe_unused]] update_types_t uts, + [[maybe_unused]] prediction_types_t pts) const { + using kt = x_z_p_q_r_us_ps>, + repack_t>>; + return kt{typename kt::state{x.value}, + typename kt::estimate_uncertainty{p.value}, + typename kt::process_uncertainty{q.value}, + typename kt::output_uncertainty{r.value}}; + } + + template + inline constexpr auto + operator()(state x, [[maybe_unused]] output_t z, + estimate_uncertainty p, + process_uncertainty q, + output_uncertainty r, + output_model h, + [[maybe_unused]] state_transition f) const { + using kt = x_z_p_q_r_us_ps; + return kt{typename kt::state{x.value}, + typename kt::estimate_uncertainty{p.value}, + typename kt::process_uncertainty{q.value}, + typename kt::output_uncertainty{r.value}, + typename kt::output_model{h.value}, + typename kt::state_transition{f.value}}; + } + + template + inline constexpr auto + operator()(state x, [[maybe_unused]] output_t z, + estimate_uncertainty p, + output_uncertainty r) const { + using kt = x_z; + return kt{typename kt::state{x.value}, + typename kt::estimate_uncertainty{p.value}, + typename kt::output_uncertainty{r.value}}; + } + + template + inline constexpr auto + operator()(state x, [[maybe_unused]] output_t z, + estimate_uncertainty p, + process_uncertainty q, + output_uncertainty r) const { + using kt = x_z_p_q_r_us_ps; + return kt{typename kt::state{x.value}, + typename kt::estimate_uncertainty{p.value}, + typename kt::process_uncertainty{q.value}, + typename kt::output_uncertainty{r.value}}; + } + + template + inline constexpr auto + operator()(state x, [[maybe_unused]] output_t z, + [[maybe_unused]] input_t u, + estimate_uncertainty p, + process_uncertainty q, + output_uncertainty r) const { + using kt = x_z_u_q_r_us_ps; + return kt{typename kt::state{x.value}, + typename kt::estimate_uncertainty{p.value}, + typename kt::process_uncertainty{q.value}, + typename kt::output_uncertainty{r.value}}; + } + + template + inline constexpr auto + operator()(state x, [[maybe_unused]] output_t z, + [[maybe_unused]] input_t u, + estimate_uncertainty p, + process_uncertainty q, + output_uncertainty r, + [[maybe_unused]] update_types_t uts, + [[maybe_unused]] prediction_types_t pts) const { + + using kt = + x_z_u_q_r_us_ps>, + repack_t>>; + return kt{typename kt::state{x.value}, + typename kt::estimate_uncertainty{p.value}, + typename kt::process_uncertainty{q.value}, + typename kt::output_uncertainty{r.value}}; + } +}; + +template inline constexpr make_filter filter{}; + +template +using filter_t = std::invoke_result_t, Arguments...>; +} // namespace fcarouge::internal + +#endif // FCAROUGE_INTERNAL_FACTORY_HPP diff --git a/include/fcarouge/internal/format.hpp b/include/fcarouge/internal/format.hpp index cd77062a9..0bd634269 100644 --- a/include/fcarouge/internal/format.hpp +++ b/include/fcarouge/internal/format.hpp @@ -45,40 +45,34 @@ For more information, please refer to */ #include namespace fcarouge { -template class kalman; +template class kalman; } // namespace fcarouge -template +template // It is allowed to add template specializations for any standard library class // template to the namespace std only if the declaration depends on at least one // program-defined type and the specialization satisfies all requirements for // the original template, except where such specializations are prohibited. // NOLINTNEXTLINE(cert-dcl58-cpp) -struct std::formatter< - fcarouge::kalman, - Char> { - using kalman = - fcarouge::kalman; - +struct std::formatter, Char> { constexpr auto parse(std::basic_format_parse_context &parse_context) { return parse_context.begin(); } //! @todo P2585 may be useful in simplifying and standardizing the support. template - auto format(const kalman &filter, + auto format(const fcarouge::kalman &filter, std::basic_format_context &format_context) const -> OutputIt { format_context.advance_to( format_to(format_context.out(), R"({{"f": {}, )", filter.f())); - if constexpr (fcarouge::internal::has_input_control_method) { + if constexpr (fcarouge::has_input_control) { format_context.advance_to( format_to(format_context.out(), R"("g": {}, )", filter.g())); } - if constexpr (fcarouge::internal::has_output_model_method) { + if constexpr (fcarouge::has_output_model) { format_context.advance_to( format_to(format_context.out(), R"("h": {}, )", filter.h())); } @@ -86,8 +80,9 @@ struct std::formatter< format_context.advance_to(format_to( format_context.out(), R"("k": {}, "p": {}, )", filter.k(), filter.p())); - { - constexpr auto end{fcarouge::internal::repack_s}; + if constexpr (fcarouge::internal::has_prediction_types) { + constexpr auto end{ + fcarouge::internal::repack_s}; constexpr decltype(end) begin{0}; constexpr decltype(end) next{1}; fcarouge::internal::for_constexpr( @@ -98,19 +93,24 @@ struct std::formatter< }); } - format_context.advance_to(format_to(format_context.out(), - R"("q": {}, "r": {}, "s": {}, )", - filter.q(), filter.r(), filter.s())); + if constexpr (fcarouge::has_process_uncertainty) { + format_context.advance_to( + format_to(format_context.out(), R"("q": {}, )", filter.q())); + } + + format_context.advance_to(format_to( + format_context.out(), R"("r": {}, "s": {}, )", filter.r(), filter.s())); //! @todo Generalize out internal method concept when MSVC has better //! if-constexpr-requires support. - if constexpr (fcarouge::internal::has_input_method) { + if constexpr (fcarouge::has_input) { format_context.advance_to( format_to(format_context.out(), R"("u": {}, )", filter.u())); } - { - constexpr auto end{fcarouge::internal::repack_s}; + if constexpr (fcarouge::internal::has_update_types) { + constexpr auto end{ + fcarouge::internal::repack_s}; constexpr decltype(end) begin{0}; constexpr decltype(end) next{1}; fcarouge::internal::for_constexpr( diff --git a/include/fcarouge/internal/kalman.tpp b/include/fcarouge/internal/kalman.tpp index 2ee63a175..f7e2441d6 100644 --- a/include/fcarouge/internal/kalman.tpp +++ b/include/fcarouge/internal/kalman.tpp @@ -40,99 +40,91 @@ For more information, please refer to */ #define FCAROUGE_INTERNAL_KALMAN_TPP namespace fcarouge { -template + +template +template +inline constexpr kalman::kalman(Arguments... arguments) + : filter(internal::filter(arguments...)) {} + +template [[nodiscard("The returned state estimate column vector X is unexpectedly " "discarded.")]] inline constexpr auto -kalman::x() const - -> const state & { +kalman::x() const -> const state & { return filter.x; } -template +template [[nodiscard("The returned state estimate column vector X is unexpectedly " "discarded.")]] inline constexpr auto -kalman::x() -> state & { +kalman::x() -> state & { return filter.x; } -template -inline constexpr void -kalman::x( - const auto &value, const auto &...values) { +template +inline constexpr void kalman::x(const auto &value, + const auto &...values) { filter.x = std::move(state{value, values...}); } -template +template [[nodiscard("The returned observation column vector Z is unexpectedly " "discarded.")]] inline constexpr auto -kalman::z() const - -> const output & { +kalman::z() const -> const output & { return filter.z; } -template +template [[nodiscard("The returned control column vector U is unexpectedly " "discarded.")]] inline constexpr const auto & -kalman::u() const - requires(has_input) +kalman::u() const + requires(has_input) { return filter.u; } -template +template [[nodiscard("The returned estimated covariance matrix P is unexpectedly " "discarded.")]] inline constexpr auto -kalman::p() const - -> const estimate_uncertainty & { +kalman::p() const -> const estimate_uncertainty & { return filter.p; } -template +template [[nodiscard("The returned estimated covariance matrix P is unexpectedly " "discarded.")]] inline constexpr auto -kalman::p() - -> estimate_uncertainty & { +kalman::p() -> estimate_uncertainty & { return filter.p; } -template -inline constexpr void -kalman::p( - const auto &value, const auto &...values) { +template +inline constexpr void kalman::p(const auto &value, + const auto &...values) { filter.p = std::move(estimate_uncertainty{value, values...}); } -template +template [[nodiscard("The returned process noise covariance matrix Q is unexpectedly " - "discarded.")]] inline constexpr auto -kalman::q() const - -> const process_uncertainty & { + "discarded.")]] inline constexpr const auto & +kalman::q() const + requires(has_process_uncertainty) +{ return filter.q; } -template +template [[nodiscard("The returned process noise covariance matrix Q is unexpectedly " - "discarded.")]] inline constexpr auto -kalman::q() - -> process_uncertainty & { + "discarded.")]] inline constexpr auto & +kalman::q() + requires(has_process_uncertainty) +{ return filter.q; } -template -inline constexpr void -kalman::q( - const auto &value, const auto &...values) { +template +inline constexpr void kalman::q(const auto &value, + const auto &...values) + requires(has_process_uncertainty) +{ if constexpr (std::is_convertible_v) { filter.q = std::move(process_uncertainty{value, values...}); } else { @@ -142,29 +134,23 @@ kalman::q( } } -template +template [[nodiscard("The returned observation noise covariance matrix R is " "unexpectedly discarded.")]] inline constexpr auto -kalman::r() const - -> const output_uncertainty & { +kalman::r() const -> const output_uncertainty & { return filter.r; } -template +template [[nodiscard("The returned observation noise covariance matrix R is " "unexpectedly discarded.")]] inline constexpr auto -kalman::r() - -> output_uncertainty & { +kalman::r() -> output_uncertainty & { return filter.r; } -template -inline constexpr void -kalman::r( - const auto &value, const auto &...values) { +template +inline constexpr void kalman::r(const auto &value, + const auto &...values) { if constexpr (std::is_convertible_v) { filter.r = std::move(output_uncertainty{value, values...}); } else { @@ -174,29 +160,23 @@ kalman::r( } } -template +template [[nodiscard("The returned state transition matrix F is unexpectedly " "discarded.")]] inline constexpr auto -kalman::f() const - -> const state_transition & { +kalman::f() const -> const state_transition & { return filter.f; } -template +template [[nodiscard("The returned state transition matrix F is unexpectedly " "discarded.")]] inline constexpr auto -kalman::f() - -> state_transition & { +kalman::f() -> state_transition & { return filter.f; } -template -inline constexpr void -kalman::f( - const auto &value, const auto &...values) { +template +inline constexpr void kalman::f(const auto &value, + const auto &...values) { if constexpr (std::is_convertible_v) { filter.f = std::move(state_transition{value, values...}); } else { @@ -206,40 +186,32 @@ kalman::f( } } -template +template [[nodiscard("The returned observation transition matrix H is unexpectedly " "discarded.")]] inline constexpr const auto & -kalman::h() const - requires(has_output_model) +kalman::h() const + requires(has_output_model) { return filter.h; } -template +template [[nodiscard("The returned observation transition matrix H is unexpectedly " "discarded.")]] inline constexpr auto & -kalman::h() - requires(has_output_model) +kalman::h() + requires(has_output_model) { return filter.h; } -template -inline constexpr void -kalman::h( - const auto &value, const auto &...values) - requires(has_output_model) +template +inline constexpr void kalman::h(const auto &value, + const auto &...values) + requires(has_output_model) { - if constexpr (std::is_convertible_v< - decltype(value), - typename kalman::output_model>) { - filter.h = std::move( - typename kalman::output_model{value, values...}); + if constexpr (std::is_convertible_v) { + filter.h = std::move(typename Filter::output_model{value, values...}); } else { using observation_state_function = decltype(filter.observation_state_h); filter.observation_state_h = @@ -247,112 +219,88 @@ kalman::h( } } -template +template [[nodiscard("The returned control transition matrix G is unexpectedly " "discarded.")]] inline constexpr const auto & -kalman::g() const - requires(has_input_control) +kalman::g() const + requires(has_input_control) { return filter.g; } -template +template [[nodiscard("The returned control transition matrix G is unexpectedly " "discarded.")]] inline constexpr auto & -kalman::g() - requires(has_input_control) +kalman::g() + requires(has_input_control) { return filter.g; } -template -inline constexpr void -kalman::g( - const auto &value, const auto &...values) - requires(has_input_control) +template +inline constexpr void kalman::g(const auto &value, + const auto &...values) + requires(has_input_control) { using transition_control_function = decltype(filter.transition_control_g); filter.transition_control_g = std::move(transition_control_function{value, values...}); } -template +template [[nodiscard("The returned gain matrix K is unexpectedly " "discarded.")]] inline constexpr auto -kalman::k() const - -> const gain & { +kalman::k() const -> const gain & { return filter.k; } -template +template [[nodiscard("The returned innovation column vector Y is unexpectedly " "discarded.")]] inline constexpr auto -kalman::y() const - -> const innovation & { +kalman::y() const -> const innovation & { return filter.y; } -template +template [[nodiscard("The returned innovation uncertainty matrix S is unexpectedly " "discarded.")]] inline constexpr auto -kalman::s() const - -> const innovation_uncertainty & { +kalman::s() const -> const innovation_uncertainty & { return filter.s; } -template -inline constexpr void -kalman::transition( - const auto &callable) { +template +inline constexpr void kalman::transition(const auto &callable) { filter.transition = callable; } -template -inline constexpr void -kalman::observation( - const auto &callable) { +template +inline constexpr void kalman::observation(const auto &callable) { filter.observation = callable; } -template -inline constexpr void -kalman::update( - const auto &...arguments) { +template +inline constexpr void kalman::update(const auto &...arguments) { filter.update(arguments...); } -template +template template [[nodiscard("The returned update argument is unexpectedly " "discarded.")]] inline constexpr auto -kalman::update() const { +kalman::update() const { return std::get(filter.update_arguments); } -template -inline constexpr void -kalman::predict( - const auto &...arguments) { +template +inline constexpr void kalman::predict(const auto &...arguments) { filter.predict(arguments...); } -template +template template [[nodiscard("The returned prediction argument is unexpectedly " "discarded.")]] inline constexpr auto -kalman::predict() const { +kalman::predict() const { return std::get(filter.prediction_arguments); } } // namespace fcarouge diff --git a/include/fcarouge/internal/type.hpp b/include/fcarouge/internal/type.hpp new file mode 100644 index 000000000..3867e4cf2 --- /dev/null +++ b/include/fcarouge/internal/type.hpp @@ -0,0 +1,193 @@ +/* __ _ __ __ _ _ +| |/ / /\ | | | \/ | /\ | \ | | +| ' / / \ | | | \ / | / \ | \| | +| < / /\ \ | | | |\/| | / /\ \ | . ` | +| . \ / ____ \| |____| | | |/ ____ \| |\ | +|_|\_\/_/ \_\______|_| |_/_/ \_\_| \_| + +Kalman Filter +Version 0.3.0 +https://github.com/FrancoisCarouge/Kalman + +SPDX-License-Identifier: Unlicense + +This is free and unencumbered software released into the public domain. + +Anyone is free to copy, modify, publish, use, compile, sell, or +distribute this software, either in source code form or as a compiled +binary, for any purpose, commercial or non-commercial, and by any +means. + +In jurisdictions that recognize copyright laws, the author or authors +of this software dedicate any and all copyright interest in the +software to the public domain. We make this dedication for the benefit +of the public at large and to the detriment of our heirs and +successors. We intend this dedication to be an overt act of +relinquishment in perpetuity of all present and future rights to this +software under copyright law. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR +OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +OTHER DEALINGS IN THE SOFTWARE. + +For more information, please refer to */ + +#ifndef FCAROUGE_INTERNAL_TYPE_HPP +#define FCAROUGE_INTERNAL_TYPE_HPP + +#include "fcarouge/utility.hpp" + +//! @todo Remove the dependency on `std::initializer_list` if possible? +#include +#include + +#include + +namespace fcarouge::internal { + +template struct state { + Type value; + + constexpr explicit state(Type v) : value{v} {} + + template + requires(sizeof...(Types) > 1) + constexpr state(Types... elements) : value{elements...} {} +}; + +template state(Type) -> state; + +template +state(Types... elements) + -> state>[sizeof...(Types)]>; + +template struct estimate_uncertainty { + Type value; + + template + constexpr explicit estimate_uncertainty( + std::initializer_list> p) + : value(p) {} + + template + constexpr explicit estimate_uncertainty(Element p) : value{p} {} +}; + +template +estimate_uncertainty(Element) -> estimate_uncertainty; + +template +estimate_uncertainty(std::initializer_list>) + -> estimate_uncertainty< + std::initializer_list>>; + +template struct output_uncertainty { + Type value; + + template + constexpr explicit output_uncertainty( + std::initializer_list> r) + : value(r) {} + + template + constexpr explicit output_uncertainty(Element r) : value{r} {} +}; + +template +output_uncertainty(Element) -> output_uncertainty; + +template +output_uncertainty(std::initializer_list>) + -> output_uncertainty< + std::initializer_list>>; + +template struct process_uncertainty { + Type value; + + template + constexpr explicit process_uncertainty( + std::initializer_list> q) + : value(q) {} + + template + constexpr explicit process_uncertainty(Element q) : value{q} {} +}; + +template +process_uncertainty(Element) -> process_uncertainty; + +template +process_uncertainty(std::initializer_list>) + -> process_uncertainty< + std::initializer_list>>; + +template struct input_t {}; + +template inline input_t input{}; + +template struct output_t {}; + +template inline output_t output{}; + +template struct output_model { + std::initializer_list> value; + + constexpr explicit output_model( + std::initializer_list> h) + : value(h) {} +}; + +template struct state_transition { + Type value; + + template + constexpr explicit state_transition( + std::initializer_list> f) + : value(f) {} + + template + constexpr explicit state_transition(Element f) : value{f} {} +}; + +template +state_transition(Element) -> state_transition; + +template +state_transition(std::initializer_list>) + -> state_transition>>; + +template struct input_control { + Type value; + + template + constexpr explicit input_control( + std::initializer_list> g) + : value(g) {} + + template + constexpr explicit input_control(Element g) : value{g} {} +}; + +template input_control(Element) -> input_control; + +template +input_control(std::initializer_list>) + -> input_control>>; + +//! @todo Better name not ending by *_types? +template struct update_types_t {}; + +template inline update_types_t update_types{}; + +template struct prediction_types_t {}; + +template +inline prediction_types_t prediction_types{}; + +} // namespace fcarouge::internal + +#endif // FCAROUGE_INTERNAL_TYPE_HPP diff --git a/include/fcarouge/internal/utility.hpp b/include/fcarouge/internal/utility.hpp index 142d7cd02..73b4a8967 100644 --- a/include/fcarouge/internal/utility.hpp +++ b/include/fcarouge/internal/utility.hpp @@ -55,6 +55,8 @@ concept eigen = requires { typename Type::PlainMatrix; }; template concept has_input_member = requires(Filter filter) { filter.u; }; +//! @todo The _method concept be extraneous or incorrect? + template concept has_input_method = requires(Filter filter) { filter.u(); }; @@ -62,6 +64,18 @@ concept has_input_method = requires(Filter filter) { filter.u(); }; template concept has_input = has_input_member || has_input_method; +template +concept has_process_uncertainty_member = requires(Filter filter) { filter.q; }; + +template +concept has_process_uncertainty_method = + requires(Filter filter) { filter.q(); }; + +//! @todo Shorten when MSVC has better if-constexpr-requires support. +template +concept has_process_uncertainty = has_process_uncertainty_member || + has_process_uncertainty_method; + template concept has_input_control_member = requires(Filter filter) { filter.g; }; @@ -84,6 +98,13 @@ template concept has_output_model = has_output_model_member || has_output_model_method; +template +concept has_prediction_types = + requires() { typename Filter::prediction_types; }; + +template +concept has_update_types = requires() { typename Filter::update_types; }; + // There is only one known way to do conditional member types: partial // specialization of class templates. template struct conditional_input {}; @@ -111,22 +132,26 @@ template struct conditional_output_model {}; template struct conditional_output_model { //! @brief Type of the observation transition matrix H. //! - //! @details Also known as the measurement transition matrix or C. + //! @details Also known as the measurement transition matrix or C. The + //! presence of the member depends on the filter capabilities. using output_model = Filter::output_model; }; +template struct conditional_process_uncertainty {}; + +template +struct conditional_process_uncertainty { + //! @brief Type of the process noise correlated variance matrix Q. + using process_uncertainty = Filter::process_uncertainty; +}; + // The only way to have a conditional member type is to inherit from a template // specialization on the member type. template struct conditional_member_types : public conditional_input, conditional_input_control, - conditional_output_model {}; - -struct empty { - inline constexpr explicit empty([[maybe_unused]] auto &&...any) noexcept { - // Constructs from anything for all initializations compatibility. - } -}; + conditional_output_model, + conditional_process_uncertainty {}; template struct pack {}; diff --git a/include/fcarouge/internal/x_z.hpp b/include/fcarouge/internal/x_z.hpp new file mode 100644 index 000000000..5aecfffd7 --- /dev/null +++ b/include/fcarouge/internal/x_z.hpp @@ -0,0 +1,84 @@ +/* __ _ __ __ _ _ +| |/ / /\ | | | \/ | /\ | \ | | +| ' / / \ | | | \ / | / \ | \| | +| < / /\ \ | | | |\/| | / /\ \ | . ` | +| . \ / ____ \| |____| | | |/ ____ \| |\ | +|_|\_\/_/ \_\______|_| |_/_/ \_\_| \_| + +Kalman Filter +Version 0.3.0 +https://github.com/FrancoisCarouge/Kalman + +SPDX-License-Identifier: Unlicense + +This is free and unencumbered software released into the public domain. + +Anyone is free to copy, modify, publish, use, compile, sell, or +distribute this software, either in source code form or as a compiled +binary, for any purpose, commercial or non-commercial, and by any +means. + +In jurisdictions that recognize copyright laws, the author or authors +of this software dedicate any and all copyright interest in the +software to the public domain. We make this dedication for the benefit +of the public at large and to the detriment of our heirs and +successors. We intend this dedication to be an overt act of +relinquishment in perpetuity of all present and future rights to this +software under copyright law. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR +OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +OTHER DEALINGS IN THE SOFTWARE. + +For more information, please refer to */ + +#ifndef FCAROUGE_INTERNAL_X_Z_HPP +#define FCAROUGE_INTERNAL_X_Z_HPP + +#include "fcarouge/utility.hpp" + +namespace fcarouge::internal { +//! @todo Provide optimized out versions of the filters. +template struct x_z { + using state = State; + using output = Output; + using estimate_uncertainty = quotient; + using output_uncertainty = quotient; + using state_transition = quotient; + using gain = quotient; + using innovation = output; + using innovation_uncertainty = output_uncertainty; + + static inline const auto i{identity_v>}; + + state x{zero_v}; + estimate_uncertainty p{identity_v}; + output_uncertainty r{zero_v}; + state_transition f{identity_v}; + gain k{identity_v}; + innovation y{zero_v}; + innovation_uncertainty s{identity_v}; + output z{zero_v}; + transpose t{}; + + inline constexpr void update(const output &output_z) { + z = output_z; + s = p + r; + k = p / s; + y = z - x; + x = x + k * y; + p = (i - k) * p * t(i - k) + k * r * t(k); + } + + inline constexpr void predict() { + x = f * x; + p = f * p * t(f); + } +}; +} // namespace fcarouge::internal + +#endif // FCAROUGE_INTERNAL_X_Z_HPP diff --git a/include/fcarouge/internal/x_z_p_q_r_us_ps.hpp b/include/fcarouge/internal/x_z_p_q_r_us_ps.hpp new file mode 100644 index 000000000..146483138 --- /dev/null +++ b/include/fcarouge/internal/x_z_p_q_r_us_ps.hpp @@ -0,0 +1,162 @@ +/* __ _ __ __ _ _ +| |/ / /\ | | | \/ | /\ | \ | | +| ' / / \ | | | \ / | / \ | \| | +| < / /\ \ | | | |\/| | / /\ \ | . ` | +| . \ / ____ \| |____| | | |/ ____ \| |\ | +|_|\_\/_/ \_\______|_| |_/_/ \_\_| \_| + +Kalman Filter +Version 0.3.0 +https://github.com/FrancoisCarouge/Kalman + +SPDX-License-Identifier: Unlicense + +This is free and unencumbered software released into the public domain. + +Anyone is free to copy, modify, publish, use, compile, sell, or +distribute this software, either in source code form or as a compiled +binary, for any purpose, commercial or non-commercial, and by any +means. + +In jurisdictions that recognize copyright laws, the author or authors +of this software dedicate any and all copyright interest in the +software to the public domain. We make this dedication for the benefit +of the public at large and to the detriment of our heirs and +successors. We intend this dedication to be an overt act of +relinquishment in perpetuity of all present and future rights to this +software under copyright law. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR +OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +OTHER DEALINGS IN THE SOFTWARE. + +For more information, please refer to */ + +#ifndef FCAROUGE_INTERNAL_X_Z_P_Q_R_US_PS_HPP +#define FCAROUGE_INTERNAL_X_Z_P_Q_R_US_PS_HPP + +#include "fcarouge/utility.hpp" +#include "function.hpp" + +#include + +namespace fcarouge::internal { +// Helper template to support multiple pack deduction. +template +struct x_z_p_q_r_us_ps final {}; + +template +struct x_z_p_q_r_us_ps, + pack> { + using state = State; + using output = Output; + using estimate_uncertainty = quotient; + using process_uncertainty = quotient; + using output_uncertainty = quotient; + using state_transition = quotient; + using output_model = quotient; + using gain = quotient; + using innovation = output; + using innovation_uncertainty = output_uncertainty; + using observation_state_function = + function; + using noise_observation_function = function; + using transition_state_function = + function; + using noise_process_function = + function; + using transition_function = + function; + using observation_function = + function; + using update_types = std::tuple; + using prediction_types = std::tuple; + + static inline const auto i{identity_v>}; + + state x{zero_v}; + estimate_uncertainty p{identity_v}; + process_uncertainty q{zero_v}; + output_uncertainty r{zero_v}; + output_model h{identity_v}; + state_transition f{identity_v}; + gain k{identity_v}; + innovation y{zero_v}; + innovation_uncertainty s{identity_v}; + output z{zero_v}; + update_types update_arguments{}; + prediction_types prediction_arguments{}; + transpose t{}; + + //! @todo Should we pass through the reference to the state x or have the user + //! access it through filter.x() when needed? Where does the + //! practical/performance tradeoff leans toward? For the general case? For the + //! specialized cases? Same question applies to other parameters. + //! @todo Pass the arguments by universal reference? + observation_state_function observation_state_h{ + [&hh = h]([[maybe_unused]] const auto &...arguments) -> output_model { + return hh; + }}; + noise_observation_function noise_observation_r{ + [&rr = + r]([[maybe_unused]] const auto &...arguments) -> output_uncertainty { + return rr; + }}; + transition_state_function transition_state_f{ + [&ff = f]([[maybe_unused]] const auto &...arguments) -> state_transition { + return ff; + }}; + noise_process_function noise_process_q{ + [&qq = q]([[maybe_unused]] const auto &...arguments) + -> process_uncertainty { return qq; }}; + transition_function transition{ + [&ff = f](const state &state_x, + [[maybe_unused]] const auto &...arguments) -> state { + return ff * state_x; + }}; + observation_function observation{ + [&hh = h](const state &state_x, + [[maybe_unused]] const auto &...arguments) -> output { + return hh * state_x; + }}; + + //! @todo Do we want to store i - k * h in a temporary result for reuse? Or + //! does the compiler/linker do it for us? + //! @todo Do we want to support extended custom y = output_difference(z, + //! observation(x))? + //! @todo Do we want to pass z to `observation_state_h()`? What are the use + //! cases? + //! @todo Do we want to pass z to `observation()`? What are the use cases? + //! @todo Use operator `+=` for the state update? + template + inline constexpr void update(const UpdateTypes &...update_pack, + const Output0 &output_z, + const OutputN &...outputs_z) { + update_arguments = {update_pack...}; + z = output{output_z, outputs_z...}; + h = observation_state_h(x, update_pack...); + r = noise_observation_r(x, z, update_pack...); + s = innovation_uncertainty{h * p * t(h) + r}; + k = p * t(h) / s; + y = z - observation(x, update_pack...); + x = state{x + k * y}; + p = estimate_uncertainty{(i - k * h) * p * t(i - k * h) + k * r * t(k)}; + } + + inline constexpr void predict(const PredictionTypes &...prediction_pack) { + prediction_arguments = {prediction_pack...}; + f = transition_state_f(x, prediction_pack...); + q = noise_process_q(x, prediction_pack...); + x = transition(x, prediction_pack...); + p = estimate_uncertainty{f * p * t(f) + q}; + } +}; +} // namespace fcarouge::internal + +#endif // FCAROUGE_INTERNAL_X_Z_P_Q_R_US_PS_HPP diff --git a/include/fcarouge/internal/kalman.hpp b/include/fcarouge/internal/x_z_u_p_q_r_f_g_ps.hpp similarity index 59% rename from include/fcarouge/internal/kalman.hpp rename to include/fcarouge/internal/x_z_u_p_q_r_f_g_ps.hpp index a93e4009e..f7889dc97 100644 --- a/include/fcarouge/internal/kalman.hpp +++ b/include/fcarouge/internal/x_z_u_p_q_r_f_g_ps.hpp @@ -36,137 +36,23 @@ OTHER DEALINGS IN THE SOFTWARE. For more information, please refer to */ -#ifndef FCAROUGE_INTERNAL_KALMAN_HPP -#define FCAROUGE_INTERNAL_KALMAN_HPP +#ifndef FCAROUGE_INTERNAL_X_Z_U_P_Q_R_F_G_PS_HPP +#define FCAROUGE_INTERNAL_X_Z_U_P_Q_R_F_G_PS_HPP +#include "fcarouge/utility.hpp" #include "function.hpp" -#include "utility.hpp" #include namespace fcarouge::internal { +// Helper template to support multiple pack deduction. template -struct kalman final { - //! @todo Support some more specializations, all, or disable others? - //! @todo Add a pretty compilation error? -}; - -template -struct kalman, - pack> { - using state = State; - using output = Output; - using input = empty; - using estimate_uncertainty = quotient; - using process_uncertainty = quotient; - using output_uncertainty = quotient; - using state_transition = quotient; - using output_model = quotient; - using input_control = empty; - using gain = quotient; - using innovation = output; - using innovation_uncertainty = output_uncertainty; - using observation_state_function = - function; - using noise_observation_function = function; - using transition_state_function = - function; - using noise_process_function = - function; - using transition_control_function = empty; - using transition_function = - function; - using observation_function = - function; - using update_types = std::tuple; - using prediction_types = std::tuple; - - static inline const auto i{identity_v>}; - - state x{zero_v}; - estimate_uncertainty p{identity_v}; - process_uncertainty q{zero_v}; - output_uncertainty r{zero_v}; - output_model h{identity_v}; - state_transition f{identity_v}; - gain k{identity_v}; - innovation y{zero_v}; - innovation_uncertainty s{identity_v}; - output z{zero_v}; - update_types update_arguments{}; - prediction_types prediction_arguments{}; - transpose t{}; - - //! @todo Should we pass through the reference to the state x or have the user - //! access it through filter.x() when needed? Where does the - //! practical/performance tradeoff leans toward? For the general case? For the - //! specialized cases? Same question applies to other parameters. - //! @todo Pass the arguments by universal reference? - observation_state_function observation_state_h{ - [&hh = h]([[maybe_unused]] const auto &...arguments) -> output_model { - return hh; - }}; - noise_observation_function noise_observation_r{ - [&rr = - r]([[maybe_unused]] const auto &...arguments) -> output_uncertainty { - return rr; - }}; - transition_state_function transition_state_f{ - [&ff = f]([[maybe_unused]] const auto &...arguments) -> state_transition { - return ff; - }}; - noise_process_function noise_process_q{ - [&qq = q]([[maybe_unused]] const auto &...arguments) - -> process_uncertainty { return qq; }}; - transition_function transition{ - [&ff = f](const state &state_x, - [[maybe_unused]] const auto &...arguments) -> state { - return ff * state_x; - }}; - observation_function observation{ - [&hh = h](const state &state_x, - [[maybe_unused]] const auto &...arguments) -> output { - return hh * state_x; - }}; - - //! @todo Do we want to store i - k * h in a temporary result for reuse? Or - //! does the compiler/linker do it for us? - //! @todo Do we want to support extended custom y = output_difference(z, - //! observation(x))? - //! @todo Do we want to pass z to `observation_state_h()`? What are the use - //! cases? - //! @todo Do we want to pass z to `observation()`? What are the use cases? - //! @todo Use operator `+=` for the state update? - template - inline constexpr void update(const UpdateTypes &...update_pack, - const Output0 &output_z, - const OutputN &...outputs_z) { - update_arguments = {update_pack...}; - z = output{output_z, outputs_z...}; - h = observation_state_h(x, update_pack...); - r = noise_observation_r(x, z, update_pack...); - s = innovation_uncertainty{h * p * t(h) + r}; - k = p * t(h) / s; - y = z - observation(x, update_pack...); - x = state{x + k * y}; - p = estimate_uncertainty{(i - k * h) * p * t(i - k * h) + k * r * t(k)}; - } - - inline constexpr void predict(const PredictionTypes &...prediction_pack) { - prediction_arguments = {prediction_pack...}; - f = transition_state_f(x, prediction_pack...); - q = noise_process_q(x, prediction_pack...); - x = transition(x, prediction_pack...); - p = estimate_uncertainty{f * p * t(f) + q}; - } -}; +struct x_z_u_p_q_r_f_g_ps final {}; template -struct kalman, - pack> { +struct x_z_u_p_q_r_f_g_ps, + pack> { using state = State; using output = Output; using input = Input; @@ -200,8 +86,21 @@ struct kalman, state x{zero_v}; estimate_uncertainty p{identity_v}; - process_uncertainty q{zero_v}; + noise_process_function noise_process_q{ + [&qq = q]([[maybe_unused]] const auto &...arguments) + -> process_uncertainty { return qq; }}; output_uncertainty r{zero_v}; + transition_state_function transition_state_f{ + [&ff = f]([[maybe_unused]] const auto &...arguments) -> state_transition { + return ff; + }}; + transition_control_function transition_control_g{ + [&gg = g]([[maybe_unused]] const auto &...arguments) -> input_control { + return gg; + }}; + + process_uncertainty q{zero_v}; + input u{zero_v}; output_model h{identity_v}; state_transition f{identity_v}; input_control g{identity_v}; @@ -209,7 +108,6 @@ struct kalman, innovation y{zero_v}; innovation_uncertainty s{identity_v}; output z{zero_v}; - input u{zero_v}; update_types update_arguments{}; prediction_types prediction_arguments{}; transpose t{}; @@ -228,17 +126,6 @@ struct kalman, r]([[maybe_unused]] const auto &...arguments) -> output_uncertainty { return rr; }}; - transition_state_function transition_state_f{ - [&ff = f]([[maybe_unused]] const auto &...arguments) -> state_transition { - return ff; - }}; - noise_process_function noise_process_q{ - [&qq = q]([[maybe_unused]] const auto &...arguments) - -> process_uncertainty { return qq; }}; - transition_control_function transition_control_g{ - [&gg = g]([[maybe_unused]] const auto &...arguments) -> input_control { - return gg; - }}; transition_function transition{ [&ff = f, &gg = g](const state &state_x, const input &input_u, [[maybe_unused]] const auto &...arguments) -> state { @@ -295,4 +182,4 @@ struct kalman, }; } // namespace fcarouge::internal -#endif // FCAROUGE_INTERNAL_KALMAN_HPP +#endif // FCAROUGE_INTERNAL_X_Z_U_P_Q_R_F_G_PS_HPP diff --git a/include/fcarouge/internal/x_z_u_q_r_us_ps.hpp b/include/fcarouge/internal/x_z_u_q_r_us_ps.hpp new file mode 100644 index 000000000..1bb317233 --- /dev/null +++ b/include/fcarouge/internal/x_z_u_q_r_us_ps.hpp @@ -0,0 +1,184 @@ +/* __ _ __ __ _ _ +| |/ / /\ | | | \/ | /\ | \ | | +| ' / / \ | | | \ / | / \ | \| | +| < / /\ \ | | | |\/| | / /\ \ | . ` | +| . \ / ____ \| |____| | | |/ ____ \| |\ | +|_|\_\/_/ \_\______|_| |_/_/ \_\_| \_| + +Kalman Filter +Version 0.3.0 +https://github.com/FrancoisCarouge/Kalman + +SPDX-License-Identifier: Unlicense + +This is free and unencumbered software released into the public domain. + +Anyone is free to copy, modify, publish, use, compile, sell, or +distribute this software, either in source code form or as a compiled +binary, for any purpose, commercial or non-commercial, and by any +means. + +In jurisdictions that recognize copyright laws, the author or authors +of this software dedicate any and all copyright interest in the +software to the public domain. We make this dedication for the benefit +of the public at large and to the detriment of our heirs and +successors. We intend this dedication to be an overt act of +relinquishment in perpetuity of all present and future rights to this +software under copyright law. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR +OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +OTHER DEALINGS IN THE SOFTWARE. + +For more information, please refer to */ + +#ifndef FCAROUGE_INTERNAL_X_Z_U_Q_R_US_PS_HPP +#define FCAROUGE_INTERNAL_X_Z_U_Q_R_US_PS_HPP + +#include "fcarouge/utility.hpp" +#include "function.hpp" + +#include + +namespace fcarouge::internal { +// Helper template to support multiple pack deduction. +template +struct x_z_u_q_r_us_ps final {}; + +template +struct x_z_u_q_r_us_ps, + pack> { + using state = State; + using output = Output; + using input = Input; + using estimate_uncertainty = quotient; + using process_uncertainty = quotient; + using output_uncertainty = quotient; + using state_transition = quotient; + using output_model = quotient; + using input_control = quotient; + using gain = quotient; + using innovation = output; + using innovation_uncertainty = output_uncertainty; + using observation_state_function = + function; + using noise_observation_function = function; + using transition_state_function = function; + using noise_process_function = + function; + using transition_control_function = + function; + using transition_function = + function; + using observation_function = + function; + using update_types = std::tuple; + using prediction_types = std::tuple; + + static inline const auto i{identity_v>}; + + state x{zero_v}; + estimate_uncertainty p{identity_v}; + process_uncertainty q{zero_v}; + output_uncertainty r{zero_v}; + input u{zero_v}; + output_model h{identity_v}; + state_transition f{identity_v}; + input_control g{identity_v}; + gain k{identity_v}; + innovation y{zero_v}; + innovation_uncertainty s{identity_v}; + output z{zero_v}; + update_types update_arguments{}; + prediction_types prediction_arguments{}; + transpose t{}; + + //! @todo Should we pass through the reference to the state x or have the user + //! access it through filter.x() when needed? Where does the + //! practical/performance tradeoff leans toward? For the general case? For the + //! specialized cases? Same question applies to other parameters. + //! @todo Pass the arguments by universal reference? + observation_state_function observation_state_h{ + [&hh = h]([[maybe_unused]] const auto &...arguments) -> output_model { + return hh; + }}; + noise_observation_function noise_observation_r{ + [&rr = + r]([[maybe_unused]] const auto &...arguments) -> output_uncertainty { + return rr; + }}; + transition_state_function transition_state_f{ + [&ff = f]([[maybe_unused]] const auto &...arguments) -> state_transition { + return ff; + }}; + noise_process_function noise_process_q{ + [&qq = q]([[maybe_unused]] const auto &...arguments) + -> process_uncertainty { return qq; }}; + transition_control_function transition_control_g{ + [&gg = g]([[maybe_unused]] const auto &...arguments) -> input_control { + return gg; + }}; + transition_function transition{ + [&ff = f, &gg = g](const state &state_x, const input &input_u, + [[maybe_unused]] const auto &...arguments) -> state { + return ff * state_x + gg * input_u; + }}; + observation_function observation{ + [&hh = h](const state &state_x, + [[maybe_unused]] const auto &...arguments) -> output { + return hh * state_x; + }}; + + //! @todo Do we want to store i - k * h in a temporary result for reuse? Or + //! does the compiler/linker do it for us? + //! @todo Do we want to support extended custom y = output_difference(z, + //! observation(x))? + //! @todo Do we want to pass z to `observation_state_h()`? What are the use + //! cases? + //! @todo Do we want to pass z to `observation()`? What are the use cases? + template + inline constexpr void update(const UpdateTypes &...update_pack, + const Output0 &output_z, + const OutputN &...outputs_z) { + update_arguments = {update_pack...}; + z = output{output_z, outputs_z...}; + h = observation_state_h(x, update_pack...); + r = noise_observation_r(x, z, update_pack...); + s = h * p * t(h) + r; + k = p * t(h) / s; + y = z - observation(x, update_pack...); + x = state{x + k * y}; + p = estimate_uncertainty{(i - k * h) * p * t(i - k * h) + k * r * t(k)}; + } + + //! @todo Extended support? + //! @todo Should the transition state F computation arguments be {x, u, args} + //! instead of {x, args, u} or can we benefit for allowing passing through an + //! input pack to the function? Similar parameter ordering question for + //! related functions. + //! @todo Do we want to pass u to `noise_process_q()`? What are the use cases? + //! @todo Do we want to pass x, u to `transition_control_g()`? What are the + //! use cases? + template + inline constexpr void predict(const PredictionTypes &...prediction_pack, + const Input0 &input_u, + const InputN &...inputs_u) { + prediction_arguments = {prediction_pack...}; + u = input{input_u, inputs_u...}; + f = transition_state_f(x, u, prediction_pack...); + q = noise_process_q(x, prediction_pack...); + g = transition_control_g(prediction_pack...); + x = transition(x, u, prediction_pack...); + p = estimate_uncertainty{f * p * t(f) + q}; + } +}; +} // namespace fcarouge::internal + +#endif // FCAROUGE_INTERNAL_X_Z_U_Q_R_US_PS_HPP diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp index 34de56e85..2fc21c82a 100644 --- a/include/fcarouge/kalman.hpp +++ b/include/fcarouge/kalman.hpp @@ -47,8 +47,8 @@ For more information, please refer to */ //! inclusion in third party software. #include "algorithm.hpp" +#include "internal/factory.hpp" #include "internal/format.hpp" -#include "internal/kalman.hpp" #include "utility.hpp" #include @@ -58,6 +58,9 @@ For more information, please refer to */ #include namespace fcarouge { + +//! @name Types +//! @{ //! @brief A generic Kalman filter. //! //! @details The Kalman filter is a Bayesian filter that uses multivariate @@ -87,34 +90,9 @@ namespace fcarouge { //! require a linear algebra backend. Customization points and type injections //! allow for implementation tradeoffs. //! -//! @tparam State The type template parameter of the state column vector x. -//! State variables can be observed (measured), or hidden variables (inferred). -//! This is the the mean of the multivariate Gaussian. Defaults to `double`. -//! @tparam Output The type template parameter of the measurement column vector -//! z. Defaults to `double`. -//! @tparam Input The type template parameter of the control u. A `void` input -//! type can be used for systems with no input control to disable all of the -//! input control features, the control transition matrix G support, and the -//! other related computations from the filter. Defaults to `void`. -//! @tparam UpdateTypes The additional update function parameter types passed in -//! through a tuple-like parameter type, composing zero or more types. -//! Parameters such as delta times, variances, or linearized values. The -//! parameters are propagated to the function objects used to compute the state -//! observation H and the observation noise R matrices. The parameters are also -//! propagated to the state observation function object h. Defaults to no -//! parameter types, the empty pack. -//! @tparam PredictionTypes The additional prediction function parameter types -//! passed in through a tuple-like parameter type, composing zero or more types. -//! Parameters such as delta times, variances, or linearized values. The -//! parameters are propagated to the function objects used to compute the -//! process noise Q, the state transition F, and the control transition G -//! matrices. The parameters are also propagated to the state transition -//! function object f. Defaults to no parameter types, the empty pack. -//! -//! @note This class could be usable in constant expressions if `std::function` -//! could too. The polymorphic function wrapper was used in place of function -//! pointers to enable default initialization from this class, captured member -//! variables. +//! @tparam Filter Exposition only. The deduced internal filter template +//! parameter. Class template argument deduction (CTAD) figures out the filter +//! type based on the declared configuration. See deduction guide. //! //! @todo Make this class usable in constant expressions. //! @todo Is this filter restricted to Newton's equations of motion? That is @@ -154,12 +132,8 @@ namespace fcarouge { //! Sarkka, Senior Member, IEEE, Angel F. Garc ıa-Fernandez, //! https://arxiv.org/pdf/1905.13002.pdf ? GPU implementation? Parallel //! implementation? -template -class kalman final : public internal::conditional_member_types, - internal::repack_t>> { +template +class kalman final : public internal::conditional_member_types { private: //! @name Private Member Types //! @{ @@ -167,16 +141,18 @@ class kalman final : public internal::conditional_member_types, - internal::repack_t>; + //! of members vary based on the constructed, configured, declared, deduced + //! filter. + using implementation = Filter; //! @} //! @name Private Member Variables //! @{ //! @brief Encapsulates the implementation details of the filter. + //! + //! @details Optionally exposes a variety of members and methods according to + //! the selected implementation. implementation filter; //! @} @@ -197,9 +173,6 @@ class kalman final : public internal::conditional_member_types + inline constexpr kalman(Arguments... arguments); //! @brief Copy constructs a filter. //! @@ -335,7 +311,7 @@ class kalman final : public internal::conditional_member_types); + requires(has_input); //! @brief Returns the estimated covariance matrix P. //! @@ -360,8 +336,10 @@ class kalman final : public internal::conditional_member_types const process_uncertainty &; - inline constexpr auto q() -> process_uncertainty &; + inline constexpr const auto &q() const + requires(has_process_uncertainty); + inline constexpr auto &q() + requires(has_process_uncertainty); //! @brief Sets the process noise covariance matrix Q. //! @@ -377,7 +355,8 @@ class kalman final : public internal::conditional_member_types); //! @brief Returns the observation noise covariance //! matrix R. @@ -444,9 +423,9 @@ class kalman final : public internal::conditional_member_types); + requires(has_output_model); inline constexpr auto &h() - requires(has_output_model); + requires(has_output_model); //! @brief Sets the observation transition matrix H. //! @@ -469,7 +448,7 @@ class kalman final : public internal::conditional_member_types); + requires(has_output_model); //! @brief Returns the control transition matrix G. //! @@ -478,11 +457,13 @@ class kalman final : public internal::conditional_member_types); + requires(has_input_control); inline constexpr auto &g() - requires(has_input_control); + requires(has_input_control); //! @brief Sets the control transition matrix G. //! @@ -501,7 +482,7 @@ class kalman final : public internal::conditional_member_types); + requires(has_input_control); //! @brief Returns the gain matrix K. //! @@ -625,6 +606,56 @@ class kalman final : public internal::conditional_member_types inline constexpr auto update() const; //! @} }; + +//! @brief State type wrapper for filter declaration support. +//! +//! @todo Use alias from internal when Clang supports CTAD for alias? +using internal::state; + +//! @brief Estimate uncertainty type wrapper for filter declaration support. +using internal::estimate_uncertainty; + +//! @brief Output uncertainty type wrapper for filter declaration support. +using internal::output_uncertainty; + +//! @brief Process uncertainty type wrapper for filter declaration support. +using internal::process_uncertainty; + +//! @brief Input type wrapper for filter declaration support. +using internal::input; + +//! @brief Output type wrapper for filter declaration support. +using internal::output; + +//! @brief Output model type wrapper for filter declaration support. +using internal::output_model; + +//! @brief Update types wrapper for filter declaration support. +using internal::update_types; + +//! @brief Prediction types wrapper for filter declaration support. +using internal::prediction_types; + +using internal::state_transition; + +using internal::input_control; +//! @} + +//! @name Deduction Guides +//! @{ +//! @brief Deduces the filter type from its declared configuration. +//! +//! @details The configuration arguments passed are used to determine at compile +//! time the type of fiter to use. The objecive is to select the most performant +//! filter within the defined configuraton parameters. +//! +//! @tparam Arguments The declarations of the filter configuration. +//! +//! @todo Should the parameter be named configurations? +//! @todo Should the configuration examples, supports be documented here? +template +kalman(Arguments... arguments) -> kalman>; +//! @} } // namespace fcarouge #include "internal/kalman.tpp" diff --git a/include/fcarouge/utility.hpp b/include/fcarouge/utility.hpp index c8b156ef8..18472e360 100644 --- a/include/fcarouge/utility.hpp +++ b/include/fcarouge/utility.hpp @@ -78,6 +78,13 @@ concept eigen = internal::eigen; template concept has_input = internal::has_input; +//! @brief Filter process uncertainty support concept. +//! +//! @details The filter supports the process uncertainty related functionality: +//! `process_uncertainty` type member and `q()` method. +template +concept has_process_uncertainty = internal::has_process_uncertainty; + //! @brief Filter input control support concept. //! //! @details The filter supports the input control related functionality: diff --git a/linalg/naive/fcarouge/linalg.hpp b/linalg/naive/fcarouge/linalg.hpp index 8dd262a6f..b8be83e9e 100644 --- a/linalg/naive/fcarouge/linalg.hpp +++ b/linalg/naive/fcarouge/linalg.hpp @@ -263,6 +263,21 @@ template return result; } +// template +// [[nodiscard]] inline constexpr auto operator*(arithmetic auto lhs, +// matrix rhs) +// { +// matrix result; + +// for (decltype(Row) i{0}; i < Row; ++i) { +// for (decltype(Column) j{0}; j < Column; ++j) { +// result.data[i][j] = lhs * rhs.data[i][j]; +// } +// } + +// return result; +// } + template inline constexpr auto &operator*=(matrix &lhs, arithmetic auto rhs) { diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index 1d4365766..6bd84bfbe 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -73,7 +73,8 @@ endforeach() foreach(BACKEND IN ITEMS "eigen") foreach(SAMPLE "kf_6x2x0_vehicle_location.cpp" - "kf_8x4x0_deep_sort_bounding_box.cpp") + # "kf_8x4x0_deep_sort_bounding_box.cpp" + ) get_filename_component(NAME ${SAMPLE} NAME_WE) add_executable(kalman_sample_${BACKEND}_${NAME}_driver ${SAMPLE}) set_target_properties( diff --git a/sample/ekf_4x1x0_soaring.cpp b/sample/ekf_4x1x0_soaring.cpp index 280a98151..3836f722c 100644 --- a/sample/ekf_4x1x0_soaring.cpp +++ b/sample/ekf_4x1x0_soaring.cpp @@ -7,6 +7,7 @@ namespace fcarouge::sample { namespace { template using vector = column_vector; +using state = fcarouge::state>; //! @brief ArduPilot plane soaring. //! @@ -31,33 +32,43 @@ template using vector = column_vector; //! //! @todo Add a data set and assert for correctness of results. [[maybe_unused]] auto sample{[] { - // 4x1 extended filter with additional parameter for prediction: driftX [m], - // driftY [m]. Constant time step. - using state = vector<4>; - using output = float; - using no_input = void; - using kalman = kalman, - std::tuple>; - - kalman filter; - - // Initialization const float trigger_strength{0}; const float thermal_radius{80}; const float thermal_position_x{5}; const float thermal_position_y{0}; - filter.x(trigger_strength, thermal_radius, thermal_position_x, - thermal_position_y); - const float strength_covariance{0.0049F}; const float radius_covariance{400}; const float position_covariance{400}; - filter.p(kalman::estimate_uncertainty{{strength_covariance, 0.F, 0.F, 0.F}, - {0.F, radius_covariance, 0.F, 0.F}, - {0.F, 0.F, position_covariance, 0.F}, - {0.F, 0.F, 0.F, position_covariance}}); + const float strength_noise{std::pow(0.001F, 2.F)}; + const float distance_noise{std::pow(0.03F, 2.F)}; + const float measure_noise{std::pow(0.45F, 2.F)}; - // No process dynamics: F = ∂f/∂X = I4 Default. + // 4x1 extended filter with additional parameter for prediction: driftX [m], + // driftY [m]. Constant time step. + kalman filter{// The state X: + state{trigger_strength, thermal_radius, thermal_position_x, + thermal_position_y}, + // The output Z: + output, + // The estimate uncertainty P: + estimate_uncertainty{{strength_covariance, 0.F, 0.F, 0.F}, + {0.F, radius_covariance, 0.F, 0.F}, + {0.F, 0.F, position_covariance, 0.F}, + {0.F, 0.F, 0.F, position_covariance}}, + // The process uncertainty Q: + process_uncertainty{{strength_noise, 0.F, 0.F, 0.F}, + {0.F, distance_noise, 0.F, 0.F}, + {0.F, 0.F, distance_noise, 0.F}, + {0.F, 0.F, 0.F, distance_noise}}, + // The output uncertainty R: + output_uncertainty{measure_noise}, + // No process dynamics: the state transition F = ∂f/∂X = I4 + // Default. The additional parameters for update. + update_types, + // The additional parameters for prediction. + prediction_types}; + + using kalman = decltype(filter); filter.transition([](const kalman::state &x, const float &drift_x, const float &drift_y) -> kalman::state { @@ -66,16 +77,6 @@ template using vector = column_vector; return x + drifts; }); - const float strength_noise{std::pow(0.001F, 2.F)}; - const float distance_noise{std::pow(0.03F, 2.F)}; - filter.q(kalman::process_uncertainty{{strength_noise, 0.F, 0.F, 0.F}, - {0.F, distance_noise, 0.F, 0.F}, - {0.F, 0.F, distance_noise, 0.F}, - {0.F, 0.F, 0.F, distance_noise}}); - - const float measure_noise{std::pow(0.45F, 2.F)}; - filter.r(kalman::output_uncertainty{measure_noise}); - // Observation Z: [w] vertical air velocity w at the aircraft’s // position w.r.t. the thermal center [m.s^-1]. filter.observation([](const kalman::state &x, const float &position_x, diff --git a/sample/kf_1x1x0_building_height.cpp b/sample/kf_1x1x0_building_height.cpp index 5d6c5f21b..db0dd55f1 100644 --- a/sample/kf_1x1x0_building_height.cpp +++ b/sample/kf_1x1x0_building_height.cpp @@ -24,22 +24,20 @@ namespace { //! @example kf_1x1x0_building_height.cpp [[maybe_unused]] auto sample{[] { // A one-dimensional filter, constant system dynamic model. - kalman filter; + kalman filter{// One can estimate the building height simply by looking at it. + // The estimated state building height is: X = 60 meters. + state{60.}, + // The building height measurement Z. + output, + // A human’s estimation error (standard deviation) is about 15 + // meters: σ = 15. Consequently the variance is σ^2 = 225. The + // estimate uncertainty is: P = 225. + estimate_uncertainty{225.}, + // Since the standard deviation σ of the altimeter measurement + // error is 5, the variance σ^2 would be 25, thus the + // measurement, output uncertainty is: R = 25. + output_uncertainty{25.}}; - // Initialization - // One can estimate the building height simply by looking at it. The estimated - // building height is: 60 meters. - filter.x(60.); - - // Now we shall initialize the estimate uncertainty. A human’s estimation - // error (standard deviation) is about 15 meters: σ = 15. Consequently the - // variance is 225: σ^2 = 225. - filter.p(225.); - - // Prediction - // Now, we shall predict the next state based on the initialization values. - // Note: The prediction operation needs not be performed since the process - // noise covariance Q is null in this example. assert(60 == filter.x() && "Since our system's dynamic model is constant, i.e. the building " "doesn't change its height: 60 meters."); @@ -47,12 +45,10 @@ namespace { "The extrapolated estimate uncertainty (variance) also doesn't " "change: 225"); - // Measure and Update - // The first measurement is: z1 = 48.54m. Since the standard deviation σ of - // the altimeter measurement error is 5, the variance σ^2 would be 25, thus - // the measurement uncertainty is: r1 = 25. - filter.r(25.); - + // Now, we shall predict the next state based on the initialization values. + // Note: The prediction operation needs not be performed since the process + // noise covariance Q is null in this example. + // Measure and update: the first measurement is: z1 = 48.54m. filter.update(48.54); // And so on. diff --git a/sample/kf_1x1x0_liquid_temperature.cpp b/sample/kf_1x1x0_liquid_temperature.cpp index 19aba09a0..cae8bfa89 100644 --- a/sample/kf_1x1x0_liquid_temperature.cpp +++ b/sample/kf_1x1x0_liquid_temperature.cpp @@ -27,27 +27,30 @@ namespace { //! @example kf_1x1x0_liquid_temperature.cpp [[maybe_unused]] auto sample{[] { // A one-dimensional filter, constant system dynamic model. - kalman filter; - - // Initialization - // Before the first iteration, we must initialize the Kalman filter and - // predict the next state (which is the first state). We don't know what the - // temperature of the liquid is, and our guess is 10°C. - filter.x(10.); - - // Our guess is very imprecise, so we set our initialization estimate error σ - // to 100. The estimate uncertainty of the initialization is the error - // variance σ^2: p0,0 = 100^2 = 10,000. This variance is very high. If we - // initialize with a more meaningful value, we will get faster Kalman filter - // convergence. - filter.p(100 * 100.); - - // Prediction - // Now, we shall predict the next state based on the initialization values. We - // think that we have an accurate model, thus we set the process noise - // variance q to 0.0001. - filter.q(0.0001); + kalman filter{ + // Before the first iteration, we must initialize the Kalman filter and + // predict the next state (which is the first state). We don't know what + // the temperature of the liquid is, and our guess is: estimated state X = + // 10°C. + state{10.}, + // The measured liquid temperature Z. + output, + // Our guess is very imprecise, so we set our initialization estimate + // error σ to 100. The estimate uncertainty p of the initialization is the + // error variance σ^2: P = p0,0 = 100^2 = 10,000. This variance is very + // high. If we initialize with a more meaningful value, we will get faster + // Kalman filter convergence. + estimate_uncertainty{100 * 100.}, + // We have an accurate model, thus we set the process uncertainty noise + // variance Q to 0.0001. + process_uncertainty{0.0001}, + // Since the measurement error of the thermometer is σ = 0.1, the variance + // σ^2 would be 0.01, thus the measurement, output uncertainty is: R = r1 + // = 0.01. The measurement error (standard deviation) is 0.1 degrees + // Celsius. + output_uncertainty{0.1 * 0.1}}; + // Now, we shall predict the next state based on the initialization values. filter.predict(); assert(10 == filter.x() && @@ -57,13 +60,7 @@ namespace { "The extrapolated estimate uncertainty (variance): p1,0 = p0,0 + q = " "10000 + 0.0001 = 10000.0001."); - // Measure and Update - // The measurement value: z1 = 49.95°C. Since the measurement error is σ = - // 0.1, the variance σ^2 would be 0.01, thus the measurement uncertainty is: - // r1 = 0.01. The measurement error (standard deviation) is 0.1 degrees - // Celsius. - filter.r(0.1 * 0.1); - + // The first measurement value: z1 = 49.95°C. Measure and update. filter.update(49.95); assert(std::abs(1 - filter.k() / 0.999999) < 0.0001 && diff --git a/sample/kf_1x1x1_dog_position.cpp b/sample/kf_1x1x1_dog_position.cpp index c44f495d7..9b6b52885 100644 --- a/sample/kf_1x1x1_dog_position.cpp +++ b/sample/kf_1x1x1_dog_position.cpp @@ -31,36 +31,34 @@ namespace { //! //! @example kf_1x1x1_dog_position.cpp [[maybe_unused]] auto sample{[] { - using kalman = kalman; - kalman filter; + kalman filter{ + // This is the dog's initial position expressed as a Gaussian. The + // state X position is 0 meters. + state{1.}, + // The measured output position Z. + output, + // We are predicting that at each time step the dog moves forward one + // meter. This is the input U process model - the description of how we + // think the + // dog moves. How do I know the velocity? Magic? Consider it a prediction, + // or perhaps we have a secondary velocity sensor. Please accept this + // simplification for now. + input, + // The variance to 400 m, which is a standard deviation of 20 meters. You + // can think of this as saying "I believe with 99.7% accuracy the position + // is 0 plus or minus 60 meters". This is because with Gaussians ~99.7% of + // values fall within of the mean. The estimate uncertainty P: + estimate_uncertainty{20 * 20.}, + // Variance in the dog's movement. The process variance is how much error + // there is in the process model. Dogs rarely do what we expect, and + // things like hills or the whiff of a squirrel will change his progress. + // The process uncertainty Q: + process_uncertainty{1.}, + // Variance in the sensor. The meaning of sensor variance is how much + // variance there is in each measurement. The output uncertainty R: + output_uncertainty{2.}}; - // Initialization - // This is the dog's initial position expressed as a Gaussian. The position is - // 0 meters, and the variance to 400 m, which is a standard deviation of 20 - // meters. You can think of this as saying "I believe with 99.7% accuracy the - // position is 0 plus or minus 60 meters". This is because with Gaussians - // ~99.7% of values fall within of the mean. - filter.x(1.); - filter.p(20 * 20.); - - // Prediction - // Variance in the dog's movement. The process variance is how much error - // there is in the process model. Dogs rarely do what we expect, and things - // like hills or the whiff of a squirrel will change his progress. - filter.q(1.); - - // Measure and Update - // Variance in the sensor. The meaning of sensor variance should be clear - it - // is how much variance there is in each measurement. - filter.r(2.); - - // We are predicting that at each time step the dog moves forward one meter. - // This is the process model - the description of how we think the dog moves. - // How do I know the velocity? Magic? Consider it a prediction, or perhaps we - // have a secondary velocity sensor. Please accept this simplification for - // now. filter.predict(1.); - filter.update(1.354); filter.predict(1.); filter.update(1.882); diff --git a/sample/kf_2x1x1_rocket_altitude.cpp b/sample/kf_2x1x1_rocket_altitude.cpp index faee1da0d..ee0000754 100644 --- a/sample/kf_2x1x1_rocket_altitude.cpp +++ b/sample/kf_2x1x1_rocket_altitude.cpp @@ -8,6 +8,11 @@ namespace fcarouge::sample { namespace { template using vector = column_vector; +template using matrix = matrix; +using altitude = double; +using acceleration = double; +using milliseconds = std::chrono::milliseconds; +using state = fcarouge::state>; //! @brief Estimating the rocket altitude. //! @@ -37,65 +42,64 @@ template using vector = column_vector; //! Let us assume a rocket boosting vertically with constant acceleration. The //! rocket is equipped with an altimeter that provides altitude measurements and //! an accelerometer that serves as a control input. -//! - The measurements period: Δt = 0.25s +//! - The constant measurements period: Δt = 0.25s //! - The rocket acceleration: a= 30 m.s^-2 //! - The altimeter measurement error standard deviation: σxm = 20m //! - The accelerometer measurement error standard deviation: ϵ = 0.1 m.s^-2 //! //! @example kf_2x1x1_rocket_altitude.cpp [[maybe_unused]] auto sample{[] { - // A 2x1x1 filter, constant acceleration dynamic model, no control, step time. - using state = vector<2>; - using output = double; - using input = double; - using kalman = kalman, - std::tuple>; - kalman filter; - - // Initialization - // We don't know the rocket location; we will set initial position and - // velocity to 0. - filter.x(0., 0.); - - // Since our initial state vector is a guess, we will set a very high estimate - // uncertainty. The high estimate uncertainty results in high Kalman gain, - // giving a high weight to the measurement. - filter.p(kalman::estimate_uncertainty{{500., 0.}, {0., 500.}}); - - // Prediction - // We will assume a discrete noise model - the noise is different at each time - // period, but it is constant between time periods. In our previous example, - // we used the system's random variance in acceleration σ^2 as a multiplier of - // the process noise matrix. But here, we have an accelerometer that measures - // the system random acceleration. The accelerometer error v is much lower - // than system's random acceleration, therefore we use ϵ^2 as a multiplier of - // the process noise matrix. This makes our estimation uncertainty much lower! - filter.q([]([[maybe_unused]] const kalman::state &x, - const std::chrono::milliseconds &delta_time) { - const auto dt{std::chrono::duration(delta_time).count()}; - return kalman::process_uncertainty{ - {0.1 * 0.1 * dt * dt * dt * dt / 4, 0.1 * 0.1 * dt * dt * dt / 2}, - {0.1 * 0.1 * dt * dt * dt / 2, 0.1 * 0.1 * dt * dt}}; - }); - - // The state transition matrix F would be: - filter.f([]([[maybe_unused]] const kalman::state &x, - [[maybe_unused]] const kalman::input &u, - const std::chrono::milliseconds &delta_time) { - const auto dt{std::chrono::duration(delta_time).count()}; - return kalman::state_transition{{1., dt}, {0., 1.}}; - }); - - // The control matrix G would be: - filter.g([](const std::chrono::milliseconds &delta_time) { - const auto dt{std::chrono::duration(delta_time).count()}; - return kalman::input_control{0.0313, dt}; - }); + // A 2x1x1 filter, constant acceleration dynamic model, no control, step + // time. + kalman filter{ + // We don't know the rocket location; we will set initial position and + // velocity to state X = [0.0, 0.0]. + state{0., 0.}, + // The filter estimates the output Z altitude as a double [m]. + output, + // The filter receives in the input U accelerometer as a double [m.s^-2]. + input, + // Since our initial state vector is a guess, we will set a very high + // estimate uncertainty. The high estimate uncertainty P results in high + // Kalman gain, giving a high weight to the measurement. + estimate_uncertainty{{500., 0.}, {0., 500.}}, + // We will assume a discrete noise model - the noise is different at each + // time period, but it is constant between time periods. In our previous + // example, we used the system's random variance in acceleration σ^2 as a + // multiplier of the process noise matrix. But here, we have an + // accelerometer that measures the system random acceleration. The + // accelerometer error v is much lower than system's random acceleration, + // therefore we use ϵ^2 as a multiplier of the process noise matrix. This + // makes our estimation uncertainty much lower! + process_uncertainty{[]([[maybe_unused]] const vector<2> &x, + const milliseconds &delta_time) { + const auto dt{std::chrono::duration(delta_time).count()}; + return matrix<2, 2>{ + {0.1 * 0.1 * dt * dt * dt * dt / 4, 0.1 * 0.1 * dt * dt * dt / 2}, + {0.1 * 0.1 * dt * dt * dt / 2, 0.1 * 0.1 * dt * dt}}; + }}, + // For the sake of the example simplicity, we will assume a constant + // measurement uncertainty: R1 = R2...Rn-1 = Rn = R. + output_uncertainty{400.}, + // The state transition matrix F would be: + state_transition{[]([[maybe_unused]] const vector<2> &x, + [[maybe_unused]] const acceleration &u, + const milliseconds &delta_time) { + const auto dt{std::chrono::duration(delta_time).count()}; + return matrix<2, 2>{{1., dt}, {0., 1.}}; + }}, + // The control matrix G would be: + input_control{[](const milliseconds &delta_time) { + const auto dt{std::chrono::duration(delta_time).count()}; + return vector<2>{0.0313, dt}; + }}, + // The filter prediction uses a delta time [ms] parameter. + prediction_types}; // We also don't know what the rocket acceleration is, but we can assume that // it's greater than zero. Let's assume: u0 = g const double gravity{-9.8}; // [m.s^-2] - const std::chrono::milliseconds delta_time{250}; + const milliseconds delta_time{250}; filter.predict(delta_time, -gravity); assert(std::abs(1 - filter.x()[0] / 0.3) < 0.03 && @@ -110,11 +114,7 @@ template using vector = column_vector; // Measure and Update // The dimension of zn is 1x1 and the dimension of xn is 2x1, so the dimension // of the observation matrix H will be 1x2. - filter.h(kalman::output_model{1., 0.}); - - // For the sake of the example simplicity, we will assume a constant - // measurement uncertainty: R1 = R2...Rn-1 = Rn = R. - filter.r(kalman::output_uncertainty{400.}); + // filter.h(kalman::output_model{1., 0.}); filter.update(-32.4); @@ -142,14 +142,13 @@ template using vector = column_vector; // measurements period: Δt = 250ms. The period is constant but passed as // variable for the example. The lambda helper shows how to simplify the // filter step call. - const auto step{[&filter](double altitude, - std::chrono::milliseconds step_time, - double acceleration) { - filter.update(altitude); - filter.predict(step_time, acceleration); + const auto step{[&filter, &delta_time](altitude measured_altitude, + acceleration measured_acceleration) { + filter.update(measured_altitude); + filter.predict(delta_time, measured_acceleration); }}; - step(-11.1, delta_time, 40.02 + gravity); + step(-11.1, 40.02 + gravity); assert(std::abs(1 - filter.x()[0] / -12.3) < 0.002 && std::abs(1 - filter.x()[1] / 14.8) < 0.002 && @@ -160,33 +159,33 @@ template using vector = column_vector; std::abs(1 - filter.p()(1, 1) / 438.8) < 0.001 && "The estimate uncertainty expected at 0.1% accuracy."); - step(18., delta_time, 39.97 + gravity); - step(22.9, delta_time, 39.81 + gravity); - step(19.5, delta_time, 39.75 + gravity); - step(28.5, delta_time, 39.6 + gravity); - step(46.5, delta_time, 39.77 + gravity); - step(68.9, delta_time, 39.83 + gravity); - step(48.2, delta_time, 39.73 + gravity); - step(56.1, delta_time, 39.87 + gravity); - step(90.5, delta_time, 39.81 + gravity); - step(104.9, delta_time, 39.92 + gravity); - step(140.9, delta_time, 39.78 + gravity); - step(148., delta_time, 39.98 + gravity); - step(187.6, delta_time, 39.76 + gravity); - step(209.2, delta_time, 39.86 + gravity); - step(244.6, delta_time, 39.61 + gravity); - step(276.4, delta_time, 39.86 + gravity); - step(323.5, delta_time, 39.74 + gravity); - step(357.3, delta_time, 39.87 + gravity); - step(357.4, delta_time, 39.63 + gravity); - step(398.3, delta_time, 39.67 + gravity); - step(446.7, delta_time, 39.96 + gravity); - step(465.1, delta_time, 39.8 + gravity); - step(529.4, delta_time, 39.89 + gravity); - step(570.4, delta_time, 39.85 + gravity); - step(636.8, delta_time, 39.9 + gravity); - step(693.3, delta_time, 39.81 + gravity); - step(707.3, delta_time, 39.81 + gravity); + step(18., 39.97 + gravity); + step(22.9, 39.81 + gravity); + step(19.5, 39.75 + gravity); + step(28.5, 39.6 + gravity); + step(46.5, 39.77 + gravity); + step(68.9, 39.83 + gravity); + step(48.2, 39.73 + gravity); + step(56.1, 39.87 + gravity); + step(90.5, 39.81 + gravity); + step(104.9, 39.92 + gravity); + step(140.9, 39.78 + gravity); + step(148., 39.98 + gravity); + step(187.6, 39.76 + gravity); + step(209.2, 39.86 + gravity); + step(244.6, 39.61 + gravity); + step(276.4, 39.86 + gravity); + step(323.5, 39.74 + gravity); + step(357.3, 39.87 + gravity); + step(357.4, 39.63 + gravity); + step(398.3, 39.67 + gravity); + step(446.7, 39.96 + gravity); + step(465.1, 39.8 + gravity); + step(529.4, 39.89 + gravity); + step(570.4, 39.85 + gravity); + step(636.8, 39.9 + gravity); + step(693.3, 39.81 + gravity); + step(707.3, 39.81 + gravity); filter.update(748.5); diff --git a/sample/kf_6x2x0_vehicle_location.cpp b/sample/kf_6x2x0_vehicle_location.cpp index 519dedc6d..c314c880e 100644 --- a/sample/kf_6x2x0_vehicle_location.cpp +++ b/sample/kf_6x2x0_vehicle_location.cpp @@ -16,6 +16,8 @@ constexpr auto fcarouge::operator/(const Numerator &lhs, const Denominator &rhs) namespace fcarouge::sample { namespace { template using vector = column_vector; +template using matrix = matrix; +using state = fcarouge::state>; //! @brief Estimating the vehicle location. //! @@ -32,72 +34,68 @@ template using vector = column_vector; //! X direction with a constant velocity. After traveling 400 meters the vehicle //! turns right, with a turning radius of 300 meters. During the turning //! maneuver, the vehicle experiences acceleration due to the circular motion -//! (an angular acceleration). The measurements period: Δt = 1s (constant). The -//! random acceleration standard deviation: σa = 0.2 m.s^-2. +//! (an angular acceleration). The measurements period: Δt = 1s (constant). //! //! @example kf_6x2x0_vehicle_location.cpp [[maybe_unused]] auto sample{[] { // A 6x2x0 filter, constant acceleration dynamic model, no control. - using state = vector<6>; - using output = vector<2>; - using no_input = void; - using kalman = kalman; - - kalman filter; - - // Initialization - // The state is chosen to be the position, velocity, acceleration in the XY - // plane: [px, vx, ax, py, vy, ay]. We don't know the vehicle location; we - // will set initial position, velocity and acceleration to 0. - filter.x(0., 0., 0., 0., 0., 0.); - - // Since our initial state vector is a guess, we will set a very high estimate - // uncertainty. The high estimate uncertainty results in a high Kalman Gain, - // giving a high weight to the measurement. - filter.p(kalman::estimate_uncertainty{{500., 0., 0., 0., 0., 0.}, - {0., 500., 0., 0., 0., 0.}, - {0., 0., 500., 0., 0., 0.}, - {0., 0., 0., 500., 0., 0.}, - {0., 0., 0., 0., 500., 0.}, - {0., 0., 0., 0., 0., 500.}}); - - // Prediction - // The process noise matrix Q would be: - kalman::process_uncertainty q{ - {0.25, 0.5, 0.5, 0., 0., 0.}, {0.5, 1., 1., 0., 0., 0.}, - {0.5, 1., 1., 0., 0., 0.}, {0., 0., 0., 0.25, 0.5, 0.5}, - {0., 0., 0., 0.5, 1., 1.}, {0., 0., 0., 0.5, 1., 1.}}; - q *= 0.2 * 0.2; - filter.q(q); - - // The state transition matrix F would be: - filter.f(kalman::state_transition{{1., 1., 0.5, 0., 0., 0.}, - {0., 1., 1., 0., 0., 0.}, - {0., 0., 1., 0., 0., 0.}, - {0., 0., 0., 1., 1., 0.5}, - {0., 0., 0., 0., 1., 1.}, - {0., 0., 0., 0., 0., 1.}}); + kalman filter{ + // The state X is chosen to be the position, velocity, acceleration in the + // XY plane: [px, vx, ax, py, vy, ay]. We don't know the vehicle location; + // we will set initial position, velocity and acceleration to 0. + state{0., 0., 0., 0., 0., 0.}, + // The vehicle has an onboard location sensor that reports output Z as X + // and Y + // coordinates of the system. + output>, + // The estimate uncertainty matrix P. + // Since our initial state vector is a guess, we will set a very high + // estimate uncertainty. The high estimate uncertainty results in a high + // Kalman Gain, giving a high weight to the measurement. + estimate_uncertainty{{500., 0., 0., 0., 0., 0.}, + {0., 500., 0., 0., 0., 0.}, + {0., 0., 500., 0., 0., 0.}, + {0., 0., 0., 500., 0., 0.}, + {0., 0., 0., 0., 500., 0.}, + {0., 0., 0., 0., 0., 500.}}, + // The process uncertainty noise matrix Q, constant, computed in place, + // with the + // random acceleration standard deviation: σa = 0.2 m.s^-2. + process_uncertainty{[]() -> matrix<6, 6> { + return 0.2 * 0.2 * matrix<6, 6>{{0.25, 0.5, 0.5, 0., 0., 0.}, + {0.5, 1., 1., 0., 0., 0.}, + {0.5, 1., 1., 0., 0., 0.}, + {0., 0., 0., 0.25, 0.5, 0.5}, + {0., 0., 0., 0.5, 1., 1.}, + {0., 0., 0., 0.5, 1., 1.}}; + }()}, + // The output uncertainty matrix R. Assume that the x and y measurements + // are uncorrelated, i.e. error in the x coordinate measurement doesn't + // depend on the error in the y coordinate measurement. In real-life + // applications, the measurement uncertainty can differ between + // measurements. In many systems the measurement uncertainty depends on + // the measurement SNR (signal-to-noise ratio), angle between sensor (or + // sensors) and target, signal frequency and many other parameters. + // For the sake of the example simplicity, we will assume a constant + // measurement uncertainty: R1 = R2...Rn-1 = Rn = R The measurement error + // standard deviation: σxm = σym = 3m. The variance 9. + output_uncertainty{{9., 0.}, {0., 9.}}, + // The output model matrix H. The dimension of zn is 2x1 and the dimension + // of xn is 6x1. Therefore the dimension of the observation matrix H shall + // be 2x6. + // COULD THIS BE DEFAULTED IF STATE REORGANIZED? PERFORMANCE? + output_model{{1., 0., 0., 0., 0., 0.}, {0., 0., 0., 1., 0., 0.}}, + // // The state transition matrix F would be: + state_transition{{1., 1., 0.5, 0., 0., 0.}, + {0., 1., 1., 0., 0., 0.}, + {0., 0., 1., 0., 0., 0.}, + {0., 0., 0., 1., 1., 0.5}, + {0., 0., 0., 0., 1., 1.}, + {0., 0., 0., 0., 0., 1.}}}; // Now we can predict the next state based on the initialization values. filter.predict(); - // Measure and Update - // The dimension of zn is 2x1 and the dimension of xn is 6x1. Therefore the - // dimension of the observation matrix H shall be 2x6. - filter.h( - kalman::output_model{{1., 0., 0., 0., 0., 0.}, {0., 0., 0., 1., 0., 0.}}); - - // Assume that the x and y measurements are uncorrelated, i.e. error in the x - // coordinate measurement doesn't depend on the error in the y coordinate - // measurement. In real-life applications, the measurement uncertainty can - // differ between measurements. In many systems the measurement uncertainty - // depends on the measurement SNR (signal-to-noise ratio), angle between - // sensor (or sensors) and target, signal frequency and many other parameters. - // For the sake of the example simplicity, we will assume a constant - // measurement uncertainty: R1 = R2...Rn-1 = Rn = R The measurement error - // standard deviation: σxm = σym = 3m. The variance 9. - filter.r(kalman::output_uncertainty{{9., 0.}, {0., 9.}}); - // The measurement values: z1 = [-393.66, 300.4] filter.update(-393.66, 300.4); filter.predict(); diff --git a/sample/kf_8x4x0_deep_sort_bounding_box.cpp b/sample/kf_8x4x0_deep_sort_bounding_box.cpp index caae1f6fc..4f1ffc893 100644 --- a/sample/kf_8x4x0_deep_sort_bounding_box.cpp +++ b/sample/kf_8x4x0_deep_sort_bounding_box.cpp @@ -16,6 +16,7 @@ constexpr auto fcarouge::operator/(const Numerator &lhs, const Denominator &rhs) namespace fcarouge::sample { namespace { template using vector = column_vector; +using state = fcarouge::state>; //! @brief Estimating the position of bounding boxes in image space. //! @@ -33,13 +34,46 @@ template using vector = column_vector; //! //! @example kf_8x4x0_deep_sort_bounding_box.cpp [[maybe_unused]] auto sample{[] { - // A 8x4x0 filter, constant velocity, linear. - using state = vector<8>; - using output = vector<4>; - using no_input = void; - using kalman = kalman; + const vector<4> initial_box{605.0F, 248.0F, 0.20481927710843373F, 332.0F}; + // Experimental position and velocity uncertainty standard deviation weights. + const float position_weight{1.F / 20.F}; + const float velocity_weight{1.F / 160.F}; + // Constant velocity, linear state transition model. From one image frame to + // the other. + const float delta_time{1.F}; - kalman filter; + // A 8x4x0 filter, constant velocity, linear. + kalman filter{ + // The filter is initialized with the first observed output. + // Bounding box position and velocity estimated state X: [px, + // py, pa, ph, vx, vy, va, vh]. + state{initial_box(0), initial_box(1), initial_box(2), initial_box(3), 0.F, + 0.F, 0.F, 0.F}, + // The output Z: + output>, + // The estimate uncertainty P: + estimate_uncertainty{ + vector<8>{2 * position_weight * initial_box(3), + 2 * position_weight * initial_box(3), 1e-2F, + 2 * position_weight * initial_box(3), + 10 * velocity_weight * initial_box(3), + 10 * velocity_weight * initial_box(3), 1e-5F, + 10 * velocity_weight * initial_box(3)} + .array() + .square() + .matrix() + .asDiagonal()}, + // The state transition F: + state_transition{{1.F, 0.F, 0.F, 0.F, delta_time, 0.F, 0.F, 0.F}, + {0.F, 1.F, 0.F, 0.F, 0.F, delta_time, 0.F, 0.F}, + {0.F, 0.F, 1.F, 0.F, 0.F, 0.F, delta_time, 0.F}, + {0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F, delta_time}, + {0.F, 0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F}, + {0.F, 0.F, 0.F, 0.F, 0.F, 1.F, 0.F, 0.F}, + {0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 1.F, 0.F}, + {0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 1.F}}}; + + using kalman = decltype(filter); // A hundred bounding box output measurements `(x, y, a, h)` from Deep SORT's // MOT16 sample, tracker #201. @@ -144,54 +178,16 @@ template using vector = column_vector; {368.F, 252.5F, 0.303886925795053F, 283.F}, {369.F, 250.5F, 0.29757785467128F, 289.F}}; - // Initialization - // The filter is initialized at runtime, on bounding box detection, with the - // first observed output. Bounding box position and velocity estimated state: - // [px, py, pa, ph, vx, vy, va, vh]. - const kalman::output initial_box{605.0F, 248.0F, 0.20481927710843373F, - 332.0F}; - filter.x(initial_box(0), initial_box(1), initial_box(2), initial_box(3), 0.F, - 0.F, 0.F, 0.F); - - // Experimental position and velocity uncertainty standard deviation weights. - const float position_weight{1.F / 20.F}; - const float velocity_weight{1.F / 160.F}; - - filter.p(kalman::estimate_uncertainty{ - state{2 * position_weight * initial_box(3), - 2 * position_weight * initial_box(3), 1e-2F, - 2 * position_weight * initial_box(3), - 10 * velocity_weight * initial_box(3), - 10 * velocity_weight * initial_box(3), 1e-5F, - 10 * velocity_weight * initial_box(3)} - .array() - .square() - .matrix() - .asDiagonal()}); - - // Constant velocity, linear state transition model. From one image frame to - // the other. - const float delta_time{1.F}; - filter.f( - kalman::state_transition{{1.F, 0.F, 0.F, 0.F, delta_time, 0.F, 0.F, 0.F}, - {0.F, 1.F, 0.F, 0.F, 0.F, delta_time, 0.F, 0.F}, - {0.F, 0.F, 1.F, 0.F, 0.F, 0.F, delta_time, 0.F}, - {0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F, delta_time}, - {0.F, 0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F}, - {0.F, 0.F, 0.F, 0.F, 0.F, 1.F, 0.F, 0.F}, - {0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 1.F, 0.F}, - {0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 1.F}}); - filter.q([position_weight, velocity_weight]( const kalman::state &x) -> kalman::process_uncertainty { - return state{position_weight * x(3), - position_weight * x(3), - 1e-2F, - position_weight * x(3), - velocity_weight * x(3), - velocity_weight * x(3), - 1e-5F, - velocity_weight * x(3)} + return vector<8>{position_weight * x(3), + position_weight * x(3), + 1e-2F, + position_weight * x(3), + velocity_weight * x(3), + velocity_weight * x(3), + 1e-5F, + velocity_weight * x(3)} .array() .square() .matrix() @@ -203,17 +199,17 @@ template using vector = column_vector; // Measure and Update // Direct linear observation transition model. - filter.h(kalman::output_model{{1.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F}, - {0.F, 1.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F}, - {0.F, 0.F, 1.F, 0.F, 0.F, 0.F, 0.F, 0.F}, - {0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F, 0.F}}); + // filter.h(kalman::output_model{{1.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F}, + // {0.F, 1.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F}, + // {0.F, 0.F, 1.F, 0.F, 0.F, 0.F, 0.F, 0.F}, + // {0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F, 0.F}}); // Observation, measurement noise covariance. filter.r([position_weight](const kalman::state &x, [[maybe_unused]] const kalman::output &z) -> kalman::output_uncertainty { - return output{position_weight * x(3), position_weight * x(3), 1e-1F, - position_weight * x(3)} + return vector<4>{position_weight * x(3), position_weight * x(3), 1e-1F, + position_weight * x(3)} .array() .square() .matrix() diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 1b8f99cdd..0a12ce5cf 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -44,7 +44,7 @@ foreach( "kalman_format_arguments.cpp" "kalman_format_float_1x1x1.cpp" "kalman_format.cpp" - "kalman_h.cpp" + # "kalman_h.cpp" "kalman_println.cpp") get_filename_component(NAME ${TEST} NAME_WE) add_executable(kalman_test_${NAME}_driver ${TEST}) diff --git a/test/kalman_assign_move_5x4x3.cpp b/test/kalman_assign_move_5x4x3.cpp index 844d0079f..aa78385be 100644 --- a/test/kalman_assign_move_5x4x3.cpp +++ b/test/kalman_assign_move_5x4x3.cpp @@ -49,7 +49,6 @@ template using matrix = matrix; //! @test Verifies the multi-dimension filter is move-assignable. [[maybe_unused]] auto test{[] { - using kalman = kalman, vector<4>, vector<3>>; const auto z3x1{zero_v>}; const auto i4x4{identity_v>}; const auto i4x5{identity_v>}; @@ -60,8 +59,8 @@ template using matrix = matrix; const auto z4x4{zero_v>}; const auto z5x1{zero_v>}; const auto z5x5{zero_v>}; - kalman filter; - + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output>, + input>}; { decltype(filter) another_filter; diff --git a/test/kalman_constructor_default.cpp b/test/kalman_constructor_default.cpp index 9e52275f1..4babd9d3b 100644 --- a/test/kalman_constructor_default.cpp +++ b/test/kalman_constructor_default.cpp @@ -48,10 +48,8 @@ namespace { kalman filter; assert(filter.f() == 1); - assert(filter.h() == 1); assert(filter.k() == 1); assert(filter.p() == 1); - assert(filter.q() == 0 && "No process noise by default."); assert(filter.r() == 0 && "No observation noise by default."); assert(filter.s() == 1); assert(filter.x() == 0 && "Origin state."); diff --git a/test/kalman_constructor_default_1x1x3.cpp b/test/kalman_constructor_default_1x1x3.cpp index 1489e54fc..af0173add 100644 --- a/test/kalman_constructor_default_1x1x3.cpp +++ b/test/kalman_constructor_default_1x1x3.cpp @@ -48,8 +48,7 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters. [[maybe_unused]] auto test{[] { - using kalman = kalman>; - kalman filter; + kalman filter{state{0.0}, output, input>}; const auto z3x1{zero_v>}; const auto i1x3{identity_v>}; diff --git a/test/kalman_constructor_default_1x4x1.cpp b/test/kalman_constructor_default_1x4x1.cpp index d81d6fcae..b73a47261 100644 --- a/test/kalman_constructor_default_1x4x1.cpp +++ b/test/kalman_constructor_default_1x4x1.cpp @@ -49,8 +49,7 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters, //! single state and input edge case. [[maybe_unused]] auto test{[] { - using kalman = kalman, double>; - kalman filter; + kalman filter{state{0.0}, output>, input}; const auto i4x4{identity_v>}; const auto i4x1{identity_v>}; diff --git a/test/kalman_constructor_default_1x4x3.cpp b/test/kalman_constructor_default_1x4x3.cpp index 6bb7d4f3e..dfdecd201 100644 --- a/test/kalman_constructor_default_1x4x3.cpp +++ b/test/kalman_constructor_default_1x4x3.cpp @@ -49,8 +49,7 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters, //! single state edge case. [[maybe_unused]] auto test{[] { - using kalman = kalman, vector<3>>; - kalman filter; + kalman filter{state{0.0}, output>, input>}; const auto z3x1{zero_v>}; const auto i4x4{identity_v>}; diff --git a/test/kalman_constructor_default_5x1x1.cpp b/test/kalman_constructor_default_5x1x1.cpp index 8147d7b5b..5418fb652 100644 --- a/test/kalman_constructor_default_5x1x1.cpp +++ b/test/kalman_constructor_default_5x1x1.cpp @@ -49,8 +49,8 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters, //! single output and input edge case. [[maybe_unused]] auto test{[] { - using kalman = kalman, double, double>; - kalman filter; + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output, + input}; const auto i1x5{identity_v>}; const auto i5x1{identity_v>}; diff --git a/test/kalman_constructor_default_5x1x3.cpp b/test/kalman_constructor_default_5x1x3.cpp index 5cade03e3..eb404e1f5 100644 --- a/test/kalman_constructor_default_5x1x3.cpp +++ b/test/kalman_constructor_default_5x1x3.cpp @@ -49,8 +49,8 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters, //! single output edge case. [[maybe_unused]] auto test{[] { - using kalman = kalman, double, vector<3>>; - kalman filter; + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output, + input>}; const auto z3x1{zero_v>}; const auto i1x5{identity_v>}; diff --git a/test/kalman_constructor_default_5x4x0.cpp b/test/kalman_constructor_default_5x4x0.cpp index abe11bcdb..22bc2ce6c 100644 --- a/test/kalman_constructor_default_5x4x0.cpp +++ b/test/kalman_constructor_default_5x4x0.cpp @@ -49,11 +49,9 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters, //! no input. [[maybe_unused]] auto test{[] { - using kalman = kalman, vector<4>>; - kalman filter; + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output>}; const auto i4x4{identity_v>}; - const auto i4x5{identity_v>}; const auto i5x4{identity_v>}; const auto i5x5{identity_v>}; const auto z4x1{zero_v>}; @@ -62,10 +60,8 @@ template using matrix = matrix; const auto z5x5{zero_v>}; assert(filter.f() == i5x5); - assert(filter.h() == i4x5); assert(filter.k() == i5x4); assert(filter.p() == i5x5); - assert(filter.q() == z5x5 && "No process noise by default."); assert(filter.r() == z4x4 && "No observation noise by default."); assert(filter.s() == i4x4); assert(filter.x() == z5x1 && "Origin state."); diff --git a/test/kalman_constructor_default_5x4x1.cpp b/test/kalman_constructor_default_5x4x1.cpp index e4d515807..1f10c19ac 100644 --- a/test/kalman_constructor_default_5x4x1.cpp +++ b/test/kalman_constructor_default_5x4x1.cpp @@ -49,8 +49,8 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters, //! single input edge case. [[maybe_unused]] auto test{[] { - using kalman = kalman, vector<4>, double>; - kalman filter; + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output>, + input}; const auto i4x4{identity_v>}; const auto i4x5{identity_v>}; diff --git a/test/kalman_constructor_default_5x4x3.cpp b/test/kalman_constructor_default_5x4x3.cpp index 579c9cfb9..50a43fcbe 100644 --- a/test/kalman_constructor_default_5x4x3.cpp +++ b/test/kalman_constructor_default_5x4x3.cpp @@ -48,8 +48,8 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters. [[maybe_unused]] auto test{[] { - using kalman = kalman, vector<4>, vector<3>>; - kalman filter; + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output>, + input>}; const auto z3x1{zero_v>}; const auto i4x4{identity_v>}; diff --git a/test/kalman_constructor_default_float_1x1x1.cpp b/test/kalman_constructor_default_float_1x1x1.cpp index b276115d3..6fddc38ed 100644 --- a/test/kalman_constructor_default_float_1x1x1.cpp +++ b/test/kalman_constructor_default_float_1x1x1.cpp @@ -45,8 +45,7 @@ namespace { //! @test Verifies default values are initialized for single-dimension filters //! with input control. [[maybe_unused]] auto test{[] { - using kalman = kalman; - kalman filter; + kalman filter{state{0.F}, output, input}; assert(filter.f() == 1); assert(filter.g() == 1); diff --git a/test/kalman_constructor_move_5x4x3.cpp b/test/kalman_constructor_move_5x4x3.cpp index 856ede213..a5cd95a16 100644 --- a/test/kalman_constructor_move_5x4x3.cpp +++ b/test/kalman_constructor_move_5x4x3.cpp @@ -49,7 +49,6 @@ template using matrix = matrix; //! @test Verifies the multi-dimension filter is move-constructible. [[maybe_unused]] auto test{[] { - using kalman = kalman, vector<4>, vector<3>>; const auto z3x1{zero_v>}; const auto i4x4{identity_v>}; const auto i4x5{identity_v>}; @@ -60,7 +59,8 @@ template using matrix = matrix; const auto z4x4{zero_v>}; const auto z5x1{zero_v>}; const auto z5x5{zero_v>}; - kalman another_filter; + kalman another_filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, + output>, input>}; another_filter.f(z5x5); diff --git a/test/kalman_f.cpp b/test/kalman_f.cpp index d892b0339..fff336730 100644 --- a/test/kalman_f.cpp +++ b/test/kalman_f.cpp @@ -46,7 +46,6 @@ namespace { //! the default filter type. [[maybe_unused]] auto test{[] { kalman filter; - using kalman = decltype(filter); assert(filter.f() == 1); @@ -62,24 +61,6 @@ namespace { assert(filter.f() == 3); } - { - const auto f{[]([[maybe_unused]] const kalman::state &x) - -> kalman::state_transition { return 4.; }}; - filter.f(f); - assert(filter.f() == 3); - filter.predict(); - assert(filter.f() == 4); - } - - { - const auto f{[]([[maybe_unused]] const kalman::state &x) - -> kalman::state_transition { return 5.; }}; - filter.f(std::move(f)); - assert(filter.f() == 4); - filter.predict(); - assert(filter.f() == 5); - } - return 0; }()}; } // namespace diff --git a/test/kalman_f_5x4x3.cpp b/test/kalman_f_5x4x3.cpp index 33f0b92a6..8bfad45e7 100644 --- a/test/kalman_f_5x4x3.cpp +++ b/test/kalman_f_5x4x3.cpp @@ -58,15 +58,13 @@ template using matrix = matrix; //! @test Verifies the state transition matrix F management overloads for //! the Eigen filter type. [[maybe_unused]] auto test{[] { - using kalman = - kalman, vector<4>, vector<3>, std::tuple, - std::tuple>; - - kalman filter; - const auto i5x5{identity_v>}; const auto z5x5{zero_v>}; const vector<3> z3{zero_v>}; + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output>, + input>, update_types, + prediction_types}; + using kalman = decltype(filter); assert(filter.f() == i5x5); diff --git a/test/kalman_format.cpp b/test/kalman_format.cpp index 01ea77e36..6b46a9af4 100644 --- a/test/kalman_format.cpp +++ b/test/kalman_format.cpp @@ -48,9 +48,8 @@ namespace { [[maybe_unused]] auto test{[] { kalman filter; - assert( - std::format("{}", filter) == - R"({"f": 1, "h": 1, "k": 1, "p": 1, "q": 0, "r": 0, "s": 1, "x": 0, "y": 0, "z": 0})"); + assert(std::format("{}", filter) == + R"({"f": 1, "k": 1, "p": 1, "r": 0, "s": 1, "x": 0, "y": 0, "z": 0})"); return 0; }()}; diff --git a/test/kalman_format_arguments.cpp b/test/kalman_format_arguments.cpp index c782e8bd7..7a87ad289 100644 --- a/test/kalman_format_arguments.cpp +++ b/test/kalman_format_arguments.cpp @@ -46,9 +46,14 @@ namespace { //! @test Verifies formatting filters for single-dimension filters with input //! control and additional arguments. [[maybe_unused]] auto test{[] { - using kalman = kalman, - std::tuple>; - kalman filter; + kalman filter{state{0.}, + output, + input, + estimate_uncertainty{1.}, + process_uncertainty{0.}, + output_uncertainty{0.}, + update_types, + prediction_types}; assert( std::format("{}", filter) == diff --git a/test/kalman_format_float_1x1x1.cpp b/test/kalman_format_float_1x1x1.cpp index 5477573a4..18d50dc26 100644 --- a/test/kalman_format_float_1x1x1.cpp +++ b/test/kalman_format_float_1x1x1.cpp @@ -46,7 +46,7 @@ namespace { //! @test Verifies formatting filters for single-dimension filters with input //! control without additional arguments. [[maybe_unused]] auto test{[] { - kalman filter; + kalman filter{state{0.F}, output, input}; assert( std::format("{}", filter) == diff --git a/test/kalman_h.cpp b/test/kalman_h.cpp index 119eacf20..433ed9e85 100644 --- a/test/kalman_h.cpp +++ b/test/kalman_h.cpp @@ -45,7 +45,7 @@ namespace { //! @test Verifies the observation transition matrix H management overloads for //! supported filter types. [[maybe_unused]] auto test{[] { - kalman filter; + kalman filter{state{0.}, output}; using kalman = decltype(filter); assert(filter.h() == 1); diff --git a/test/kalman_h_5x4x3.cpp b/test/kalman_h_5x4x3.cpp index 94b8ac21a..698f8808d 100644 --- a/test/kalman_h_5x4x3.cpp +++ b/test/kalman_h_5x4x3.cpp @@ -57,14 +57,13 @@ template using matrix = matrix; //! @test Verifies the observation transition matrix H management overloads for //! the Eigen filter type. [[maybe_unused]] auto test{[] { - using kalman = - kalman, vector<4>, vector<3>, std::tuple, - std::tuple>; - - kalman filter; const auto i4x5{identity_v>}; const auto z4x5{zero_v>}; const auto z4{zero_v>}; + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output>, + input>, update_types, + prediction_types}; + using kalman = decltype(filter); assert(filter.h() == i4x5);