From 87f5db29628b668d7f9cb6ee39b59848bb29c334 Mon Sep 17 00:00:00 2001 From: Francois Carouge Date: Sat, 23 Apr 2022 16:38:17 -0700 Subject: [PATCH] [sample] add 2D vehicule location example --- .../deploy_test_coverage_coveralls.yml | 2 +- .../workflows/verify_test_memory_valgrind.yml | 2 +- .../verify_test_sanitizer_address.yml | 2 +- .../workflows/verify_test_sanitizer_leak.yml | 2 +- .../verify_test_sanitizer_thread.yml | 2 +- ...rify_test_sanitizer_undefined_behavior.yml | 2 +- .../verify_test_ubuntu-20-04_gcc-trunk.yml | 2 +- .../verify_test_windows-2019_msvc.yml | 6 +- README.md | 16 +- include/fcarouge/kalman.hpp | 4 +- sample/building_height.cpp | 12 +- sample/liquid_temperature.cpp | 22 +- sample/vehicule_location.cpp | 193 ++++++++++++++++++ 13 files changed, 232 insertions(+), 35 deletions(-) create mode 100644 sample/vehicule_location.cpp diff --git a/.github/workflows/deploy_test_coverage_coveralls.yml b/.github/workflows/deploy_test_coverage_coveralls.yml index 351d51e34..2f06192fa 100644 --- a/.github/workflows/deploy_test_coverage_coveralls.yml +++ b/.github/workflows/deploy_test_coverage_coveralls.yml @@ -26,7 +26,7 @@ jobs: cmake --build . ; sudo make install ) ) - name: Build - run: gcc-11 sample/*.cpp source/*.cpp test/*.cpp -Iinclude -I/usr/local/include/eigen3 -O0 -g -std=c++23 -fmodules-ts --coverage -lstdc++ + run: gcc-11 sample/*.cpp source/*.cpp test/*.cpp -Iinclude -I/usr/local/include/eigen3 -O0 -g -std=c++23 -fmodules-ts --coverage -lstdc++ -lm - name: "Coverage: Base" run: | lcov --rc lcov_branch_coverage=1 --gcov-tool gcov-11 --capture --initial --directory . --output-file base.info diff --git a/.github/workflows/verify_test_memory_valgrind.yml b/.github/workflows/verify_test_memory_valgrind.yml index 43c397dc0..69675f571 100644 --- a/.github/workflows/verify_test_memory_valgrind.yml +++ b/.github/workflows/verify_test_memory_valgrind.yml @@ -26,6 +26,6 @@ jobs: cmake --build . ; sudo make install ) ) - name: Build - run: gcc-11 sample/*.cpp test/*.cpp source/*.cpp -Iinclude -I/usr/local/include/eigen3 -g -O0 -std=c++23 -lstdc++ + run: gcc-11 sample/*.cpp test/*.cpp source/*.cpp -Iinclude -I/usr/local/include/eigen3 -g -O0 -std=c++23 -lstdc++ -lm - name: Test run: valgrind --verbose ./a.out diff --git a/.github/workflows/verify_test_sanitizer_address.yml b/.github/workflows/verify_test_sanitizer_address.yml index 49a76effe..a6547c304 100644 --- a/.github/workflows/verify_test_sanitizer_address.yml +++ b/.github/workflows/verify_test_sanitizer_address.yml @@ -26,6 +26,6 @@ jobs: cmake --build . ; sudo make install ) ) - name: Build - run: gcc-11 sample/*.cpp test/*.cpp source/*.cpp -Iinclude -I/usr/local/include/eigen3 -std=c++23 -g -fsanitize=address -fsanitize-address-use-after-scope -fno-omit-frame-pointer -lstdc++ + run: gcc-11 sample/*.cpp test/*.cpp source/*.cpp -Iinclude -I/usr/local/include/eigen3 -std=c++23 -g -fsanitize=address -fsanitize-address-use-after-scope -fno-omit-frame-pointer -lstdc++ -lm - name: Test run: ASAN_OPTIONS=verbosity=2:strict_string_checks=1:detect_stack_use_after_return=1:check_initialization_order=1:strict_init_order=1 ./a.out diff --git a/.github/workflows/verify_test_sanitizer_leak.yml b/.github/workflows/verify_test_sanitizer_leak.yml index 0e8f91a90..518af20ee 100644 --- a/.github/workflows/verify_test_sanitizer_leak.yml +++ b/.github/workflows/verify_test_sanitizer_leak.yml @@ -26,6 +26,6 @@ jobs: cmake --build . ; sudo make install ) ) - name: Build - run: gcc-11 sample/*.cpp test/*.cpp source/*.cpp -Iinclude -I/usr/local/include/eigen3 -std=c++23 -g -fsanitize=leak -lstdc++ + run: gcc-11 sample/*.cpp test/*.cpp source/*.cpp -Iinclude -I/usr/local/include/eigen3 -std=c++23 -g -fsanitize=leak -lstdc++ -lm - name: Test run: LSAN_OPTIONS=verbosity=2 ./a.out diff --git a/.github/workflows/verify_test_sanitizer_thread.yml b/.github/workflows/verify_test_sanitizer_thread.yml index 464df5860..a80c6ee68 100644 --- a/.github/workflows/verify_test_sanitizer_thread.yml +++ b/.github/workflows/verify_test_sanitizer_thread.yml @@ -26,6 +26,6 @@ jobs: cmake --build . ; sudo make install ) ) - name: Build - run: gcc-11 sample/*.cpp test/*.cpp source/*.cpp -Iinclude -I/usr/local/include/eigen3 -std=c++23 -g -fsanitize=thread -lstdc++ + run: gcc-11 sample/*.cpp test/*.cpp source/*.cpp -Iinclude -I/usr/local/include/eigen3 -std=c++23 -g -fsanitize=thread -lstdc++ -lm - name: Test run: TSAN_OPTIONS=verbosity=2 ./a.out diff --git a/.github/workflows/verify_test_sanitizer_undefined_behavior.yml b/.github/workflows/verify_test_sanitizer_undefined_behavior.yml index 9334165f2..a8ac55c43 100644 --- a/.github/workflows/verify_test_sanitizer_undefined_behavior.yml +++ b/.github/workflows/verify_test_sanitizer_undefined_behavior.yml @@ -26,6 +26,6 @@ jobs: cmake --build . ; sudo make install ) ) - name: Build - run: gcc-11 sample/*.cpp test/*.cpp source/*.cpp -Iinclude -I/usr/local/include/eigen3 -std=c++23 -g -fsanitize=undefined -lstdc++ + run: gcc-11 sample/*.cpp test/*.cpp source/*.cpp -Iinclude -I/usr/local/include/eigen3 -std=c++23 -g -fsanitize=undefined -lstdc++ -lm - name: Test run: UBSAN_OPTIONS=verbosity=2 ./a.out diff --git a/.github/workflows/verify_test_ubuntu-20-04_gcc-trunk.yml b/.github/workflows/verify_test_ubuntu-20-04_gcc-trunk.yml index 6de7a3add..bc2f3defb 100644 --- a/.github/workflows/verify_test_ubuntu-20-04_gcc-trunk.yml +++ b/.github/workflows/verify_test_ubuntu-20-04_gcc-trunk.yml @@ -25,6 +25,6 @@ jobs: cmake --build . ; sudo make install ) ) - name: Build - run: LD_LIBRARY_PATH=/opt/gcc-latest/lib64 LD_RUN_PATH=/opt/gcc-latest/lib64 /opt/gcc-latest/bin/gcc sample/*.cpp test/*.cpp source/*.cpp -Iinclude -I/usr/local/include/eigen3 -Wall -Wextra -pedantic -std=c++23 -lstdc++ + run: LD_LIBRARY_PATH=/opt/gcc-latest/lib64 LD_RUN_PATH=/opt/gcc-latest/lib64 /opt/gcc-latest/bin/gcc sample/*.cpp test/*.cpp source/*.cpp -Iinclude -I/usr/local/include/eigen3 -Wall -Wextra -pedantic -std=c++23 -lstdc++ -lm - name: Run run: ./a.out diff --git a/.github/workflows/verify_test_windows-2019_msvc.yml b/.github/workflows/verify_test_windows-2019_msvc.yml index 2439c9bf0..b70a2fc3f 100644 --- a/.github/workflows/verify_test_windows-2019_msvc.yml +++ b/.github/workflows/verify_test_windows-2019_msvc.yml @@ -14,7 +14,9 @@ jobs: uses: actions/checkout@v3 - name: Install uses: ilammy/msvc-dev-cmd@v1.10.0 - - name: Version - run: cl + - name: Install Eigen + run: vcpkg install eigen3 + - name: Install Eigen + run: env - name: Build run: cl /EHsc /std:c++latest /I include /TP sample/*.cpp source/*.cpp test/*.cpp diff --git a/README.md b/README.md index 3f9c6baa6..2ad4ad18b 100644 --- a/README.md +++ b/README.md @@ -52,11 +52,11 @@ using kalman = fcarouge::kalman; kalman k; -k.state_x = kalman::state{ 60 }; -k.estimate_uncertainty_p = kalman::estimate_uncertainty{ 15 * 15 }; -k.transition_observation_h = []() { return kalman::observation{ 1 }; }; -k.noise_observation_r = []() { - return kalman::observation_noise_uncertainty{ 5 * 5 }; +k.state_x = 60; +k.estimate_uncertainty_p = 225; +k.transition_observation_h = [] { return kalman::observation{ 1 }; }; +k.noise_observation_r = [] { + return kalman::observation_noise_uncertainty{ 25 }; }; k.update(48.54); @@ -74,7 +74,7 @@ const double gravitational_acceleration{ -9.8 }; // m.s^-2 const std::chrono::milliseconds delta_time{ 250 }; kalman k; -k.state_x = kalman::state{ 0, 0 }; +k.state_x = { 0, 0 }; k.estimate_uncertainty_p = kalman::estimate_uncertainty{ { 500, 0 }, { 0, 500 } }; k.transition_state_f = [](const std::chrono::milliseconds &delta_time) { @@ -92,8 +92,8 @@ k.transition_control_g = [](const std::chrono::milliseconds &delta_time) { const auto dt{ std::chrono::duration(delta_time).count() }; return kalman::control{ 0.0313, dt }; }; -k.transition_observation_h = []() { return kalman::observation{ { 1, 0 } }; }; -k.noise_observation_r = []() { +k.transition_observation_h = [] { return kalman::observation{ { 1, 0 } }; }; +k.noise_observation_r = [] { return kalman::observation_noise_uncertainty{ 400 }; }; diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp index 996472386..40b385469 100644 --- a/include/fcarouge/kalman.hpp +++ b/include/fcarouge/kalman.hpp @@ -90,11 +90,11 @@ class kalman // Functors could be replaced by the standard general-purpose polymorphic // function wrapper `std::function` if lambda captures are needed. - observation (*transition_observation_h)() = []() { + observation (*transition_observation_h)() = [] { return observation{ Identity()() }; }; - observation_noise_uncertainty (*noise_observation_r)() = []() { + observation_noise_uncertainty (*noise_observation_r)() = [] { return observation_noise_uncertainty{}; }; diff --git a/sample/building_height.cpp b/sample/building_height.cpp index 2036f1b17..0e2a75013 100644 --- a/sample/building_height.cpp +++ b/sample/building_height.cpp @@ -21,20 +21,20 @@ namespace //! 55.01m, 55.15m, 49.89m, 40.85m, 46.72m, 50.05m, 51.27m, 49.95m. //! //! @example building_height.cpp -[[maybe_unused]] constexpr auto building_height{ []() { - // One-dimensional filter, constant system dynamic model. +[[maybe_unused]] constexpr auto building_height{ [] { + // A one-dimensional filter, constant system dynamic model. using kalman = kalman; kalman k; // Initialization // One can estimate the building height simply by looking at it. The estimated // building height is: 60 meters. - k.state_x = kalman::state{ 60 }; + k.state_x = 60; // Now we shall initialize the estimate uncertainty. A human’s estimation // error (standard deviation) is about 15 meters: σ = 15. Consequently the // variance is 225: σ^2 = 225. - k.estimate_uncertainty_p = kalman::estimate_uncertainty{ 15 * 15 }; + k.estimate_uncertainty_p = 225; // Prediction // Now, we shall predict the next state based on the initialization values. @@ -49,8 +49,8 @@ namespace // 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 }; + k.noise_observation_r = [] { + return kalman::observation_noise_uncertainty{ 25 }; }; k.update(48.54); diff --git a/sample/liquid_temperature.cpp b/sample/liquid_temperature.cpp index 467336966..02dea9b59 100644 --- a/sample/liquid_temperature.cpp +++ b/sample/liquid_temperature.cpp @@ -26,8 +26,8 @@ namespace //! 50.023°C, and 49.99°C. //! //! @example liquid_temperature.cpp -[[maybe_unused]] constexpr auto liquid_temperature{ []() { - // One-dimensional filter, constant system dynamic model. +[[maybe_unused]] constexpr auto liquid_temperature{ [] { + // A one-dimensional filter, constant system dynamic model. using kalman = kalman; kalman k; @@ -35,20 +35,20 @@ namespace // 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 }; + k.state_x = 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 }; + k.estimate_uncertainty_p = 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 = []() { + k.noise_process_q = [] { return kalman::process_noise_uncertainty{ 0.0001 }; }; @@ -67,13 +67,13 @@ namespace // 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 = []() { + k.noise_observation_r = [] { return kalman::observation_noise_uncertainty{ 0.1 * 0.1 }; }; k.update(49.95); - // And so on. + // And so on, every measurements period: Δt = 5s (constant). k.predict(); k.update(49.967); k.predict(); @@ -92,15 +92,17 @@ namespace 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."); + + k.predict(); + + 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."); // 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 diff --git a/sample/vehicule_location.cpp b/sample/vehicule_location.cpp new file mode 100644 index 000000000..10f0791e1 --- /dev/null +++ b/sample/vehicule_location.cpp @@ -0,0 +1,193 @@ +#include "fcarouge/kalman.hpp" +#include "fcarouge/kalman_operator_eigen.hpp" + +#include + +namespace fcarouge::sample +{ +namespace +{ +//! @test Estimating the Vehicule Location +//! +//! @copyright This example is transcribed from KalmanFilter.NET copyright Alex +//! Becker. +//! +//! @see https://www.kalmanfilter.net/multiExamples.html#ex9 +//! +//! @details In this example, we would like to estimate the location of the +//! vehicle in the XY plane. The vehicle has an onboard location sensor that +//! reports X and Y coordinates of the system. We assume constant acceleration +//! dynamics. In this example we don't have a control variable u since we don't +//! have control input. Let us assume a vehicle moving in a straight line in the +//! X direction with a constant velocity. After traveling 400 meters the vehicle +//! turns right, with a turning radius of 300 meters. During the turning +//! maneuver, the vehicle experiences acceleration due to the circular motion +//! (an angular acceleration). The measurements period: Δt = 1s (constant). The +//! random acceleration standard deviation: σa = 0.2 m.s^-2. +[[maybe_unused]] auto vehicule_location{ [] { + // A 6x2x0 filter, constant acceleration dynamic model, no control. + using kalman = eigen::kalman; + kalman k; + + // Initialization + // The state is chosen to be the position, velocity, acceleration in the XY + // plane: [px, vx, ax, py, vy, ay]. We don't know the vehicle location; we + // will set initial position, velocity and acceleration to 0. + k.state_x = { 0, 0, 0, 0, 0, 0 }; + + // Since our initial state vector is a guess, we will set a very high estimate + // uncertainty. The high estimate uncertainty results in a high Kalman Gain, + // giving a high weight to the measurement. + k.estimate_uncertainty_p = kalman::estimate_uncertainty{ + { 500, 0, 0, 0, 0, 0 }, { 0, 500, 0, 0, 0, 0 }, { 0, 0, 500, 0, 0, 0 }, + { 0, 0, 0, 500, 0, 0 }, { 0, 0, 0, 0, 500, 0 }, { 0, 0, 0, 0, 0, 500 } + }; + + // Prediction + // The process noise matrix Q would be: + k.noise_process_q = [] { + return kalman::process_noise_uncertainty{ + { 1, 0.5, 0.5, 0, 0, 0 }, { 0.5, 1, 1, 0, 0, 0 }, + { 0.5, 1, 1, 0, 0, 0 }, { 0, 0, 0, 0.25, 0.5, 0.5 }, + { 0, 0, 0, 0.5, 1, 1 }, { 0, 0, 0, 0.5, 1, 1 } + }; + }; + // The state transition matrix F would be: + k.transition_state_f = [] { + return kalman::state_transition{ + { 1, 1, 0.5, 0, 0, 0 }, { 0, 1, 1, 0, 0, 0 }, { 0, 0, 1, 0, 0, 0 }, + { 0, 0, 0, 1, 1, 0.5 }, { 0, 0, 0, 0, 1, 1 }, { 0, 0, 0, 0, 0, 1 } + }; + }; + + // Now we can predict the next state based on the initialization values. + k.predict(); + + // Measure and Update + // The dimension of zn is 2x1 and the dimension of xn is 6x1. Therefore the + // dimension of the observation matrix H shall be 2x6. + k.transition_observation_h = [] { + return kalman::observation{ { 1, 0, 0, 0, 0, 0 }, { 0, 0, 0, 1, 0, 0 } }; + }; + // Assume that the x and y measurements are uncorrelated, i.e. error in the x + // coordinate measurement doesn't depend on the error in the y coordinate + // measurement. In real-life applications, the measurement uncertainty can + // differ between measurements. In many systems the measurement uncertainty + // depends on the measurement SNR (signal-to-noise ratio), angle between + // sensor (or sensors) and target, signal frequency and many other parameters. + // For the sake of the example simplicity, we will assume a constant + // measurement uncertainty: R1 = R2...Rn-1 = Rn = R The measurement error + // standard deviation: σxm = σym = 3m. The variance 9. + k.noise_observation_r = [] { + return kalman::observation_noise_uncertainty{ { 9, 0 }, { 0, 9 } }; + }; + + // The measurement values: z1 = [-393.66, 300.4] + k.update({ -393.66, 300.4 }); + + // And so on, every measurements period: Δt = 1s (constant). + k.predict(); + k.update({ -375.93, 301.78 }); + k.predict(); + k.update({ -351.04, 295.1 }); + k.predict(); + k.update({ -328.96, 305.19 }); + k.predict(); + k.update({ -299.35, 301.06 }); + k.predict(); + k.update({ -273.36, 302.05 }); + k.predict(); + k.update({ -245.89, 300 }); + k.predict(); + k.update({ -222.58, 303.57 }); + k.predict(); + k.update({ -198.03, 296.33 }); + k.predict(); + k.update({ -174.17, 297.65 }); + k.predict(); + k.update({ -146.32, 297.41 }); + k.predict(); + k.update({ -123.72, 299.61 }); + k.predict(); + k.update({ -103.47, 299.6 }); + k.predict(); + k.update({ -78.23, 302.39 }); + k.predict(); + k.update({ -52.63, 295.04 }); + k.predict(); + k.update({ -23.34, 300.09 }); + k.predict(); + k.update({ 25.96, 294.72 }); + k.predict(); + k.update({ 49.72, 298.61 }); + k.predict(); + k.update({ 76.94, 294.64 }); + k.predict(); + k.update({ 95.38, 284.88 }); + k.predict(); + k.update({ 119.83, 272.82 }); + k.predict(); + k.update({ 144.01, 264.93 }); + k.predict(); + k.update({ 161.84, 251.46 }); + k.predict(); + k.update({ 180.56, 241.27 }); + k.predict(); + k.update({ 201.42, 222.98 }); + k.predict(); + k.update({ 222.62, 203.73 }); + k.predict(); + k.update({ 239.4, 184.1 }); + k.predict(); + k.update({ 252.51, 166.12 }); + k.predict(); + k.update({ 266.26, 138.71 }); + k.predict(); + k.update({ 271.75, 119.71 }); + k.predict(); + k.update({ 277.4, 100.41 }); + k.predict(); + k.update({ 294.12, 79.76 }); + k.predict(); + k.update({ 301.23, 50.62 }); + k.predict(); + k.update({ 291.8, 32.99 }); + k.predict(); + k.update({ 299.89, 2.14 }); + + assert(5 - 0.0001 < k.estimate_uncertainty_p(0, 0) && + k.estimate_uncertainty_p(0, 0) < 5 + 1.84 && + 5 - 0.0001 < k.estimate_uncertainty_p(3, 3) && + k.estimate_uncertainty_p(3, 3) < 5 + 1.751 && + "At this point, the position uncertainty px = py = 5, which means " + "that the standard deviation of the prediction is square root of 5m."); + + k.predict(); + + assert(298.5 - 1.7 < k.state_x(0) && k.state_x(0) < 298.5 + 0.0001 && + -1.65 - 1 < k.state_x(1) && k.state_x(1) < -1.65 + 0.0001 && + -1.9 - 0.3 < k.state_x(2) && k.state_x(2) < -1.9 + 0.0001 && + -22.5 - 0.5 < k.state_x(3) && k.state_x(3) < -22.5 + 0.0001 && + -26.1 - 1.5 < k.state_x(4) && k.state_x(4) < -26.1 + 0.0001 && + -0.64 - 0.7 < k.state_x(5) && k.state_x(5) < -0.64 + 0.0001); + + // As you can see, the Kalman Filter tracks the vehicle quite well. However, + // when the vehicle starts the turning maneuver, the estimates are not so + // accurate. After a while, the Kalman Filter accuracy improves. While the + // vehicle travels along the straight line, the acceleration is constant and + // equal to zero. However, during the turn maneuver, the vehicle experiences + // acceleration due to the circular motion - the angular acceleration. + // Although the angular acceleration is constant, the angular acceleration + // projection on the x and y axes is not constant, therefore ax and ay are not + // constant. Our Kalman Filter is designed for a constant acceleration model. + // Nevertheless, it succeeds in tracking maneuvering vehicle due to a properly + // chosen σ2a parameter. I would like to encourage the readers to implement + // this example in software and see how different values of σ2a of R influence + // the actual Kalman Filter accuracy, Kalman Gain convergence, and estimation + // uncertainty. + + return 0; +}() }; + +} // namespace +} // namespace fcarouge::sample