feedc0de/mp-units
1
0
Fork 0
forked from mpusz/mp-units
Code Pull Requests Activity

feat: Kalman Filter tutorials 1-8 added

Browse Source
This commit is contained in:
Mateusz Pusz
2021-04-13 18:16:11 +02:00
parent 47ad0d7cdf
commit 97245a8c96
28 changed files with 985 additions and 214 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
docs/CMakeLists.txt
View File

@@ -63,7 +63,16 @@ set(unitsSphinxDocs
"${CMAKE_CURRENT_SOURCE_DIR}/examples/foot_pound_second.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/glide_computer.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/hello_units.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/kalman_filter-alpha_beta_filter_example2.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/kalman_filter.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/kalman_filter/example_1.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/kalman_filter/example_2.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/kalman_filter/example_3.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/kalman_filter/example_4.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/kalman_filter/example_5.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/kalman_filter/example_6.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/kalman_filter/example_7.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/kalman_filter/example_8.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/kalman_filter/kalman.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/linear_algebra.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/measurement.rst"
"${CMAKE_CURRENT_SOURCE_DIR}/examples/total_energy.rst"

3
docs/examples.rst
View File

@@ -18,5 +18,6 @@ Examples
examples/clcpp_response
examples/conversion_factor
examples/experimental_angle
examples/kalman_filter-alpha_beta_filter_example2
examples/total_energy
examples/kalman_filter

10
docs/examples/kalman_filter-alpha_beta_filter_example2.rst
View File

@@ -1,10 +0,0 @@
kalman_filter-alpha_beta_filter_example2
========================================
This example is based on the 1d aircraft α-β filter example 2 from the
`Kalman Filter <https://www.kalmanfilter.net/alphabeta.html#ex2>`_ tutorial.
.. literalinclude:: ../../example/references/kalman_filter-alpha_beta_filter_example2.cpp
:caption: kalman_filter-alpha_beta_filter_example2.cpp
:start-at: #include
:linenos:

17
docs/examples/kalman_filter.rst Normal file
View File

@@ -0,0 +1,17 @@
Kalman Filter Tutorial
======================
The following examples are based on the `Kalman Filter <https://www.kalmanfilter.net>`_ tutorial.
.. toctree::
:maxdepth: 1
kalman_filter/kalman
kalman_filter/example_1
kalman_filter/example_2
kalman_filter/example_3
kalman_filter/example_4
kalman_filter/example_5
kalman_filter/example_6
kalman_filter/example_7
kalman_filter/example_8

9
docs/examples/kalman_filter/example_1.rst Normal file
View File

@@ -0,0 +1,9 @@
Example 1 - Weighting the gold
==============================
Implementation of: https://www.kalmanfilter.net/alphabeta.html#ex1
.. literalinclude:: ../../../example/kalman_filter/kalman_filter-example_1.cpp
:caption: kalman_filter-example_1.cpp
:start-at: #include
:linenos:

9
docs/examples/kalman_filter/example_2.rst Normal file
View File

@@ -0,0 +1,9 @@
Example 2 - Tracking the constant velocity aircraft in one dimension
====================================================================
Implementation of: https://www.kalmanfilter.net/alphabeta.html#ex2
.. literalinclude:: ../../../example/kalman_filter/kalman_filter-example_2.cpp
:caption: kalman_filter-example_2.cpp
:start-at: #include
:linenos:

9
docs/examples/kalman_filter/example_3.rst Normal file
View File

@@ -0,0 +1,9 @@
Example 3 - Tracking accelerating aircraft in one dimension
===========================================================
Implementation of: https://www.kalmanfilter.net/alphabeta.html#ex3
.. literalinclude:: ../../../example/kalman_filter/kalman_filter-example_3.cpp
:caption: kalman_filter-example_3.cpp
:start-at: #include
:linenos:

9
docs/examples/kalman_filter/example_4.rst Normal file
View File

@@ -0,0 +1,9 @@
Example 4 - Tracking accelerating aircraft with the α−β−γ filter
================================================================
Implementation of: https://www.kalmanfilter.net/alphabeta.html#ex4
.. literalinclude:: ../../../example/kalman_filter/kalman_filter-example_4.cpp
:caption: kalman_filter-example_4.cpp
:start-at: #include
:linenos:

9
docs/examples/kalman_filter/example_5.rst Normal file
View File

@@ -0,0 +1,9 @@
Example 5 - Estimating the building height
==========================================
Implementation of: https://www.kalmanfilter.net/kalman1d.html#ex5
.. literalinclude:: ../../../example/kalman_filter/kalman_filter-example_5.cpp
:caption: kalman_filter-example_5.cpp
:start-at: #include
:linenos:

9
docs/examples/kalman_filter/example_6.rst Normal file
View File

@@ -0,0 +1,9 @@
Example 6 - Estimating the temperature of the liquid in a tank
==============================================================
Implementation of: https://www.kalmanfilter.net/kalman1d.html#ex6
.. literalinclude:: ../../../example/kalman_filter/kalman_filter-example_6.cpp
:caption: kalman_filter-example_6.cpp
:start-at: #include
:linenos:

9
docs/examples/kalman_filter/example_7.rst Normal file
View File

@@ -0,0 +1,9 @@
Example 7 - Estimating the temperature of the heaing liquid I
=============================================================
Implementation of: https://www.kalmanfilter.net/kalman1d.html#ex7
.. literalinclude:: ../../../example/kalman_filter/kalman_filter-example_7.cpp
:caption: kalman_filter-example_7.cpp
:start-at: #include
:linenos:

9
docs/examples/kalman_filter/example_8.rst Normal file
View File

@@ -0,0 +1,9 @@
Example 8 - Estimating the temperature of the heaing liquid II
==============================================================
Implementation of: https://www.kalmanfilter.net/kalman1d.html#ex8
.. literalinclude:: ../../../example/kalman_filter/kalman_filter-example_8.cpp
:caption: kalman_filter-example_8.cpp
:start-at: #include
:linenos:

7
docs/examples/kalman_filter/kalman.rst Normal file
View File

@@ -0,0 +1,7 @@
Common utility header file (``kalman.h``)
=========================================
.. literalinclude:: ../../../example/kalman_filter/kalman.h
:caption: kalman.h
:start-at: #include
:linenos:

1
example/CMakeLists.txt
View File

@@ -39,5 +39,6 @@ if(NOT UNITS_LIBCXX)
endif()
add_subdirectory(alternative_namespaces)
add_subdirectory(kalman_filter)
add_subdirectory(literals)
add_subdirectory(references)

41
example/kalman_filter/CMakeLists.txt Normal file
View File

@@ -0,0 +1,41 @@
# The MIT License (MIT)
#
# Copyright (c) 2018 Mateusz Pusz
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
cmake_minimum_required(VERSION 3.2)
#
# add_example(target <depependencies>...)
#
function(add_example target)
add_executable(${target} ${target}.cpp)
target_link_libraries(${target} PRIVATE ${ARGN})
target_compile_definitions(${target} PRIVATE UNITS_REFERENCES)
endfunction()
add_example(kalman_filter-example_1 mp-units::core-fmt mp-units::si)
add_example(kalman_filter-example_2 mp-units::core-fmt mp-units::si)
add_example(kalman_filter-example_3 mp-units::core-fmt mp-units::si)
add_example(kalman_filter-example_4 mp-units::core-fmt mp-units::si)
add_example(kalman_filter-example_5 mp-units::core-fmt mp-units::si)
add_example(kalman_filter-example_6 mp-units::core-fmt mp-units::si)
add_example(kalman_filter-example_7 mp-units::core-fmt mp-units::si)
add_example(kalman_filter-example_8 mp-units::core-fmt mp-units::si)

180
example/kalman_filter/kalman.h Normal file
View File

@@ -0,0 +1,180 @@
// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
#pragma once
#include <units/format.h>
#include <units/generic/dimensionless.h>
#include <units/isq/dimensions/time.h>
#include <units/math.h>
#include <units/quantity.h>
#include <units/quantity_point.h>
#include <tuple>
namespace kalman {
template<typename T>
concept QuantityOrQuantityPoint = units::Quantity<T> || units::QuantityPoint<T>; // TODO Should it also account for `kinds`?
template<typename... Qs>
inline constexpr bool are_derivatives = false;
template<typename Q>
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
are_derivatives<Q2, Qs...>;
// state
template<QuantityOrQuantityPoint... 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)...) {}
};
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_); }
template<std::size_t Idx, typename... Qs>
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>;
public:
kalman::state<QQP, QQPs...> state; // TODO extend kalman functions to work with this variadic patermater list
uncertainty_type uncertainty;
};
// kalman gain
template<units::Quantity Q>
constexpr 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>
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>
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));
const auto q2 = get<1>(predicted) + get<1>(gain) * (measured - get<0>(predicted)) / interval;
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)
{
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 };
}
// covariance update
template<units::Quantity Q, 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>
constexpr state<Q1, Q2> state_extrapolation(const state<Q1, Q2>& estimated, T interval)
{
const auto q1 = get<0>(estimated) + get<1>(estimated) * interval;
const auto q2 = get<1>(estimated);
return { q1, q2 };
}
template<typename Q1, typename Q2, typename Q3, 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;
const auto q2 = get<1>(estimated) + get<2>(estimated) * interval;
const auto q3 = get<2>(estimated);
return { q1, q2, q3 };
}
// covariance extrapolation
template<units::Quantity Q>
constexpr Q covariance_extrapolation(Q uncertainty, Q process_noise_variance)
{
return uncertainty + process_noise_variance;
}
} // namespace kalman
template<typename... Qs>
struct fmt::formatter<kalman::state<Qs...>> : formatter<std::string> {
template<typename FormatContext>
auto format(const kalman::state<Qs...>& s, FormatContext& ctx)
{
std::string txt = [&]{
if constexpr(sizeof...(Qs) == 1)
return fmt::format("{1:%.{0}Q %q}", precision, kalman::get<0>(s));
else if constexpr(sizeof...(Qs) == 2)
return fmt::format("{{ {1:%.{0}Q %q}, {2:%.{0}Q %q} }}", precision, kalman::get<0>(s), kalman::get<1>(s));
else
return fmt::format("{{ {1:%.{0}Q %q}, {2:%.{0}Q %q}, {3:%.{0}Q %q} }}", precision, kalman::get<0>(s), kalman::get<1>(s), kalman::get<2>(s));
}();
return formatter<std::string>::format(txt, ctx);
}
private:
int precision = 1; // TODO find the easiest way to parse it from context
};
template<typename Q>
struct fmt::formatter<kalman::estimation<Q>> : formatter<std::string> {
template<typename FormatContext>
auto format(kalman::estimation<Q> e, FormatContext& ctx)
{
units::Quantity auto q = [](const Q& t) {
if constexpr(units::Quantity<Q>)
return t;
else
return t.relative();
}(kalman::get<0>(e.state));
std::string txt = fmt::format("{0:%.{2}Q} ± {1:%.{2}Q} {0:%q}", q, sqrt(e.uncertainty), precision);
return formatter<std::string>::format(txt, ctx);
}
private:
int precision = 3; // TODO find the easiest way to parse it from context
};

