diff --git a/README.md b/README.md index 84b1872130..4acce51eca 100644 --- a/README.md +++ b/README.md @@ -382,6 +382,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 874634d98a..c0b7b65899 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 0000000000..d64be60256 --- /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 cd77062a90..0bd6342696 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 2ee63a1758..5f590709ca 100644 --- a/include/fcarouge/internal/kalman.tpp +++ b/include/fcarouge/internal/kalman.tpp @@ -40,101 +40,95 @@ 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) { - if constexpr (std::is_convertible_v) { - filter.q = std::move(process_uncertainty{value, 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(typename Filter::process_uncertainty{value, values...}); } else { using noise_process_function = decltype(filter.noise_process_q); filter.noise_process_q = @@ -142,31 +136,26 @@ 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) { - if constexpr (std::is_convertible_v) { - filter.r = std::move(output_uncertainty{value, values...}); +template +inline constexpr void kalman::r(const auto &value, + const auto &...values) { + if constexpr (std::is_convertible_v) { + filter.r = std::move(typename Filter::output_uncertainty{value, values...}); } else { using noise_observation_function = decltype(filter.noise_observation_r); filter.noise_observation_r = @@ -174,31 +163,26 @@ 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) { - if constexpr (std::is_convertible_v) { - filter.f = std::move(state_transition{value, values...}); +template +inline constexpr void kalman::f(const auto &value, + const auto &...values) { + if constexpr (std::is_convertible_v) { + filter.f = std::move(typename Filter::state_transition{value, values...}); } else { using transition_state_function = decltype(filter.transition_state_f); filter.transition_state_f = @@ -206,40 +190,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 +223,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 0000000000..3867e4cf2e --- /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 142d7cd02b..73b4a89674 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 0000000000..5aecfffd7b --- /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 0000000000..1464831382 --- /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 a93e4009e4..f7889dc97d 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 0000000000..1bb3172330 --- /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 34de56e854..2fc21c82a7 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 c8b156ef8f..18472e360b 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 8dd262a6f3..b8be83e9e9 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 1d43657669..6bd84bfbed 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 280a981518..3836f722cc 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 5d6c5f21b0..db0dd55f18 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 19aba09a03..cae8bfa893 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 c44f495d73..9b6b528852 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 faee1da0d2..ee00007549 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 519dedc6d5..c314c880e0 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 caae1f6fc5..4f1ffc893f 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 1b8f99cdd2..0a12ce5cfb 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 844d0079fd..aa78385be5 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 9e52275f1f..4babd9d3b2 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 1489e54fc6..af0173add1 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 d81d6fcae0..b73a472612 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 6bb7d4f3e4..dfdecd2013 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 8147d7b5b8..5418fb6528 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 5cade03e38..eb404e1f55 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 abe11bcdb8..22bc2ce6ce 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 e4d515807b..1f10c19aca 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 579c9cfb97..50a43fcbe2 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 b276115d32..6fddc38edd 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 856ede2133..a5cd95a16c 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 d892b0339d..fff336730a 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 33f0b92a61..8bfad45e7d 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 01ea77e364..6b46a9af4b 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 c782e8bd7d..7a87ad2898 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 5477573a45..18d50dc262 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 119eacf206..433ed9e85f 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 94b8ac21a7..698f8808d5 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);