From 6caee68756090fbb7b87bd958d6592b50bf625a8 Mon Sep 17 00:00:00 2001 From: FrancoisCarouge Date: Fri, 27 Oct 2023 23:33:18 -0700 Subject: [PATCH] [filter] declarative paradigm --- benchmark/predict_1x1x0.cpp | 4 +- benchmark/predict_1x1x1.cpp | 6 +- benchmark/predict_linalg_x1x.cpp | 7 +- benchmark/update_1x1x0.cpp | 6 +- benchmark/update_1x1x1.cpp | 6 +- benchmark/update_linalg_xx0.cpp | 6 +- documentation/Doxyfile | 2 +- include/fcarouge/internal/format.hpp | 30 +- include/fcarouge/internal/kalman.hpp | 297 +++++++++++++++++- include/fcarouge/internal/kalman.tpp | 232 +++++--------- include/fcarouge/internal/utility.hpp | 16 +- include/fcarouge/kalman.hpp | 121 ++++--- linalg/naive/fcarouge/linalg.hpp | 13 + sample/ekf_4x1x0_soaring.cpp | 9 +- sample/kf_1x1x0_building_height.cpp | 31 +- sample/kf_1x1x0_liquid_temperature.cpp | 29 +- sample/kf_1x1x1_dog_position.cpp | 15 +- sample/kf_2x1x1_rocket_altitude.cpp | 117 +++---- sample/kf_6x2x0_vehicle_location.cpp | 8 +- sample/kf_8x4x0_deep_sort_bounding_box.cpp | 48 ++- test/kalman_assign_move_5x4x3.cpp | 5 +- test/kalman_constructor_default.cpp | 1 - test/kalman_constructor_default_1x1x3.cpp | 3 +- test/kalman_constructor_default_1x4x1.cpp | 3 +- test/kalman_constructor_default_1x4x3.cpp | 3 +- test/kalman_constructor_default_5x1x1.cpp | 4 +- test/kalman_constructor_default_5x1x3.cpp | 4 +- test/kalman_constructor_default_5x4x0.cpp | 3 +- test/kalman_constructor_default_5x4x1.cpp | 4 +- test/kalman_constructor_default_5x4x3.cpp | 4 +- ...kalman_constructor_default_float_1x1x1.cpp | 3 +- test/kalman_constructor_move_5x4x3.cpp | 4 +- test/kalman_f.cpp | 2 +- test/kalman_f_5x4x3.cpp | 10 +- test/kalman_format.cpp | 2 +- test/kalman_format_arguments.cpp | 10 +- test/kalman_format_float_1x1x1.cpp | 2 +- test/kalman_h.cpp | 6 +- test/kalman_h_5x4x3.cpp | 9 +- 39 files changed, 649 insertions(+), 436 deletions(-) diff --git a/benchmark/predict_1x1x0.cpp b/benchmark/predict_1x1x0.cpp index 8407a4b51..2c0aa50b3 100644 --- a/benchmark/predict_1x1x0.cpp +++ b/benchmark/predict_1x1x0.cpp @@ -48,9 +48,7 @@ namespace fcarouge::benchmark { namespace { //! @benchmark Measure predict, empty benchmark performance. void bench(::benchmark::State &benchmark_state) { - using kalman = kalman; - - kalman filter; + kalman filter{state{0.F}, output}; for (auto _ : benchmark_state) { ::benchmark::ClobberMemory(); diff --git a/benchmark/predict_1x1x1.cpp b/benchmark/predict_1x1x1.cpp index 8c297ebcb..1fa0bbcef 100644 --- a/benchmark/predict_1x1x1.cpp +++ b/benchmark/predict_1x1x1.cpp @@ -49,13 +49,13 @@ namespace fcarouge::benchmark { namespace { //! @benchmark Measure predict, empty benchmark performance. void bench(::benchmark::State &benchmark_state) { - using kalman = kalman; - - kalman filter; + kalman filter{state{0.F}, output, input}; std::random_device random_device; std::mt19937 random_generator{random_device()}; std::uniform_real_distribution uniformly_distributed; + using kalman = decltype(filter); + for (auto _ : benchmark_state) { const typename kalman::output u{uniformly_distributed(random_generator)}; diff --git a/benchmark/predict_linalg_x1x.cpp b/benchmark/predict_linalg_x1x.cpp index 88ad88a86..9329838ac 100644 --- a/benchmark/predict_linalg_x1x.cpp +++ b/benchmark/predict_linalg_x1x.cpp @@ -59,13 +59,14 @@ template using vector = column_vector; //! states and inputs with the Eigen linear algebra backend. template void bench(::benchmark::State &benchmark_state) { - using kalman = kalman, float, vector>; - - kalman filter; + kalman filter{state{vector{}}, output, + input>}; std::random_device random_device; std::mt19937 random_generator{random_device()}; std::uniform_real_distribution uniformly_distributed; + using kalman = decltype(filter); + for (auto _ : benchmark_state) { float uv[InputSize]; diff --git a/benchmark/update_1x1x0.cpp b/benchmark/update_1x1x0.cpp index 9fa9ae690..8079f33da 100644 --- a/benchmark/update_1x1x0.cpp +++ b/benchmark/update_1x1x0.cpp @@ -49,13 +49,13 @@ namespace fcarouge::benchmark { namespace { //! @benchmark Measure update, empty benchmark performance. void bench(::benchmark::State &benchmark_state) { - using kalman = kalman; - - kalman filter; + kalman filter{state{0.F}, output}; std::random_device random_device; std::mt19937 random_generator{random_device()}; std::uniform_real_distribution uniformly_distributed; + using kalman = decltype(filter); + for (auto _ : benchmark_state) { const typename kalman::output z{uniformly_distributed(random_generator)}; diff --git a/benchmark/update_1x1x1.cpp b/benchmark/update_1x1x1.cpp index 6c3dead2d..ebc497341 100644 --- a/benchmark/update_1x1x1.cpp +++ b/benchmark/update_1x1x1.cpp @@ -49,13 +49,13 @@ namespace fcarouge::benchmark { namespace { //! @benchmark Measure update, empty benchmark performance. void bench(::benchmark::State &benchmark_state) { - using kalman = kalman; - - kalman filter; + kalman filter{state{0.F}, output, input}; std::random_device random_device; std::mt19937 random_generator{random_device()}; std::uniform_real_distribution uniformly_distributed; + using kalman = decltype(filter); + for (auto _ : benchmark_state) { const typename kalman::output z{uniformly_distributed(random_generator)}; diff --git a/benchmark/update_linalg_xx0.cpp b/benchmark/update_linalg_xx0.cpp index c9750de75..f7567eda8 100644 --- a/benchmark/update_linalg_xx0.cpp +++ b/benchmark/update_linalg_xx0.cpp @@ -68,13 +68,13 @@ template using vector = column_vector; //! states and outputs with the Eigen linear algebra backend. template void bench(::benchmark::State &benchmark_state) { - using kalman = kalman, vector, void>; - - kalman filter; + kalman filter{state{vector{}}, output>}; std::random_device random_device; std::mt19937 random_generator{random_device()}; std::uniform_real_distribution uniformly_distributed; + using kalman = decltype(filter); + for (auto _ : benchmark_state) { float zv[OutputSize]; diff --git a/documentation/Doxyfile b/documentation/Doxyfile index 7a59d4656..19f0b299e 100644 --- a/documentation/Doxyfile +++ b/documentation/Doxyfile @@ -117,7 +117,7 @@ WARN_IF_UNDOCUMENTED = YES WARN_IF_DOC_ERROR = YES WARN_IF_INCOMPLETE_DOC = YES WARN_NO_PARAMDOC = YES -WARN_AS_ERROR = NO +WARN_AS_ERROR = FAIL_ON_WARNINGS WARN_FORMAT = "$file:$line: $text" WARN_LINE_FORMAT = "at line $line of file $file" WARN_LOGFILE = diff --git a/include/fcarouge/internal/format.hpp b/include/fcarouge/internal/format.hpp index cd77062a9..19d944dcd 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( @@ -104,13 +99,14 @@ struct std::formatter< //! @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.hpp b/include/fcarouge/internal/kalman.hpp index a93e4009e..d73a6702b 100644 --- a/include/fcarouge/internal/kalman.hpp +++ b/include/fcarouge/internal/kalman.hpp @@ -43,27 +43,24 @@ For more information, please refer to */ #include "utility.hpp" #include +#include namespace fcarouge::internal { -template -struct kalman final { - //! @todo Support some more specializations, all, or disable others? - //! @todo Add a pretty compilation error? -}; +// Helper template to support multiple pack deduction. +template +struct kalman_s_o_us_ps final {}; template -struct kalman, - pack> { +struct kalman_s_o_us_ps, + 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; @@ -75,7 +72,6 @@ struct kalman, function; using noise_process_function = function; - using transition_control_function = empty; using transition_function = function; using observation_function = @@ -163,10 +159,14 @@ struct kalman, } }; +// Helper template to support multiple pack deduction. +template +struct kalman_s_o_i_us_ps final {}; + template -struct kalman, - pack> { +struct kalman_s_o_i_us_ps, + pack> { using state = State; using output = Output; using input = Input; @@ -202,6 +202,7 @@ struct kalman, 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}; @@ -209,7 +210,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{}; @@ -293,6 +293,277 @@ struct kalman, p = estimate_uncertainty{f * p * t(f) + q}; } }; +//! @todo What is the minimal meaningful filter? Should further details be +//! optimized out? +template struct kalman_s_o { + using state = State; + using output = Output; + using estimate_uncertainty = quotient; + using process_uncertainty = quotient; + using output_uncertainty = quotient; + using state_transition = quotient; + using gain = quotient; + using innovation = output; + using innovation_uncertainty = output_uncertainty; + using noise_observation_function = + function; + using transition_state_function = function; + using noise_process_function = function; + using transition_function = function; + using observation_function = function; + + static inline const auto i{identity_v>}; + + state x{zero_v}; + estimate_uncertainty p{identity_v}; + output_uncertainty r{zero_v}; + + process_uncertainty q{zero_v}; ////// REMOVE ME + state_transition f{identity_v}; + gain k{identity_v}; + innovation y{zero_v}; + innovation_uncertainty s{identity_v}; + output z{zero_v}; + transpose t{}; + + 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; + }}; + + inline constexpr void update(const output &output_z) { + z = output_z; + r = noise_observation_r(x, z); + s = innovation_uncertainty{p + r}; + k = p / s; + y = z - x; + x = state{x + k * y}; + p = estimate_uncertainty{(i - k) * p * t(i - k) + k * r * t(k)}; + } + + inline constexpr void predict() { + f = transition_state_f(x); + q = noise_process_q(x); + x = transition(x); + p = estimate_uncertainty{f * p * t(f) + q}; + } +}; + +template struct state { + Type value; + + // Remove if/not array? + state(Type v) : value{v} {} + + // same? + template + requires(sizeof...(Types) > 1) + state(Types &&...elements) : value{std::forward(elements)...} {} +}; + +template state(Type) -> state; + +template +state(const Types &...elements) + -> state>[sizeof...(Types)]>; + +template struct estimate_uncertainty { + Type value; + + // Remove if/not array? + estimate_uncertainty(Type v) : value{v} {} + + template + estimate_uncertainty([[maybe_unused]] const Types (&...rows)[Columns]) + // requires(std::conjunction_v, Types>...> + // && + // ((Columns == first_v) && ... && true)) + { + int i{0}; + ( + [&]([[maybe_unused]] auto row) { + // for constexpr? + for (std::remove_cv_t)> j{0}; + j < first_v; ++j) { + value[i * first_v + j] = row[j]; + } + ++i; + }(rows), + ...); + } +}; + +template +estimate_uncertainty(Type) -> estimate_uncertainty; + +template +estimate_uncertainty([[maybe_unused]] const Types (&...rows)[Columns]) + -> estimate_uncertainty>[sizeof...(Columns) * first_v]>; + +// std::remove_cvref_t>,sizeof...(Columns), +// first_v + +template struct output_uncertainty { + Type value; +}; + +template struct process_uncertainty { + Type value; +}; + +template struct input_t {}; + +template inline input_t input{}; + +template struct output_t {}; + +template inline output_t output{}; + +template struct output_model { + Type value; +}; + +//! @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{}; + +//! @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 { + //! @todo Rename, clean the concet: undeduced? + inline constexpr auto operator()() const -> kalman_s_o + requires(std::same_as); + + inline constexpr auto operator()() const { return Filter{}; } + + template + inline constexpr auto operator()(state x, + [[maybe_unused]] input_t u) const { + return kalman_s_o_i_us_ps{ + x.value}; + } + + template + inline constexpr auto operator()(state x, + [[maybe_unused]] output_t z) const { + return kalman_s_o_us_ps{x.value}; + } + + template + inline constexpr auto + operator()(state x, [[maybe_unused]] output_t z, + [[maybe_unused]] update_types_t uts, + [[maybe_unused]] prediction_types_t pts) const { + return kalman_s_o_us_ps>, + repack_t>>{x.value}; + } + + template + inline constexpr auto operator()(state x, + [[maybe_unused]] output_t z, + [[maybe_unused]] input_t u) const { + return kalman_s_o_i_us_ps{ + x.value}; + } + + template + inline constexpr auto + operator()(state x, [[maybe_unused]] output_t z, + [[maybe_unused]] input_t u, + estimate_uncertainty p, + [[maybe_unused]] prediction_types_t pts) const { + using kt = kalman_s_o_i_us_ps>>; + return kt{x.value, typename kt::estimate_uncertainty{p.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 { + return kalman_s_o_i_us_ps>, + repack_t>>{x.value}; + } + + template + inline constexpr auto + operator()(state x, estimate_uncertainty p, + output_uncertainty r) const { + return kalman_s_o{x.value, p.value, r.value}; + } + + template + inline constexpr auto + operator()(state x, estimate_uncertainty p, + process_uncertainty q, + output_uncertainty r) const { + return kalman_s_o_us_ps{ + x.value, p.value, q.value, r.value}; + } + + template + inline constexpr auto + operator()(state x, [[maybe_unused]] input_t u, + estimate_uncertainty p, + process_uncertainty q, + output_uncertainty r) const { + return kalman_s_o_i_us_ps{ + x.value, p.value, q.value, r.value}; + } + template + inline constexpr auto + operator()(state x, [[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 { + + return kalman_s_o_i_us_ps>, + repack_t>>{ + x.value, p.value, q.value, r.value}; + } +}; + +template inline constexpr make_filter filter{}; + +template +using filter_t = std::invoke_result_t, Arguments...>; + } // namespace fcarouge::internal #endif // FCAROUGE_INTERNAL_KALMAN_HPP diff --git a/include/fcarouge/internal/kalman.tpp b/include/fcarouge/internal/kalman.tpp index 2ee63a175..88c4100b4 100644 --- a/include/fcarouge/internal/kalman.tpp +++ b/include/fcarouge/internal/kalman.tpp @@ -40,99 +40,85 @@ 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 & { +kalman::q() const -> const 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 & { +kalman::q() -> process_uncertainty & { return filter.q; } -template -inline constexpr void -kalman::q( - const auto &value, const auto &...values) { +template +inline constexpr void kalman::q(const auto &value, + const auto &...values) { if constexpr (std::is_convertible_v) { filter.q = std::move(process_uncertainty{value, values...}); } else { @@ -142,29 +128,23 @@ kalman::q( } } -template +template [[nodiscard("The returned observation noise covariance matrix R is " "unexpectedly discarded.")]] inline constexpr auto -kalman::r() const - -> const output_uncertainty & { +kalman::r() const -> const output_uncertainty & { return filter.r; } -template +template [[nodiscard("The returned observation noise covariance matrix R is " "unexpectedly discarded.")]] inline constexpr auto -kalman::r() - -> output_uncertainty & { +kalman::r() -> output_uncertainty & { return filter.r; } -template -inline constexpr void -kalman::r( - const auto &value, const auto &...values) { +template +inline constexpr void kalman::r(const auto &value, + const auto &...values) { if constexpr (std::is_convertible_v) { filter.r = std::move(output_uncertainty{value, values...}); } else { @@ -174,29 +154,23 @@ kalman::r( } } -template +template [[nodiscard("The returned state transition matrix F is unexpectedly " "discarded.")]] inline constexpr auto -kalman::f() const - -> const state_transition & { +kalman::f() const -> const state_transition & { return filter.f; } -template +template [[nodiscard("The returned state transition matrix F is unexpectedly " "discarded.")]] inline constexpr auto -kalman::f() - -> state_transition & { +kalman::f() -> state_transition & { return filter.f; } -template -inline constexpr void -kalman::f( - const auto &value, const auto &...values) { +template +inline constexpr void kalman::f(const auto &value, + const auto &...values) { if constexpr (std::is_convertible_v) { filter.f = std::move(state_transition{value, values...}); } else { @@ -206,40 +180,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 +213,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/utility.hpp b/include/fcarouge/internal/utility.hpp index 142d7cd02..3ce645815 100644 --- a/include/fcarouge/internal/utility.hpp +++ b/include/fcarouge/internal/utility.hpp @@ -84,6 +84,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,7 +118,8 @@ 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; }; @@ -122,12 +130,6 @@ 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. - } -}; - template struct pack {}; using empty_pack = pack<>; diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp index d0475bd4a..dd2559c4a 100644 --- a/include/fcarouge/kalman.hpp +++ b/include/fcarouge/kalman.hpp @@ -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,27 +132,25 @@ 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 //! @{ //! @brief Implementation details of the filter. //! //! @details The internal implementation, filtering strategies, and presence - //! of members vary based on the configured filter. - using implementation = - internal::kalman, - 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; //! @} @@ -217,10 +193,13 @@ class kalman final : public internal::conditional_member_types + inline constexpr kalman(Arguments... arguments); //! @brief Copy constructs a filter. //! @@ -330,7 +309,7 @@ class kalman final : public internal::conditional_member_types); + requires(has_input); //! @brief Returns the estimated covariance matrix P. //! @@ -439,9 +418,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. //! @@ -464,7 +443,7 @@ class kalman final : public internal::conditional_member_types); + requires(has_output_model); //! @brief Returns the control transition matrix G. //! @@ -473,11 +452,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. //! @@ -496,7 +477,7 @@ class kalman final : public internal::conditional_member_types); + requires(has_input_control); //! @brief Returns the gain matrix K. //! @@ -619,6 +600,52 @@ 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; +//! @} + +//! @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/linalg/naive/fcarouge/linalg.hpp b/linalg/naive/fcarouge/linalg.hpp index 209937a1d..7541f4d7e 100644 --- a/linalg/naive/fcarouge/linalg.hpp +++ b/linalg/naive/fcarouge/linalg.hpp @@ -90,11 +90,23 @@ template struct matrix { inline constexpr explicit matrix(Type (&elements)[Row]) requires(Row != 1 && Column == 1) { + // constexpr for? for (decltype(Row) i{0}; i < Row; ++i) { data[i][0] = elements[i]; } } + inline constexpr explicit matrix(Type (&elements)[Row * Column]) + requires(Row != 1 && Column != 1) + { + // constexpr for? + for (decltype(Row) i{0}; i < Row; ++i) { + for (decltype(Column) j{0}; j < Row; ++j) { + data[i][j] = elements[i * Column + j]; + } + } + } + template matrix([[maybe_unused]] const Types (&...rows)[Columns]) requires(std::conjunction_v...> && @@ -173,6 +185,7 @@ template ((Columns == first_v) && ... && true)) matrix(const Types (&...rows)[Columns]) -> matrix>, sizeof...(Columns), + // first_v (Columns, ...)>; //! @} diff --git a/sample/ekf_4x1x0_soaring.cpp b/sample/ekf_4x1x0_soaring.cpp index 280a98151..569e1e96c 100644 --- a/sample/ekf_4x1x0_soaring.cpp +++ b/sample/ekf_4x1x0_soaring.cpp @@ -33,13 +33,10 @@ template using vector = column_vector; [[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{state{vector<4>{0.F, 0.F, 0.F, 0.F}}, output, + update_types, prediction_types}; - kalman filter; + using kalman = decltype(filter); // Initialization const float trigger_strength{0}; diff --git a/sample/kf_1x1x0_building_height.cpp b/sample/kf_1x1x0_building_height.cpp index 5d6c5f21b..8cce13335 100644 --- a/sample/kf_1x1x0_building_height.cpp +++ b/sample/kf_1x1x0_building_height.cpp @@ -24,22 +24,15 @@ namespace { //! @example kf_1x1x0_building_height.cpp [[maybe_unused]] auto sample{[] { // A one-dimensional filter, constant system dynamic model. - kalman filter; - - // 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.); + // state building height is: x = 60 meters. A human’s estimation error + // (standard deviation) is about 15 meters: σ = 15. Consequently the variance + // is σ^2 = 225. The estimate uncertainty is: p = 225. Since the standard + // deviation σ of the altimeter measurement error is 5, the variance σ^2 would + // be 25, thus the measurement, output uncertainty is: r1 = 25. + kalman filter{state{60.}, estimate_uncertainty{225.}, + output_uncertainty{25.}}; - // 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 +40,10 @@ namespace { "The extrapolated estimate uncertainty (variance) also doesn't " "change: 225"); - // Measure and Update - // The first measurement is: z1 = 48.54m. Since the standard deviation σ of - // the altimeter measurement error is 5, the variance σ^2 would be 25, thus - // the measurement uncertainty is: r1 = 25. - filter.r(25.); - + // Now, we shall predict the next state based on the initialization values. + // Note: The prediction operation needs not be performed since the process + // noise covariance Q is null in this example. + // Measure and update: the first measurement is: z1 = 48.54m. filter.update(48.54); // And so on. diff --git a/sample/kf_1x1x0_liquid_temperature.cpp b/sample/kf_1x1x0_liquid_temperature.cpp index 19aba09a0..a59aa5c33 100644 --- a/sample/kf_1x1x0_liquid_temperature.cpp +++ b/sample/kf_1x1x0_liquid_temperature.cpp @@ -27,27 +27,24 @@ 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.); - + // temperature of the liquid is, and our guess is: estimated state x = 10°C. // 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 + // We have an accurate model, thus we set the process uncertainty noise // variance q to 0.0001. - filter.q(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: r1 = 0.01. The measurement error (standard deviation) is + // 0.1 degrees Celsius. + kalman filter{state{10.}, estimate_uncertainty{100 * 100.}, + process_uncertainty{0.0001}, 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 +54,7 @@ namespace { "The extrapolated estimate uncertainty (variance): p1,0 = p0,0 + q = " "10000 + 0.0001 = 10000.0001."); - // Measure and Update - // The measurement value: z1 = 49.95°C. Since the measurement error is σ = - // 0.1, the variance σ^2 would be 0.01, thus the measurement uncertainty is: - // r1 = 0.01. The measurement error (standard deviation) is 0.1 degrees - // Celsius. - filter.r(0.1 * 0.1); - + // The first measurement value: z1 = 49.95°C. Measure and update. filter.update(49.95); assert(std::abs(1 - filter.k() / 0.999999) < 0.0001 && diff --git a/sample/kf_1x1x1_dog_position.cpp b/sample/kf_1x1x1_dog_position.cpp index c44f495d7..613ce7614 100644 --- a/sample/kf_1x1x1_dog_position.cpp +++ b/sample/kf_1x1x1_dog_position.cpp @@ -31,28 +31,18 @@ namespace { //! //! @example kf_1x1x1_dog_position.cpp [[maybe_unused]] auto sample{[] { - using kalman = kalman; - kalman filter; - - // 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.); + kalman filter{state{1.}, input, estimate_uncertainty{20 * 20.}, + process_uncertainty{1.}, output_uncertainty{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. @@ -60,7 +50,6 @@ namespace { // have a secondary velocity sensor. Please accept this simplification for // now. filter.predict(1.); - filter.update(1.354); filter.predict(1.); filter.update(1.882); diff --git a/sample/kf_2x1x1_rocket_altitude.cpp b/sample/kf_2x1x1_rocket_altitude.cpp index faee1da0d..ed72eb12c 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,30 +42,31 @@ 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.}}); + // 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 altitude as a double [m]. + output, + // The filter receives in the 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 results in high + // Kalman gain, giving a high weight to the measurement. + estimate_uncertainty{{500., 0.}, {0., 500.}}, + // The filter prediction uses a delta time [ms]. + prediction_types}; + + using kalman = decltype(filter); // Prediction // We will assume a discrete noise model - the noise is different at each time @@ -71,7 +77,7 @@ template using vector = column_vector; // 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 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}, @@ -81,13 +87,13 @@ template using vector = column_vector; // 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 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) { + filter.g([](const milliseconds &delta_time) { const auto dt{std::chrono::duration(delta_time).count()}; return kalman::input_control{0.0313, dt}; }); @@ -95,7 +101,7 @@ template using vector = column_vector; // 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,7 +116,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.}); + // 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. @@ -142,14 +148,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 +165,33 @@ template using vector = column_vector; std::abs(1 - filter.p()(1, 1) / 438.8) < 0.001 && "The estimate uncertainty expected at 0.1% accuracy."); - step(18., delta_time, 39.97 + gravity); - step(22.9, delta_time, 39.81 + gravity); - step(19.5, delta_time, 39.75 + gravity); - step(28.5, delta_time, 39.6 + gravity); - step(46.5, delta_time, 39.77 + gravity); - step(68.9, delta_time, 39.83 + gravity); - step(48.2, delta_time, 39.73 + gravity); - step(56.1, delta_time, 39.87 + gravity); - step(90.5, delta_time, 39.81 + gravity); - step(104.9, delta_time, 39.92 + gravity); - step(140.9, delta_time, 39.78 + gravity); - step(148., delta_time, 39.98 + gravity); - step(187.6, delta_time, 39.76 + gravity); - step(209.2, delta_time, 39.86 + gravity); - step(244.6, delta_time, 39.61 + gravity); - step(276.4, delta_time, 39.86 + gravity); - step(323.5, delta_time, 39.74 + gravity); - step(357.3, delta_time, 39.87 + gravity); - step(357.4, delta_time, 39.63 + gravity); - step(398.3, delta_time, 39.67 + gravity); - step(446.7, delta_time, 39.96 + gravity); - step(465.1, delta_time, 39.8 + gravity); - step(529.4, delta_time, 39.89 + gravity); - step(570.4, delta_time, 39.85 + gravity); - step(636.8, delta_time, 39.9 + gravity); - step(693.3, delta_time, 39.81 + gravity); - step(707.3, delta_time, 39.81 + gravity); + step(18., 39.97 + gravity); + step(22.9, 39.81 + gravity); + step(19.5, 39.75 + gravity); + step(28.5, 39.6 + gravity); + step(46.5, 39.77 + gravity); + step(68.9, 39.83 + gravity); + step(48.2, 39.73 + gravity); + step(56.1, 39.87 + gravity); + step(90.5, 39.81 + gravity); + step(104.9, 39.92 + gravity); + step(140.9, 39.78 + gravity); + step(148., 39.98 + gravity); + step(187.6, 39.76 + gravity); + step(209.2, 39.86 + gravity); + step(244.6, 39.61 + gravity); + step(276.4, 39.86 + gravity); + step(323.5, 39.74 + gravity); + step(357.3, 39.87 + gravity); + step(357.4, 39.63 + gravity); + step(398.3, 39.67 + gravity); + step(446.7, 39.96 + gravity); + step(465.1, 39.8 + gravity); + step(529.4, 39.89 + gravity); + step(570.4, 39.85 + gravity); + step(636.8, 39.9 + gravity); + step(693.3, 39.81 + gravity); + step(707.3, 39.81 + gravity); filter.update(748.5); diff --git a/sample/kf_6x2x0_vehicle_location.cpp b/sample/kf_6x2x0_vehicle_location.cpp index 519dedc6d..96f7cfa5d 100644 --- a/sample/kf_6x2x0_vehicle_location.cpp +++ b/sample/kf_6x2x0_vehicle_location.cpp @@ -38,12 +38,9 @@ template using vector = column_vector; //! @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{state{vector<6>{0., 0., 0., 0., 0., 0.}}, output>}; - kalman filter; + using kalman = decltype(filter); // Initialization // The state is chosen to be the position, velocity, acceleration in the XY @@ -84,6 +81,7 @@ template using vector = column_vector; // 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. + // COULD THIS BE DEFAULTED IF STATE REORGANIZED? PERFORMANCE? filter.h( kalman::output_model{{1., 0., 0., 0., 0., 0.}, {0., 0., 0., 1., 0., 0.}}); diff --git a/sample/kf_8x4x0_deep_sort_bounding_box.cpp b/sample/kf_8x4x0_deep_sort_bounding_box.cpp index caae1f6fc..e1dc909e3 100644 --- a/sample/kf_8x4x0_deep_sort_bounding_box.cpp +++ b/sample/kf_8x4x0_deep_sort_bounding_box.cpp @@ -34,12 +34,10 @@ 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; + kalman filter{state{vector<8>{0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F}}, + output>}; - kalman filter; + using kalman = decltype(filter); // A hundred bounding box output measurements `(x, y, a, h)` from Deep SORT's // MOT16 sample, tracker #201. @@ -158,12 +156,12 @@ template using vector = column_vector; 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)} + 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() @@ -184,14 +182,14 @@ template using vector = column_vector; 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 +201,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/kalman_assign_move_5x4x3.cpp b/test/kalman_assign_move_5x4x3.cpp index 844d0079f..aa78385be 100644 --- a/test/kalman_assign_move_5x4x3.cpp +++ b/test/kalman_assign_move_5x4x3.cpp @@ -49,7 +49,6 @@ template using matrix = matrix; //! @test Verifies the multi-dimension filter is move-assignable. [[maybe_unused]] auto test{[] { - using kalman = kalman, vector<4>, vector<3>>; const auto z3x1{zero_v>}; const auto i4x4{identity_v>}; const auto i4x5{identity_v>}; @@ -60,8 +59,8 @@ template using matrix = matrix; const auto z4x4{zero_v>}; const auto z5x1{zero_v>}; const auto z5x5{zero_v>}; - kalman filter; - + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output>, + input>}; { decltype(filter) another_filter; diff --git a/test/kalman_constructor_default.cpp b/test/kalman_constructor_default.cpp index 9e52275f1..182d57202 100644 --- a/test/kalman_constructor_default.cpp +++ b/test/kalman_constructor_default.cpp @@ -48,7 +48,6 @@ 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."); diff --git a/test/kalman_constructor_default_1x1x3.cpp b/test/kalman_constructor_default_1x1x3.cpp index 1489e54fc..af0173add 100644 --- a/test/kalman_constructor_default_1x1x3.cpp +++ b/test/kalman_constructor_default_1x1x3.cpp @@ -48,8 +48,7 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters. [[maybe_unused]] auto test{[] { - using kalman = kalman>; - kalman filter; + kalman filter{state{0.0}, output, input>}; const auto z3x1{zero_v>}; const auto i1x3{identity_v>}; diff --git a/test/kalman_constructor_default_1x4x1.cpp b/test/kalman_constructor_default_1x4x1.cpp index d81d6fcae..b73a47261 100644 --- a/test/kalman_constructor_default_1x4x1.cpp +++ b/test/kalman_constructor_default_1x4x1.cpp @@ -49,8 +49,7 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters, //! single state and input edge case. [[maybe_unused]] auto test{[] { - using kalman = kalman, double>; - kalman filter; + kalman filter{state{0.0}, output>, input}; const auto i4x4{identity_v>}; const auto i4x1{identity_v>}; diff --git a/test/kalman_constructor_default_1x4x3.cpp b/test/kalman_constructor_default_1x4x3.cpp index 6bb7d4f3e..dfdecd201 100644 --- a/test/kalman_constructor_default_1x4x3.cpp +++ b/test/kalman_constructor_default_1x4x3.cpp @@ -49,8 +49,7 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters, //! single state edge case. [[maybe_unused]] auto test{[] { - using kalman = kalman, vector<3>>; - kalman filter; + kalman filter{state{0.0}, output>, input>}; const auto z3x1{zero_v>}; const auto i4x4{identity_v>}; diff --git a/test/kalman_constructor_default_5x1x1.cpp b/test/kalman_constructor_default_5x1x1.cpp index 8147d7b5b..5418fb652 100644 --- a/test/kalman_constructor_default_5x1x1.cpp +++ b/test/kalman_constructor_default_5x1x1.cpp @@ -49,8 +49,8 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters, //! single output and input edge case. [[maybe_unused]] auto test{[] { - using kalman = kalman, double, double>; - kalman filter; + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output, + input}; const auto i1x5{identity_v>}; const auto i5x1{identity_v>}; diff --git a/test/kalman_constructor_default_5x1x3.cpp b/test/kalman_constructor_default_5x1x3.cpp index 5cade03e3..eb404e1f5 100644 --- a/test/kalman_constructor_default_5x1x3.cpp +++ b/test/kalman_constructor_default_5x1x3.cpp @@ -49,8 +49,8 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters, //! single output edge case. [[maybe_unused]] auto test{[] { - using kalman = kalman, double, vector<3>>; - kalman filter; + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output, + input>}; const auto z3x1{zero_v>}; const auto i1x5{identity_v>}; diff --git a/test/kalman_constructor_default_5x4x0.cpp b/test/kalman_constructor_default_5x4x0.cpp index abe11bcdb..dbf6eabcd 100644 --- a/test/kalman_constructor_default_5x4x0.cpp +++ b/test/kalman_constructor_default_5x4x0.cpp @@ -49,8 +49,7 @@ 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>}; diff --git a/test/kalman_constructor_default_5x4x1.cpp b/test/kalman_constructor_default_5x4x1.cpp index e4d515807..1f10c19ac 100644 --- a/test/kalman_constructor_default_5x4x1.cpp +++ b/test/kalman_constructor_default_5x4x1.cpp @@ -49,8 +49,8 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters, //! single input edge case. [[maybe_unused]] auto test{[] { - using kalman = kalman, vector<4>, double>; - kalman filter; + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output>, + input}; const auto i4x4{identity_v>}; const auto i4x5{identity_v>}; diff --git a/test/kalman_constructor_default_5x4x3.cpp b/test/kalman_constructor_default_5x4x3.cpp index 579c9cfb9..50a43fcbe 100644 --- a/test/kalman_constructor_default_5x4x3.cpp +++ b/test/kalman_constructor_default_5x4x3.cpp @@ -48,8 +48,8 @@ template using matrix = matrix; //! @test Verifies default values are initialized for multi-dimension filters. [[maybe_unused]] auto test{[] { - using kalman = kalman, vector<4>, vector<3>>; - kalman filter; + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output>, + input>}; const auto z3x1{zero_v>}; const auto i4x4{identity_v>}; diff --git a/test/kalman_constructor_default_float_1x1x1.cpp b/test/kalman_constructor_default_float_1x1x1.cpp index b276115d3..2528e0158 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}, input}; assert(filter.f() == 1); assert(filter.g() == 1); diff --git a/test/kalman_constructor_move_5x4x3.cpp b/test/kalman_constructor_move_5x4x3.cpp index 856ede213..a5cd95a16 100644 --- a/test/kalman_constructor_move_5x4x3.cpp +++ b/test/kalman_constructor_move_5x4x3.cpp @@ -49,7 +49,6 @@ template using matrix = matrix; //! @test Verifies the multi-dimension filter is move-constructible. [[maybe_unused]] auto test{[] { - using kalman = kalman, vector<4>, vector<3>>; const auto z3x1{zero_v>}; const auto i4x4{identity_v>}; const auto i4x5{identity_v>}; @@ -60,7 +59,8 @@ template using matrix = matrix; const auto z4x4{zero_v>}; const auto z5x1{zero_v>}; const auto z5x5{zero_v>}; - kalman another_filter; + kalman another_filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, + output>, input>}; another_filter.f(z5x5); diff --git a/test/kalman_f.cpp b/test/kalman_f.cpp index e27921819..d892b0339 100644 --- a/test/kalman_f.cpp +++ b/test/kalman_f.cpp @@ -45,8 +45,8 @@ namespace { //! @test Verifies the state transition matrix F management overloads for //! the default filter type. [[maybe_unused]] auto test{[] { - using kalman = kalman<>; kalman filter; + using kalman = decltype(filter); assert(filter.f() == 1); diff --git a/test/kalman_f_5x4x3.cpp b/test/kalman_f_5x4x3.cpp index 33f0b92a6..8bfad45e7 100644 --- a/test/kalman_f_5x4x3.cpp +++ b/test/kalman_f_5x4x3.cpp @@ -58,15 +58,13 @@ template using matrix = matrix; //! @test Verifies the state transition matrix F management overloads for //! the Eigen filter type. [[maybe_unused]] auto test{[] { - using kalman = - kalman, vector<4>, vector<3>, std::tuple, - std::tuple>; - - kalman filter; - const auto i5x5{identity_v>}; const auto z5x5{zero_v>}; const vector<3> z3{zero_v>}; + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output>, + input>, update_types, + prediction_types}; + using kalman = decltype(filter); assert(filter.f() == i5x5); diff --git a/test/kalman_format.cpp b/test/kalman_format.cpp index 01ea77e36..0b3d14f83 100644 --- a/test/kalman_format.cpp +++ b/test/kalman_format.cpp @@ -50,7 +50,7 @@ namespace { 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})"); + R"({"f": 1, "k": 1, "p": 1, "q": 0, "r": 0, "s": 1, "x": 0, "y": 0, "z": 0})"); return 0; }()}; diff --git a/test/kalman_format_arguments.cpp b/test/kalman_format_arguments.cpp index c782e8bd7..1fdaadf52 100644 --- a/test/kalman_format_arguments.cpp +++ b/test/kalman_format_arguments.cpp @@ -46,9 +46,13 @@ 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.}, + input, + estimate_uncertainty{1.}, + process_uncertainty{0.}, + output_uncertainty{0.}, + update_types, + prediction_types}; assert( std::format("{}", filter) == diff --git a/test/kalman_format_float_1x1x1.cpp b/test/kalman_format_float_1x1x1.cpp index 5477573a4..64d2bc76b 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}, input}; assert( std::format("{}", filter) == diff --git a/test/kalman_h.cpp b/test/kalman_h.cpp index dd0c5a3b6..433ed9e85 100644 --- a/test/kalman_h.cpp +++ b/test/kalman_h.cpp @@ -43,10 +43,10 @@ For more information, please refer to */ namespace fcarouge::test { namespace { //! @test Verifies the observation transition matrix H management overloads for -//! the default filter type. +//! supported filter types. [[maybe_unused]] auto test{[] { - using kalman = kalman<>; - kalman filter; + kalman filter{state{0.}, output}; + using kalman = decltype(filter); assert(filter.h() == 1); diff --git a/test/kalman_h_5x4x3.cpp b/test/kalman_h_5x4x3.cpp index 94b8ac21a..698f8808d 100644 --- a/test/kalman_h_5x4x3.cpp +++ b/test/kalman_h_5x4x3.cpp @@ -57,14 +57,13 @@ template using matrix = matrix; //! @test Verifies the observation transition matrix H management overloads for //! the Eigen filter type. [[maybe_unused]] auto test{[] { - using kalman = - kalman, vector<4>, vector<3>, std::tuple, - std::tuple>; - - kalman filter; const auto i4x5{identity_v>}; const auto z4x5{zero_v>}; const auto z4{zero_v>}; + kalman filter{state{vector<5>{0.0, 0.0, 0.0, 0.0, 0.0}}, output>, + input>, update_types, + prediction_types}; + using kalman = decltype(filter); assert(filter.h() == i4x5);