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

refactor(example): kalman filter 1-5 examples refactored for V2

Browse Source
This commit is contained in:
Mateusz Pusz
2022-12-20 17:29:31 +01:00
parent 82b1f7ffc0
commit 2b760b5d46
6 changed files with 97 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
example/kalman_filter/kalman.h
View File

@@ -24,8 +24,7 @@
#include <units/bits/fmt_hacks.h>
#include <units/format.h>
#include <units/generic/dimensionless.h>
#include <units/isq/dimensions/time.h>
#include <units/isq/space_and_time.h>
#include <units/math.h>
#include <units/quantity.h>
#include <units/quantity_point.h>
@@ -37,21 +36,19 @@ template<typename T>
concept QuantityOrQuantityPoint =
units::Quantity<T> || units::QuantityPoint<T>; // TODO Should it also account for `kinds`?
template<typename... Qs>
template<units::Dimension auto... Ds>
inline constexpr bool are_time_derivatives = false;
template<typename Q>
inline constexpr bool are_time_derivatives<Q> = true;
template<units::Dimension auto D>
inline constexpr bool are_time_derivatives<D> = true;
template<typename Q1, typename Q2, typename... Qs>
inline constexpr bool are_time_derivatives<Q1, Q2, Qs...> =
units::DimensionOfT<typename decltype(Q1::reference / Q2::reference)::dimension,
units::isq::dim_time> && // TODO Think on how to simplify this
are_time_derivatives<Q2, Qs...>;
template<units::Dimension auto D1, units::Dimension auto D2, units::Dimension auto... Ds>
inline constexpr bool are_time_derivatives<D1, D2, Ds...> =
(D1 / D2 == units::isq::dim_time) && are_time_derivatives<D2, Ds...>;
// state
template<QuantityOrQuantityPoint... QQPs>
requires(sizeof...(QQPs) > 0) && (sizeof...(QQPs) <= 3) && are_time_derivatives<QQPs...>
requires(sizeof...(QQPs) > 0) && (sizeof...(QQPs) <= 3) && are_time_derivatives<QQPs::dimension...>
struct state {
std::tuple<QQPs...> variables_;
constexpr state(QQPs... qqps) : variables_(std::move(qqps)...) {}
@@ -76,11 +73,10 @@ constexpr const auto& get(const state<Qs...>& s)
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>;
static constexpr auto uncertainty_ref = QQP::reference * QQP::reference;
using uncertainty_type = units::quantity<uncertainty_ref, 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 parameter list
uncertainty_type uncertainty;
};
@@ -93,21 +89,23 @@ estimation(state<QQP>, U) -> estimation<QQP>;
// kalman gain
template<units::Quantity Q>
constexpr units::dimensionless<units::one> kalman_gain(Q estimate_uncertainty, Q measurement_uncertainty)
constexpr units::quantity<units::dimensionless[units::one]> kalman_gain(Q estimate_uncertainty,
Q measurement_uncertainty)
{
return estimate_uncertainty / (estimate_uncertainty + measurement_uncertainty);
}
// state update
template<typename Q, QuantityOrQuantityPoint QM, units::Dimensionless K>
requires units::equivalent<typename Q::dimension, typename QM::dimension>
template<typename Q, QuantityOrQuantityPoint QM, units::quantity_of<units::dimensionless> K>
requires(Q::quantity_spec == QM::quantity_spec)
constexpr state<Q> state_update(const state<Q>& predicted, QM measured, K gain)
{
return {get<0>(predicted) + gain * (measured - get<0>(predicted))};
}
template<typename Q1, typename Q2, QuantityOrQuantityPoint QM, units::Dimensionless K, units::isq::Time T>
requires units::equivalent<typename Q1::dimension, typename QM::dimension>
template<typename Q1, typename Q2, QuantityOrQuantityPoint QM, units::quantity_of<units::dimensionless> K,
units::quantity_of<units::isq::time> T>
requires(Q1::quantity_spec == QM::quantity_spec)
constexpr state<Q1, Q2> state_update(const state<Q1, Q2>& predicted, QM measured, std::array<K, 2> gain, T interval)
{
const auto q1 = get<0>(predicted) + get<0>(gain) * (measured - get<0>(predicted));
@@ -115,8 +113,9 @@ constexpr state<Q1, Q2> state_update(const state<Q1, Q2>& predicted, QM measured
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>
template<typename Q1, typename Q2, typename Q3, QuantityOrQuantityPoint QM, units::quantity_of<units::dimensionless> K,
units::quantity_of<units::isq::time> T>
requires(Q1::quantity_spec == QM::quantity_spec)
constexpr state<Q1, Q2, Q3> state_update(const state<Q1, Q2, Q3>& predicted, QM measured, std::array<K, 3> gain,
T interval)
{
@@ -127,14 +126,14 @@ constexpr state<Q1, Q2, Q3> state_update(const state<Q1, Q2, Q3>& predicted, QM
}
// covariance update
template<units::Quantity Q, units::Dimensionless K>
template<units::Quantity Q, units::quantity_of<units::dimensionless> K>
constexpr Q covariance_update(Q uncertainty, K gain)
{
return (1 - gain) * uncertainty;
}
// state extrapolation
template<typename Q1, typename Q2, units::isq::Time T>
template<typename Q1, typename Q2, units::quantity_of<units::isq::time> T>
constexpr state<Q1, Q2> state_extrapolation(const state<Q1, Q2>& estimated, T interval)
{
const auto q1 = get<0>(estimated) + get<1>(estimated) * interval;
@@ -142,7 +141,7 @@ constexpr state<Q1, Q2> state_extrapolation(const state<Q1, Q2>& estimated, T in
return {q1, q2};
}
template<typename Q1, typename Q2, typename Q3, units::isq::Time T>
template<typename Q1, typename Q2, typename Q3, units::quantity_of<units::isq::time> T>
constexpr state<Q1, Q2, Q3> state_extrapolation(const state<Q1, Q2, Q3>& estimated, T interval)
{
const auto q1 = get<0>(estimated) + get<1>(estimated) * interval + get<2>(estimated) * pow<2>(interval) / 2;

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

@@ -22,8 +22,8 @@
#include "kalman.h"
#include <units/format.h>
#include <units/generic/dimensionless.h>
#include <units/isq/si/mass.h>
#include <units/isq/space_and_time.h>
#include <units/si/unit_symbols.h>
#include <array>
#include <iostream>
@@ -38,27 +38,27 @@ void print_header(const kalman::State auto& initial)
"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, quantity_of<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);
}
int main()
{
using namespace units::isq;
using namespace units::isq::si::references;
using state = kalman::state<si::mass<si::gram>>;
using namespace units::si::unit_symbols;
using state = kalman::state<quantity<isq::mass[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};
const state initial = {1 * isq::mass[kg]};
const std::array measurements = {1030 * isq::mass[g], 989 * isq::mass[g], 1017 * isq::mass[g], 1009 * isq::mass[g],
1013 * isq::mass[g], 979 * isq::mass[g], 1008 * isq::mass[g], 1042 * isq::mass[g],
1012 * isq::mass[g], 1011 * isq::mass[g]};
print_header(initial);
state next = initial;
for (int index = 1; const auto& m : measurements) {
const auto& previous = next;
const dimensionless<one> gain = 1. / index;
const quantity gain = 1. / index;
const auto current = state_update(previous, m, gain);
next = current;
print(index++, gain, m, current, next);

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

@@ -22,15 +22,17 @@
#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/isq/space_and_time.h>
#include <units/si/unit_symbols.h>
#include <array>
#include <iostream>
// Based on: https://www.kalmanfilter.net/alphabeta.html#ex2
template<class T>
requires units::is_scalar<T>
inline constexpr bool units::is_vector<T> = true;
using namespace units;
void print_header(const kalman::State auto& initial)
@@ -46,15 +48,17 @@ void print(auto iteration, Quantity auto measured, const kalman::State auto& cur
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>>;
using namespace units::si::unit_symbols;
using state = kalman::state<quantity<isq::position_vector[m]>, quantity<isq::velocity[m / s]>>;
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};
std::array gain = {dimensionless<one>(0.2), dimensionless<one>(0.1)};
const auto interval = 5 * isq::period_duration[s];
const state initial = {30 * isq::position_vector[km], 40 * isq::velocity[m / s]};
const std::array measurements = {30110 * isq::position_vector[m], 30265 * isq::position_vector[m],
30740 * isq::position_vector[m], 30750 * isq::position_vector[m],
31135 * isq::position_vector[m], 31015 * isq::position_vector[m],
31180 * isq::position_vector[m], 31610 * isq::position_vector[m],
31960 * isq::position_vector[m], 31865 * isq::position_vector[m]};
std::array gain = {quantity{0.2}, quantity{0.1}};
print_header(initial);
state next = state_extrapolation(initial, interval);

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

@@ -22,15 +22,17 @@
#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/isq/space_and_time.h>
#include <units/si/unit_symbols.h>
#include <array>
#include <iostream>
// Based on: https://www.kalmanfilter.net/alphabeta.html#ex3
template<class T>
requires units::is_scalar<T>
inline constexpr bool units::is_vector<T> = true;
using namespace units;
void print_header(const kalman::State auto& initial)
@@ -46,15 +48,17 @@ void print(auto iteration, Quantity auto measured, const kalman::State auto& cur
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>>;
using namespace units::si::unit_symbols;
using state = kalman::state<quantity<isq::position_vector[m]>, quantity<isq::velocity[m / s]>>;
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};
std::array gain = {dimensionless<one>(0.2), dimensionless<one>(0.1)};
const auto interval = 5 * isq::period_duration[s];
const state initial = {30 * isq::position_vector[km], 50 * isq::velocity[m / s]};
const std::array measurements = {30160 * isq::position_vector[m], 30365 * isq::position_vector[m],
30890 * isq::position_vector[m], 31050 * isq::position_vector[m],
31785 * isq::position_vector[m], 32215 * isq::position_vector[m],
33130 * isq::position_vector[m], 34510 * isq::position_vector[m],
36010 * isq::position_vector[m], 37265 * isq::position_vector[m]};
std::array gain = {quantity{0.2}, quantity{0.1}};
print_header(initial);
state next = state_extrapolation(initial, interval);

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

@@ -22,16 +22,17 @@
#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/isq/space_and_time.h>
#include <units/si/unit_symbols.h>
#include <array>
#include <iostream>
// Based on: https://www.kalmanfilter.net/alphabeta.html#ex4
template<class T>
requires units::is_scalar<T>
inline constexpr bool units::is_vector<T> = true;
using namespace units;
void print_header(const kalman::State auto& initial)
@@ -47,18 +48,18 @@ void print(auto iteration, Quantity auto measured, const kalman::State auto& cur
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>>;
constexpr auto mps = m / s;
constexpr auto mps2 = mps / s;
using namespace units::si::unit_symbols;
using state = kalman::state<quantity<isq::position_vector[m]>, quantity<isq::velocity[m / s]>,
quantity<isq::acceleration[m / s2]>>;
const auto interval = 5. * isq::period_duration[s];
const state initial = {30 * isq::position_vector[km], 50 * isq::velocity[m / s], 0 * isq::acceleration[m / s2]};
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};
std::array gain = {dimensionless<one>(0.5), dimensionless<one>(0.4), dimensionless<one>(0.1)};
const std::array measurements = {30160 * isq::position_vector[m], 30365 * isq::position_vector[m],
30890 * isq::position_vector[m], 31050 * isq::position_vector[m],
31785 * isq::position_vector[m], 32215 * isq::position_vector[m],
33130 * isq::position_vector[m], 34510 * isq::position_vector[m],
36010 * isq::position_vector[m], 37265 * isq::position_vector[m]};
std::array gain = {quantity{0.5}, quantity{0.4}, quantity{0.1}};
print_header(initial);
state next = state_extrapolation(initial, interval);

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

@@ -22,8 +22,9 @@
#include "kalman.h"
#include <units/format.h>
#include <units/isq/si/length.h>
#include <units/isq/space_and_time.h>
#include <units/math.h>
#include <units/si/unit_symbols.h>
#include <array>
#include <iostream>
@@ -39,7 +40,7 @@ void print_header(kalman::estimation<Q> initial)
"Next Estimate");
}
template<Quantity Q, Dimensionless K>
template<Quantity Q, quantity_of<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,
@@ -49,16 +50,18 @@ void print(auto iteration, K gain, Q measured, kalman::estimation<Q> current, ka
int main()
{
using namespace kalman;
using namespace units::isq;
using namespace units::isq::si::references;
using namespace units::si::unit_symbols;
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);
const estimation initial = {state{60. * isq::height[m]}, pow<2>(15. * isq::height[m])};
const std::array measurements = {48.54 * isq::height[m], 47.11 * isq::height[m], 55.01 * isq::height[m],
55.15 * isq::height[m], 49.89 * isq::height[m], 40.85 * isq::height[m],
46.72 * isq::height[m], 50.05 * isq::height[m], 51.27 * isq::height[m],
49.95 * isq::height[m]};
const auto measurement_uncertainty = pow<2>(5. * isq::height[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)};
auto update = [=]<Quantity Q>(const estimation<Q>& previous, const Q& measurement,
quantity_of<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; };