From 97245a8c96491c6eb39f200ccdcb8172ab386995 Mon Sep 17 00:00:00 2001 From: Mateusz Pusz Date: Tue, 13 Apr 2021 18:16:11 +0200 Subject: [PATCH] feat: Kalman Filter tutorials 1-8 added --- docs/CMakeLists.txt | 11 +- docs/examples.rst | 3 +- ...lman_filter-alpha_beta_filter_example2.rst | 10 - docs/examples/kalman_filter.rst | 17 ++ docs/examples/kalman_filter/example_1.rst | 9 + docs/examples/kalman_filter/example_2.rst | 9 + docs/examples/kalman_filter/example_3.rst | 9 + docs/examples/kalman_filter/example_4.rst | 9 + docs/examples/kalman_filter/example_5.rst | 9 + docs/examples/kalman_filter/example_6.rst | 9 + docs/examples/kalman_filter/example_7.rst | 9 + docs/examples/kalman_filter/example_8.rst | 9 + docs/examples/kalman_filter/kalman.rst | 7 + example/CMakeLists.txt | 1 + example/kalman_filter/CMakeLists.txt | 41 ++++ example/kalman_filter/kalman.h | 180 ++++++++++++++++++ .../kalman_filter/kalman_filter-example_1.cpp | 63 ++++++ .../kalman_filter/kalman_filter-example_2.cpp | 66 +++++++ .../kalman_filter/kalman_filter-example_3.cpp | 66 +++++++ .../kalman_filter/kalman_filter-example_4.cpp | 69 +++++++ .../kalman_filter/kalman_filter-example_5.cpp | 73 +++++++ .../kalman_filter/kalman_filter-example_6.cpp | 106 +++++++++++ .../kalman_filter/kalman_filter-example_7.cpp | 106 +++++++++++ .../kalman_filter/kalman_filter-example_8.cpp | 106 +++++++++++ example/literals/CMakeLists.txt | 1 - ...lman_filter-alpha_beta_filter_example2.cpp | 100 ---------- example/references/CMakeLists.txt | 1 - ...lman_filter-alpha_beta_filter_example2.cpp | 100 ---------- 28 files changed, 985 insertions(+), 214 deletions(-) delete mode 100644 docs/examples/kalman_filter-alpha_beta_filter_example2.rst create mode 100644 docs/examples/kalman_filter.rst create mode 100644 docs/examples/kalman_filter/example_1.rst create mode 100644 docs/examples/kalman_filter/example_2.rst create mode 100644 docs/examples/kalman_filter/example_3.rst create mode 100644 docs/examples/kalman_filter/example_4.rst create mode 100644 docs/examples/kalman_filter/example_5.rst create mode 100644 docs/examples/kalman_filter/example_6.rst create mode 100644 docs/examples/kalman_filter/example_7.rst create mode 100644 docs/examples/kalman_filter/example_8.rst create mode 100644 docs/examples/kalman_filter/kalman.rst create mode 100644 example/kalman_filter/CMakeLists.txt create mode 100644 example/kalman_filter/kalman.h create mode 100644 example/kalman_filter/kalman_filter-example_1.cpp create mode 100644 example/kalman_filter/kalman_filter-example_2.cpp create mode 100644 example/kalman_filter/kalman_filter-example_3.cpp create mode 100644 example/kalman_filter/kalman_filter-example_4.cpp create mode 100644 example/kalman_filter/kalman_filter-example_5.cpp create mode 100644 example/kalman_filter/kalman_filter-example_6.cpp create mode 100644 example/kalman_filter/kalman_filter-example_7.cpp create mode 100644 example/kalman_filter/kalman_filter-example_8.cpp delete mode 100644 example/literals/kalman_filter-alpha_beta_filter_example2.cpp delete mode 100644 example/references/kalman_filter-alpha_beta_filter_example2.cpp diff --git a/docs/CMakeLists.txt b/docs/CMakeLists.txt index 0f301ee8..3b9f0c73 100644 --- a/docs/CMakeLists.txt +++ b/docs/CMakeLists.txt @@ -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" diff --git a/docs/examples.rst b/docs/examples.rst index a508a1f6..e5c9f6ad 100644 --- a/docs/examples.rst +++ b/docs/examples.rst @@ -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 diff --git a/docs/examples/kalman_filter-alpha_beta_filter_example2.rst b/docs/examples/kalman_filter-alpha_beta_filter_example2.rst deleted file mode 100644 index 3643ccfe..00000000 --- a/docs/examples/kalman_filter-alpha_beta_filter_example2.rst +++ /dev/null @@ -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 `_ tutorial. - -.. literalinclude:: ../../example/references/kalman_filter-alpha_beta_filter_example2.cpp - :caption: kalman_filter-alpha_beta_filter_example2.cpp - :start-at: #include - :linenos: diff --git a/docs/examples/kalman_filter.rst b/docs/examples/kalman_filter.rst new file mode 100644 index 00000000..690b452d --- /dev/null +++ b/docs/examples/kalman_filter.rst @@ -0,0 +1,17 @@ +Kalman Filter Tutorial +====================== + +The following examples are based on the `Kalman Filter `_ 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 diff --git a/docs/examples/kalman_filter/example_1.rst b/docs/examples/kalman_filter/example_1.rst new file mode 100644 index 00000000..28a7da5b --- /dev/null +++ b/docs/examples/kalman_filter/example_1.rst @@ -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: diff --git a/docs/examples/kalman_filter/example_2.rst b/docs/examples/kalman_filter/example_2.rst new file mode 100644 index 00000000..58312b4b --- /dev/null +++ b/docs/examples/kalman_filter/example_2.rst @@ -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: diff --git a/docs/examples/kalman_filter/example_3.rst b/docs/examples/kalman_filter/example_3.rst new file mode 100644 index 00000000..a0adfe76 --- /dev/null +++ b/docs/examples/kalman_filter/example_3.rst @@ -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: diff --git a/docs/examples/kalman_filter/example_4.rst b/docs/examples/kalman_filter/example_4.rst new file mode 100644 index 00000000..a97a1f06 --- /dev/null +++ b/docs/examples/kalman_filter/example_4.rst @@ -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: diff --git a/docs/examples/kalman_filter/example_5.rst b/docs/examples/kalman_filter/example_5.rst new file mode 100644 index 00000000..e1e10d78 --- /dev/null +++ b/docs/examples/kalman_filter/example_5.rst @@ -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: diff --git a/docs/examples/kalman_filter/example_6.rst b/docs/examples/kalman_filter/example_6.rst new file mode 100644 index 00000000..0f2b5b54 --- /dev/null +++ b/docs/examples/kalman_filter/example_6.rst @@ -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: diff --git a/docs/examples/kalman_filter/example_7.rst b/docs/examples/kalman_filter/example_7.rst new file mode 100644 index 00000000..3fd54a15 --- /dev/null +++ b/docs/examples/kalman_filter/example_7.rst @@ -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: diff --git a/docs/examples/kalman_filter/example_8.rst b/docs/examples/kalman_filter/example_8.rst new file mode 100644 index 00000000..758851a8 --- /dev/null +++ b/docs/examples/kalman_filter/example_8.rst @@ -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: diff --git a/docs/examples/kalman_filter/kalman.rst b/docs/examples/kalman_filter/kalman.rst new file mode 100644 index 00000000..cabf2043 --- /dev/null +++ b/docs/examples/kalman_filter/kalman.rst @@ -0,0 +1,7 @@ +Common utility header file (``kalman.h``) +========================================= + +.. literalinclude:: ../../../example/kalman_filter/kalman.h + :caption: kalman.h + :start-at: #include + :linenos: diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index 17fb0211..e34072ab 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -39,5 +39,6 @@ if(NOT UNITS_LIBCXX) endif() add_subdirectory(alternative_namespaces) +add_subdirectory(kalman_filter) add_subdirectory(literals) add_subdirectory(references) diff --git a/example/kalman_filter/CMakeLists.txt b/example/kalman_filter/CMakeLists.txt new file mode 100644 index 00000000..2ee45504 --- /dev/null +++ b/example/kalman_filter/CMakeLists.txt @@ -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 ...) +# +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) diff --git a/example/kalman_filter/kalman.h b/example/kalman_filter/kalman.h new file mode 100644 index 00000000..439dc3a4 --- /dev/null +++ b/example/kalman_filter/kalman.h @@ -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 +#include +#include +#include +#include +#include +#include + +namespace kalman { + +template +concept QuantityOrQuantityPoint = units::Quantity || units::QuantityPoint; // TODO Should it also account for `kinds`? + +template +inline constexpr bool are_derivatives = false; + +template +inline constexpr bool are_derivatives = true; + +template +inline constexpr bool are_derivatives = + units::DimensionOfT && // TODO Think on how to simplify this + are_derivatives; + +// state +template + requires (sizeof...(QQPs) > 0) && (sizeof...(QQPs) <= 3) && are_derivatives +struct state { + std::tuple variables_; + constexpr state(QQPs... qqps): variables_(std::move(qqps)...) {} +}; + +template +concept State = units::is_specialization_of; + +template +constexpr auto& get(state& s) { return get(s.variables_); } + +template +constexpr const auto& get(const state& s) { return get(s.variables_); } + +// estimation +template +struct estimation { +private: + using uncertainty_ref = decltype(QQP::reference * QQP::reference); + using uncertainty_type = units::quantity; +public: + kalman::state state; // TODO extend kalman functions to work with this variadic patermater list + uncertainty_type uncertainty; +}; + +// kalman gain +template +constexpr units::dimensionless kalman_gain(Q estimate_uncertainty, Q measurement_uncertainty) +{ + return estimate_uncertainty / (estimate_uncertainty + measurement_uncertainty); +} + +// state update +template + requires units::equivalent +constexpr state state_update(const state& predicted, QM measured, K gain) +{ + return { get<0>(predicted) + gain * (measured - get<0>(predicted)) }; +} + +template + requires units::equivalent +constexpr state state_update(const state& predicted, QM measured, std::array 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 + requires units::equivalent +constexpr state state_update(const state& predicted, QM measured, std::array 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 +constexpr Q covariance_update(Q uncertainty, K gain) +{ + return (1 - gain) * uncertainty; +} + +// state extrapolation +template +constexpr state state_extrapolation(const state& estimated, T interval) +{ + const auto q1 = get<0>(estimated) + get<1>(estimated) * interval; + const auto q2 = get<1>(estimated); + return { q1, q2 }; +} + +template +constexpr state state_extrapolation(const state& 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 +constexpr Q covariance_extrapolation(Q uncertainty, Q process_noise_variance) +{ + return uncertainty + process_noise_variance; +} + +} // namespace kalman + +template +struct fmt::formatter> : formatter { + template + auto format(const kalman::state& 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::format(txt, ctx); + } +private: + int precision = 1; // TODO find the easiest way to parse it from context +}; + +template +struct fmt::formatter> : formatter { + template + auto format(kalman::estimation e, FormatContext& ctx) + { + units::Quantity auto q = [](const Q& t) { + if constexpr(units::Quantity) + 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::format(txt, ctx); + } +private: + int precision = 3; // TODO find the easiest way to parse it from context +}; diff --git a/example/kalman_filter/kalman_filter-example_1.cpp b/example/kalman_filter/kalman_filter-example_1.cpp new file mode 100644 index 00000000..7ae4bce9 --- /dev/null +++ b/example/kalman_filter/kalman_filter-example_1.cpp @@ -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 +#include +#include +#include + +// 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>; + + 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 gain = 1. / index; + const auto current = state_update(previous, m, gain); + next = current; + print(index++, gain, m, current, next); + } +} diff --git a/example/kalman_filter/kalman_filter-example_2.cpp b/example/kalman_filter/kalman_filter-example_2.cpp new file mode 100644 index 00000000..e7679dae --- /dev/null +++ b/example/kalman_filter/kalman_filter-example_2.cpp @@ -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 +#include +#include +#include +#include +#include + +// 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::speed>; + + 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(0.2), dimensionless(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); + } +} diff --git a/example/kalman_filter/kalman_filter-example_3.cpp b/example/kalman_filter/kalman_filter-example_3.cpp new file mode 100644 index 00000000..3a8bf91d --- /dev/null +++ b/example/kalman_filter/kalman_filter-example_3.cpp @@ -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 +#include +#include +#include +#include +#include + +// 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::speed>; + + 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(0.2), dimensionless(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); + } +} diff --git a/example/kalman_filter/kalman_filter-example_4.cpp b/example/kalman_filter/kalman_filter-example_4.cpp new file mode 100644 index 00000000..9ae3a62a --- /dev/null +++ b/example/kalman_filter/kalman_filter-example_4.cpp @@ -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 +#include +#include +#include +#include +#include +#include + +// 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::speed, si::acceleration>; + 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(0.5), dimensionless(0.4), dimensionless(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); + } +} diff --git a/example/kalman_filter/kalman_filter-example_5.cpp b/example/kalman_filter/kalman_filter-example_5.cpp new file mode 100644 index 00000000..b818f19a --- /dev/null +++ b/example/kalman_filter/kalman_filter-example_5.cpp @@ -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 +#include +#include +#include +#include + +// Based on: https://www.kalmanfilter.net/kalman1d.html#ex5 + +using namespace units; + +template +void print_header(kalman::estimation initial) +{ + fmt::print("Initial: {}\n", initial); + fmt::print("{:>2} | {:>5} | {:>8} | {:>16} | {:>16}\n", "N", "Gain", "Measured", "Curr. Estimate", "Next Estimate"); +} + +template +void print(auto iteration, K gain, Q measured, kalman::estimation current, kalman::estimation 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 = [=](const estimation& previous, const Q& meassurement, Dimensionless auto gain) { + return estimation{ state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, gain) }; + }; + + auto predict = [](const estimation& 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); + } +} diff --git a/example/kalman_filter/kalman_filter-example_6.cpp b/example/kalman_filter/kalman_filter-example_6.cpp new file mode 100644 index 00000000..06c291d4 --- /dev/null +++ b/example/kalman_filter/kalman_filter-example_6.cpp @@ -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 +#include +#include +#include +#include +#include + +// TODO Fix when Celsius is properly supported (#232) +namespace units::isq::si { + +struct degree_celsius : alias_unit {}; + +namespace thermodynamic_temperature_references { + +inline constexpr auto deg_C = reference{}; + +} // 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 +void print_header(kalman::estimation initial) +{ + fmt::print("Initial: {}\n", initial); + fmt::print("{:>2} | {:>7} | {:>10} | {:>18} | {:>18}\n", "N", "Gain", "Measured", "Curr. Estimate", "Next Estimate"); +} + +template +void print(auto iteration, K gain, QP measured, kalman::estimation current, kalman::estimation 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 = [=](const estimation& previous, const QP& meassurement, Dimensionless auto gain) { + return estimation{ state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, gain) }; + }; + + auto predict = [=](const estimation& 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); + } +} diff --git a/example/kalman_filter/kalman_filter-example_7.cpp b/example/kalman_filter/kalman_filter-example_7.cpp new file mode 100644 index 00000000..8911c020 --- /dev/null +++ b/example/kalman_filter/kalman_filter-example_7.cpp @@ -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 +#include +#include +#include +#include +#include + +// TODO Fix when Celsius is properly supported (#232) +namespace units::isq::si { + +struct degree_celsius : alias_unit {}; + +namespace thermodynamic_temperature_references { + +inline constexpr auto deg_C = reference{}; + +} // 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 +void print_header(kalman::estimation initial) +{ + fmt::print("Initial: {}\n", initial); + fmt::print("{:>2} | {:>7} | {:>10} | {:>18} | {:>18}\n", "N", "Gain", "Measured", "Curr. Estimate", "Next Estimate"); +} + +template +void print(auto iteration, K gain, QP measured, kalman::estimation current, kalman::estimation 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 = [=](const estimation& previous, const QP& meassurement, Dimensionless auto gain) { + return estimation{ state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, gain) }; + }; + + auto predict = [=](const estimation& 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); + } +} diff --git a/example/kalman_filter/kalman_filter-example_8.cpp b/example/kalman_filter/kalman_filter-example_8.cpp new file mode 100644 index 00000000..7d7100ae --- /dev/null +++ b/example/kalman_filter/kalman_filter-example_8.cpp @@ -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 +#include +#include +#include +#include +#include + +// TODO Fix when Celsius is properly supported (#232) +namespace units::isq::si { + +struct degree_celsius : alias_unit {}; + +namespace thermodynamic_temperature_references { + +inline constexpr auto deg_C = reference{}; + +} // 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 +void print_header(kalman::estimation initial) +{ + fmt::print("Initial: {}\n", initial); + fmt::print("{:>2} | {:>7} | {:>10} | {:>18} | {:>18}\n", "N", "Gain", "Measured", "Curr. Estimate", "Next Estimate"); +} + +template +void print(auto iteration, K gain, QP measured, kalman::estimation current, kalman::estimation 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 = [=](const estimation& previous, const QP& meassurement, Dimensionless auto gain) { + return estimation{ state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, gain) }; + }; + + auto predict = [=](const estimation& 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); + } +} diff --git a/example/literals/CMakeLists.txt b/example/literals/CMakeLists.txt index 26d32f77..f3f032dd 100644 --- a/example/literals/CMakeLists.txt +++ b/example/literals/CMakeLists.txt @@ -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) diff --git a/example/literals/kalman_filter-alpha_beta_filter_example2.cpp b/example/literals/kalman_filter-alpha_beta_filter_example2.cpp deleted file mode 100644 index 52f53ead..00000000 --- a/example/literals/kalman_filter-alpha_beta_filter_example2.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include - -/* - kalman filter tutorial - 1d aircraft α-β filter example2 from https://www.kalmanfilter.net/alphabeta.html#ex2 -*/ - -namespace { - -template -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> range; - state_variable> speed; - - constexpr void estimate(const state& previous_state, const si::length& 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 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); - } -} diff --git a/example/references/CMakeLists.txt b/example/references/CMakeLists.txt index 6e75a0e8..71f37cfc 100644 --- a/example/references/CMakeLists.txt +++ b/example/references/CMakeLists.txt @@ -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) diff --git a/example/references/kalman_filter-alpha_beta_filter_example2.cpp b/example/references/kalman_filter-alpha_beta_filter_example2.cpp deleted file mode 100644 index 68f0fdf0..00000000 --- a/example/references/kalman_filter-alpha_beta_filter_example2.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include - -/* - kalman filter tutorial - 1d aircraft α-β filter example2 from https://www.kalmanfilter.net/alphabeta.html#ex2 -*/ - -namespace { - -template -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> range; - state_variable> speed; - - constexpr void estimate(const state& previous_state, const si::length& 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 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); - } -}