63
example/kalman_filter/kalman_filter-example_1.cpp Normal file
View File

@@ -0,0 +1,63 @@
// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
#include "kalman.h"
#include <units/isq/si/mass.h>
#include <units/format.h>
#include <array>
#include <iostream>
// Based on: https://www.kalmanfilter.net/alphabeta.html#ex1
using namespace units;
void print_header(const kalman::State auto& initial)
{
fmt::print("Initial: {}\n", initial);
fmt::print("{:>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)
{
fmt::print("{: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>>;
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) {
const auto& previous = next;
const dimensionless<one> gain = 1. / index;
const auto current = state_update(previous, m, gain);
next = current;
print(index++, gain, m, current, next);
}
}

66
example/kalman_filter/kalman_filter-example_2.cpp Normal file
View File

@@ -0,0 +1,66 @@
// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
#include "kalman.h"
#include <units/isq/si/length.h>
#include <units/isq/si/speed.h>
#include <units/isq/si/time.h>
#include <units/format.h>
#include <array>
#include <iostream>
// Based on: https://www.kalmanfilter.net/alphabeta.html#ex2
using namespace units;
void print_header(const kalman::State auto& initial)
{
fmt::print("Initial: {}\n", initial);
fmt::print("{:>2} | {:>8} | {:>23} | {:>23}\n", "N", "Measured", "Curr. Estimate", "Next Estimate");
}
void print(auto iteration, Quantity auto measured, const kalman::State auto& current, const kalman::State auto& next)
{
fmt::print("{:2} | {:8} | {} | {}\n", iteration, measured, current, next);
}
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>>;
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)};
print_header(initial);
state next = state_extrapolation(initial, interval);
for(int index = 1; const auto& measured : measurements) {
const auto& previous = next;
const auto current = state_update(previous, measured, gain, interval);
next = state_extrapolation(current, interval);
print(index++, measured, current, next);
}
}

