style: clang-format applied to the remaining files
This commit is contained in:
1 parent
0dca41b5ae
commit
b221dace3f
109 files changed
+4546
-3140
No files matched your search
@@ -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);
|
||||
|
||||
Reference in new issue
Block a user