From 6b4951f526beed8ac47b25edfc350e407c6c839d 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 +- include/fcarouge/internal/format.hpp | 30 +- include/fcarouge/internal/kalman.hpp | 434 +++++++++++++++++- include/fcarouge/internal/kalman.tpp | 232 ++++------ include/fcarouge/internal/utility.hpp | 16 +- include/fcarouge/kalman.hpp | 123 +++-- linalg/naive/fcarouge/linalg.hpp | 37 +- sample/CMakeLists.txt | 3 +- sample/ekf_4x1x0_soaring.cpp | 59 +-- sample/kf_1x1x0_building_height.cpp | 36 +- sample/kf_1x1x0_liquid_temperature.cpp | 51 +- sample/kf_1x1x1_dog_position.cpp | 52 +-- sample/kf_2x1x1_rocket_altitude.cpp | 117 ++--- sample/kf_6x2x0_vehicle_location.cpp | 114 +++-- sample/kf_8x4x0_deep_sort_bounding_box.cpp | 112 +++-- 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, 962 insertions(+), 571 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/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..237ff0ce4 100644 --- a/include/fcarouge/internal/kalman.hpp +++ b/include/fcarouge/internal/kalman.hpp @@ -42,28 +42,27 @@ For more information, please refer to */ #include "function.hpp" #include "utility.hpp" +//! @todo Remove the dependency on `std::initializer_list` if possible? +#include #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 +74,6 @@ struct kalman, function; using noise_process_function = function; - using transition_control_function = empty; using transition_function = function; using observation_function = @@ -163,10 +161,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 +204,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 +212,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 +295,412 @@ 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? + explicit state(Type v) : value{v} {} + + // same? + template + requires(sizeof...(Types) > 1) + 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> r) { + value = r; + } + + template constexpr explicit estimate_uncertainty(T r) { + value = r; + } +}; + +template estimate_uncertainty(T) -> estimate_uncertainty; + +template +estimate_uncertainty(std::initializer_list>) + -> estimate_uncertainty>>; + +template struct output_uncertainty { + Type value; + + template + constexpr explicit output_uncertainty( + std::initializer_list> r) { + value = r; + } + + template constexpr explicit output_uncertainty(T r) { + value = r; + } +}; + +template output_uncertainty(T) -> output_uncertainty; + +template +output_uncertainty(std::initializer_list>) + -> output_uncertainty>>; + +template struct process_uncertainty { + Type value; + + template + constexpr explicit process_uncertainty( + std::initializer_list> q) { + value = q; + } + + template constexpr explicit process_uncertainty(T q) { + value = q; + } +}; + +template process_uncertainty(T) -> process_uncertainty; + +template +process_uncertainty(std::initializer_list>) + -> process_uncertainty>>; + +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 { + std::initializer_list> value; + + constexpr explicit state_transition( + std::initializer_list> f) { + value = f; + } +}; + +//! @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 { + template + inline constexpr auto + 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 Dumb out dummy: nil filter. + return kalman_s_o{}; + } + + //! @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 { + + using kt = kalman_s_o_us_ps>, + repack_t>>; + return kt{typename kt::state{x.value}}; + } + + template + inline constexpr auto operator()(state x, + [[maybe_unused]] output_t z, + [[maybe_unused]] input_t u) const { + using kt = kalman_s_o_i_us_ps; + return kt{typename kt::state{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{typename kt::state{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 { + using kt = kalman_s_o_i_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, + [[maybe_unused]] update_types_t uts, + [[maybe_unused]] prediction_types_t pts) const { + using kt = kalman_s_o_us_ps>, + repack_t>>; + return kt{typename kt::state{x.value}, + typename kt::estimate_uncertainty{p.value}}; + } + + template + inline constexpr auto + operator()(state x, [[maybe_unused]] output_t z, + estimate_uncertainty p, + process_uncertainty q) const { + using kt = kalman_s_o_us_ps; + return kt{typename kt::state{x.value}, + typename kt::estimate_uncertainty{p.value}, + typename kt::process_uncertainty{q.value}}; + } + + template + inline constexpr auto + operator()(state x, [[maybe_unused]] output_t z, + estimate_uncertainty p, + process_uncertainty q, + [[maybe_unused]] update_types_t uts, + [[maybe_unused]] prediction_types_t pts) const { + using kt = kalman_s_o_us_ps>, + repack_t>>; + return kt{typename kt::state{x.value}, + typename kt::estimate_uncertainty{p.value}, + typename kt::process_uncertainty{q.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 = kalman_s_o_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 = kalman_s_o_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, estimate_uncertainty p, + output_uncertainty r) const { + using kt = kalman_s_o; + 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, estimate_uncertainty p, + process_uncertainty q, + output_uncertainty r) const { + using kt = kalman_s_o_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]] input_t u, + estimate_uncertainty p, + process_uncertainty q, + output_uncertainty r) const { + using kt = kalman_s_o_i_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]] 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 = + kalman_s_o_i_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_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 34de56e85..82e9482f1 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,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; //! @} @@ -221,10 +197,13 @@ class kalman final : public internal::conditional_member_types + inline constexpr kalman(Arguments... arguments); //! @brief Copy constructs a filter. //! @@ -335,7 +314,7 @@ class kalman final : public internal::conditional_member_types); + requires(has_input); //! @brief Returns the estimated covariance matrix P. //! @@ -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,54 @@ 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; +//! @} + +//! @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..41afa14d6 100644 --- a/linalg/naive/fcarouge/linalg.hpp +++ b/linalg/naive/fcarouge/linalg.hpp @@ -46,6 +46,9 @@ For more information, please refer to */ #include "fcarouge/utility.hpp" +//! @todo Remove the dependency on `std::initializer_list` if possible? +#include + namespace fcarouge { //! @name Algebraic Types //! @{ @@ -90,19 +93,20 @@ template struct matrix { inline constexpr explicit matrix(Type (&elements)[Row]) requires(Row != 1 && Column == 1) { + //! @todo Perhaps this is too naive and can be improved? for (decltype(Row) i{0}; i < Row; ++i) { data[i][0] = elements[i]; } } template - matrix([[maybe_unused]] const Types (&...rows)[Columns]) + matrix(const Types (&...rows)[Columns]) requires(std::conjunction_v...> && ((Columns == Column) && ... && true)) { decltype(Row) i{0}; ( - [&]([[maybe_unused]] auto row) { + [&](const auto &row) { for (decltype(Column) j{0}; j < Column; ++j) { data[i][j] = row[j]; } @@ -111,6 +115,20 @@ template struct matrix { ...); } + inline constexpr explicit matrix( + std::initializer_list> rows) { + + decltype(Row) i{0}; + for (const auto &row : rows) { + decltype(Column) j{0}; + for (const auto &element : row) { + data[i][j] = element; + ++j; + } + ++i; + } + } + [[nodiscard]] inline constexpr explicit(false) operator Type() const requires(Row == 1 && Column == 1) { @@ -173,6 +191,7 @@ template ((Columns == first_v) && ... && true)) matrix(const Types (&...rows)[Columns]) -> matrix>, sizeof...(Columns), + //! @todo Should this be `first_v` instead? (Columns, ...)>; //! @} @@ -246,6 +265,20 @@ template return result; } +template +[[nodiscard]] inline constexpr auto operator*(arithmetic auto lhs, + matrix rhs) { + matrix result; + + for (decltype(Row) i{0}; i < Row; ++i) { + for (decltype(Column) j{0}; j < Column; ++j) { + result.data[i][j] = lhs * rhs.data[i][j]; + } + } + + return result; +} + template inline constexpr auto &operator*=(matrix &lhs, arithmetic auto rhs) { diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index 1d4365766..6bd84bfbe 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -73,7 +73,8 @@ endforeach() foreach(BACKEND IN ITEMS "eigen") foreach(SAMPLE "kf_6x2x0_vehicle_location.cpp" - "kf_8x4x0_deep_sort_bounding_box.cpp") + # "kf_8x4x0_deep_sort_bounding_box.cpp" + ) get_filename_component(NAME ${SAMPLE} NAME_WE) add_executable(kalman_sample_${BACKEND}_${NAME}_driver ${SAMPLE}) set_target_properties( diff --git a/sample/ekf_4x1x0_soaring.cpp b/sample/ekf_4x1x0_soaring.cpp index 280a98151..3836f722c 100644 --- a/sample/ekf_4x1x0_soaring.cpp +++ b/sample/ekf_4x1x0_soaring.cpp @@ -7,6 +7,7 @@ namespace fcarouge::sample { namespace { template using vector = column_vector; +using state = fcarouge::state>; //! @brief ArduPilot plane soaring. //! @@ -31,33 +32,43 @@ template using vector = column_vector; //! //! @todo Add a data set and assert for correctness of results. [[maybe_unused]] auto sample{[] { - // 4x1 extended filter with additional parameter for prediction: driftX [m], - // driftY [m]. Constant time step. - using state = vector<4>; - using output = float; - using no_input = void; - using kalman = kalman, - std::tuple>; - - kalman filter; - - // Initialization const float trigger_strength{0}; const float thermal_radius{80}; const float thermal_position_x{5}; const float thermal_position_y{0}; - filter.x(trigger_strength, thermal_radius, thermal_position_x, - thermal_position_y); - const float strength_covariance{0.0049F}; const float radius_covariance{400}; const float position_covariance{400}; - filter.p(kalman::estimate_uncertainty{{strength_covariance, 0.F, 0.F, 0.F}, - {0.F, radius_covariance, 0.F, 0.F}, - {0.F, 0.F, position_covariance, 0.F}, - {0.F, 0.F, 0.F, position_covariance}}); + const float strength_noise{std::pow(0.001F, 2.F)}; + const float distance_noise{std::pow(0.03F, 2.F)}; + const float measure_noise{std::pow(0.45F, 2.F)}; - // No process dynamics: F = ∂f/∂X = I4 Default. + // 4x1 extended filter with additional parameter for prediction: driftX [m], + // driftY [m]. Constant time step. + kalman filter{// The state X: + state{trigger_strength, thermal_radius, thermal_position_x, + thermal_position_y}, + // The output Z: + output, + // The estimate uncertainty P: + estimate_uncertainty{{strength_covariance, 0.F, 0.F, 0.F}, + {0.F, radius_covariance, 0.F, 0.F}, + {0.F, 0.F, position_covariance, 0.F}, + {0.F, 0.F, 0.F, position_covariance}}, + // The process uncertainty Q: + process_uncertainty{{strength_noise, 0.F, 0.F, 0.F}, + {0.F, distance_noise, 0.F, 0.F}, + {0.F, 0.F, distance_noise, 0.F}, + {0.F, 0.F, 0.F, distance_noise}}, + // The output uncertainty R: + output_uncertainty{measure_noise}, + // No process dynamics: the state transition F = ∂f/∂X = I4 + // Default. The additional parameters for update. + update_types, + // The additional parameters for prediction. + prediction_types}; + + using kalman = decltype(filter); filter.transition([](const kalman::state &x, const float &drift_x, const float &drift_y) -> kalman::state { @@ -66,16 +77,6 @@ template using vector = column_vector; return x + drifts; }); - const float strength_noise{std::pow(0.001F, 2.F)}; - const float distance_noise{std::pow(0.03F, 2.F)}; - filter.q(kalman::process_uncertainty{{strength_noise, 0.F, 0.F, 0.F}, - {0.F, distance_noise, 0.F, 0.F}, - {0.F, 0.F, distance_noise, 0.F}, - {0.F, 0.F, 0.F, distance_noise}}); - - const float measure_noise{std::pow(0.45F, 2.F)}; - filter.r(kalman::output_uncertainty{measure_noise}); - // Observation Z: [w] vertical air velocity w at the aircraft’s // position w.r.t. the thermal center [m.s^-1]. filter.observation([](const kalman::state &x, const float &position_x, diff --git a/sample/kf_1x1x0_building_height.cpp b/sample/kf_1x1x0_building_height.cpp index 5d6c5f21b..5f37eddc8 100644 --- a/sample/kf_1x1x0_building_height.cpp +++ b/sample/kf_1x1x0_building_height.cpp @@ -24,22 +24,18 @@ 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.}, + // 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 +43,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..8d984a44d 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.}, + // Our guess is very imprecise, so we set our initialization estimate + // error σ to 100. The estimate uncertainty p of the initialization is the + // error variance σ^2: P = p0,0 = 100^2 = 10,000. This variance is very + // high. + // If we initialize with a more meaningful value, we will get faster + // Kalman filter convergence. + estimate_uncertainty{100 * 100.}, + // We have an accurate model, thus we set the process uncertainty noise + // variance Q to 0.0001. + process_uncertainty{0.0001}, + // Since the measurement error of the thermometer is σ = 0.1, the variance + // σ^2 would be 0.01, thus the measurement, output uncertainty is: R = r1 + // = + // 0.01. The measurement error (standard deviation) is 0.1 degrees + // Celsius. + output_uncertainty{0.1 * 0.1}}; + // Now, we shall predict the next state based on the initialization values. filter.predict(); assert(10 == filter.x() && @@ -57,13 +60,7 @@ namespace { "The extrapolated estimate uncertainty (variance): p1,0 = p0,0 + q = " "10000 + 0.0001 = 10000.0001."); - // Measure and Update - // The measurement value: z1 = 49.95°C. Since the measurement error is σ = - // 0.1, the variance σ^2 would be 0.01, thus the measurement uncertainty is: - // r1 = 0.01. The measurement error (standard deviation) is 0.1 degrees - // Celsius. - filter.r(0.1 * 0.1); - + // The first measurement value: z1 = 49.95°C. Measure and update. filter.update(49.95); assert(std::abs(1 - filter.k() / 0.999999) < 0.0001 && diff --git a/sample/kf_1x1x1_dog_position.cpp b/sample/kf_1x1x1_dog_position.cpp index c44f495d7..98da2cb86 100644 --- a/sample/kf_1x1x1_dog_position.cpp +++ b/sample/kf_1x1x1_dog_position.cpp @@ -31,36 +31,32 @@ 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.}, + // We are predicting that at each time step the dog moves forward one + // meter. This is the input U process model - the description of how we + // think the + // dog moves. How do I know the velocity? Magic? Consider it a prediction, + // or perhaps we have a secondary velocity sensor. Please accept this + // simplification for now. + input, + // The variance to 400 m, which is a standard deviation of 20 meters. You + // can think of this as saying "I believe with 99.7% accuracy the position + // is 0 plus or minus 60 meters". This is because with Gaussians ~99.7% of + // values fall within of the mean. The estimate uncertainty P: + estimate_uncertainty{20 * 20.}, + // Variance in the dog's movement. The process variance is how much error + // there is in the process model. Dogs rarely do what we expect, and + // things like hills or the whiff of a squirrel will change his progress. + // The process uncertainty Q: + process_uncertainty{1.}, + // Variance in the sensor. The meaning of sensor variance is how much + // variance there is in each measurement. The output uncertainty R: + output_uncertainty{2.}}; - // Initialization - // This is the dog's initial position expressed as a Gaussian. The position is - // 0 meters, and the variance to 400 m, which is a standard deviation of 20 - // meters. You can think of this as saying "I believe with 99.7% accuracy the - // position is 0 plus or minus 60 meters". This is because with Gaussians - // ~99.7% of values fall within of the mean. - filter.x(1.); - filter.p(20 * 20.); - - // Prediction - // Variance in the dog's movement. The process variance is how much error - // there is in the process model. Dogs rarely do what we expect, and things - // like hills or the whiff of a squirrel will change his progress. - filter.q(1.); - - // Measure and Update - // Variance in the sensor. The meaning of sensor variance should be clear - it - // is how much variance there is in each measurement. - filter.r(2.); - - // We are predicting that at each time step the dog moves forward one meter. - // This is the process model - the description of how we think the dog moves. - // How do I know the velocity? Magic? Consider it a prediction, or perhaps we - // have a secondary velocity sensor. Please accept this simplification for - // now. filter.predict(1.); - filter.update(1.354); filter.predict(1.); filter.update(1.882); diff --git a/sample/kf_2x1x1_rocket_altitude.cpp b/sample/kf_2x1x1_rocket_altitude.cpp index faee1da0d..983865450 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 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.}}, + // The filter prediction uses a delta time [ms] parameter. + 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..c314c880e 100644 --- a/sample/kf_6x2x0_vehicle_location.cpp +++ b/sample/kf_6x2x0_vehicle_location.cpp @@ -16,6 +16,8 @@ constexpr auto fcarouge::operator/(const Numerator &lhs, const Denominator &rhs) namespace fcarouge::sample { namespace { template using vector = column_vector; +template using matrix = matrix; +using state = fcarouge::state>; //! @brief Estimating the vehicle location. //! @@ -32,72 +34,68 @@ template using vector = column_vector; //! X direction with a constant velocity. After traveling 400 meters the vehicle //! turns right, with a turning radius of 300 meters. During the turning //! maneuver, the vehicle experiences acceleration due to the circular motion -//! (an angular acceleration). The measurements period: Δt = 1s (constant). The -//! random acceleration standard deviation: σa = 0.2 m.s^-2. +//! (an angular acceleration). The measurements period: Δt = 1s (constant). //! //! @example kf_6x2x0_vehicle_location.cpp [[maybe_unused]] auto sample{[] { // A 6x2x0 filter, constant acceleration dynamic model, no control. - using state = vector<6>; - using output = vector<2>; - using no_input = void; - using kalman = kalman; - - kalman filter; - - // Initialization - // The state is chosen to be the position, velocity, acceleration in the XY - // plane: [px, vx, ax, py, vy, ay]. We don't know the vehicle location; we - // will set initial position, velocity and acceleration to 0. - filter.x(0., 0., 0., 0., 0., 0.); - - // Since our initial state vector is a guess, we will set a very high estimate - // uncertainty. The high estimate uncertainty results in a high Kalman Gain, - // giving a high weight to the measurement. - filter.p(kalman::estimate_uncertainty{{500., 0., 0., 0., 0., 0.}, - {0., 500., 0., 0., 0., 0.}, - {0., 0., 500., 0., 0., 0.}, - {0., 0., 0., 500., 0., 0.}, - {0., 0., 0., 0., 500., 0.}, - {0., 0., 0., 0., 0., 500.}}); - - // Prediction - // The process noise matrix Q would be: - kalman::process_uncertainty q{ - {0.25, 0.5, 0.5, 0., 0., 0.}, {0.5, 1., 1., 0., 0., 0.}, - {0.5, 1., 1., 0., 0., 0.}, {0., 0., 0., 0.25, 0.5, 0.5}, - {0., 0., 0., 0.5, 1., 1.}, {0., 0., 0., 0.5, 1., 1.}}; - q *= 0.2 * 0.2; - filter.q(q); - - // The state transition matrix F would be: - filter.f(kalman::state_transition{{1., 1., 0.5, 0., 0., 0.}, - {0., 1., 1., 0., 0., 0.}, - {0., 0., 1., 0., 0., 0.}, - {0., 0., 0., 1., 1., 0.5}, - {0., 0., 0., 0., 1., 1.}, - {0., 0., 0., 0., 0., 1.}}); + kalman filter{ + // The state X is chosen to be the position, velocity, acceleration in the + // XY plane: [px, vx, ax, py, vy, ay]. We don't know the vehicle location; + // we will set initial position, velocity and acceleration to 0. + state{0., 0., 0., 0., 0., 0.}, + // The vehicle has an onboard location sensor that reports output Z as X + // and Y + // coordinates of the system. + output>, + // The estimate uncertainty matrix P. + // Since our initial state vector is a guess, we will set a very high + // estimate uncertainty. The high estimate uncertainty results in a high + // Kalman Gain, giving a high weight to the measurement. + estimate_uncertainty{{500., 0., 0., 0., 0., 0.}, + {0., 500., 0., 0., 0., 0.}, + {0., 0., 500., 0., 0., 0.}, + {0., 0., 0., 500., 0., 0.}, + {0., 0., 0., 0., 500., 0.}, + {0., 0., 0., 0., 0., 500.}}, + // The process uncertainty noise matrix Q, constant, computed in place, + // with the + // random acceleration standard deviation: σa = 0.2 m.s^-2. + process_uncertainty{[]() -> matrix<6, 6> { + return 0.2 * 0.2 * matrix<6, 6>{{0.25, 0.5, 0.5, 0., 0., 0.}, + {0.5, 1., 1., 0., 0., 0.}, + {0.5, 1., 1., 0., 0., 0.}, + {0., 0., 0., 0.25, 0.5, 0.5}, + {0., 0., 0., 0.5, 1., 1.}, + {0., 0., 0., 0.5, 1., 1.}}; + }()}, + // The output uncertainty matrix R. Assume that the x and y measurements + // are uncorrelated, i.e. error in the x coordinate measurement doesn't + // depend on the error in the y coordinate measurement. In real-life + // applications, the measurement uncertainty can differ between + // measurements. In many systems the measurement uncertainty depends on + // the measurement SNR (signal-to-noise ratio), angle between sensor (or + // sensors) and target, signal frequency and many other parameters. + // For the sake of the example simplicity, we will assume a constant + // measurement uncertainty: R1 = R2...Rn-1 = Rn = R The measurement error + // standard deviation: σxm = σym = 3m. The variance 9. + output_uncertainty{{9., 0.}, {0., 9.}}, + // The output model matrix H. The dimension of zn is 2x1 and the dimension + // of xn is 6x1. Therefore the dimension of the observation matrix H shall + // be 2x6. + // COULD THIS BE DEFAULTED IF STATE REORGANIZED? PERFORMANCE? + output_model{{1., 0., 0., 0., 0., 0.}, {0., 0., 0., 1., 0., 0.}}, + // // The state transition matrix F would be: + state_transition{{1., 1., 0.5, 0., 0., 0.}, + {0., 1., 1., 0., 0., 0.}, + {0., 0., 1., 0., 0., 0.}, + {0., 0., 0., 1., 1., 0.5}, + {0., 0., 0., 0., 1., 1.}, + {0., 0., 0., 0., 0., 1.}}}; // Now we can predict the next state based on the initialization values. filter.predict(); - // Measure and Update - // The dimension of zn is 2x1 and the dimension of xn is 6x1. Therefore the - // dimension of the observation matrix H shall be 2x6. - filter.h( - kalman::output_model{{1., 0., 0., 0., 0., 0.}, {0., 0., 0., 1., 0., 0.}}); - - // Assume that the x and y measurements are uncorrelated, i.e. error in the x - // coordinate measurement doesn't depend on the error in the y coordinate - // measurement. In real-life applications, the measurement uncertainty can - // differ between measurements. In many systems the measurement uncertainty - // depends on the measurement SNR (signal-to-noise ratio), angle between - // sensor (or sensors) and target, signal frequency and many other parameters. - // For the sake of the example simplicity, we will assume a constant - // measurement uncertainty: R1 = R2...Rn-1 = Rn = R The measurement error - // standard deviation: σxm = σym = 3m. The variance 9. - filter.r(kalman::output_uncertainty{{9., 0.}, {0., 9.}}); - // The measurement values: z1 = [-393.66, 300.4] filter.update(-393.66, 300.4); filter.predict(); diff --git a/sample/kf_8x4x0_deep_sort_bounding_box.cpp b/sample/kf_8x4x0_deep_sort_bounding_box.cpp index caae1f6fc..4f1ffc893 100644 --- a/sample/kf_8x4x0_deep_sort_bounding_box.cpp +++ b/sample/kf_8x4x0_deep_sort_bounding_box.cpp @@ -16,6 +16,7 @@ constexpr auto fcarouge::operator/(const Numerator &lhs, const Denominator &rhs) namespace fcarouge::sample { namespace { template using vector = column_vector; +using state = fcarouge::state>; //! @brief Estimating the position of bounding boxes in image space. //! @@ -33,13 +34,46 @@ template using vector = column_vector; //! //! @example kf_8x4x0_deep_sort_bounding_box.cpp [[maybe_unused]] auto sample{[] { - // A 8x4x0 filter, constant velocity, linear. - using state = vector<8>; - using output = vector<4>; - using no_input = void; - using kalman = kalman; + const vector<4> initial_box{605.0F, 248.0F, 0.20481927710843373F, 332.0F}; + // Experimental position and velocity uncertainty standard deviation weights. + const float position_weight{1.F / 20.F}; + const float velocity_weight{1.F / 160.F}; + // Constant velocity, linear state transition model. From one image frame to + // the other. + const float delta_time{1.F}; - kalman filter; + // A 8x4x0 filter, constant velocity, linear. + kalman filter{ + // The filter is initialized with the first observed output. + // Bounding box position and velocity estimated state X: [px, + // py, pa, ph, vx, vy, va, vh]. + state{initial_box(0), initial_box(1), initial_box(2), initial_box(3), 0.F, + 0.F, 0.F, 0.F}, + // The output Z: + output>, + // The estimate uncertainty P: + estimate_uncertainty{ + vector<8>{2 * position_weight * initial_box(3), + 2 * position_weight * initial_box(3), 1e-2F, + 2 * position_weight * initial_box(3), + 10 * velocity_weight * initial_box(3), + 10 * velocity_weight * initial_box(3), 1e-5F, + 10 * velocity_weight * initial_box(3)} + .array() + .square() + .matrix() + .asDiagonal()}, + // The state transition F: + state_transition{{1.F, 0.F, 0.F, 0.F, delta_time, 0.F, 0.F, 0.F}, + {0.F, 1.F, 0.F, 0.F, 0.F, delta_time, 0.F, 0.F}, + {0.F, 0.F, 1.F, 0.F, 0.F, 0.F, delta_time, 0.F}, + {0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F, delta_time}, + {0.F, 0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F}, + {0.F, 0.F, 0.F, 0.F, 0.F, 1.F, 0.F, 0.F}, + {0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 1.F, 0.F}, + {0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 1.F}}}; + + using kalman = decltype(filter); // A hundred bounding box output measurements `(x, y, a, h)` from Deep SORT's // MOT16 sample, tracker #201. @@ -144,54 +178,16 @@ template using vector = column_vector; {368.F, 252.5F, 0.303886925795053F, 283.F}, {369.F, 250.5F, 0.29757785467128F, 289.F}}; - // Initialization - // The filter is initialized at runtime, on bounding box detection, with the - // first observed output. Bounding box position and velocity estimated state: - // [px, py, pa, ph, vx, vy, va, vh]. - const kalman::output initial_box{605.0F, 248.0F, 0.20481927710843373F, - 332.0F}; - filter.x(initial_box(0), initial_box(1), initial_box(2), initial_box(3), 0.F, - 0.F, 0.F, 0.F); - - // Experimental position and velocity uncertainty standard deviation weights. - const float position_weight{1.F / 20.F}; - const float velocity_weight{1.F / 160.F}; - - filter.p(kalman::estimate_uncertainty{ - state{2 * position_weight * initial_box(3), - 2 * position_weight * initial_box(3), 1e-2F, - 2 * position_weight * initial_box(3), - 10 * velocity_weight * initial_box(3), - 10 * velocity_weight * initial_box(3), 1e-5F, - 10 * velocity_weight * initial_box(3)} - .array() - .square() - .matrix() - .asDiagonal()}); - - // Constant velocity, linear state transition model. From one image frame to - // the other. - const float delta_time{1.F}; - filter.f( - kalman::state_transition{{1.F, 0.F, 0.F, 0.F, delta_time, 0.F, 0.F, 0.F}, - {0.F, 1.F, 0.F, 0.F, 0.F, delta_time, 0.F, 0.F}, - {0.F, 0.F, 1.F, 0.F, 0.F, 0.F, delta_time, 0.F}, - {0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F, delta_time}, - {0.F, 0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F}, - {0.F, 0.F, 0.F, 0.F, 0.F, 1.F, 0.F, 0.F}, - {0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 1.F, 0.F}, - {0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 1.F}}); - filter.q([position_weight, velocity_weight]( const kalman::state &x) -> kalman::process_uncertainty { - return state{position_weight * x(3), - position_weight * x(3), - 1e-2F, - position_weight * x(3), - velocity_weight * x(3), - velocity_weight * x(3), - 1e-5F, - velocity_weight * x(3)} + return vector<8>{position_weight * x(3), + position_weight * x(3), + 1e-2F, + position_weight * x(3), + velocity_weight * x(3), + velocity_weight * x(3), + 1e-5F, + velocity_weight * x(3)} .array() .square() .matrix() @@ -203,17 +199,17 @@ template using vector = column_vector; // Measure and Update // Direct linear observation transition model. - filter.h(kalman::output_model{{1.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F}, - {0.F, 1.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F}, - {0.F, 0.F, 1.F, 0.F, 0.F, 0.F, 0.F, 0.F}, - {0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F, 0.F}}); + // filter.h(kalman::output_model{{1.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F}, + // {0.F, 1.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F}, + // {0.F, 0.F, 1.F, 0.F, 0.F, 0.F, 0.F, 0.F}, + // {0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F, 0.F}}); // Observation, measurement noise covariance. filter.r([position_weight](const kalman::state &x, [[maybe_unused]] const kalman::output &z) -> kalman::output_uncertainty { - return output{position_weight * x(3), position_weight * x(3), 1e-1F, - position_weight * x(3)} + return vector<4>{position_weight * x(3), position_weight * x(3), 1e-1F, + position_weight * x(3)} .array() .square() .matrix() diff --git a/test/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);