66
example/kalman_filter/kalman_filter-example_3.cpp Normal file
View File

@@ -0,0 +1,66 @@
// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
#include "kalman.h"
#include <units/isq/si/length.h>
#include <units/isq/si/speed.h>
#include <units/isq/si/time.h>
#include <units/format.h>
#include <array>
#include <iostream>
// Based on: https://www.kalmanfilter.net/alphabeta.html#ex3
using namespace units;
void print_header(const kalman::State auto& initial)
{
fmt::print("Initial: {}\n", initial);
fmt::print("{:>2} | {:>8} | {:>23} | {:>23}\n", "N", "Measured", "Curr. Estimate", "Next Estimate");
}
void print(auto iteration, Quantity auto measured, const kalman::State auto& current, const kalman::State auto& next)
{
fmt::print("{:2} | {:8} | {} | {}\n", iteration, measured, current, next);
}
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>>;
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)};
print_header(initial);
state next = state_extrapolation(initial, interval);
for(int index = 1; const auto& measured : measurements) {
const auto& previous = next;
const auto current = state_update(previous, measured, gain, interval);
next = state_extrapolation(current, interval);
print(index++, measured, current, next);
}
}

69
example/kalman_filter/kalman_filter-example_4.cpp Normal file
View File

@@ -0,0 +1,69 @@
// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
#include "kalman.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 <array>
#include <iostream>
// Based on: https://www.kalmanfilter.net/alphabeta.html#ex4
using namespace units;
void print_header(const kalman::State auto& initial)
{
fmt::print("Initial: {}\n", initial);
fmt::print("{:>2} | {:>8} | {:>35} | {:>35}\n", "N", "Measured", "Curr. Estimate", "Next Estimate");
}
void print(auto iteration, Quantity auto measured, const kalman::State auto& current, const kalman::State auto& next)
{
fmt::print("{:2} | {:8} | {:>35} | {:>35}\n", iteration, measured, current, next);
}
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;
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)};
print_header(initial);
state next = state_extrapolation(initial, interval);
for(int index = 1; const auto& measured : measurements) {
const auto& previous = next;
const auto current = state_update(previous, measured, gain, interval);
next = state_extrapolation(current, interval);
print(index++, measured, current, next);
}
}

