mpusz/mp-units
1
0
Fork 1
mirror of https://github.com/mpusz/mp-units.git synced 2025-08-02 11:54:27 +02:00
Code Issues Packages Projects Releases Wiki Activity

feat(example): kalman filter examples enabled after text formatting refactoring

Browse Source
This commit is contained in:
Mateusz Pusz
2024-05-29 20:25:21 +02:00
parent da0ab13b0d
commit 1cb86a2271
12 changed files with 429 additions and 343 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
example/CMakeLists.txt
View File

@@ -72,4 +72,4 @@ add_example(total_energy)
add_example(unmanned_aerial_vehicle example_utils) add_example(unmanned_aerial_vehicle example_utils)
add_subdirectory(glide_computer_lib) add_subdirectory(glide_computer_lib)
# add_subdirectory(kalman_filter) add_subdirectory(kalman_filter)

354
example/kalman_filter/kalman.h
View File

@@ -23,21 +23,22 @@
#pragma once #pragma once
#include <mp-units/compat_macros.h> #include <mp-units/compat_macros.h>
#include <algorithm>
#include <tuple> #include <tuple>
#ifdef MP_UNITS_MODULES #ifdef MP_UNITS_MODULES
import mp_units; import mp_units;
#else #else
#include <mp-units/ext/algorithm.h>
#include <mp-units/format.h> #include <mp-units/format.h>
#include <mp-units/framework/quantity.h>
#include <mp-units/framework/quantity_point.h>
#include <mp-units/math.h> #include <mp-units/math.h>
#include <mp-units/quantity.h> #include <mp-units/systems/isq/base_quantities.h>
#include <mp-units/quantity_point.h>
#include <mp-units/systems/isq/space_and_time.h>
#endif #endif
namespace kalman { namespace kalman {
template<typename T> namespace detail {
concept QuantityOrQuantityPoint = mp_units::Quantity<T> || mp_units::QuantityPoint<T>;
template<mp_units::Dimension auto... Ds> template<mp_units::Dimension auto... Ds>
inline constexpr bool are_time_derivatives = false; inline constexpr bool are_time_derivatives = false;
@@ -49,192 +50,261 @@ template<mp_units::Dimension auto D1, mp_units::Dimension auto D2, mp_units::Dim
inline constexpr bool are_time_derivatives<D1, D2, Ds...> = inline constexpr bool are_time_derivatives<D1, D2, Ds...> =
(D1 / D2 == mp_units::isq::dim_time) && are_time_derivatives<D2, Ds...>; (D1 / D2 == mp_units::isq::dim_time) && are_time_derivatives<D2, Ds...>;
// state } // namespace detail
template<QuantityOrQuantityPoint... QQPs>
requires(sizeof...(QQPs) > 0) && (sizeof...(QQPs) <= 3) && are_time_derivatives<QQPs::dimension...> // system state
struct state { template<mp_units::QuantityPoint... QPs>
std::tuple<QQPs...> variables_; requires(sizeof...(QPs) > 0) && (sizeof...(QPs) <= 3) && detail::are_time_derivatives<QPs::dimension...>
constexpr state(QQPs... qqps) : variables_(std::move(qqps)...) {} class system_state {
std::tuple<QPs...> variables_;
public:
constexpr explicit system_state(QPs... qps) : variables_(std::move(qps)...) {}
template<std::size_t Idx>
[[nodiscard]] friend constexpr auto& get(system_state<QPs...>& s)
{
return get<Idx>(s.variables_);
}
template<std::size_t Idx>
[[nodiscard]] friend constexpr const auto& get(const system_state<QPs...>& s)
{
return get<Idx>(s.variables_);
}
}; };
template<typename T> template<typename T>
concept State = mp_units::is_specialization_of<T, state>; concept SystemState = mp_units::is_specialization_of<T, system_state>;
template<std::size_t Idx, typename... Qs>
constexpr auto& get(state<Qs...>& s)
{
return get<Idx>(s.variables_);
}
template<std::size_t Idx, typename... Qs> // system state estimation
constexpr const auto& get(const state<Qs...>& s) template<mp_units::QuantityPoint QP, mp_units::QuantityPoint... Rest>
{ requires requires { typename system_state<QP, Rest...>; }
return get<Idx>(s.variables_); class system_state_estimate {
}
// estimation
template<QuantityOrQuantityPoint QQP, QuantityOrQuantityPoint... QQPs>
struct estimation {
private:
static constexpr auto uncertainty_ref = QQP::reference * QQP::reference;
using uncertainty_type = mp_units::quantity<uncertainty_ref, typename QQP::rep>;
public: public:
kalman::state<QQP, QQPs...> state; // TODO extend kalman functions to work with this variadic parameter list using state_type = system_state<QP, Rest...>;
uncertainty_type uncertainty; using standard_deviation_type = QP::quantity_type;
using variance_type =
mp_units::quantity<pow<2>(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<QuantityOrQuantityPoint QQP, mp_units::Quantity U>
estimation(state<QQP>, U) -> estimation<QQP>;
// kalman gain // kalman gain
template<mp_units::Quantity Q> template<mp_units::Quantity Q1, mp_units::Quantity Q2>
constexpr mp_units::quantity<mp_units::dimensionless[mp_units::one]> kalman_gain(Q estimate_uncertainty, requires requires { mp_units::common_reference(Q1::reference, Q2::reference); }
Q measurement_uncertainty) [[nodiscard]] constexpr mp_units::quantity<mp_units::dimensionless[mp_units::one]> 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 // state update
template<typename Q, QuantityOrQuantityPoint QM, mp_units::QuantityOf<mp_units::dimensionless> K> template<typename QP, mp_units::QuantityPoint QM, mp_units::QuantityOf<mp_units::dimensionless> K>
requires(implicitly_convertible(QM::quantity_spec, Q::quantity_spec)) requires(implicitly_convertible(QM::quantity_spec, QP::quantity_spec))
constexpr state<Q> state_update(const state<Q>& predicted, QM measured, K gain) [[nodiscard]] constexpr system_state<QP> state_update(const system_state<QP>& predicted, QM measured, K gain)
{ {
return {get<0>(predicted) + gain * (measured - get<0>(predicted))}; return system_state<QP>{get<0>(predicted) + gain * (measured - get<0>(predicted))};
} }
template<typename Q1, typename Q2, QuantityOrQuantityPoint QM, mp_units::QuantityOf<mp_units::dimensionless> K, template<typename QP1, typename QP2, mp_units::QuantityPoint QM, mp_units::QuantityOf<mp_units::dimensionless> K,
mp_units::QuantityOf<mp_units::isq::time> T> mp_units::QuantityOf<mp_units::isq::time> T>
requires(implicitly_convertible(QM::quantity_spec, Q1::quantity_spec)) requires(implicitly_convertible(QM::quantity_spec, QP1::quantity_spec))
constexpr state<Q1, Q2> state_update(const state<Q1, Q2>& predicted, QM measured, std::array<K, 2> gain, T interval) [[nodiscard]] constexpr system_state<QP1, QP2> state_update(const system_state<QP1, QP2>& predicted, QM measured,
std::array<K, 2> gain, T interval)
{ {
const auto q1 = get<0>(predicted) + get<0>(gain) * (measured - get<0>(predicted)); const auto qp1 = fma(get<0>(gain), measured - get<0>(predicted), get<0>(predicted));
const auto q2 = get<1>(predicted) + get<1>(gain) * (measured - get<0>(predicted)) / interval; const auto qp2 = fma(get<1>(gain), (measured - get<0>(predicted)) / interval, get<1>(predicted));
return {q1, q2}; return system_state<QP1, QP2>{qp1, qp2};
} }
template<typename Q1, typename Q2, typename Q3, QuantityOrQuantityPoint QM, template<typename QP1, typename QP2, typename QP3, mp_units::QuantityPoint QM,
mp_units::QuantityOf<mp_units::dimensionless> K, mp_units::QuantityOf<mp_units::isq::time> T> mp_units::QuantityOf<mp_units::dimensionless> K, mp_units::QuantityOf<mp_units::isq::time> T>
requires(implicitly_convertible(QM::quantity_spec, Q1::quantity_spec)) requires(implicitly_convertible(QM::quantity_spec, QP1::quantity_spec))
constexpr state<Q1, Q2, Q3> state_update(const state<Q1, Q2, Q3>& predicted, QM measured, std::array<K, 3> gain, [[nodiscard]] constexpr system_state<QP1, QP2, QP3> state_update(const system_state<QP1, QP2, QP3>& predicted,
T interval) QM measured, std::array<K, 3> gain, T interval)
{ {
const auto q1 = get<0>(predicted) + get<0>(gain) * (measured - get<0>(predicted)); const auto qp1 = fma(get<0>(gain), measured - get<0>(predicted), get<0>(predicted));
const auto q2 = get<1>(predicted) + get<1>(gain) * (measured - get<0>(predicted)) / interval; const auto qp2 = fma(get<1>(gain), (measured - get<0>(predicted)) / interval, get<1>(predicted));
const auto q3 = get<2>(predicted) + get<2>(gain) * (measured - get<0>(predicted)) / (interval * interval / 2); const auto qp3 = fma(get<2>(gain), (measured - get<0>(predicted)) / (interval * interval / 2), get<2>(predicted));
return {q1, q2, q3}; return system_state<QP1, QP2, QP3>{qp1, qp2, qp3};
} }
// covariance update // covariance update
template<mp_units::Quantity Q, mp_units::QuantityOf<mp_units::dimensionless> K> template<mp_units::Quantity Q, mp_units::QuantityOf<mp_units::dimensionless> 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; return (1 * mp_units::one - gain) * uncertainty;
} }
// state extrapolation template<mp_units::QuantityPoint... QPs, mp_units::QuantityPoint QP, mp_units::QuantityOf<mp_units::dimensionless> K>
template<typename Q1, typename Q2, mp_units::QuantityOf<mp_units::isq::time> T> [[nodiscard]] constexpr system_state_estimate<QPs...> state_estimate_update(
constexpr state<Q1, Q2> state_extrapolation(const state<Q1, Q2>& estimated, T interval) const system_state_estimate<QPs...>& previous, QP measurement, K gain)
{ {
const auto q1 = get<0>(estimated) + get<1>(estimated) * interval; return {state_update(previous.state(), measurement, gain), covariance_update(previous.variance(), gain)};
const auto q2 = get<1>(estimated); };
return {q1, q2};
// state extrapolation
template<typename QP1, typename QP2, mp_units::QuantityOf<mp_units::isq::time> T>
[[nodiscard]] constexpr system_state<QP1, QP2> state_extrapolation(const system_state<QP1, QP2>& 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>{qp1, qp2};
} }
template<typename Q1, typename Q2, typename Q3, mp_units::QuantityOf<mp_units::isq::time> T> template<typename QP1, typename QP2, typename QP3, mp_units::QuantityOf<mp_units::isq::time> T>
constexpr state<Q1, Q2, Q3> state_extrapolation(const state<Q1, Q2, Q3>& estimated, T interval) [[nodiscard]] constexpr system_state<QP1, QP2, QP3> state_extrapolation(const system_state<QP1, QP2, QP3>& estimated,
T interval)
{ {
const auto q1 = get<0>(estimated) + get<1>(estimated) * interval + get<2>(estimated) * pow<2>(interval) / 2; auto to_quantity = [](const auto& qp) { return qp.quantity_ref_from(qp.point_origin); };
const auto q2 = get<1>(estimated) + get<2>(estimated) * interval; const auto qp1 = to_quantity(get<2>(estimated)) * pow<2>(interval) / 2 +
const auto q3 = get<2>(estimated); fma(to_quantity(get<1>(estimated)), interval, get<0>(estimated));
return {q1, q2, q3}; 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>{qp1, qp2, qp3};
} }
// covariance extrapolation // covariance extrapolation
template<mp_units::Quantity Q> template<mp_units::Quantity Q1, mp_units::Quantity Q2>
constexpr Q covariance_extrapolation(Q uncertainty, Q process_noise_variance) 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; return uncertainty + process_noise_variance;
} }
} // namespace kalman } // namespace kalman
template<typename... Qs> template<auto R, auto PO, typename Rep, typename Char>
struct MP_UNITS_STD_FMT::formatter<kalman::state<Qs...>> { struct MP_UNITS_STD_FMT::formatter<mp_units::quantity_point<R, PO, Rep>, Char> :
constexpr auto parse(format_parse_context& ctx) MP_UNITS_STD_FMT::formatter<typename mp_units::quantity_point<R, PO, Rep>::quantity_type> {
template<typename FormatContext>
auto format(const mp_units::quantity_point<R, PO, Rep>& qp, FormatContext& ctx) const -> decltype(ctx.out())
{ {
mp_units::detail::dynamic_specs_handler handler(specs, ctx); return MP_UNITS_STD_FMT::formatter<typename mp_units::quantity_point<R, PO, Rep>::quantity_type>::format(
return mp_units::detail::parse_format_specs(ctx.begin(), ctx.end(), handler); qp.quantity_ref_from(qp.point_origin), ctx);
}
};
template<typename... QPs, typename Char>
class MP_UNITS_STD_FMT::formatter<kalman::system_state<QPs...>, Char> {
using format_specs = mp_units::detail::fill_align_width_format_specs<Char>;
format_specs specs_{};
std::array<std::basic_string<Char>, sizeof...(QPs)> format_str_;
std::tuple<MP_UNITS_STD_FMT::formatter<typename QPs::quantity_type, Char>...> formatters_{};
template<typename Formatter>
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<Char> 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<std::size_t... Is>
[[nodiscard]] constexpr const Char* parse_default_spec(const Char* begin, const Char* end, size_t idx,
std::index_sequence<Is...>)
{
auto parse = [&](bool flag, auto& f, std::basic_string<Char>& str) {
return flag ? parse_default_spec(begin, end, f, str) : begin;
};
return std::max({parse(idx == Is, std::get<Is>(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<char>('0' + sizeof...(QPs)))
throw MP_UNITS_STD_FMT::format_error(std::string("unknown `subentity-id` token '") + c + "'");
const size_t idx = static_cast<size_t>(c - '0');
begin = parse_default_spec(begin, end, idx, std::index_sequence_for<QPs...>{});
} while (begin != end && *begin != '}');
return begin;
}
template<typename OutputIt, typename FormatContext, std::size_t Idx>
OutputIt format_system_state(OutputIt out, const kalman::system_state<QPs...>& s, FormatContext& ctx,
std::index_sequence<Idx>) const
{
const std::locale locale = MP_UNITS_FMT_LOCALE(ctx.locale());
auto f = [&](const std::basic_string<Char>& 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<Idx>(format_str_), get<Idx>(s));
}
template<typename OutputIt, typename FormatContext, std::size_t Idx, std::size_t... Rest>
OutputIt format_system_state(OutputIt out, const kalman::system_state<QPs...>& s, FormatContext& ctx,
std::index_sequence<Idx, Rest...>) const
{
const std::locale locale = MP_UNITS_FMT_LOCALE(ctx.locale());
auto f = [&](const std::basic_string<Char>& 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<Idx>(format_str_), get<Idx>(s)), ((*out++ = ' ', f(get<Rest>(format_str_), get<Rest>(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<typename FormatContext> template<typename FormatContext>
auto format(const kalman::state<Qs...>& s, FormatContext& ctx) auto format(const kalman::system_state<QPs...>& s, FormatContext& ctx) const -> decltype(ctx.out())
{ {
std::string value_buffer; auto specs = specs_;
auto to_value_buffer = std::back_inserter(value_buffer); mp_units::detail::handle_dynamic_spec<mp_units::detail::width_checker>(specs.width, specs.width_ref, ctx);
if (specs.precision != -1) {
if constexpr (sizeof...(Qs) == 1) if (specs.width == 0) {
MP_UNITS_STD_FMT::format_to(to_value_buffer, "{1:%.{0}Q %q}", specs.precision, kalman::get<0>(s)); // Avoid extra copying if width is not specified
else if constexpr (sizeof...(Qs) == 2) format_system_state(ctx.out(), s, ctx, std::index_sequence_for<QPs...>{});
MP_UNITS_STD_FMT::format_to(to_value_buffer, "{{ {1:%.{0}Q %q}, {2:%.{0}Q %q} }}", specs.precision, return ctx.out();
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));
} }
std::basic_string<Char> quantity_buffer;
format_system_state(std::back_inserter(quantity_buffer), s, ctx, std::index_sequence_for<QPs...>{});
std::string global_format_buffer; std::basic_string<Char> fill_align_width_format_str;
mp_units::detail::quantity_global_format_specs<char> global_specs = {specs.fill, specs.align, specs.width}; mp_units::detail::format_global_buffer(std::back_inserter(fill_align_width_format_str), specs);
mp_units::detail::format_global_buffer(std::back_inserter(global_format_buffer), global_specs); return MP_UNITS_STD_FMT::vformat_to(ctx.out(), fill_align_width_format_str,
MP_UNITS_STD_FMT::make_format_args(quantity_buffer));
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<char> specs;
};
template<typename Q>
struct MP_UNITS_STD_FMT::formatter<kalman::estimation<Q>> {
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<typename FormatContext>
auto format(kalman::estimation<Q> e, FormatContext& ctx)
{
mp_units::Quantity auto q = [](const Q& t) {
if constexpr (mp_units::Quantity<Q>)
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<char> 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<char> specs;
}; };

33
example/kalman_filter/kalman_filter-example_1.cpp
View File

@@ -35,36 +35,37 @@ import mp_units;
using namespace 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("Initial: {}\n", initial);
std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>9} | {:>8} | {:>14} | {:>14}\n", "N", "Gain", "Measured", std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>9} | {:>8} | {:>14} | {:>14}\n", "N", "Gain", "Measured",
"Curr. Estimate", "Next Estimate"); "Curr. Estimate", "Next Estimate");
} }
void print(auto iteration, QuantityOf<dimensionless> auto gain, Quantity auto measured, void print(auto iteration, QuantityOf<dimensionless> auto gain, QuantityPoint auto measured,
const kalman::State auto& current, const kalman::State auto& next) 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, std::cout << MP_UNITS_STD_FMT::format("{:2} | {:9:N[.2f]} | {:8} | {:14:0[:N[.2f]]} | {:14:0[:N[.2f]]}\n", iteration,
next); gain, measured, current, next);
} }
int main() int main()
{ {
using namespace mp_units::si::unit_symbols; using namespace mp_units::si::unit_symbols;
using state = kalman::state<quantity<isq::mass[g]>>; using state = kalman::system_state<quantity_point<isq::mass[g]>>;
using qp = quantity_point<isq::mass[g]>;
const state initial = {1 * kg}; const state initial_guess{qp{1 * kg}};
const std::array measurements = {1'030 * g, 989 * g, 1'017 * g, 1'009 * g, 1'013 * g, const std::array measurements = {qp{996 * g}, qp{994 * g}, qp{1021 * g}, qp{1000 * g}, qp{1002 * g},
979 * g, 1'008 * g, 1'042 * g, 1'012 * g, 1'011 * g}; qp{1010 * g}, qp{983 * g}, qp{971 * g}, qp{993 * g}, qp{1023 * g}};
print_header(initial); print_header(initial_guess);
state next = initial; state next = initial_guess;
for (int index = 1; const auto& v : measurements) { for (int index = 1; const auto& m : measurements) {
const auto& previous = next; const state& previous = next;
const auto gain = 1. / index * one; const quantity gain = 1. / index * one;
const auto current = state_update(previous, v, gain); const state current = state_update(previous, m, gain);
next = current; next = current;
print(index++, gain, v, current, next); print(index++, gain, m, current, next);
} }
} }

28
example/kalman_filter/kalman_filter-example_2.cpp
View File

@@ -39,36 +39,38 @@ inline constexpr bool mp_units::is_vector<T> = true;
using namespace 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("Initial: {}\n", initial);
std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>8} | {:>23} | {:>23}\n", "N", "Measured", "Curr. Estimate", std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>8} | {:>23} | {:>23}\n", "N", "Measured", "Curr. Estimate",
"Next 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() int main()
{ {
using namespace mp_units::si::unit_symbols; using namespace mp_units::si::unit_symbols;
using state = kalman::state<quantity<isq::position_vector[m]>, quantity<isq::velocity[m / s]>>; using qp = quantity_point<isq::position_vector[m]>;
using state = kalman::system_state<qp, quantity_point<isq::velocity[m / s]>>;
const auto interval = isq::duration(5 * s); const quantity interval = isq::duration(5 * s);
const state initial = {30 * km, 40 * m / s}; const state initial{qp{30 * km}, quantity_point{40 * m / s}};
const quantity<isq::position_vector[m], int> measurements[] = {30'110 * m, 30'265 * m, 30'740 * m, 30'750 * m, const std::array measurements = {qp{30'171 * m}, qp{30'353 * m}, qp{30'756 * m}, qp{30'799 * m}, qp{31'018 * m},
31'135 * m, 31'015 * m, 31'180 * m, 31'610 * m, qp{31'278 * m}, qp{31'276 * m}, qp{31'379 * m}, qp{31'748 * m}, qp{32'175 * m}};
31'960 * m, 31'865 * m};
std::array gain = {0.2 * one, 0.1 * one}; std::array gain = {0.2 * one, 0.1 * one};
print_header(initial); print_header(initial);
state next = state_extrapolation(initial, interval); state next = state_extrapolation(initial, interval);
for (int index = 1; const auto& measured : measurements) { for (int index = 1; const auto& m : measurements) {
const auto& previous = next; const state& previous = next;
const auto current = state_update(previous, measured, gain, interval); const state current = state_update(previous, m, gain, interval);
next = state_extrapolation(current, interval); next = state_extrapolation(current, interval);
print(index++, measured, current, next); print(index++, m, current, next);
} }
} }

30
example/kalman_filter/kalman_filter-example_3.cpp
View File

@@ -39,36 +39,38 @@ inline constexpr bool mp_units::is_vector<T> = true;
using namespace 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("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"); "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() int main()
{ {
using namespace mp_units::si::unit_symbols; using namespace mp_units::si::unit_symbols;
using state = kalman::state<quantity<isq::position_vector[m]>, quantity<isq::velocity[m / s]>>; using qp = quantity_point<isq::position_vector[m]>;
using state = kalman::system_state<qp, quantity_point<isq::velocity[m / s]>>;
const auto interval = isq::duration(5 * s); const quantity interval = isq::duration(5 * s);
const state initial = {30 * km, 50 * m / s}; const state initial{qp{30 * km}, quantity_point{50 * m / s}};
const quantity<isq::position_vector[m], int> measurements[] = {30'160 * m, 30'365 * m, 30'890 * m, 31'050 * m, const std::array measurements = {qp{30'221 * m}, qp{30'453 * m}, qp{30'906 * m}, qp{30'999 * m}, qp{31'368 * m},
31'785 * m, 32'215 * m, 33'130 * m, 34'510 * m, qp{31'978 * m}, qp{32'526 * m}, qp{33'379 * m}, qp{34'698 * m}, qp{36'275 * m}};
36'010 * m, 37'265 * m};
std::array gain = {0.2 * one, 0.1 * one}; std::array gain = {0.2 * one, 0.1 * one};
print_header(initial); print_header(initial);
state next = state_extrapolation(initial, interval); state next = state_extrapolation(initial, interval);
for (int index = 1; const auto& measured : measurements) { for (int index = 1; const auto& m : measurements) {
const auto& previous = next; const state& previous = next;
const auto current = state_update(previous, measured, gain, interval); const state current = state_update(previous, m, gain, interval);
next = state_extrapolation(current, interval); next = state_extrapolation(current, interval);
print(index++, measured, current, next); print(index++, m, current, next);
} }
} }

31
example/kalman_filter/kalman_filter-example_4.cpp
View File

@@ -39,37 +39,40 @@ inline constexpr bool mp_units::is_vector<T> = true;
using namespace 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("Initial: {}\n", initial);
std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>8} | {:>35} | {:>35}\n", "N", "Measured", "Curr. Estimate", std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>8} | {:>35} | {:>35}\n", "N", "Measured", "Curr. Estimate",
"Next 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() int main()
{ {
using namespace mp_units::si::unit_symbols; using namespace mp_units::si::unit_symbols;
using state = kalman::state<quantity<isq::position_vector[m]>, quantity<isq::velocity[m / s]>, using qp = quantity_point<isq::position_vector[m]>;
quantity<isq::acceleration[m / s2]>>; using state =
const auto interval = isq::duration(5. * s); kalman::system_state<qp, quantity_point<isq::velocity[m / s]>, quantity_point<isq::acceleration[m / s2]>>;
const state initial = {30 * km, 50 * m / s, 0 * m / s2};
const quantity<isq::position_vector[m], int> measurements[] = {30'160 * m, 30'365 * m, 30'890 * m, 31'050 * m, const quantity interval = isq::duration(5. * s);
31'785 * m, 32'215 * m, 33'130 * m, 34'510 * m, const state initial{qp{30 * km}, quantity_point{50 * m / s}, quantity_point{0 * m / s2}};
36'010 * m, 37'265 * m}; 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}; std::array gain = {0.5 * one, 0.4 * one, 0.1 * one};
print_header(initial); print_header(initial);
state next = state_extrapolation(initial, interval); state next = state_extrapolation(initial, interval);
for (int index = 1; const auto& measured : measurements) { for (int index = 1; const auto& m : measurements) {
const auto& previous = next; const state& previous = next;
const auto current = state_update(previous, measured, gain, interval); const state current = state_update(previous, m, gain, interval);
next = state_extrapolation(current, interval); next = state_extrapolation(current, interval);
print(index++, measured, current, next); print(index++, m, current, next);
} }
} }

50
example/kalman_filter/kalman_filter-example_5.cpp
View File

@@ -36,45 +36,45 @@ import mp_units;
using namespace mp_units; using namespace mp_units;
template<Quantity Q> template<QuantityPoint QP>
void print_header(kalman::estimation<Q> initial) void print_header(kalman::system_state_estimate<QP> initial)
{ {
std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial); std::cout << MP_UNITS_STD_FMT::format("Initial: {} {}\n", initial.state(), initial.variance());
std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>5} | {:>8} | {:>16} | {:>16}\n", "N", "Gain", "Measured", std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>8} | {:>5} | {:>16} | {:>16}\n", "N", "Measured", "Gain",
"Curr. Estimate", "Next Estimate"); "Curr. Estimate", "Next Estimate");
} }
template<Quantity Q, QuantityOf<dimensionless> K> template<QuantityPoint QP, QuantityOf<dimensionless> K>
void print(auto iteration, K gain, Q measured, kalman::estimation<Q> current, kalman::estimation<Q> next) void print(auto iteration, QP measured, K gain, kalman::system_state_estimate<QP> current,
kalman::system_state_estimate<QP> next)
{ {
std::cout << MP_UNITS_STD_FMT::format("{:2} | {:5%.2Q} | {:8} | {:>16.2} | {:>16.2}\n", iteration, gain, measured, std::cout << MP_UNITS_STD_FMT::format(
current, next); "{: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() int main()
{ {
using namespace kalman;
using namespace mp_units::si::unit_symbols; using namespace mp_units::si::unit_symbols;
using qp = quantity_point<isq::height[m]>;
using estimate = kalman::system_state_estimate<qp>;
using state = estimate::state_type;
const estimation initial = {state{isq::height(60. * m)}, pow<2>(isq::height(15. * m))}; const estimate initial{state{qp{60. * m}}, 15. * m};
const quantity<isq::height[m]> measurements[] = {48.54 * m, 47.11 * m, 55.01 * m, 55.15 * m, 49.89 * 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},
40.85 * m, 46.72 * m, 50.05 * m, 51.27 * m, 49.95 * m}; qp{52.61 * m}, qp{45.87 * m}, qp{42.64 * m}, qp{48.26 * m}, qp{55.84 * m}};
const auto measurement_uncertainty = pow<2>(isq::height(5. * m)); const quantity measurement_error = isq::height(5. * m);
const quantity measurement_variance = pow<2>(measurement_error);
auto update = [=]<Quantity Q>(const estimation<Q>& previous, const Q& measurement, auto predict = [](const estimate& current) { return current; };
QuantityOf<dimensionless> auto gain) {
return estimation{state_update(previous.state, measurement, gain), covariance_update(previous.uncertainty, gain)};
};
auto predict = []<Quantity Q>(const estimation<Q>& current) { return current; };
print_header(initial); print_header(initial);
estimation next = predict(initial); estimate next = predict(initial);
for (int index = 1; const auto& measured : measurements) { for (int index = 1; const auto& m : measurements) {
const auto& previous = next; const estimate& previous = next;
const auto gain = kalman_gain(previous.uncertainty, measurement_uncertainty); const quantity gain = kalman::kalman_gain(previous.variance(), measurement_variance);
const estimation current = update(previous, measured, gain); const estimate current = state_estimate_update(previous, m, gain);
next = predict(current); next = predict(current);
print(index++, gain, measured, current, next); print(index++, m, gain, current, next);
} }
} }

61
example/kalman_filter/kalman_filter-example_6.cpp
View File

@@ -27,62 +27,59 @@
import mp_units; import mp_units;
#else #else
#include <mp-units/format.h> #include <mp-units/format.h>
#include <mp-units/framework/quantity_point.h>
#include <mp-units/math.h> #include <mp-units/math.h>
#include <mp-units/quantity_point.h>
#include <mp-units/systems/isq/thermodynamics.h> #include <mp-units/systems/isq/thermodynamics.h>
#include <mp-units/systems/si/si.h> #include <mp-units/systems/si.h>
#endif #endif
// Based on: https://www.kalmanfilter.net/kalman1d.html#ex6 // Based on: https://www.kalmanfilter.net/kalman1d_pn.html#ex6
using namespace mp_units; using namespace mp_units;
template<QuantityPoint QP> template<QuantityPoint QP>
void print_header(kalman::estimation<QP> initial) void print_header(kalman::system_state_estimate<QP> initial)
{ {
std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial); std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial.state(), initial.variance());
std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>7} | {:>10} | {:>18} | {:>18}\n", "N", "Gain", "Measured", std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>10} | {:>7} | {:>22} | {:>22}\n", "N", "Measured", "Gain",
"Curr. Estimate", "Next Estimate"); "Curr. Estimate", "Next Estimate");
} }
template<QuantityPoint QP, QuantityOf<dimensionless> K> template<QuantityPoint QP, QuantityOf<dimensionless> K>
void print(auto iteration, K gain, QP measured, kalman::estimation<QP> current, kalman::estimation<QP> next) void print(auto iteration, QP measured, K gain, kalman::system_state_estimate<QP> current,
kalman::system_state_estimate<QP> next)
{ {
std::cout << MP_UNITS_STD_FMT::format("{:2} | {:7%.4Q} | {:10%.3Q %q} | {:>18.3} | {:>18.3}\n", iteration, gain, std::cout << MP_UNITS_STD_FMT::format(
measured.quantity_ref_from(QP::point_origin), current, next); "{: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() int main()
{ {
constexpr auto deg_C = isq::Celsius_temperature[si::degree_Celsius]; using namespace mp_units::si::unit_symbols;
using qp = quantity_point<isq::Celsius_temperature[deg_C]>;
using estimate = kalman::system_state_estimate<qp>;
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); auto predict = [=](const estimate& current) {
const estimation initial = {state{si::ice_point + 10. * deg_C}, pow<2>(100. * deg_C)}; return estimate{current.state(), kalman::covariance_extrapolation(current.variance(), process_noise_variance)};
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 = [=]<QuantityPoint QP>(const estimation<QP>& previous, const QP& meassurement,
QuantityOf<dimensionless> auto gain) {
return estimation{state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, gain)};
};
auto predict = [=]<QuantityPoint QP>(const estimation<QP>& current) {
return estimation{current.state, covariance_extrapolation(current.uncertainty, process_noise_variance)};
}; };
print_header(initial); print_header(initial);
estimation next = predict(initial); estimate next = predict(initial);
for (int index = 1; const auto& m : measurements) { for (int index = 1; const auto& m : measurements) {
const auto& previous = next; const estimate& previous = next;
const auto gain = kalman_gain(previous.uncertainty, measurement_uncertainty); const quantity gain = kalman::kalman_gain(previous.variance(), measurement_variance);
const estimation current = update(previous, m, gain); const estimate current = state_estimate_update(previous, m, gain);
next = predict(current); next = predict(current);
print(index++, gain, m, current, next); print(index++, m, gain, current, next);
} }
} }

61
example/kalman_filter/kalman_filter-example_7.cpp
View File

@@ -27,62 +27,59 @@
import mp_units; import mp_units;
#else #else
#include <mp-units/format.h> #include <mp-units/format.h>
#include <mp-units/framework/quantity_point.h>
#include <mp-units/math.h> #include <mp-units/math.h>
#include <mp-units/quantity_point.h>
#include <mp-units/systems/isq/thermodynamics.h> #include <mp-units/systems/isq/thermodynamics.h>
#include <mp-units/systems/si/si.h> #include <mp-units/systems/si.h>
#endif #endif
// Based on: https://www.kalmanfilter.net/kalman1d.html#ex7 // Based on: https://www.kalmanfilter.net/kalman1d_pn.html#ex7
using namespace mp_units; using namespace mp_units;
template<QuantityPoint QP> template<QuantityPoint QP>
void print_header(kalman::estimation<QP> initial) void print_header(kalman::system_state_estimate<QP> initial)
{ {
std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial); std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial.state(), initial.variance());
std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>7} | {:>10} | {:>18} | {:>18}\n", "N", "Gain", "Measured", std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>10} | {:>7} | {:>22} | {:>22}\n", "N", "Measured", "Gain",
"Curr. Estimate", "Next Estimate"); "Curr. Estimate", "Next Estimate");
} }
template<QuantityPoint QP, QuantityOf<dimensionless> K> template<QuantityPoint QP, QuantityOf<dimensionless> K>
void print(auto iteration, K gain, QP measured, kalman::estimation<QP> current, kalman::estimation<QP> next) void print(auto iteration, QP measured, K gain, kalman::system_state_estimate<QP> current,
kalman::system_state_estimate<QP> next)
{ {
std::cout << MP_UNITS_STD_FMT::format("{:2} | {:7%.4Q} | {:10%.3Q %q} | {:>18.3} | {:>18.3}\n", iteration, gain, std::cout << MP_UNITS_STD_FMT::format(
measured.quantity_ref_from(QP::point_origin), current, next); "{: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() int main()
{ {
constexpr auto deg_C = isq::Celsius_temperature[si::degree_Celsius]; using namespace mp_units::si::unit_symbols;
using qp = quantity_point<isq::Celsius_temperature[deg_C]>;
using estimate = kalman::system_state_estimate<qp>;
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); auto predict = [=](const estimate& current) {
const estimation initial = {state{si::ice_point + 10. * deg_C}, pow<2>(100. * deg_C)}; return estimate{current.state(), kalman::covariance_extrapolation(current.variance(), process_noise_variance)};
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 = [=]<QuantityPoint QP>(const estimation<QP>& previous, const QP& meassurement,
QuantityOf<dimensionless> auto gain) {
return estimation{state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, gain)};
};
auto predict = [=]<QuantityPoint QP>(const estimation<QP>& current) {
return estimation{current.state, covariance_extrapolation(current.uncertainty, process_noise_variance)};
}; };
print_header(initial); print_header(initial);
estimation next = predict(initial); estimate next = predict(initial);
for (int index = 1; const auto& m : measurements) { for (int index = 1; const auto& m : measurements) {
const auto& previous = next; const estimate& previous = next;
const auto gain = kalman_gain(previous.uncertainty, measurement_uncertainty); const quantity gain = kalman::kalman_gain(previous.variance(), measurement_variance);
const estimation current = update(previous, m, gain); const estimate current = state_estimate_update(previous, m, gain);
next = predict(current); next = predict(current);
print(index++, gain, m, current, next); print(index++, m, gain, current, next);
} }
} }

61
example/kalman_filter/kalman_filter-example_8.cpp
View File

@@ -27,62 +27,59 @@
import mp_units; import mp_units;
#else #else
#include <mp-units/format.h> #include <mp-units/format.h>
#include <mp-units/framework/quantity_point.h>
#include <mp-units/math.h> #include <mp-units/math.h>
#include <mp-units/quantity_point.h>
#include <mp-units/systems/isq/thermodynamics.h> #include <mp-units/systems/isq/thermodynamics.h>
#include <mp-units/systems/si/si.h> #include <mp-units/systems/si.h>
#endif #endif
// Based on: https://www.kalmanfilter.net/kalman1d.html#ex8 // Based on: https://www.kalmanfilter.net/kalman1d_pn.html#ex8
using namespace mp_units; using namespace mp_units;
template<QuantityPoint QP> template<QuantityPoint QP>
void print_header(kalman::estimation<QP> initial) void print_header(kalman::system_state_estimate<QP> initial)
{ {
std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial); std::cout << MP_UNITS_STD_FMT::format("Initial: {}\n", initial.state(), initial.variance());
std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>7} | {:>10} | {:>16} | {:>16}\n", "N", "Gain", "Measured", std::cout << MP_UNITS_STD_FMT::format("{:>2} | {:>10} | {:>7} | {:>22} | {:>22}\n", "N", "Measured", "Gain",
"Curr. Estimate", "Next Estimate"); "Curr. Estimate", "Next Estimate");
} }
template<QuantityPoint QP, QuantityOf<dimensionless> K> template<QuantityPoint QP, QuantityOf<dimensionless> K>
void print(auto iteration, K gain, QP measured, kalman::estimation<QP> current, kalman::estimation<QP> next) void print(auto iteration, QP measured, K gain, kalman::system_state_estimate<QP> current,
kalman::system_state_estimate<QP> next)
{ {
std::cout << MP_UNITS_STD_FMT::format("{:2} | {:7%.3Q} | {:10%.3Q %q} | {:>16.2} | {:>16.2}\n", iteration, gain, std::cout << MP_UNITS_STD_FMT::format(
measured.quantity_ref_from(QP::point_origin), current, next); "{: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() int main()
{ {
constexpr auto deg_C = isq::Celsius_temperature[si::degree_Celsius]; using namespace mp_units::si::unit_symbols;
using qp = quantity_point<isq::Celsius_temperature[deg_C]>;
using estimate = kalman::system_state_estimate<qp>;
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); auto predict = [=](const estimate& current) {
const estimation initial = {state{si::ice_point + 10. * deg_C}, pow<2>(100. * deg_C)}; return estimate{current.state(), kalman::covariance_extrapolation(current.variance(), process_noise_variance)};
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 = [=]<QuantityPoint QP>(const estimation<QP>& previous, const QP& meassurement,
QuantityOf<dimensionless> auto gain) {
return estimation{state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, gain)};
};
auto predict = [=]<QuantityPoint QP>(const estimation<QP>& current) {
return estimation{current.state, covariance_extrapolation(current.uncertainty, process_noise_variance)};
}; };
print_header(initial); print_header(initial);
estimation next = predict(initial); estimate next = predict(initial);
for (int index = 1; const auto& m : measurements) { for (int index = 1; const auto& m : measurements) {
const auto& previous = next; const estimate& previous = next;
const auto gain = kalman_gain(previous.uncertainty, measurement_uncertainty); const quantity gain = kalman::kalman_gain(previous.variance(), measurement_variance);
const estimation current = update(previous, m, gain); const estimate current = state_estimate_update(previous, m, gain);
next = predict(current); next = predict(current);
print(index++, gain, m, current, next); print(index++, m, gain, current, next);
} }
} }

39
src/core/include/mp-units/bits/fmt.h
View File

@@ -30,6 +30,7 @@
// NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic, cppcoreguidelines-pro-type-union-access) // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic, cppcoreguidelines-pro-type-union-access)
#pragma once #pragma once
#include <mp-units/bits/module_macros.h>
#include <mp-units/compat_macros.h> #include <mp-units/compat_macros.h>
#include <mp-units/ext/algorithm.h> #include <mp-units/ext/algorithm.h>
@@ -45,6 +46,9 @@
namespace mp_units::detail { 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_align : std::int8_t { none, left, right, center, numeric };
enum class fmt_arg_id_kind : std::int8_t { enum class fmt_arg_id_kind : std::int8_t {
none, none,
@@ -109,6 +113,8 @@ public:
[[nodiscard]] constexpr const Char& operator[](size_t index) const { return data_[index]; } [[nodiscard]] constexpr const Char& operator[](size_t index) const { return data_[index]; }
}; };
MP_UNITS_EXPORT_END
template<typename T> template<typename T>
inline constexpr bool is_integer = inline constexpr bool is_integer =
std::is_integral_v<T> && !std::is_same_v<T, bool> && !std::is_same_v<T, char> && !std::is_same_v<T, wchar_t>; std::is_integral_v<T> && !std::is_same_v<T, bool> && !std::is_same_v<T, char> && !std::is_same_v<T, wchar_t>;
@@ -135,20 +141,6 @@ template<typename Int>
return static_cast<std::make_unsigned_t<Int>>(value); return static_cast<std::make_unsigned_t<Int>>(value);
} }
struct width_checker {
template<typename T>
[[nodiscard]] constexpr unsigned long long operator()(T value) const
{
if constexpr (is_integer<T>) {
if constexpr (std::numeric_limits<T>::is_signed)
if (value < 0) MP_UNITS_THROW(MP_UNITS_STD_FMT::format_error("negative width"));
return static_cast<unsigned long long>(value);
}
MP_UNITS_THROW(MP_UNITS_STD_FMT::format_error("width is not integer"));
return 0;
}
};
template<class Handler, typename FormatArg> template<class Handler, typename FormatArg>
[[nodiscard]] constexpr int get_dynamic_spec(FormatArg arg) [[nodiscard]] constexpr int get_dynamic_spec(FormatArg arg)
{ {
@@ -167,6 +159,9 @@ template<typename Context, typename ID>
return arg; return arg;
} }
// TODO the below should be exposed by the C++ Standard Library (used in our examples)
MP_UNITS_EXPORT_BEGIN
template<class Handler, typename Context> template<class Handler, typename Context>
constexpr void handle_dynamic_spec(int& value, fmt_arg_ref<typename Context::char_type> ref, Context& ctx) constexpr void handle_dynamic_spec(int& value, fmt_arg_ref<typename Context::char_type> ref, Context& ctx)
{ {
@@ -184,6 +179,22 @@ constexpr void handle_dynamic_spec(int& value, fmt_arg_ref<typename Context::cha
} }
} }
struct width_checker {
template<typename T>
[[nodiscard]] constexpr unsigned long long operator()(T value) const
{
if constexpr (is_integer<T>) {
if constexpr (std::numeric_limits<T>::is_signed)
if (value < 0) MP_UNITS_THROW(MP_UNITS_STD_FMT::format_error("negative width"));
return static_cast<unsigned long long>(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 // 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. // that the range is non-empty and the first character is a digit.
template<typename Char> template<typename Char>

22
src/core/include/mp-units/format.h
View File

@@ -25,6 +25,7 @@
#pragma once #pragma once
#include <mp-units/bits/fmt.h> #include <mp-units/bits/fmt.h>
#include <mp-units/bits/module_macros.h>
#include <mp-units/compat_macros.h> #include <mp-units/compat_macros.h>
#include <mp-units/ext/algorithm.h> #include <mp-units/ext/algorithm.h>
#include <mp-units/framework/customization_points.h> #include <mp-units/framework/customization_points.h>
@@ -33,14 +34,6 @@
namespace mp_units::detail { namespace mp_units::detail {
template<typename Char>
struct fill_align_width_format_specs {
fill_t<Char> fill;
fmt_align align : 4 = fmt_align::none;
int width = 0;
fmt_arg_ref<Char> width_ref;
};
template<typename Char> template<typename Char>
[[nodiscard]] constexpr const Char* at_most_one_of(const Char* begin, const Char* end, std::string_view modifiers) [[nodiscard]] constexpr const Char* at_most_one_of(const Char* begin, const Char* end, std::string_view modifiers)
{ {
@@ -51,6 +44,17 @@ template<typename Char>
return it; return it;
} }
// TODO the below should be exposed by the C++ Standard Library (used in our examples)
MP_UNITS_EXPORT_BEGIN
template<typename Char>
struct fill_align_width_format_specs {
fill_t<Char> fill;
fmt_align align : 4 = fmt_align::none;
int width = 0;
fmt_arg_ref<Char> width_ref;
};
template<typename Char, typename Specs> template<typename Char, typename Specs>
[[nodiscard]] constexpr const Char* parse_fill_align_width(MP_UNITS_STD_FMT::basic_format_parse_context<Char>& ctx, [[nodiscard]] constexpr const Char* parse_fill_align_width(MP_UNITS_STD_FMT::basic_format_parse_context<Char>& ctx,
const Char* begin, const Char* end, Specs& specs, 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, "}}"); return MP_UNITS_STD_FMT::format_to(out, "}}");
} }
MP_UNITS_EXPORT_END
} // namespace mp_units::detail } // namespace mp_units::detail
// //