diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt index f7a1067d4..d22feb0c3 100644 --- a/include/CMakeLists.txt +++ b/include/CMakeLists.txt @@ -46,6 +46,7 @@ target_sources( "HEADERS" FILES "fcarouge/kalman.hpp" + "fcarouge/printer.hpp" "fcarouge/utility.hpp") install( TARGETS kalman diff --git a/include/fcarouge/internal/format.hpp b/include/fcarouge/internal/format.hpp index 9bea11d17..b683e45b5 100644 --- a/include/fcarouge/internal/format.hpp +++ b/include/fcarouge/internal/format.hpp @@ -46,6 +46,9 @@ For more information, please refer to */ namespace fcarouge { template class kalman; +namespace decorator { +template class printer; +} } // namespace fcarouge template @@ -61,8 +64,9 @@ struct std::formatter, Char> { //! @todo P2585 may be useful in simplifying and standardizing the support. template - auto format(const fcarouge::kalman &filter, - std::basic_format_context &format_context) const + constexpr auto + format(const fcarouge::kalman &filter, + std::basic_format_context &format_context) const -> OutputIt { format_context.advance_to( @@ -141,4 +145,8 @@ struct std::formatter, Char> { } }; +template +struct std::formatter, Char> + : public std::formatter {}; + #endif // FCAROUGE_INTERNAL_FORMAT_HPP diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp index 458553556..9cb4008ad 100644 --- a/include/fcarouge/kalman.hpp +++ b/include/fcarouge/kalman.hpp @@ -48,6 +48,7 @@ For more information, please refer to */ #include "internal/factory.hpp" #include "internal/format.hpp" +#include "printer.hpp" #include "utility.hpp" #include @@ -131,7 +132,7 @@ namespace fcarouge { //! https://arxiv.org/pdf/1905.13002.pdf ? GPU implementation? Parallel //! implementation? template -class kalman final : public internal::conditional_member_types { +class kalman : public internal::conditional_member_types { private: //! @name Private Member Types //! @{ diff --git a/include/fcarouge/printer.hpp b/include/fcarouge/printer.hpp new file mode 100644 index 000000000..8dcf365cb --- /dev/null +++ b/include/fcarouge/printer.hpp @@ -0,0 +1,95 @@ +/* __ _ __ __ _ _ +| |/ / /\ | | | \/ | /\ | \ | | +| ' / / \ | | | \ / | / \ | \| | +| < / /\ \ | | | |\/| | / /\ \ | . ` | +| . \ / ____ \| |____| | | |/ ____ \| |\ | +|_|\_\/_/ \_\______|_| |_/_/ \_\_| \_| + +Kalman Filter +Version 0.4.0 +https://github.com/FrancoisCarouge/Kalman + +SPDX-License-Identifier: Unlicense + +This is free and unencumbered software released into the public domain. + +Anyone is free to copy, modify, publish, use, compile, sell, or +distribute this software, either in source code form or as a compiled +binary, for any purpose, commercial or non-commercial, and by any +means. + +In jurisdictions that recognize copyright laws, the author or authors +of this software dedicate any and all copyright interest in the +software to the public domain. We make this dedication for the benefit +of the public at large and to the detriment of our heirs and +successors. We intend this dedication to be an overt act of +relinquishment in perpetuity of all present and future rights to this +software under copyright law. + +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 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. + +For more information, please refer to */ + +#ifndef FCAROUGE_PRINTER_HPP +#define FCAROUGE_PRINTER_HPP + +#include + +namespace fcarouge { +namespace decorator { +template class printer : public Filter { +public: + using Filter::p; + using Filter::x; + + inline constexpr explicit printer([[maybe_unused]] Filter &&filter) + : Filter{std::forward(filter)} { + std::println("{{\"event\": \"construction\", \"filter\":{}}}", *this); + } + + inline constexpr ~printer() { + std::println("{{\"event\": \"destruction\", \"filter\":{}}}", *this); + } + + inline constexpr void x(const auto &value, const auto &...values) { + Filter::x(value, values...); + std::println("{{\"event\": \"x\", \"filter\":{}}}", *this); + } + + //! @todo Implement the remaining events. + inline constexpr void p(const auto &value, const auto &...values) { + Filter::p(value, values...); + std::println("{{\"event\": \"p\", \"filter\":{}}}", *this); + } + + inline constexpr void predict(const auto &...arguments) { + Filter::predict(arguments...); + std::println("{{\"event\": \"predict\", \"filter\":{}}}", *this); + } + + inline constexpr void update(const auto &...arguments) { + Filter::update(arguments...); + std::println("{{\"event\": \"update\", \"filter\":{}}}", *this); + } +}; +} // namespace decorator + +struct printer_decorator {}; + +inline constexpr printer_decorator printer; + +template +inline constexpr auto +operator|(Filter &&filter, [[maybe_unused]] const printer_decorator decorator) { + return decorator::printer(std::forward(filter)); +} + +} // namespace fcarouge + +#endif // FCAROUGE_PRINTER_HPP diff --git a/sample/kf_1x1x0_building_height.cpp b/sample/kf_1x1x0_building_height.cpp index db0dd55f1..601a8dbdd 100644 --- a/sample/kf_1x1x0_building_height.cpp +++ b/sample/kf_1x1x0_building_height.cpp @@ -3,6 +3,8 @@ #include #include +#include + namespace fcarouge::sample { namespace { //! @brief Estimating the height of a building. diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index d1101a419..4eec717f3 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -44,7 +44,8 @@ foreach( "kalman_format_arguments.cpp" "kalman_format_float_1x1x1.cpp" "kalman_format.cpp" - "kalman_println.cpp") + "kalman_println.cpp" + "printer_kf_1x1x0_building_height.cpp") get_filename_component(NAME ${TEST} NAME_WE) add_executable(kalman_test_${NAME}_driver ${TEST}) target_link_libraries(kalman_test_${NAME}_driver PRIVATE kalman kalman_main diff --git a/test/printer_kf_1x1x0_building_height.cpp b/test/printer_kf_1x1x0_building_height.cpp new file mode 100644 index 000000000..ce591d84c --- /dev/null +++ b/test/printer_kf_1x1x0_building_height.cpp @@ -0,0 +1,76 @@ +#include "fcarouge/kalman.hpp" + +#include +#include + +namespace fcarouge::sample { +namespace { +//! @brief Estimating the height of a building. +//! +//! @copyright This example is transcribed from KalmanFilter.NET copyright Alex +//! Becker. +//! +//! @see https://www.kalmanfilter.net/kalman1d.html#ex5 +//! +//! @details Assume that we would like to estimate the height of a building +//! using a very imprecise altimeter. We know for sure, that the building height +//! doesn’t change over time, at least during the short measurement process. The +//! true building height is 50 meters. The altimeter measurement error (standard +//! deviation) is 5 meters. The set of ten measurements is: 48.54m, 47.11m, +//! 55.01m, 55.15m, 49.89m, 40.85m, 46.72m, 50.05m, 51.27m, 49.95m. +//! +//! @image html ./sample/image/kf_1x1x0_building_height.svg +//! +//! @example kf_1x1x0_building_height.cpp +[[maybe_unused]] auto sample{[] { + // A one-dimensional filter, constant system dynamic model. + auto filter{ + kalman{// One can estimate the building height simply by looking at it. + // The estimated state building height is: X = 60 meters. + state{60.}, + // The building height measurement Z. + output, + // A human’s estimation error (standard deviation) is about 15 + // meters: σ = 15. Consequently the variance is σ^2 = 225. The + // estimate uncertainty is: P = 225. + estimate_uncertainty{225.}, + // Since the standard deviation σ of the altimeter measurement + // error is 5, the variance σ^2 would be 25, thus the + // measurement, output uncertainty is: R = 25. + output_uncertainty{25.}} | + printer}; + + assert(60 == filter.x() && + "Since our system's dynamic model is constant, i.e. the building " + "doesn't change its height: 60 meters."); + assert(225 == filter.p() && + "The extrapolated estimate uncertainty (variance) also doesn't " + "change: 225"); + + // Now, we shall predict the next state based on the initialization values. + // Note: The prediction operation needs not be performed since the process + // noise covariance Q is null in this example. + // Measure and update: the first measurement is: z1 = 48.54m. + filter.update(48.54); + + // And so on. + filter.update(47.11); + filter.update(55.01); + filter.update(55.15); + filter.update(49.89); + filter.update(40.85); + filter.update(46.72); + filter.update(50.05); + filter.update(51.27); + filter.update(49.95); + + // After 10 measurements the filter estimates the height of the building + // at 49.57m. + assert(std::abs(1 - filter.x() / 49.57) < 0.001 && + "After 10 measurement and update iterations, the building estimated " + "height is: 49.57m."); + + return 0; +}()}; +} // namespace +} // namespace fcarouge::sample