73
example/kalman_filter/kalman_filter-example_5.cpp Normal file
View File

@@ -0,0 +1,73 @@
// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
#include "kalman.h"
#include <units/isq/si/length.h>
#include <units/format.h>
#include <units/math.h>
#include <array>
#include <iostream>
// Based on: https://www.kalmanfilter.net/kalman1d.html#ex5
using namespace units;
template<Quantity Q>
void print_header(kalman::estimation<Q> initial)
{
fmt::print("Initial: {}\n", initial);
fmt::print("{:>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)
{
fmt::print("{:2} | {:5%.2Q} | {:8} | {:16} | {:16}\n", iteration, gain, measured, current, next);
}
int main()
{
using namespace kalman;
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 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) };
};
auto predict = []<Quantity Q>(const estimation<Q>& current) { return current; };
print_header(initial);
estimation next = predict(initial);
for(int index = 1; const auto& m : measurements) {
const auto& previous = next;
const auto gain = kalman_gain(previous.uncertainty, measurement_uncertainty);
const estimation current = update(previous, m, gain);
next = predict(current);
print(index++, gain, m, current, next);
}
}

106
example/kalman_filter/kalman_filter-example_6.cpp Normal file
View File

@@ -0,0 +1,106 @@
// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
#include "kalman.h"
#include <units/isq/si/thermodynamic_temperature.h>
#include <units/format.h>
#include <units/math.h>
#include <units/quantity_point.h>
#include <array>
#include <iostream>
// TODO Fix when Celsius is properly supported (#232)
namespace units::isq::si {
struct degree_celsius : alias_unit<kelvin, basic_symbol_text{"°C", "deg_C"}, no_prefix> {};
namespace thermodynamic_temperature_references {
inline constexpr auto deg_C = reference<dim_thermodynamic_temperature, degree_celsius>{};
} // namespace thermodynamic_temperature_references
namespace references {
using namespace thermodynamic_temperature_references;
} // namespace references
} // namespace units::isq::si
// Based on: https://www.kalmanfilter.net/kalman1d.html#ex6
using namespace units;
template<QuantityPoint QP>
void print_header(kalman::estimation<QP> initial)
{
fmt::print("Initial: {}\n", initial);
fmt::print("{:>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)
{
fmt::print("{:2} | {:7%.4Q} | {:10%.3Q %q} | {:>18} | {:>18}\n", iteration, gain, measured.relative(), current, next);
}
int main()
{
using namespace kalman;
using namespace units::isq;
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 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) };
};
auto predict = [=]<QuantityPoint QP>(const estimation<QP>& current) {
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) {
const auto& previous = next;
const auto gain = kalman_gain(previous.uncertainty, measurement_uncertainty);
const estimation current = update(previous, m, gain);
next = predict(current);
print(index++, gain, m, current, next);
}
}

