From bbb1d0d0eb9b58e88aede089cff5288986345e2d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Fran=C3=A7ois=20Carouge?= Date: Fri, 22 Apr 2022 20:09:27 -0700 Subject: [PATCH] [sample] add liquid temperature example (#15) --- include/fcarouge/kalman.hpp | 25 ++++++-- sample/building_height.cpp | 11 ++-- sample/liquid_temperature.cpp | 113 ++++++++++++++++++++++++++++++++++ sample/sample.cpp | 54 ---------------- 4 files changed, 139 insertions(+), 64 deletions(-) create mode 100644 sample/liquid_temperature.cpp delete mode 100644 sample/sample.cpp diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp index 0de124ec6..996472386 100644 --- a/include/fcarouge/kalman.hpp +++ b/include/fcarouge/kalman.hpp @@ -94,10 +94,27 @@ class kalman return observation{ Identity()() }; }; - observation_noise_uncertainty (*noise_observation_r)(); - state_transition (*transition_state_f)(const PredictionArguments &...); - process_noise_uncertainty (*noise_process_q)(const PredictionArguments &...); - control (*transition_control_g)(const PredictionArguments &...); + observation_noise_uncertainty (*noise_observation_r)() = []() { + return observation_noise_uncertainty{}; + }; + + state_transition (*transition_state_f)(const PredictionArguments &...) = + [](const PredictionArguments &...arguments) { + static_cast((arguments, ...)); + return state_transition{ Identity()() }; + }; + + process_noise_uncertainty (*noise_process_q)(const PredictionArguments &...) = + [](const PredictionArguments &...arguments) { + static_cast((arguments, ...)); + return process_noise_uncertainty{}; + }; + + control (*transition_control_g)(const PredictionArguments &...) = + [](const PredictionArguments &...arguments) { + static_cast((arguments, ...)); + return control{}; + }; inline constexpr void update(const output &output_z) { diff --git a/sample/building_height.cpp b/sample/building_height.cpp index 990adf34d..2036f1b17 100644 --- a/sample/building_height.cpp +++ b/sample/building_height.cpp @@ -39,17 +39,16 @@ namespace // Prediction // Now, we shall predict the next state based on the initialization values. assert(60 == k.state_x && - "Since our system's Dynamic Model is constant, i.e. the building " + "Since our system's dynamic model is constant, i.e. the building " "doesn't change its height: 60 meters."); assert(225 == k.estimate_uncertainty_p && "The extrapolated estimate uncertainty (variance) also doesn't " "change: 225"); // Measure and Update - // The first measurement is: z1 = 48.54m. - // Since the standard deviation (σ) of the altimeter measurement error is 5, - // the variance (σ^2) would be 25, thus the measurement uncertainty is: r1 - // = 25. + // The first measurement is: z1 = 48.54m. Since the standard deviation σ of + // the altimeter measurement error is 5, the variance σ^2 would be 25, thus + // the measurement uncertainty is: r1 = 25. k.noise_observation_r = []() { return kalman::observation_noise_uncertainty{ 5 * 5 }; }; @@ -68,7 +67,7 @@ namespace // After 10 measurements the filter estimates the height of the building // at 49.57m. - assert(49.57 - 0.01 < k.state_x && k.state_x < 49.57 + 0.01 && + assert(49.57 - 0.001 < k.state_x && k.state_x < 49.57 + 0.001 && "After 10 measurement and update iterations, the building estimated " "height is: 49.57m."); diff --git a/sample/liquid_temperature.cpp b/sample/liquid_temperature.cpp new file mode 100644 index 000000000..467336966 --- /dev/null +++ b/sample/liquid_temperature.cpp @@ -0,0 +1,113 @@ +#include "fcarouge/kalman.hpp" + +#include + +namespace fcarouge::sample +{ +namespace +{ +//! @test Estimating the Temperature of the Liquid in a Tank +//! +//! @copyright This example is transcribed from KalmanFilter.NET copyright Alex +//! Becker. +//! +//! @see https://www.kalmanfilter.net/kalman1d.html#ex6 +//! +//! @details We would like to estimate the temperature of the liquid in a tank. +//! We assume that at steady state the liquid temperature is constant. However, +//! some fluctuations in the true liquid temperature are possible. We can +//! describe the system dynamics by the following equation: xn = T + wn where: T +//! is the constant temperature wn is a random process noise with variance q. +//! Let us assume a true temperature of 50 degrees Celsius. The measurements are +//! taken every 5 seconds. The true liquid temperature at the measurement points +//! is: 49.979°C, 50.025°C, 50°C, 50.003°C, 49.994°C, 50.002°C, 49.999°C, +//! 50.006°C, 49.998°C, and 49.991°C. The set of measurements is: 49.95°C, +//! 49.967°C, 50.1°C, 50.106°C, 49.992°C, 49.819°C, 49.933°C, 50.007°C, +//! 50.023°C, and 49.99°C. +//! +//! @example liquid_temperature.cpp +[[maybe_unused]] constexpr auto liquid_temperature{ []() { + // One-dimensional filter, constant system dynamic model. + using kalman = kalman; + kalman k; + + // Initialization + // Before the first iteration, we must initialize the Kalman filter and + // predict the next state (which is the first state). We don't know what the + // temperature of the liquid is, and our guess is 10°C. + k.state_x = kalman::state{ 10 }; + + // Our guess is very imprecise, so we set our initialization estimate error σ + // to 100. The estimate uncertainty of the initialization is the error + // variance σ^2: p0,0 = 100^2 = 10,000. This variance is very high. If we + // initialize with a more meaningful value, we will get faster Kalman filter + // convergence. + k.estimate_uncertainty_p = kalman::estimate_uncertainty{ 100 * 100 }; + + // Prediction + // Now, we shall predict the next state based on the initialization values. We + // think that we have an accurate model, thus we set the process noise + // variance q to 0.0001. + k.noise_process_q = []() { + return kalman::process_noise_uncertainty{ 0.0001 }; + }; + + k.predict(); + + assert(10 == k.state_x && + "Since our model has constant dynamics, the predicted estimate is " + "equal to the current estimate: x^1,0 = 10°C."); + assert(10000.0001 - 0.001 < k.estimate_uncertainty_p && + k.estimate_uncertainty_p < 10000.0001 + 0.001 && + "The extrapolated estimate uncertainty (variance): p1,0 = p0,0 + q = " + "10000 + 0.0001 = 10000.0001."); + + // Measure and Update + // The measurement value: z1 = 49.95°C. Since the measurement error is σ = + // 0.1, the variance σ^2 would be 0.01, thus the measurement uncertainty is: + // r1 = 0.01. The measurement error (standard deviation) is 0.1 degrees + // Celsius. + k.noise_observation_r = []() { + return kalman::observation_noise_uncertainty{ 0.1 * 0.1 }; + }; + + k.update(49.95); + + // And so on. + k.predict(); + k.update(49.967); + k.predict(); + k.update(50.1); + k.predict(); + k.update(50.106); + k.predict(); + k.update(49.992); + k.predict(); + k.update(49.819); + k.predict(); + k.update(49.933); + k.predict(); + k.update(50.007); + k.predict(); + k.update(50.023); + k.predict(); + k.update(49.99); + k.predict(); + + // The estimate uncertainty quickly goes down, after 10 measurements: + assert(49.988 - 0.0001 < k.state_x && k.state_x < 49.988 + 0.0001 && + "The filter estimates the liquid temperature at 49.988°C."); + assert(0.0013 - 0.0001 < k.estimate_uncertainty_p && + k.estimate_uncertainty_p < 0.0013 + 0.0001 && + "The estimate uncertainty is 0.0013, i.e. the estimate error standard " + "deviation is: 0.036°C."); + // So we can say that the liquid temperature estimate is: 49.988 ± 0.036°C. + // In this example we've measured a liquid temperature using the + // one-dimensional Kalman filter. Although the system dynamics include a + // random process noise, the Kalman filter can provide good estimation. + + return 0; +}() }; + +} // namespace +} // namespace fcarouge::sample diff --git a/sample/sample.cpp b/sample/sample.cpp deleted file mode 100644 index 5ac85df30..000000000 --- a/sample/sample.cpp +++ /dev/null @@ -1,54 +0,0 @@ -/*_ __ _ __ __ _ _ - | |/ / /\ | | | \/ | /\ | \ | | - | ' / / \ | | | \ / | / \ | \| | - | < / /\ \ | | | |\/| | / /\ \ | . ` | - | . \ / ____ \| |____| | | |/ ____ \| |\ | - |_|\_\/_/ \_\______|_| |_/_/ \_\_| \_| - -Kalman Filter for C++ -Version 0.1.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 */ - -#include - -namespace fcarouge::sample -{ -namespace -{ -//! @test Demonstrate the code. -[[maybe_unused]] constexpr auto sample = []() { - static_assert(true, "Compile-time demonstration."); - assert(true && "Run-time demonstration."); - - return 0; -}(); - -} // namespace -} // namespace fcarouge::sample