style: clang-format applied to the remaining files

This commit is contained in:
Mateusz Pusz committed 2022-04-02 21:36:42 +02:00
1 parent 0dca41b5ae
commit b221dace3f
109 files changed
+4546 -3140

No files matched your search

+39 -28
View File
@@ -34,7 +34,8 @@
namespace kalman {
template<typename T>
concept QuantityOrQuantityPoint = units::Quantity<T> || units::QuantityPoint<T>; // TODO Should it also account for `kinds`?
concept QuantityOrQuantityPoint =
units::Quantity<T> || units::QuantityPoint<T>; // TODO Should it also account for `kinds`?
template<typename... Qs>
inline constexpr bool are_derivatives = false;
@@ -44,34 +45,42 @@ inline constexpr bool are_derivatives<Q> = true;
template<typename Q1, typename Q2, typename... Qs>
inline constexpr bool are_derivatives<Q1, Q2, Qs...> =
units::DimensionOfT<typename decltype(Q1::reference / Q2::reference)::dimension, units::isq::dim_time> && // TODO Think on how to simplify this
units::DimensionOfT<typename decltype(Q1::reference / Q2::reference)::dimension,
units::isq::dim_time> && // TODO Think on how to simplify this
are_derivatives<Q2, Qs...>;
// state
template<QuantityOrQuantityPoint... QQPs>
requires (sizeof...(QQPs) > 0) && (sizeof...(QQPs) <= 3) && are_derivatives<QQPs...>
requires(sizeof...(QQPs) > 0) && (sizeof...(QQPs) <= 3) && are_derivatives<QQPs...>
struct state {
std::tuple<QQPs...> variables_;
constexpr state(QQPs... qqps): variables_(std::move(qqps)...) {}
constexpr state(QQPs... qqps) : variables_(std::move(qqps)...) {}
};
template<typename T>
concept State = units::is_specialization_of<T, state>;
template<std::size_t Idx, typename... Qs>
constexpr auto& get(state<Qs...>& s) { return get<Idx>(s.variables_); }
constexpr auto& get(state<Qs...>& s)
{
return get<Idx>(s.variables_);
}
template<std::size_t Idx, typename... Qs>
constexpr const auto& get(const state<Qs...>& s) { return get<Idx>(s.variables_); }
constexpr const auto& get(const state<Qs...>& s)
{
return get<Idx>(s.variables_);
}
// estimation
template<QuantityOrQuantityPoint QQP, QuantityOrQuantityPoint... QQPs>
struct estimation {
private:
using uncertainty_ref = decltype(QQP::reference * QQP::reference);
using uncertainty_type = units::quantity<typename uncertainty_ref::dimension, typename uncertainty_ref::unit, typename QQP::rep>;
using uncertainty_type =
units::quantity<typename uncertainty_ref::dimension, typename uncertainty_ref::unit, typename QQP::rep>;
public:
kalman::state<QQP, QQPs...> state; // TODO extend kalman functions to work with this variadic patermater list
kalman::state<QQP, QQPs...> state; // TODO extend kalman functions to work with this variadic patermater list
uncertainty_type uncertainty;
};
@@ -94,7 +103,7 @@ template<typename Q, QuantityOrQuantityPoint QM, units::Dimensionless K>
requires units::equivalent<typename Q::dimension, typename QM::dimension>
constexpr state<Q> state_update(const state<Q>& predicted, QM measured, K gain)
{
return { get<0>(predicted) + gain * (measured - get<0>(predicted)) };
return {get<0>(predicted) + gain * (measured - get<0>(predicted))};
}
template<typename Q1, typename Q2, QuantityOrQuantityPoint QM, units::Dimensionless K, units::isq::Time T>
@@ -103,17 +112,18 @@ constexpr state<Q1, Q2> state_update(const state<Q1, Q2>& predicted, QM measured
{
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 };
return {q1, q2};
}
template<typename Q1, typename Q2, typename Q3, QuantityOrQuantityPoint QM, units::Dimensionless K, units::isq::Time T>
requires units::equivalent<typename Q1::dimension, typename QM::dimension>
constexpr state<Q1, Q2, Q3> state_update(const state<Q1, Q2, Q3>& predicted, QM measured, std::array<K, 3> gain, T interval)
constexpr state<Q1, Q2, Q3> state_update(const state<Q1, Q2, Q3>& predicted, QM measured, std::array<K, 3> 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 };
return {q1, q2, q3};
}
// covariance update
@@ -129,7 +139,7 @@ constexpr state<Q1, Q2> state_extrapolation(const state<Q1, Q2>& estimated, T in
{
const auto q1 = get<0>(estimated) + get<1>(estimated) * interval;
const auto q2 = get<1>(estimated);
return { q1, q2 };
return {q1, q2};
}
template<typename Q1, typename Q2, typename Q3, units::isq::Time T>
@@ -138,7 +148,7 @@ constexpr state<Q1, Q2, Q3> state_extrapolation(const state<Q1, Q2, Q3>& estimat
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 };
return {q1, q2, q3};
}
// covariance extrapolation
@@ -164,24 +174,26 @@ struct STD_FMT::formatter<kalman::state<Qs...>> {
std::string value_buffer;
auto to_value_buffer = std::back_inserter(value_buffer);
if (specs.precision != -1) {
if constexpr(sizeof...(Qs) == 1)
if constexpr (sizeof...(Qs) == 1)
STD_FMT::format_to(to_value_buffer, "{1:%.{0}Q %q}", specs.precision, kalman::get<0>(s));
else if constexpr(sizeof...(Qs) == 2)
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 if constexpr (sizeof...(Qs) == 2)
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
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)
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)
STD_FMT::format_to(to_value_buffer, "{}", kalman::get<0>(s));
else if constexpr(sizeof...(Qs) == 2)
else if constexpr (sizeof...(Qs) == 2)
STD_FMT::format_to(to_value_buffer, "{{ {}, {} }}", kalman::get<0>(s), kalman::get<1>(s));
else
STD_FMT::format_to(to_value_buffer, "{{ {}, {}, {} }}", kalman::get<0>(s), kalman::get<1>(s), kalman::get<2>(s));
STD_FMT::format_to(to_value_buffer, "{{ {}, {}, {} }}", kalman::get<0>(s), kalman::get<1>(s),
kalman::get<2>(s));
}
std::string global_format_buffer;
units::detail::quantity_global_format_specs<char> global_specs = { specs.fill, specs.align, specs.width };
units::detail::quantity_global_format_specs<char> global_specs = {specs.fill, specs.align, specs.width};
units::detail::format_global_buffer(std::back_inserter(global_format_buffer), global_specs);
return STD_FMT::vformat_to(ctx.out(), global_format_buffer, STD_FMT::make_format_args(value_buffer));
@@ -202,7 +214,7 @@ struct STD_FMT::formatter<kalman::estimation<Q>> {
auto format(kalman::estimation<Q> e, FormatContext& ctx)
{
units::Quantity auto q = [](const Q& t) {
if constexpr(units::Quantity<Q>)
if constexpr (units::Quantity<Q>)
return t;
else
return t.relative();
@@ -212,13 +224,12 @@ struct STD_FMT::formatter<kalman::estimation<Q>> {
auto to_value_buffer = std::back_inserter(value_buffer);
if (specs.precision != -1) {
STD_FMT::format_to(to_value_buffer, "{0:%.{2}Q} ± {1:%.{2}Q} {0:%q}", q, sqrt(e.uncertainty), specs.precision);
}
else {
} else {
STD_FMT::format_to(to_value_buffer, "{0:%Q} ± {1:%Q} {0:%q}", q, sqrt(e.uncertainty));
}
std::string global_format_buffer;
units::detail::quantity_global_format_specs<char> global_specs = { specs.fill, specs.align, specs.width };
units::detail::quantity_global_format_specs<char> global_specs = {specs.fill, specs.align, specs.width};
units::detail::format_global_buffer(std::back_inserter(global_format_buffer), global_specs);
return STD_FMT::vformat_to(ctx.out(), global_format_buffer, STD_FMT::make_format_args(value_buffer));
@@ -21,9 +21,9 @@
// SOFTWARE.
#include "kalman.h"
#include <units/isq/si/mass.h>
#include <units/format.h>
#include <units/generic/dimensionless.h>
#include <units/isq/si/mass.h>
#include <array>
#include <iostream>
@@ -34,10 +34,12 @@ using namespace units;
void print_header(const kalman::State auto& initial)
{
std::cout << STD_FMT::format("Initial: {}\n", initial);
std::cout << STD_FMT::format("{:>2} | {:>9} | {:>8} | {:>14} | {:>14}\n", "N", "Gain", "Measured", "Curr. Estimate", "Next Estimate");
std::cout << STD_FMT::format("{:>2} | {:>9} | {:>8} | {:>14} | {:>14}\n", "N", "Gain", "Measured", "Curr. Estimate",
"Next Estimate");
}
void print(auto iteration, Dimensionless auto gain, Quantity auto measured, const kalman::State auto& current, const kalman::State auto& next)
void print(auto iteration, Dimensionless auto gain, Quantity auto measured, const kalman::State auto& current,
const kalman::State auto& next)
{
std::cout << STD_FMT::format("{:2} | {:9} | {:8} | {:14} | {:14}\n", iteration, gain, measured, current, next);
}
@@ -48,13 +50,13 @@ int main()
using namespace units::isq::si::references;
using state = kalman::state<si::mass<si::gram>>;
const state initial = { 1 * kg };
const std::array measurements = { 1030 * g, 989 * g, 1017 * g, 1009 * g, 1013 * g,
979 * g, 1008 * g, 1042 * g, 1012 * g, 1011 * g };
const state initial = {1 * kg};
const std::array measurements = {1030 * g, 989 * g, 1017 * g, 1009 * g, 1013 * g,
979 * g, 1008 * g, 1042 * g, 1012 * g, 1011 * g};
print_header(initial);
state next = initial;
for(int index = 1; const auto& m : measurements) {
for (int index = 1; const auto& m : measurements) {
const auto& previous = next;
const dimensionless<one> gain = 1. / index;
const auto current = state_update(previous, m, gain);
@@ -21,11 +21,11 @@
// SOFTWARE.
#include "kalman.h"
#include <units/format.h>
#include <units/generic/dimensionless.h>
#include <units/isq/si/length.h>
#include <units/isq/si/speed.h>
#include <units/isq/si/time.h>
#include <units/format.h>
#include <units/generic/dimensionless.h>
#include <array>
#include <iostream>
@@ -51,14 +51,14 @@ int main()
using state = kalman::state<si::length<si::metre>, si::speed<si::metre_per_second>>;
const auto interval = 5 * s;
const state initial = { 30 * km, 40 * (m / s) };
const std::array measurements = { 30110 * m, 30265 * m, 30740 * m, 30750 * m, 31135 * m,
31015 * m, 31180 * m, 31610 * m, 31960 * m, 31865 * m };
const state initial = {30 * km, 40 * (m / s)};
const std::array measurements = {30110 * m, 30265 * m, 30740 * m, 30750 * m, 31135 * m,
31015 * m, 31180 * m, 31610 * m, 31960 * m, 31865 * m};
std::array gain = {dimensionless<one>(0.2), dimensionless<one>(0.1)};
print_header(initial);
state next = state_extrapolation(initial, interval);
for(int index = 1; const auto& measured : measurements) {
for (int index = 1; const auto& measured : measurements) {
const auto& previous = next;
const auto current = state_update(previous, measured, gain, interval);
next = state_extrapolation(current, interval);
@@ -21,11 +21,11 @@
// SOFTWARE.
#include "kalman.h"
#include <units/format.h>
#include <units/generic/dimensionless.h>
#include <units/isq/si/length.h>
#include <units/isq/si/speed.h>
#include <units/isq/si/time.h>
#include <units/format.h>
#include <units/generic/dimensionless.h>
#include <array>
#include <iostream>
@@ -51,14 +51,14 @@ int main()
using state = kalman::state<si::length<si::metre>, si::speed<si::metre_per_second>>;
const auto interval = 5 * s;
const state initial = { 30 * km, 50 * (m / s) };
const std::array measurements = { 30160 * m, 30365 * m, 30890 * m, 31050 * m, 31785 * m,
32215 * m, 33130 * m, 34510 * m, 36010 * m, 37265 * m };
const state initial = {30 * km, 50 * (m / s)};
const std::array measurements = {30160 * m, 30365 * m, 30890 * m, 31050 * m, 31785 * m,
32215 * m, 33130 * m, 34510 * m, 36010 * m, 37265 * m};
std::array gain = {dimensionless<one>(0.2), dimensionless<one>(0.1)};
print_header(initial);
state next = state_extrapolation(initial, interval);
for(int index = 1; const auto& measured : measurements) {
for (int index = 1; const auto& measured : measurements) {
const auto& previous = next;
const auto current = state_update(previous, measured, gain, interval);
next = state_extrapolation(current, interval);
@@ -21,12 +21,12 @@
// SOFTWARE.
#include "kalman.h"
#include <units/format.h>
#include <units/generic/dimensionless.h>
#include <units/isq/si/acceleration.h>
#include <units/isq/si/length.h>
#include <units/isq/si/speed.h>
#include <units/isq/si/time.h>
#include <units/format.h>
#include <units/generic/dimensionless.h>
#include <array>
#include <iostream>
@@ -49,19 +49,20 @@ int main()
{
using namespace units::isq;
using namespace units::isq::si::references;
using state = kalman::state<si::length<si::metre>, si::speed<si::metre_per_second>, si::acceleration<si::metre_per_second_sq>>;
using state =
kalman::state<si::length<si::metre>, si::speed<si::metre_per_second>, si::acceleration<si::metre_per_second_sq>>;
constexpr auto mps = m / s;
constexpr auto mps2 = mps / s;
const auto interval = 5. * s;
const state initial = { 30 * km, 50 * mps, 0 * mps2 };
const std::array measurements = { 30160 * m, 30365 * m, 30890 * m, 31050 * m, 31785 * m,
32215 * m, 33130 * m, 34510 * m, 36010 * m, 37265 * m };
const state initial = {30 * km, 50 * mps, 0 * mps2};
const std::array measurements = {30160 * m, 30365 * m, 30890 * m, 31050 * m, 31785 * m,
32215 * m, 33130 * m, 34510 * m, 36010 * m, 37265 * m};
std::array gain = {dimensionless<one>(0.5), dimensionless<one>(0.4), dimensionless<one>(0.1)};
print_header(initial);
state next = state_extrapolation(initial, interval);
for(int index = 1; const auto& measured : measurements) {
for (int index = 1; const auto& measured : measurements) {
const auto& previous = next;
const auto current = state_update(previous, measured, gain, interval);
next = state_extrapolation(current, interval);
@@ -21,8 +21,8 @@
// SOFTWARE.
#include "kalman.h"
#include <units/isq/si/length.h>
#include <units/format.h>
#include <units/isq/si/length.h>
#include <units/math.h>
#include <array>
#include <iostream>
@@ -35,13 +35,15 @@ template<Quantity Q>
void print_header(kalman::estimation<Q> initial)
{
std::cout << STD_FMT::format("Initial: {}\n", initial);
std::cout << STD_FMT::format("{:>2} | {:>5} | {:>8} | {:>16} | {:>16}\n", "N", "Gain", "Measured", "Curr. Estimate", "Next Estimate");
std::cout << STD_FMT::format("{:>2} | {:>5} | {:>8} | {:>16} | {:>16}\n", "N", "Gain", "Measured", "Curr. Estimate",
"Next Estimate");
}
template<Quantity Q, Dimensionless K>
void print(auto iteration, K gain, Q measured, kalman::estimation<Q> current, kalman::estimation<Q> next)
{
std::cout << STD_FMT::format("{:2} | {:5%.2Q} | {:8} | {:>16.2} | {:>16.2}\n", iteration, gain, measured, current, next);
std::cout << STD_FMT::format("{:2} | {:5%.2Q} | {:8} | {:>16.2} | {:>16.2}\n", iteration, gain, measured, current,
next);
}
int main()
@@ -50,20 +52,20 @@ int main()
using namespace units::isq;
using namespace units::isq::si::references;
const estimation initial = { state{ 60. * m }, pow<2>(15. * m) };
const std::array 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 estimation initial = {state{60. * m}, pow<2>(15. * m)};
const std::array 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>(5. * m);
auto update = [=]<Quantity Q>(const estimation<Q>& previous, const Q& meassurement, Dimensionless auto gain) {
return estimation{ state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, gain) };
return estimation{state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, gain)};
};
auto predict = []<Quantity Q>(const estimation<Q>& current) { return current; };
print_header(initial);
estimation next = predict(initial);
for(int index = 1; const auto& measured : measurements) {
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);
@@ -21,8 +21,8 @@
// SOFTWARE.
#include "kalman.h"
#include <units/isq/si/thermodynamic_temperature.h>
#include <units/format.h>
#include <units/isq/si/thermodynamic_temperature.h>
#include <units/math.h>
#include <units/quantity_point.h>
#include <units/unit.h>
@@ -56,13 +56,15 @@ template<QuantityPoint QP>
void print_header(kalman::estimation<QP> initial)
{
std::cout << STD_FMT::format("Initial: {}\n", initial);
std::cout << STD_FMT::format("{:>2} | {:>7} | {:>10} | {:>18} | {:>18}\n", "N", "Gain", "Measured", "Curr. Estimate", "Next Estimate");
std::cout << STD_FMT::format("{:>2} | {:>7} | {:>10} | {:>18} | {:>18}\n", "N", "Gain", "Measured", "Curr. Estimate",
"Next Estimate");
}
template<QuantityPoint QP, Dimensionless K>
void print(auto iteration, K gain, QP measured, kalman::estimation<QP> current, kalman::estimation<QP> next)
{
std::cout << STD_FMT::format("{:2} | {:7%.4Q} | {:10%.3Q %q} | {:>18.3} | {:>18.3}\n", iteration, gain, measured.relative(), current, next);
std::cout << STD_FMT::format("{:2} | {:7%.4Q} | {:10%.3Q %q} | {:>18.3} | {:>18.3}\n", iteration, gain,
measured.relative(), current, next);
}
int main()
@@ -72,32 +74,25 @@ int main()
using namespace units::isq::si::references;
const auto process_noise_variance = 0.0001 * (deg_C * deg_C);
const estimation initial = { state{ quantity_point(10. * deg_C) }, pow<2>(100. * deg_C) };
const std::array measurements = {
quantity_point(49.95 * deg_C),
quantity_point(49.967 * deg_C),
quantity_point(50.1 * deg_C),
quantity_point(50.106 * deg_C),
quantity_point(49.992 * deg_C),
quantity_point(49.819 * deg_C),
quantity_point(49.933 * deg_C),
quantity_point(50.007 * deg_C),
quantity_point(50.023 * deg_C),
quantity_point(49.99 * deg_C)
};
const estimation initial = {state{quantity_point(10. * deg_C)}, pow<2>(100. * deg_C)};
const std::array measurements = {quantity_point(49.95 * deg_C), quantity_point(49.967 * deg_C),
quantity_point(50.1 * deg_C), quantity_point(50.106 * deg_C),
quantity_point(49.992 * deg_C), quantity_point(49.819 * deg_C),
quantity_point(49.933 * deg_C), quantity_point(50.007 * deg_C),
quantity_point(50.023 * deg_C), quantity_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, Dimensionless auto gain) {
return estimation{ state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, 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) };
return estimation{current.state, covariance_extrapolation(current.uncertainty, process_noise_variance)};
};
print_header(initial);
estimation next = predict(initial);
for(int index = 1; const auto& m : measurements) {
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);
@@ -21,8 +21,8 @@
// SOFTWARE.
#include "kalman.h"
#include <units/isq/si/thermodynamic_temperature.h>
#include <units/format.h>
#include <units/isq/si/thermodynamic_temperature.h>
#include <units/math.h>
#include <units/quantity_point.h>
#include <units/unit.h>
@@ -56,13 +56,15 @@ template<QuantityPoint QP>
void print_header(kalman::estimation<QP> initial)
{
std::cout << STD_FMT::format("Initial: {}\n", initial);
std::cout << STD_FMT::format("{:>2} | {:>7} | {:>10} | {:>18} | {:>18}\n", "N", "Gain", "Measured", "Curr. Estimate", "Next Estimate");
std::cout << STD_FMT::format("{:>2} | {:>7} | {:>10} | {:>18} | {:>18}\n", "N", "Gain", "Measured", "Curr. Estimate",
"Next Estimate");
}
template<QuantityPoint QP, Dimensionless K>
void print(auto iteration, K gain, QP measured, kalman::estimation<QP> current, kalman::estimation<QP> next)
{
std::cout << STD_FMT::format("{:2} | {:7%.4Q} | {:10%.3Q %q} | {:>18.3} | {:>18.3}\n", iteration, gain, measured.relative(), current, next);
std::cout << STD_FMT::format("{:2} | {:7%.4Q} | {:10%.3Q %q} | {:>18.3} | {:>18.3}\n", iteration, gain,
measured.relative(), current, next);
}
int main()
@@ -72,32 +74,25 @@ int main()
using namespace units::isq::si::references;
const auto process_noise_variance = 0.0001 * (deg_C * deg_C);
const estimation initial = { state{ quantity_point(10. * deg_C) }, pow<2>(100. * deg_C) };
const std::array measurements = {
quantity_point(50.45 * deg_C),
quantity_point(50.967 * deg_C),
quantity_point(51.6 * deg_C),
quantity_point(52.106 * deg_C),
quantity_point(52.492 * deg_C),
quantity_point(52.819 * deg_C),
quantity_point(53.433 * deg_C),
quantity_point(54.007 * deg_C),
quantity_point(54.523 * deg_C),
quantity_point(54.99 * deg_C)
};
const estimation initial = {state{quantity_point(10. * deg_C)}, pow<2>(100. * deg_C)};
const std::array measurements = {quantity_point(50.45 * deg_C), quantity_point(50.967 * deg_C),
quantity_point(51.6 * deg_C), quantity_point(52.106 * deg_C),
quantity_point(52.492 * deg_C), quantity_point(52.819 * deg_C),
quantity_point(53.433 * deg_C), quantity_point(54.007 * deg_C),
quantity_point(54.523 * deg_C), quantity_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, Dimensionless auto gain) {
return estimation{ state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, 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) };
return estimation{current.state, covariance_extrapolation(current.uncertainty, process_noise_variance)};
};
print_header(initial);
estimation next = predict(initial);
for(int index = 1; const auto& m : measurements) {
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);
@@ -21,8 +21,8 @@
// SOFTWARE.
#include "kalman.h"
#include <units/isq/si/thermodynamic_temperature.h>
#include <units/format.h>
#include <units/isq/si/thermodynamic_temperature.h>
#include <units/math.h>
#include <units/quantity_point.h>
#include <units/unit.h>
@@ -56,13 +56,15 @@ template<QuantityPoint QP>
void print_header(kalman::estimation<QP> initial)
{
std::cout << STD_FMT::format("Initial: {}\n", initial);
std::cout << STD_FMT::format("{:>2} | {:>7} | {:>10} | {:>16} | {:>16}\n", "N", "Gain", "Measured", "Curr. Estimate", "Next Estimate");
std::cout << STD_FMT::format("{:>2} | {:>7} | {:>10} | {:>16} | {:>16}\n", "N", "Gain", "Measured", "Curr. Estimate",
"Next Estimate");
}
template<QuantityPoint QP, Dimensionless K>
void print(auto iteration, K gain, QP measured, kalman::estimation<QP> current, kalman::estimation<QP> next)
{
std::cout << STD_FMT::format("{:2} | {:7%.3Q} | {:10%.3Q %q} | {:>16.2} | {:>16.2}\n", iteration, gain, measured.relative(), current, next);
std::cout << STD_FMT::format("{:2} | {:7%.3Q} | {:10%.3Q %q} | {:>16.2} | {:>16.2}\n", iteration, gain,
measured.relative(), current, next);
}
int main()
@@ -72,32 +74,25 @@ int main()
using namespace units::isq::si::references;
const auto process_noise_variance = 0.15 * (deg_C * deg_C);
const estimation initial = { state{ quantity_point(10. * deg_C) }, pow<2>(100. * deg_C) };
const std::array measurements = {
quantity_point(50.45 * deg_C),
quantity_point(50.967 * deg_C),
quantity_point(51.6 * deg_C),
quantity_point(52.106 * deg_C),
quantity_point(52.492 * deg_C),
quantity_point(52.819 * deg_C),
quantity_point(53.433 * deg_C),
quantity_point(54.007 * deg_C),
quantity_point(54.523 * deg_C),
quantity_point(54.99 * deg_C)
};
const estimation initial = {state{quantity_point(10. * deg_C)}, pow<2>(100. * deg_C)};
const std::array measurements = {quantity_point(50.45 * deg_C), quantity_point(50.967 * deg_C),
quantity_point(51.6 * deg_C), quantity_point(52.106 * deg_C),
quantity_point(52.492 * deg_C), quantity_point(52.819 * deg_C),
quantity_point(53.433 * deg_C), quantity_point(54.007 * deg_C),
quantity_point(54.523 * deg_C), quantity_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, Dimensionless auto gain) {
return estimation{ state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, 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) };
return estimation{current.state, covariance_extrapolation(current.uncertainty, process_noise_variance)};
};
print_header(initial);
estimation next = predict(initial);
for(int index = 1; const auto& m : measurements) {
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);