106
example/kalman_filter/kalman_filter-example_7.cpp Normal file
View File

@@ -0,0 +1,106 @@
// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
#include "kalman.h"
#include <units/isq/si/thermodynamic_temperature.h>
#include <units/format.h>
#include <units/math.h>
#include <units/quantity_point.h>
#include <array>
#include <iostream>
// TODO Fix when Celsius is properly supported (#232)
namespace units::isq::si {
struct degree_celsius : alias_unit<kelvin, basic_symbol_text{"°C", "deg_C"}, no_prefix> {};
namespace thermodynamic_temperature_references {
inline constexpr auto deg_C = reference<dim_thermodynamic_temperature, degree_celsius>{};
} // namespace thermodynamic_temperature_references
namespace references {
using namespace thermodynamic_temperature_references;
} // namespace references
} // namespace units::isq::si
// Based on: https://www.kalmanfilter.net/kalman1d.html#ex7
using namespace units;
template<QuantityPoint QP>
void print_header(kalman::estimation<QP> initial)
{
fmt::print("Initial: {}\n", initial);
fmt::print("{:>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)
{
fmt::print("{:2} | {:7%.4Q} | {:10%.3Q %q} | {:>18} | {:>18}\n", iteration, gain, measured.relative(), current, next);
}
int main()
{
using namespace kalman;
using namespace units::isq;
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 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) };
};
auto predict = [=]<QuantityPoint QP>(const estimation<QP>& current) {
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) {
const auto& previous = next;
const auto gain = kalman_gain(previous.uncertainty, measurement_uncertainty);
const estimation current = update(previous, m, gain);
next = predict(current);
print(index++, gain, m, current, next);
}
}

106
example/kalman_filter/kalman_filter-example_8.cpp Normal file
View File

@@ -0,0 +1,106 @@
// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
#include "kalman.h"
#include <units/isq/si/thermodynamic_temperature.h>
#include <units/format.h>
#include <units/math.h>
#include <units/quantity_point.h>
#include <array>
#include <iostream>
// TODO Fix when Celsius is properly supported (#232)
namespace units::isq::si {
struct degree_celsius : alias_unit<kelvin, basic_symbol_text{"°C", "deg_C"}, no_prefix> {};
namespace thermodynamic_temperature_references {
inline constexpr auto deg_C = reference<dim_thermodynamic_temperature, degree_celsius>{};
} // namespace thermodynamic_temperature_references
namespace references {
using namespace thermodynamic_temperature_references;
} // namespace references
} // namespace units::isq::si
// Based on: https://www.kalmanfilter.net/kalman1d.html#ex8
using namespace units;
template<QuantityPoint QP>
void print_header(kalman::estimation<QP> initial)
{
fmt::print("Initial: {}\n", initial);
fmt::print("{:>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)
{
fmt::print("{:2} | {:7%.4Q} | {:10%.3Q %q} | {:>18} | {:>18}\n", iteration, gain, measured.relative(), current, next);
}
int main()
{
using namespace kalman;
using namespace units::isq;
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 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) };
};
auto predict = [=]<QuantityPoint QP>(const estimation<QP>& current) {
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) {
const auto& previous = next;
const auto gain = kalman_gain(previous.uncertainty, measurement_uncertainty);
const estimation current = update(previous, m, gain);
next = predict(current);
print(index++, gain, m, current, next);
}
}

