From 1cb86a227110cddd7816afc1210f9f8f3de09f08 Mon Sep 17 00:00:00 2001 From: Mateusz Pusz Date: Wed, 29 May 2024 20:25:21 +0200 Subject: [PATCH] feat(example): kalman filter examples enabled after text formatting refactoring --- example/CMakeLists.txt | 2 +- example/kalman_filter/kalman.h | 354 +++++++++++------- .../kalman_filter/kalman_filter-example_1.cpp | 33 +- .../kalman_filter/kalman_filter-example_2.cpp | 28 +- .../kalman_filter/kalman_filter-example_3.cpp | 30 +- .../kalman_filter/kalman_filter-example_4.cpp | 31 +- .../kalman_filter/kalman_filter-example_5.cpp | 50 +-- .../kalman_filter/kalman_filter-example_6.cpp | 61 ++- .../kalman_filter/kalman_filter-example_7.cpp | 61 ++- .../kalman_filter/kalman_filter-example_8.cpp | 61 ++- src/core/include/mp-units/bits/fmt.h | 39 +- src/core/include/mp-units/format.h | 22 +- 12 files changed, 429 insertions(+), 343 deletions(-) diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index 9e08103d..36364f12 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -72,4 +72,4 @@ add_example(total_energy) add_example(unmanned_aerial_vehicle example_utils) add_subdirectory(glide_computer_lib) -# add_subdirectory(kalman_filter) +add_subdirectory(kalman_filter) diff --git a/example/kalman_filter/kalman.h b/example/kalman_filter/kalman.h index 9ae5b462..bcf19646 100644 --- a/example/kalman_filter/kalman.h +++ b/example/kalman_filter/kalman.h @@ -23,21 +23,22 @@ #pragma once #include +#include #include #ifdef MP_UNITS_MODULES import mp_units; #else +#include #include +#include +#include #include -#include -#include -#include +#include #endif namespace kalman { -template -concept QuantityOrQuantityPoint = mp_units::Quantity || mp_units::QuantityPoint; +namespace detail { template inline constexpr bool are_time_derivatives = false; @@ -49,192 +50,261 @@ template = (D1 / D2 == mp_units::isq::dim_time) && are_time_derivatives; -// state -template - requires(sizeof...(QQPs) > 0) && (sizeof...(QQPs) <= 3) && are_time_derivatives -struct state { - std::tuple variables_; - constexpr state(QQPs... qqps) : variables_(std::move(qqps)...) {} +} // namespace detail + +// system state +template + requires(sizeof...(QPs) > 0) && (sizeof...(QPs) <= 3) && detail::are_time_derivatives +class system_state { + std::tuple variables_; +public: + constexpr explicit system_state(QPs... qps) : variables_(std::move(qps)...) {} + + template + [[nodiscard]] friend constexpr auto& get(system_state& s) + { + return get(s.variables_); + } + + template + [[nodiscard]] friend constexpr const auto& get(const system_state& s) + { + return get(s.variables_); + } }; template -concept State = mp_units::is_specialization_of; +concept SystemState = mp_units::is_specialization_of; -template -constexpr auto& get(state& s) -{ - return get(s.variables_); -} -template -constexpr const auto& get(const state& s) -{ - return get(s.variables_); -} - -// estimation -template -struct estimation { -private: - static constexpr auto uncertainty_ref = QQP::reference * QQP::reference; - using uncertainty_type = mp_units::quantity; +// system state estimation +template + requires requires { typename system_state; } +class system_state_estimate { public: - kalman::state state; // TODO extend kalman functions to work with this variadic parameter list - uncertainty_type uncertainty; + using state_type = system_state; + using standard_deviation_type = QP::quantity_type; + using variance_type = + mp_units::quantity(standard_deviation_type::reference), typename standard_deviation_type::rep>; +private: + state_type state_; + variance_type variance_; +public: + constexpr system_state_estimate(state_type state, standard_deviation_type standard_deviation) : + state_(state), variance_(pow<2>(standard_deviation)) + { + } + constexpr system_state_estimate(state_type state, variance_type variance) : state_(state), variance_(variance) {} + [[nodiscard]] constexpr const state_type& state() const { return state_; } + [[nodiscard]] constexpr const variance_type& variance() const { return variance_; } + [[nodiscard]] constexpr standard_deviation_type standard_deviation() const { return sqrt(variance_); } }; -template -estimation(state, U) -> estimation; - // kalman gain -template -constexpr mp_units::quantity kalman_gain(Q estimate_uncertainty, - Q measurement_uncertainty) +template + requires requires { mp_units::common_reference(Q1::reference, Q2::reference); } +[[nodiscard]] constexpr mp_units::quantity kalman_gain( + Q1 variance_in_estimate, Q2 variance_in_measurement) { - return estimate_uncertainty / (estimate_uncertainty + measurement_uncertainty); + return variance_in_estimate / (variance_in_estimate + variance_in_measurement); } // state update -template K> - requires(implicitly_convertible(QM::quantity_spec, Q::quantity_spec)) -constexpr state state_update(const state& predicted, QM measured, K gain) +template K> + requires(implicitly_convertible(QM::quantity_spec, QP::quantity_spec)) +[[nodiscard]] constexpr system_state state_update(const system_state& predicted, QM measured, K gain) { - return {get<0>(predicted) + gain * (measured - get<0>(predicted))}; + return system_state{get<0>(predicted) + gain * (measured - get<0>(predicted))}; } -template K, +template K, mp_units::QuantityOf T> - requires(implicitly_convertible(QM::quantity_spec, Q1::quantity_spec)) -constexpr state state_update(const state& predicted, QM measured, std::array gain, T interval) + requires(implicitly_convertible(QM::quantity_spec, QP1::quantity_spec)) +[[nodiscard]] constexpr system_state state_update(const system_state& predicted, QM measured, + std::array gain, T interval) { - const auto q1 = get<0>(predicted) + get<0>(gain) * (measured - get<0>(predicted)); - const auto q2 = get<1>(predicted) + get<1>(gain) * (measured - get<0>(predicted)) / interval; - return {q1, q2}; + const auto qp1 = fma(get<0>(gain), measured - get<0>(predicted), get<0>(predicted)); + const auto qp2 = fma(get<1>(gain), (measured - get<0>(predicted)) / interval, get<1>(predicted)); + return system_state{qp1, qp2}; } -template K, mp_units::QuantityOf T> - requires(implicitly_convertible(QM::quantity_spec, Q1::quantity_spec)) -constexpr state state_update(const state& predicted, QM measured, std::array gain, - T interval) + requires(implicitly_convertible(QM::quantity_spec, QP1::quantity_spec)) +[[nodiscard]] constexpr system_state state_update(const system_state& predicted, + QM measured, std::array gain, T interval) { - const auto q1 = get<0>(predicted) + get<0>(gain) * (measured - get<0>(predicted)); - const auto q2 = get<1>(predicted) + get<1>(gain) * (measured - get<0>(predicted)) / interval; - const auto q3 = get<2>(predicted) + get<2>(gain) * (measured - get<0>(predicted)) / (interval * interval / 2); - return {q1, q2, q3}; + const auto qp1 = fma(get<0>(gain), measured - get<0>(predicted), get<0>(predicted)); + const auto qp2 = fma(get<1>(gain), (measured - get<0>(predicted)) / interval, get<1>(predicted)); + const auto qp3 = fma(get<2>(gain), (measured - get<0>(predicted)) / (interval * interval / 2), get<2>(predicted)); + return system_state{qp1, qp2, qp3}; } // covariance update template K> -constexpr Q covariance_update(Q uncertainty, K gain) +[[nodiscard]] constexpr Q covariance_update(Q uncertainty, K gain) { return (1 * mp_units::one - gain) * uncertainty; } -// state extrapolation -template T> -constexpr state state_extrapolation(const state& estimated, T interval) +template K> +[[nodiscard]] constexpr system_state_estimate state_estimate_update( + const system_state_estimate& previous, QP measurement, K gain) { - const auto q1 = get<0>(estimated) + get<1>(estimated) * interval; - const auto q2 = get<1>(estimated); - return {q1, q2}; + return {state_update(previous.state(), measurement, gain), covariance_update(previous.variance(), gain)}; +}; + + +// state extrapolation +template T> +[[nodiscard]] constexpr system_state state_extrapolation(const system_state& estimated, T interval) +{ + auto to_quantity = [](const auto& qp) { return qp.quantity_ref_from(qp.point_origin); }; + const auto qp1 = fma(to_quantity(get<1>(estimated)), interval, get<0>(estimated)); + const auto qp2 = get<1>(estimated); + return system_state{qp1, qp2}; } -template T> -constexpr state state_extrapolation(const state& estimated, T interval) +template T> +[[nodiscard]] constexpr system_state state_extrapolation(const system_state& estimated, + T interval) { - const auto q1 = get<0>(estimated) + get<1>(estimated) * interval + get<2>(estimated) * pow<2>(interval) / 2; - const auto q2 = get<1>(estimated) + get<2>(estimated) * interval; - const auto q3 = get<2>(estimated); - return {q1, q2, q3}; + auto to_quantity = [](const auto& qp) { return qp.quantity_ref_from(qp.point_origin); }; + const auto qp1 = to_quantity(get<2>(estimated)) * pow<2>(interval) / 2 + + fma(to_quantity(get<1>(estimated)), interval, get<0>(estimated)); + const auto qp2 = fma(to_quantity(get<2>(estimated)), interval, get<1>(estimated)); + const auto qp3 = get<2>(estimated); + return system_state{qp1, qp2, qp3}; } // covariance extrapolation -template -constexpr Q covariance_extrapolation(Q uncertainty, Q process_noise_variance) +template + requires requires { mp_units::common_reference(Q1::reference, Q2::reference); } +[[nodiscard]] constexpr mp_units::Quantity auto covariance_extrapolation(Q1 uncertainty, Q2 process_noise_variance) { return uncertainty + process_noise_variance; } } // namespace kalman -template -struct MP_UNITS_STD_FMT::formatter> { - constexpr auto parse(format_parse_context& ctx) +template +struct MP_UNITS_STD_FMT::formatter, Char> : + MP_UNITS_STD_FMT::formatter::quantity_type> { + template + auto format(const mp_units::quantity_point& qp, FormatContext& ctx) const -> decltype(ctx.out()) { - mp_units::detail::dynamic_specs_handler handler(specs, ctx); - return mp_units::detail::parse_format_specs(ctx.begin(), ctx.end(), handler); + return MP_UNITS_STD_FMT::formatter::quantity_type>::format( + qp.quantity_ref_from(qp.point_origin), ctx); + } +}; + +template +class MP_UNITS_STD_FMT::formatter, Char> { + using format_specs = mp_units::detail::fill_align_width_format_specs; + format_specs specs_{}; + std::array, sizeof...(QPs)> format_str_; + std::tuple...> formatters_{}; + + template + constexpr const Char* parse_default_spec(const Char* begin, const Char* end, Formatter& f, std::string& format_str) + { + if (begin == end || *begin++ != '[') + throw MP_UNITS_STD_FMT::format_error("`default-spec` should contain a `[` character"); + auto it = begin; + for (int nested_brackets = 0; it != end && !(*it == ']' && nested_brackets == 0); it++) { + if (*it == '[') ++nested_brackets; + if (*it == ']') { + if (nested_brackets == 0) throw MP_UNITS_STD_FMT::format_error("unmatched ']' in format string"); + --nested_brackets; + } + } + format_str = "{:" + std::string(begin, it) + '}'; + if (it == end) throw MP_UNITS_STD_FMT::format_error("unmatched '[' in format string"); + MP_UNITS_STD_FMT::basic_format_parse_context ctx(std::string_view(begin, it)); + auto ptr = f.parse(ctx); + if (ptr != it) throw MP_UNITS_STD_FMT::format_error("invalid subentity format '" + std::string(begin, it) + "'"); + return ++it; // skip `]` + } + + template + [[nodiscard]] constexpr const Char* parse_default_spec(const Char* begin, const Char* end, size_t idx, + std::index_sequence) + { + auto parse = [&](bool flag, auto& f, std::basic_string& str) { + return flag ? parse_default_spec(begin, end, f, str) : begin; + }; + return std::max({parse(idx == Is, std::get(formatters_), format_str_[Is])...}); + } + + [[nodiscard]] constexpr const Char* parse_defaults_specs(const Char* begin, const Char* end) + { + if (begin == end || *begin == '}') return begin; + if (*begin++ != ':') throw MP_UNITS_STD_FMT::format_error("`defaults-specs` should start with a `:`"); + do { + auto c = *begin++; + if (c < '0' || c >= static_cast('0' + sizeof...(QPs))) + throw MP_UNITS_STD_FMT::format_error(std::string("unknown `subentity-id` token '") + c + "'"); + const size_t idx = static_cast(c - '0'); + begin = parse_default_spec(begin, end, idx, std::index_sequence_for{}); + } while (begin != end && *begin != '}'); + return begin; + } + + template + OutputIt format_system_state(OutputIt out, const kalman::system_state& s, FormatContext& ctx, + std::index_sequence) const + { + const std::locale locale = MP_UNITS_FMT_LOCALE(ctx.locale()); + auto f = [&](const std::basic_string& str, const mp_units::QuantityPoint auto& qp) { + return MP_UNITS_STD_FMT::vformat_to(out, locale, str, MP_UNITS_STD_FMT::make_format_args(qp)); + }; + return f(get(format_str_), get(s)); + } + + template + OutputIt format_system_state(OutputIt out, const kalman::system_state& s, FormatContext& ctx, + std::index_sequence) const + { + const std::locale locale = MP_UNITS_FMT_LOCALE(ctx.locale()); + auto f = [&](const std::basic_string& str, const mp_units::QuantityPoint auto& qp) { + return MP_UNITS_STD_FMT::vformat_to(out, locale, str, MP_UNITS_STD_FMT::make_format_args(qp)); + }; + return f(get(format_str_), get(s)), ((*out++ = ' ', f(get(format_str_), get(s))), ...); + } + +public: + constexpr formatter() + { + for (auto& str : format_str_) str = "{}"; + } + + constexpr auto parse(format_parse_context& ctx) -> decltype(ctx.begin()) + { + auto begin = ctx.begin(), end = ctx.end(); + + begin = parse_fill_align_width(ctx, begin, end, specs_, mp_units::detail::fmt_align::right); + if (begin == end) return begin; + + return parse_defaults_specs(begin, end); } template - auto format(const kalman::state& s, FormatContext& ctx) + auto format(const kalman::system_state& s, FormatContext& ctx) const -> decltype(ctx.out()) { - std::string value_buffer; - auto to_value_buffer = std::back_inserter(value_buffer); - if (specs.precision != -1) { - if constexpr (sizeof...(Qs) == 1) - MP_UNITS_STD_FMT::format_to(to_value_buffer, "{1:%.{0}Q %q}", specs.precision, kalman::get<0>(s)); - else if constexpr (sizeof...(Qs) == 2) - MP_UNITS_STD_FMT::format_to(to_value_buffer, "{{ {1:%.{0}Q %q}, {2:%.{0}Q %q} }}", specs.precision, - kalman::get<0>(s), kalman::get<1>(s)); - else - MP_UNITS_STD_FMT::format_to(to_value_buffer, "{{ {1:%.{0}Q %q}, {2:%.{0}Q %q}, {3:%.{0}Q %q} }}", - specs.precision, kalman::get<0>(s), kalman::get<1>(s), kalman::get<2>(s)); - } else { - if constexpr (sizeof...(Qs) == 1) - MP_UNITS_STD_FMT::format_to(to_value_buffer, "{}", kalman::get<0>(s)); - else if constexpr (sizeof...(Qs) == 2) - MP_UNITS_STD_FMT::format_to(to_value_buffer, "{{ {}, {} }}", kalman::get<0>(s), kalman::get<1>(s)); - else - MP_UNITS_STD_FMT::format_to(to_value_buffer, "{{ {}, {}, {} }}", kalman::get<0>(s), kalman::get<1>(s), - kalman::get<2>(s)); + auto specs = specs_; + mp_units::detail::handle_dynamic_spec(specs.width, specs.width_ref, ctx); + + if (specs.width == 0) { + // Avoid extra copying if width is not specified + format_system_state(ctx.out(), s, ctx, std::index_sequence_for{}); + return ctx.out(); } + std::basic_string quantity_buffer; + format_system_state(std::back_inserter(quantity_buffer), s, ctx, std::index_sequence_for{}); - std::string global_format_buffer; - mp_units::detail::quantity_global_format_specs global_specs = {specs.fill, specs.align, specs.width}; - mp_units::detail::format_global_buffer(std::back_inserter(global_format_buffer), global_specs); - - return MP_UNITS_STD_FMT::vformat_to(ctx.out(), global_format_buffer, - MP_UNITS_STD_FMT::make_format_args(value_buffer)); + std::basic_string fill_align_width_format_str; + mp_units::detail::format_global_buffer(std::back_inserter(fill_align_width_format_str), specs); + return MP_UNITS_STD_FMT::vformat_to(ctx.out(), fill_align_width_format_str, + MP_UNITS_STD_FMT::make_format_args(quantity_buffer)); } -private: - mp_units::detail::dynamic_format_specs specs; -}; - -template -struct MP_UNITS_STD_FMT::formatter> { - constexpr auto parse(format_parse_context& ctx) - { - mp_units::detail::dynamic_specs_handler handler(specs, ctx); - return mp_units::detail::parse_format_specs(ctx.begin(), ctx.end(), handler); - } - - template - auto format(kalman::estimation e, FormatContext& ctx) - { - mp_units::Quantity auto q = [](const Q& t) { - if constexpr (mp_units::Quantity) - return t; - else - return t.quantity_ref_from(t.point_origin); - }(kalman::get<0>(e.state)); - - std::string value_buffer; - auto to_value_buffer = std::back_inserter(value_buffer); - if (specs.precision != -1) { - MP_UNITS_STD_FMT::format_to(to_value_buffer, "{0:%.{2}Q} ± {1:%.{2}Q} {0:%q}", q, sqrt(e.uncertainty), - specs.precision); - } else { - MP_UNITS_STD_FMT::format_to(to_value_buffer, "{0:%Q} ± {1:%Q} {0:%q}", q, sqrt(e.uncertainty)); - } - - std::string global_format_buffer; - mp_units::detail::quantity_global_format_specs global_specs = {specs.fill, specs.align, specs.width}; - mp_units::detail::format_global_buffer(std::back_inserter(global_format_buffer), global_specs); - - return MP_UNITS_STD_FMT::vformat_to(ctx.out(), global_format_buffer, - MP_UNITS_STD_FMT::make_format_args(value_buffer)); - } -private: - mp_units::detail::dynamic_format_specs specs; }; diff --git a/example/kalman_filter/kalman_filter-example_1.cpp b/example/kalman_filter/kalman_filter-example_1.cpp index c038f253..26746ace 100644 --- a/example/kalman_filter/kalman_filter-example_1.cpp +++ b/example/kalman_filter/kalman_filter-example_1.cpp @@ -35,36 +35,37 @@ import mp_units; using namespace mp_units; -void print_header(const kalman::State auto& initial) +void print_header(const kalman::SystemState auto& initial) { std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial); std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>9} | {:>8} | {:>14} | {:>14}\n", "N", "Gain", "Measured", "Curr. Estimate", "Next Estimate"); } -void print(auto iteration, QuantityOf auto gain, Quantity auto measured, - const kalman::State auto& current, const kalman::State auto& next) +void print(auto iteration, QuantityOf auto gain, QuantityPoint auto measured, + const kalman::SystemState auto& current, const kalman::SystemState auto& next) { - std::cout << MP_UNITS_STD_FMT::format("{:2} | {:9} | {:8} | {:14} | {:14}\n", iteration, gain, measured, current, - next); + std::cout << MP_UNITS_STD_FMT::format("{:2} | {:9:N[.2f]} | {:8} | {:14:0[:N[.2f]]} | {:14:0[:N[.2f]]}\n", iteration, + gain, measured, current, next); } int main() { using namespace mp_units::si::unit_symbols; - using state = kalman::state>; + using state = kalman::system_state>; + using qp = quantity_point; - const state initial = {1 * kg}; - const std::array measurements = {1'030 * g, 989 * g, 1'017 * g, 1'009 * g, 1'013 * g, - 979 * g, 1'008 * g, 1'042 * g, 1'012 * g, 1'011 * g}; + const state initial_guess{qp{1 * kg}}; + const std::array measurements = {qp{996 * g}, qp{994 * g}, qp{1021 * g}, qp{1000 * g}, qp{1002 * g}, + qp{1010 * g}, qp{983 * g}, qp{971 * g}, qp{993 * g}, qp{1023 * g}}; - print_header(initial); - state next = initial; - for (int index = 1; const auto& v : measurements) { - const auto& previous = next; - const auto gain = 1. / index * one; - const auto current = state_update(previous, v, gain); + print_header(initial_guess); + state next = initial_guess; + for (int index = 1; const auto& m : measurements) { + const state& previous = next; + const quantity gain = 1. / index * one; + const state current = state_update(previous, m, gain); next = current; - print(index++, gain, v, current, next); + print(index++, gain, m, current, next); } } diff --git a/example/kalman_filter/kalman_filter-example_2.cpp b/example/kalman_filter/kalman_filter-example_2.cpp index 0260d2de..83489d76 100644 --- a/example/kalman_filter/kalman_filter-example_2.cpp +++ b/example/kalman_filter/kalman_filter-example_2.cpp @@ -39,36 +39,38 @@ inline constexpr bool mp_units::is_vector = true; using namespace mp_units; -void print_header(const kalman::State auto& initial) +void print_header(const kalman::SystemState auto& initial) { std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial); std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>8} | {:>23} | {:>23}\n", "N", "Measured", "Curr. Estimate", "Next Estimate"); } -void print(auto iteration, Quantity auto measured, const kalman::State auto& current, const kalman::State auto& next) +void print(auto iteration, QuantityPoint auto measured, const kalman::SystemState auto& current, + const kalman::SystemState auto& next) { - std::cout << MP_UNITS_STD_FMT::format("{:2} | {:8} | {:.1} | {:.1}\n", iteration, measured, current, next); + std::cout << MP_UNITS_STD_FMT::vformat("{:2} | {:8} | {:23:0[:N[.2f]]1[:N[.2f]]} | {:23:0[:N[.2f]]1[:N[.2f]]}\n", + MP_UNITS_STD_FMT::make_format_args(iteration, measured, current, next)); } int main() { using namespace mp_units::si::unit_symbols; - using state = kalman::state, quantity>; + using qp = quantity_point; + using state = kalman::system_state>; - const auto interval = isq::duration(5 * s); - const state initial = {30 * km, 40 * m / s}; - const quantity measurements[] = {30'110 * m, 30'265 * m, 30'740 * m, 30'750 * m, - 31'135 * m, 31'015 * m, 31'180 * m, 31'610 * m, - 31'960 * m, 31'865 * m}; + const quantity interval = isq::duration(5 * s); + const state initial{qp{30 * km}, quantity_point{40 * m / s}}; + const std::array measurements = {qp{30'171 * m}, qp{30'353 * m}, qp{30'756 * m}, qp{30'799 * m}, qp{31'018 * m}, + qp{31'278 * m}, qp{31'276 * m}, qp{31'379 * m}, qp{31'748 * m}, qp{32'175 * m}}; std::array gain = {0.2 * one, 0.1 * one}; print_header(initial); state next = state_extrapolation(initial, interval); - for (int index = 1; const auto& measured : measurements) { - const auto& previous = next; - const auto current = state_update(previous, measured, gain, interval); + for (int index = 1; const auto& m : measurements) { + const state& previous = next; + const state current = state_update(previous, m, gain, interval); next = state_extrapolation(current, interval); - print(index++, measured, current, next); + print(index++, m, current, next); } } diff --git a/example/kalman_filter/kalman_filter-example_3.cpp b/example/kalman_filter/kalman_filter-example_3.cpp index 5d453d48..352d603e 100644 --- a/example/kalman_filter/kalman_filter-example_3.cpp +++ b/example/kalman_filter/kalman_filter-example_3.cpp @@ -39,36 +39,38 @@ inline constexpr bool mp_units::is_vector = true; using namespace mp_units; -void print_header(const kalman::State auto& initial) +void print_header(const kalman::SystemState auto& initial) { std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial); - std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>8} | {:>24} | {:>24}\n", "N", "Measured", "Curr. Estimate", + std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>8} | {:>23} | {:>23}\n", "N", "Measured", "Curr. Estimate", "Next Estimate"); } -void print(auto iteration, Quantity auto measured, const kalman::State auto& current, const kalman::State auto& next) +void print(auto iteration, QuantityPoint auto measured, const kalman::SystemState auto& current, + const kalman::SystemState auto& next) { - std::cout << MP_UNITS_STD_FMT::format("{:2} | {:8} | {:>24.1} | {:>24.1}\n", iteration, measured, current, next); + std::cout << MP_UNITS_STD_FMT::format("{:2} | {:8} | {:23:0[:N[.2f]]1[:N[.2f]]} | {:23:0[:N[.2f]]1[:N[.2f]]}\n", + iteration, measured, current, next); } int main() { using namespace mp_units::si::unit_symbols; - using state = kalman::state, quantity>; + using qp = quantity_point; + using state = kalman::system_state>; - const auto interval = isq::duration(5 * s); - const state initial = {30 * km, 50 * m / s}; - const quantity measurements[] = {30'160 * m, 30'365 * m, 30'890 * m, 31'050 * m, - 31'785 * m, 32'215 * m, 33'130 * m, 34'510 * m, - 36'010 * m, 37'265 * m}; + const quantity interval = isq::duration(5 * s); + const state initial{qp{30 * km}, quantity_point{50 * m / s}}; + const std::array measurements = {qp{30'221 * m}, qp{30'453 * m}, qp{30'906 * m}, qp{30'999 * m}, qp{31'368 * m}, + qp{31'978 * m}, qp{32'526 * m}, qp{33'379 * m}, qp{34'698 * m}, qp{36'275 * m}}; std::array gain = {0.2 * one, 0.1 * one}; print_header(initial); state next = state_extrapolation(initial, interval); - for (int index = 1; const auto& measured : measurements) { - const auto& previous = next; - const auto current = state_update(previous, measured, gain, interval); + for (int index = 1; const auto& m : measurements) { + const state& previous = next; + const state current = state_update(previous, m, gain, interval); next = state_extrapolation(current, interval); - print(index++, measured, current, next); + print(index++, m, current, next); } } diff --git a/example/kalman_filter/kalman_filter-example_4.cpp b/example/kalman_filter/kalman_filter-example_4.cpp index 00696c02..99492954 100644 --- a/example/kalman_filter/kalman_filter-example_4.cpp +++ b/example/kalman_filter/kalman_filter-example_4.cpp @@ -39,37 +39,40 @@ inline constexpr bool mp_units::is_vector = true; using namespace mp_units; -void print_header(const kalman::State auto& initial) +void print_header(const kalman::SystemState auto& initial) { std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial); std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>8} | {:>35} | {:>35}\n", "N", "Measured", "Curr. Estimate", "Next Estimate"); } -void print(auto iteration, Quantity auto measured, const kalman::State auto& current, const kalman::State auto& next) +void print(auto iteration, QuantityPoint auto measured, const kalman::SystemState auto& current, + const kalman::SystemState auto& next) { - std::cout << MP_UNITS_STD_FMT::format("{:2} | {:8} | {:>35.1} | {:>35.1}\n", iteration, measured, current, next); + std::cout << MP_UNITS_STD_FMT::format( + "{:2} | {:8} | {:35:0[:N[.2f]]1[:N[.2f]]2[:N[.2f]]} | {:35:0[:N[.2f]]1[:N[.2f]]2[:N[.2f]]}\n", iteration, measured, + current, next); } int main() { using namespace mp_units::si::unit_symbols; - using state = kalman::state, quantity, - quantity>; - const auto interval = isq::duration(5. * s); - const state initial = {30 * km, 50 * m / s, 0 * m / s2}; + using qp = quantity_point; + using state = + kalman::system_state, quantity_point>; - const quantity measurements[] = {30'160 * m, 30'365 * m, 30'890 * m, 31'050 * m, - 31'785 * m, 32'215 * m, 33'130 * m, 34'510 * m, - 36'010 * m, 37'265 * m}; + const quantity interval = isq::duration(5. * s); + const state initial{qp{30 * km}, quantity_point{50 * m / s}, quantity_point{0 * m / s2}}; + const std::array measurements = {qp{30'221 * m}, qp{30'453 * m}, qp{30'906 * m}, qp{30'999 * m}, qp{31'368 * m}, + qp{31'978 * m}, qp{32'526 * m}, qp{33'379 * m}, qp{34'698 * m}, qp{36'275 * m}}; std::array gain = {0.5 * one, 0.4 * one, 0.1 * one}; print_header(initial); state next = state_extrapolation(initial, interval); - for (int index = 1; const auto& measured : measurements) { - const auto& previous = next; - const auto current = state_update(previous, measured, gain, interval); + for (int index = 1; const auto& m : measurements) { + const state& previous = next; + const state current = state_update(previous, m, gain, interval); next = state_extrapolation(current, interval); - print(index++, measured, current, next); + print(index++, m, current, next); } } diff --git a/example/kalman_filter/kalman_filter-example_5.cpp b/example/kalman_filter/kalman_filter-example_5.cpp index b4be7e3c..861e6596 100644 --- a/example/kalman_filter/kalman_filter-example_5.cpp +++ b/example/kalman_filter/kalman_filter-example_5.cpp @@ -36,45 +36,45 @@ import mp_units; using namespace mp_units; -template -void print_header(kalman::estimation initial) +template +void print_header(kalman::system_state_estimate initial) { - std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial); - std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>5} | {:>8} | {:>16} | {:>16}\n", "N", "Gain", "Measured", + std::cout << MP_UNITS_STD_FMT::format("Initial: {} {}\n", initial.state(), initial.variance()); + std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>8} | {:>5} | {:>16} | {:>16}\n", "N", "Measured", "Gain", "Curr. Estimate", "Next Estimate"); } -template K> -void print(auto iteration, K gain, Q measured, kalman::estimation current, kalman::estimation next) +template K> +void print(auto iteration, QP measured, K gain, kalman::system_state_estimate current, + kalman::system_state_estimate next) { - std::cout << MP_UNITS_STD_FMT::format("{:2} | {:5%.2Q} | {:8} | {:>16.2} | {:>16.2}\n", iteration, gain, measured, - current, next); + std::cout << MP_UNITS_STD_FMT::format( + "{:2} | {:8} | {:5:N[.2f]} | {:6:0[:N[.2f]]} {:8:N[.2f]} | {:6:0[:N[.2f]]} {:8:N[.2f]}\n", iteration, measured, + gain, current.state(), current.variance(), next.state(), next.variance()); } int main() { - using namespace kalman; using namespace mp_units::si::unit_symbols; + using qp = quantity_point; + using estimate = kalman::system_state_estimate; + using state = estimate::state_type; - const estimation initial = {state{isq::height(60. * m)}, pow<2>(isq::height(15. * m))}; - const quantity measurements[] = {48.54 * m, 47.11 * m, 55.01 * m, 55.15 * m, 49.89 * m, - 40.85 * m, 46.72 * m, 50.05 * m, 51.27 * m, 49.95 * m}; - const auto measurement_uncertainty = pow<2>(isq::height(5. * m)); + const estimate initial{state{qp{60. * m}}, 15. * m}; + const std::array measurements = {qp{49.03 * m}, qp{48.44 * m}, qp{55.21 * m}, qp{49.98 * m}, qp{50.6 * m}, + qp{52.61 * m}, qp{45.87 * m}, qp{42.64 * m}, qp{48.26 * m}, qp{55.84 * m}}; + const quantity measurement_error = isq::height(5. * m); + const quantity measurement_variance = pow<2>(measurement_error); - auto update = [=](const estimation& previous, const Q& measurement, - QuantityOf auto gain) { - return estimation{state_update(previous.state, measurement, gain), covariance_update(previous.uncertainty, gain)}; - }; - - auto predict = [](const estimation& current) { return current; }; + auto predict = [](const estimate& current) { return current; }; print_header(initial); - estimation next = predict(initial); - for (int index = 1; const auto& measured : measurements) { - const auto& previous = next; - const auto gain = kalman_gain(previous.uncertainty, measurement_uncertainty); - const estimation current = update(previous, measured, gain); + estimate next = predict(initial); + for (int index = 1; const auto& m : measurements) { + const estimate& previous = next; + const quantity gain = kalman::kalman_gain(previous.variance(), measurement_variance); + const estimate current = state_estimate_update(previous, m, gain); next = predict(current); - print(index++, gain, measured, current, next); + print(index++, m, gain, current, next); } } diff --git a/example/kalman_filter/kalman_filter-example_6.cpp b/example/kalman_filter/kalman_filter-example_6.cpp index a6ab2131..1881d55e 100644 --- a/example/kalman_filter/kalman_filter-example_6.cpp +++ b/example/kalman_filter/kalman_filter-example_6.cpp @@ -27,62 +27,59 @@ import mp_units; #else #include +#include #include -#include #include -#include +#include #endif -// Based on: https://www.kalmanfilter.net/kalman1d.html#ex6 +// Based on: https://www.kalmanfilter.net/kalman1d_pn.html#ex6 using namespace mp_units; template -void print_header(kalman::estimation initial) +void print_header(kalman::system_state_estimate initial) { - std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial); - std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>7} | {:>10} | {:>18} | {:>18}\n", "N", "Gain", "Measured", + std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial.state(), initial.variance()); + std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>10} | {:>7} | {:>22} | {:>22}\n", "N", "Measured", "Gain", "Curr. Estimate", "Next Estimate"); } template K> -void print(auto iteration, K gain, QP measured, kalman::estimation current, kalman::estimation next) +void print(auto iteration, QP measured, K gain, kalman::system_state_estimate current, + kalman::system_state_estimate next) { - std::cout << MP_UNITS_STD_FMT::format("{:2} | {:7%.4Q} | {:10%.3Q %q} | {:>18.3} | {:>18.3}\n", iteration, gain, - measured.quantity_ref_from(QP::point_origin), current, next); + std::cout << MP_UNITS_STD_FMT::format( + "{:2} | {:10} | {:7:N[.4f]} | {:10:0[:N[.3f]]} {:11:N[.4f]} | {:10:0[:N[.3f]]} {:11:N[.4f]}\n", iteration, measured, + gain, current.state(), current.variance(), next.state(), next.variance()); } int main() { - constexpr auto deg_C = isq::Celsius_temperature[si::degree_Celsius]; + using namespace mp_units::si::unit_symbols; + using qp = quantity_point; + using estimate = kalman::system_state_estimate; + using state = estimate::state_type; - using namespace kalman; + const quantity process_noise_variance = 0.0001 * pow<2>(deg_C); + const estimate initial{state{qp{60. * deg_C}}, 100. * deg_C}; + const std::array measurements = {qp{49.986 * deg_C}, qp{49.963 * deg_C}, qp{50.09 * deg_C}, qp{50.001 * deg_C}, + qp{50.018 * deg_C}, qp{50.05 * deg_C}, qp{49.938 * deg_C}, qp{49.858 * deg_C}, + qp{49.965 * deg_C}, qp{50.114 * deg_C}}; + const quantity measurement_error = 0.1 * deg_C; + const quantity measurement_variance = pow<2>(measurement_error); - const auto process_noise_variance = 0.0001 * (deg_C * deg_C); - const estimation initial = {state{si::ice_point + 10. * deg_C}, pow<2>(100. * deg_C)}; - const std::array measurements = {si::ice_point + 49.95 * deg_C, si::ice_point + 49.967 * deg_C, - si::ice_point + 50.1 * deg_C, si::ice_point + 50.106 * deg_C, - si::ice_point + 49.992 * deg_C, si::ice_point + 49.819 * deg_C, - si::ice_point + 49.933 * deg_C, si::ice_point + 50.007 * deg_C, - si::ice_point + 50.023 * deg_C, si::ice_point + 49.99 * deg_C}; - const auto measurement_uncertainty = pow<2>(0.1 * deg_C); - - auto update = [=](const estimation& previous, const QP& meassurement, - QuantityOf auto gain) { - return estimation{state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, gain)}; - }; - - auto predict = [=](const estimation& current) { - return estimation{current.state, covariance_extrapolation(current.uncertainty, process_noise_variance)}; + auto predict = [=](const estimate& current) { + return estimate{current.state(), kalman::covariance_extrapolation(current.variance(), process_noise_variance)}; }; print_header(initial); - estimation next = predict(initial); + estimate next = predict(initial); for (int index = 1; const auto& m : measurements) { - const auto& previous = next; - const auto gain = kalman_gain(previous.uncertainty, measurement_uncertainty); - const estimation current = update(previous, m, gain); + const estimate& previous = next; + const quantity gain = kalman::kalman_gain(previous.variance(), measurement_variance); + const estimate current = state_estimate_update(previous, m, gain); next = predict(current); - print(index++, gain, m, current, next); + print(index++, m, gain, current, next); } } diff --git a/example/kalman_filter/kalman_filter-example_7.cpp b/example/kalman_filter/kalman_filter-example_7.cpp index 49c832fe..f7ab2450 100644 --- a/example/kalman_filter/kalman_filter-example_7.cpp +++ b/example/kalman_filter/kalman_filter-example_7.cpp @@ -27,62 +27,59 @@ import mp_units; #else #include +#include #include -#include #include -#include +#include #endif -// Based on: https://www.kalmanfilter.net/kalman1d.html#ex7 +// Based on: https://www.kalmanfilter.net/kalman1d_pn.html#ex7 using namespace mp_units; template -void print_header(kalman::estimation initial) +void print_header(kalman::system_state_estimate initial) { - std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial); - std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>7} | {:>10} | {:>18} | {:>18}\n", "N", "Gain", "Measured", + std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial.state(), initial.variance()); + std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>10} | {:>7} | {:>22} | {:>22}\n", "N", "Measured", "Gain", "Curr. Estimate", "Next Estimate"); } template K> -void print(auto iteration, K gain, QP measured, kalman::estimation current, kalman::estimation next) +void print(auto iteration, QP measured, K gain, kalman::system_state_estimate current, + kalman::system_state_estimate next) { - std::cout << MP_UNITS_STD_FMT::format("{:2} | {:7%.4Q} | {:10%.3Q %q} | {:>18.3} | {:>18.3}\n", iteration, gain, - measured.quantity_ref_from(QP::point_origin), current, next); + std::cout << MP_UNITS_STD_FMT::format( + "{:2} | {:10} | {:7:N[.4f]} | {:10:0[:N[.3f]]} {:11:N[.4f]} | {:10:0[:N[.3f]]} {:11:N[.4f]}\n", iteration, measured, + gain, current.state(), current.variance(), next.state(), next.variance()); } int main() { - constexpr auto deg_C = isq::Celsius_temperature[si::degree_Celsius]; + using namespace mp_units::si::unit_symbols; + using qp = quantity_point; + using estimate = kalman::system_state_estimate; + using state = estimate::state_type; - using namespace kalman; + const quantity process_noise_variance = 0.0001 * pow<2>(deg_C); + const estimate initial{state{qp{10. * deg_C}}, 100. * deg_C}; + const std::array measurements = {qp{50.486 * deg_C}, qp{50.963 * deg_C}, qp{51.597 * deg_C}, qp{52.001 * deg_C}, + qp{52.518 * deg_C}, qp{53.05 * deg_C}, qp{53.438 * deg_C}, qp{53.858 * deg_C}, + qp{54.465 * deg_C}, qp{55.114 * deg_C}}; + const quantity measurement_error = 0.1 * deg_C; + const quantity measurement_variance = pow<2>(measurement_error); - const auto process_noise_variance = 0.0001 * (deg_C * deg_C); - const estimation initial = {state{si::ice_point + 10. * deg_C}, pow<2>(100. * deg_C)}; - const std::array measurements = {si::ice_point + 50.45 * deg_C, si::ice_point + 50.967 * deg_C, - si::ice_point + 51.6 * deg_C, si::ice_point + 52.106 * deg_C, - si::ice_point + 52.492 * deg_C, si::ice_point + 52.819 * deg_C, - si::ice_point + 53.433 * deg_C, si::ice_point + 54.007 * deg_C, - si::ice_point + 54.523 * deg_C, si::ice_point + 54.99 * deg_C}; - const auto measurement_uncertainty = pow<2>(0.1 * deg_C); - - auto update = [=](const estimation& previous, const QP& meassurement, - QuantityOf auto gain) { - return estimation{state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, gain)}; - }; - - auto predict = [=](const estimation& current) { - return estimation{current.state, covariance_extrapolation(current.uncertainty, process_noise_variance)}; + auto predict = [=](const estimate& current) { + return estimate{current.state(), kalman::covariance_extrapolation(current.variance(), process_noise_variance)}; }; print_header(initial); - estimation next = predict(initial); + estimate next = predict(initial); for (int index = 1; const auto& m : measurements) { - const auto& previous = next; - const auto gain = kalman_gain(previous.uncertainty, measurement_uncertainty); - const estimation current = update(previous, m, gain); + const estimate& previous = next; + const quantity gain = kalman::kalman_gain(previous.variance(), measurement_variance); + const estimate current = state_estimate_update(previous, m, gain); next = predict(current); - print(index++, gain, m, current, next); + print(index++, m, gain, current, next); } } diff --git a/example/kalman_filter/kalman_filter-example_8.cpp b/example/kalman_filter/kalman_filter-example_8.cpp index 474ecff7..e7cedfdb 100644 --- a/example/kalman_filter/kalman_filter-example_8.cpp +++ b/example/kalman_filter/kalman_filter-example_8.cpp @@ -27,62 +27,59 @@ import mp_units; #else #include +#include #include -#include #include -#include +#include #endif -// Based on: https://www.kalmanfilter.net/kalman1d.html#ex8 +// Based on: https://www.kalmanfilter.net/kalman1d_pn.html#ex8 using namespace mp_units; template -void print_header(kalman::estimation initial) +void print_header(kalman::system_state_estimate initial) { - std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial); - std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>7} | {:>10} | {:>16} | {:>16}\n", "N", "Gain", "Measured", + std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial.state(), initial.variance()); + std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>10} | {:>7} | {:>22} | {:>22}\n", "N", "Measured", "Gain", "Curr. Estimate", "Next Estimate"); } template K> -void print(auto iteration, K gain, QP measured, kalman::estimation current, kalman::estimation next) +void print(auto iteration, QP measured, K gain, kalman::system_state_estimate current, + kalman::system_state_estimate next) { - std::cout << MP_UNITS_STD_FMT::format("{:2} | {:7%.3Q} | {:10%.3Q %q} | {:>16.2} | {:>16.2}\n", iteration, gain, - measured.quantity_ref_from(QP::point_origin), current, next); + std::cout << MP_UNITS_STD_FMT::format( + "{:2} | {:10} | {:7:N[.4f]} | {:10:0[:N[.3f]]} {:11:N[.4f]} | {:10:0[:N[.3f]]} {:11:N[.4f]}\n", iteration, measured, + gain, current.state(), current.variance(), next.state(), next.variance()); } int main() { - constexpr auto deg_C = isq::Celsius_temperature[si::degree_Celsius]; + using namespace mp_units::si::unit_symbols; + using qp = quantity_point; + using estimate = kalman::system_state_estimate; + using state = estimate::state_type; - using namespace kalman; + const quantity process_noise_variance = 0.15 * pow<2>(deg_C); + const estimate initial{state{qp{10. * deg_C}}, 100. * deg_C}; + const std::array measurements = {qp{50.486 * deg_C}, qp{50.963 * deg_C}, qp{51.597 * deg_C}, qp{52.001 * deg_C}, + qp{52.518 * deg_C}, qp{53.05 * deg_C}, qp{53.438 * deg_C}, qp{53.858 * deg_C}, + qp{54.465 * deg_C}, qp{55.114 * deg_C}}; + const quantity measurement_error = 0.1 * deg_C; + const quantity measurement_variance = pow<2>(measurement_error); - const auto process_noise_variance = 0.15 * (deg_C * deg_C); - const estimation initial = {state{si::ice_point + 10. * deg_C}, pow<2>(100. * deg_C)}; - const std::array measurements = {si::ice_point + 50.45 * deg_C, si::ice_point + 50.967 * deg_C, - si::ice_point + 51.6 * deg_C, si::ice_point + 52.106 * deg_C, - si::ice_point + 52.492 * deg_C, si::ice_point + 52.819 * deg_C, - si::ice_point + 53.433 * deg_C, si::ice_point + 54.007 * deg_C, - si::ice_point + 54.523 * deg_C, si::ice_point + 54.99 * deg_C}; - const auto measurement_uncertainty = pow<2>(0.1 * deg_C); - - auto update = [=](const estimation& previous, const QP& meassurement, - QuantityOf auto gain) { - return estimation{state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, gain)}; - }; - - auto predict = [=](const estimation& current) { - return estimation{current.state, covariance_extrapolation(current.uncertainty, process_noise_variance)}; + auto predict = [=](const estimate& current) { + return estimate{current.state(), kalman::covariance_extrapolation(current.variance(), process_noise_variance)}; }; print_header(initial); - estimation next = predict(initial); + estimate next = predict(initial); for (int index = 1; const auto& m : measurements) { - const auto& previous = next; - const auto gain = kalman_gain(previous.uncertainty, measurement_uncertainty); - const estimation current = update(previous, m, gain); + const estimate& previous = next; + const quantity gain = kalman::kalman_gain(previous.variance(), measurement_variance); + const estimate current = state_estimate_update(previous, m, gain); next = predict(current); - print(index++, gain, m, current, next); + print(index++, m, gain, current, next); } } diff --git a/src/core/include/mp-units/bits/fmt.h b/src/core/include/mp-units/bits/fmt.h index 16d98363..3e59cc03 100644 --- a/src/core/include/mp-units/bits/fmt.h +++ b/src/core/include/mp-units/bits/fmt.h @@ -30,6 +30,7 @@ // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic, cppcoreguidelines-pro-type-union-access) #pragma once +#include #include #include @@ -45,6 +46,9 @@ namespace mp_units::detail { +// TODO the below should be exposed by the C++ Standard Library (used in our examples) +MP_UNITS_EXPORT_BEGIN + enum class fmt_align : std::int8_t { none, left, right, center, numeric }; enum class fmt_arg_id_kind : std::int8_t { none, @@ -109,6 +113,8 @@ public: [[nodiscard]] constexpr const Char& operator[](size_t index) const { return data_[index]; } }; +MP_UNITS_EXPORT_END + template inline constexpr bool is_integer = std::is_integral_v && !std::is_same_v && !std::is_same_v && !std::is_same_v; @@ -135,20 +141,6 @@ template return static_cast>(value); } -struct width_checker { - template - [[nodiscard]] constexpr unsigned long long operator()(T value) const - { - if constexpr (is_integer) { - if constexpr (std::numeric_limits::is_signed) - if (value < 0) MP_UNITS_THROW(MP_UNITS_STD_FMT::format_error("negative width")); - return static_cast(value); - } - MP_UNITS_THROW(MP_UNITS_STD_FMT::format_error("width is not integer")); - return 0; - } -}; - template [[nodiscard]] constexpr int get_dynamic_spec(FormatArg arg) { @@ -167,6 +159,9 @@ template return arg; } +// TODO the below should be exposed by the C++ Standard Library (used in our examples) +MP_UNITS_EXPORT_BEGIN + template constexpr void handle_dynamic_spec(int& value, fmt_arg_ref ref, Context& ctx) { @@ -184,6 +179,22 @@ constexpr void handle_dynamic_spec(int& value, fmt_arg_ref + [[nodiscard]] constexpr unsigned long long operator()(T value) const + { + if constexpr (is_integer) { + if constexpr (std::numeric_limits::is_signed) + if (value < 0) MP_UNITS_THROW(MP_UNITS_STD_FMT::format_error("negative width")); + return static_cast(value); + } + MP_UNITS_THROW(MP_UNITS_STD_FMT::format_error("width is not integer")); + return 0; + } +}; + +MP_UNITS_EXPORT_END + // Parses the range [begin, end) as an unsigned integer. This function assumes // that the range is non-empty and the first character is a digit. template diff --git a/src/core/include/mp-units/format.h b/src/core/include/mp-units/format.h index 544ce69c..88616c1f 100644 --- a/src/core/include/mp-units/format.h +++ b/src/core/include/mp-units/format.h @@ -25,6 +25,7 @@ #pragma once #include +#include #include #include #include @@ -33,14 +34,6 @@ namespace mp_units::detail { -template -struct fill_align_width_format_specs { - fill_t fill; - fmt_align align : 4 = fmt_align::none; - int width = 0; - fmt_arg_ref width_ref; -}; - template [[nodiscard]] constexpr const Char* at_most_one_of(const Char* begin, const Char* end, std::string_view modifiers) { @@ -51,6 +44,17 @@ template return it; } +// TODO the below should be exposed by the C++ Standard Library (used in our examples) +MP_UNITS_EXPORT_BEGIN + +template +struct fill_align_width_format_specs { + fill_t fill; + fmt_align align : 4 = fmt_align::none; + int width = 0; + fmt_arg_ref width_ref; +}; + template [[nodiscard]] constexpr const Char* parse_fill_align_width(MP_UNITS_STD_FMT::basic_format_parse_context& ctx, const Char* begin, const Char* end, Specs& specs, @@ -89,6 +93,8 @@ OutputIt format_global_buffer(OutputIt out, const fill_align_width_format_specs< return MP_UNITS_STD_FMT::format_to(out, "}}"); } +MP_UNITS_EXPORT_END + } // namespace mp_units::detail //