1
example/literals/CMakeLists.txt
View File

@@ -37,7 +37,6 @@ add_example(capacitor_time_curve mp-units::core-io mp-units::si)
add_example(clcpp_response mp-units::core-fmt mp-units::core-io mp-units::si mp-units::si-iau mp-units::si-imperial mp-units::si-international mp-units::si-typographic mp-units::si-us)
add_example(experimental_angle mp-units::core-fmt mp-units::core-io mp-units::si)
add_example(foot_pound_second mp-units::core-fmt mp-units::si-fps)
add_example(kalman_filter-alpha_beta_filter_example2 mp-units::core-fmt mp-units::si)
add_example(measurement mp-units::core-io mp-units::si)
add_example(total_energy mp-units::core-io mp-units::si mp-units::isq-natural)
add_example(unknown_dimension mp-units::core-io mp-units::si)

100
example/literals/kalman_filter-alpha_beta_filter_example2.cpp
View File

@@ -1,100 +0,0 @@
// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
#include <units/format.h>
#include <units/isq/si/length.h>
#include <units/isq/si/speed.h>
#include <units/isq/si/time.h>
#include <iostream>
#include <array>
/*
kalman filter tutorial
1d aircraft α-β filter example2 from https://www.kalmanfilter.net/alphabeta.html#ex2
*/
namespace {
template<units::Quantity Q>
struct state_variable {
Q estimated_current_state;
Q predicted_next_state;
};
using namespace units::isq;
using namespace units::isq::si::literals;
constexpr auto radar_transmit_interval = 5.0_q_s;
constexpr double kalman_range_gain = 0.2;
constexpr double kalman_speed_gain = 0.1;
struct state {
state_variable<si::length<si::metre>> range;
state_variable<si::speed<si::metre_per_second>> speed;
constexpr void estimate(const state& previous_state, const si::length<si::metre>& measurement)
{
auto const innovation = measurement - previous_state.range.predicted_next_state;
range.estimated_current_state = previous_state.range.predicted_next_state + kalman_range_gain * innovation;
speed.estimated_current_state =
previous_state.speed.predicted_next_state + kalman_speed_gain * innovation / radar_transmit_interval;
}
constexpr void predict()
{
range.predicted_next_state =
range.estimated_current_state + speed.estimated_current_state * radar_transmit_interval;
speed.predicted_next_state = speed.estimated_current_state;
}
};
} // namespace
int main()
{
std::cout << "\n\n1d aircraft α-β filter example2 from https://www.kalmanfilter.net/alphabeta.html#ex2";
std::cout << "\n\n";
constexpr auto measurements = std::array{0.0_q_m, // N.B measurement[0] is unknown and unused
30110.0_q_m, 30265.0_q_m, 30740.0_q_m, 30750.0_q_m, 31135.0_q_m,
31015.0_q_m, 31180.0_q_m, 31610.0_q_m, 31960.0_q_m, 31865.0_q_m};
constexpr auto num_measurements = measurements.size();
std::array<state, num_measurements> track;
// We need an initial estimate of track[0] as there is no previous state to get a prediction from
track[0].range.estimated_current_state = 30'000_q_m;
track[0].speed.estimated_current_state = 40.0_q_m_per_s;
for (auto n = 0U; n < num_measurements; ++n) {
if (n > 0) {
track[n].estimate(track[n - 1], measurements[n]);
}
track[n].predict();
std::cout << fmt::format("measurement[{}] = {:.0}\n", n, measurements[n]);
std::cout << fmt::format("range.estimated_current_state[{}] = {:.1}\n", n, track[n].range.estimated_current_state);
std::cout << fmt::format("speed.estimated_current_state[{}] = {:.1}\n", n, track[n].speed.estimated_current_state);
std::cout << fmt::format("range.predicted_next_state[{}] = {:.1}\n", n, track[n].range.predicted_next_state);
std::cout << fmt::format("speed.predicted_next_state[{}] = {:.1}\n\n", n, track[n].speed.predicted_next_state);
}
}

1
example/references/CMakeLists.txt
View File

@@ -37,7 +37,6 @@ add_example(capacitor_time_curve mp-units::core-io mp-units::si)
add_example(clcpp_response mp-units::core-fmt mp-units::core-io mp-units::si mp-units::si-iau mp-units::si-imperial mp-units::si-international mp-units::si-typographic mp-units::si-us)
add_example(experimental_angle mp-units::core-fmt mp-units::core-io mp-units::si)
add_example(foot_pound_second mp-units::core-fmt mp-units::si-fps)
add_example(kalman_filter-alpha_beta_filter_example2 mp-units::core-fmt mp-units::si)
add_example(measurement mp-units::core-io mp-units::si)
add_example(total_energy mp-units::core-io mp-units::si mp-units::isq-natural)
add_example(unknown_dimension mp-units::core-io mp-units::si)

100
example/references/kalman_filter-alpha_beta_filter_example2.cpp
View File

@@ -1,100 +0,0 @@
// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
#include <units/format.h>
#include <units/isq/si/length.h>
#include <units/isq/si/speed.h>
#include <units/isq/si/time.h>
#include <iostream>
#include <array>
/*
kalman filter tutorial
1d aircraft α-β filter example2 from https://www.kalmanfilter.net/alphabeta.html#ex2
*/
namespace {
template<units::Quantity Q>
struct state_variable {
Q estimated_current_state;
Q predicted_next_state;
};
using namespace units::isq;
using namespace units::isq::si::references;
constexpr auto radar_transmit_interval = 5.0 * s;
constexpr double kalman_range_gain = 0.2;
constexpr double kalman_speed_gain = 0.1;
struct state {
state_variable<si::length<si::metre>> range;
state_variable<si::speed<si::metre_per_second>> speed;
constexpr void estimate(const state& previous_state, const si::length<si::metre>& measurement)
{
auto const innovation = measurement - previous_state.range.predicted_next_state;
range.estimated_current_state = previous_state.range.predicted_next_state + kalman_range_gain * innovation;
speed.estimated_current_state =
previous_state.speed.predicted_next_state + kalman_speed_gain * innovation / radar_transmit_interval;
}
constexpr void predict()
{
range.predicted_next_state =
range.estimated_current_state + speed.estimated_current_state * radar_transmit_interval;
speed.predicted_next_state = speed.estimated_current_state;
}
};
} // namespace
int main()
{
std::cout << "\n\n1d aircraft α-β filter example2 from https://www.kalmanfilter.net/alphabeta.html#ex2";
std::cout << "\n\n";
constexpr auto measurements = std::array{0.0 * m, // N.B measurement[0] is unknown and unused
30110.0 * m, 30265.0 * m, 30740.0 * m, 30750.0 * m, 31135.0 * m,
31015.0 * m, 31180.0 * m, 31610.0 * m, 31960.0 * m, 31865.0 * m};
constexpr auto num_measurements = measurements.size();
std::array<state, num_measurements> track;
// We need an initial estimate of track[0] as there is no previous state to get a prediction from
track[0].range.estimated_current_state = 30'000 * m;
track[0].speed.estimated_current_state = 40.0 * (m / s);
for (auto n = 0U; n < num_measurements; ++n) {
if (n > 0) {
track[n].estimate(track[n - 1], measurements[n]);
}
track[n].predict();
std::cout << fmt::format("measurement[{}] = {:.0}\n", n, measurements[n]);
std::cout << fmt::format("range.estimated_current_state[{}] = {:.1}\n", n, track[n].range.estimated_current_state);
std::cout << fmt::format("speed.estimated_current_state[{}] = {:.1}\n", n, track[n].speed.estimated_current_state);
std::cout << fmt::format("range.predicted_next_state[{}] = {:.1}\n", n, track[n].range.predicted_next_state);
std::cout << fmt::format("speed.predicted_next_state[{}] = {:.1}\n\n", n, track[n].speed.predicted_next_state);
}
}