From 1dd0a95831c5c85d72fb2e045277da974f2986f1 Mon Sep 17 00:00:00 2001 From: Francois Carouge Date: Tue, 1 Nov 2022 21:20:47 -0700 Subject: [PATCH] [sample] building height plots --- sample/image/kf_1x1x0_building_height.svg | 759 ++++++++++++++++++++ sample/kf_1x1x0_building_height.cpp | 2 + sample/result/kf_1x1x0_building_height.json | 134 ++++ sample/script/kf_1x1x0_building_height.plt | 61 ++ sample/script/plot.sh | 57 ++ 5 files changed, 1013 insertions(+) create mode 100644 sample/image/kf_1x1x0_building_height.svg create mode 100644 sample/result/kf_1x1x0_building_height.json create mode 100644 sample/script/kf_1x1x0_building_height.plt create mode 100644 sample/script/plot.sh diff --git a/sample/image/kf_1x1x0_building_height.svg b/sample/image/kf_1x1x0_building_height.svg new file mode 100644 index 0000000000..e8ea975793 --- /dev/null +++ b/sample/image/kf_1x1x0_building_height.svg @@ -0,0 +1,759 @@ + + + +Gnuplot +Produced by GNUPLOT 5.4 patchlevel 2 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + + + + + + + + + + + + + 10 + + + + + + + + + + + + + 20 + + + + + + + + + + + + + 30 + + + + + + + + + + + + + 40 + + + + + + + + + + + + + 50 + + + + + + + + + + + + + 60 + + + + + 0 + + + + + 1 + + + + + 2 + + + + + 3 + + + + + 4 + + + + + 5 + + + + + 6 + + + + + 7 + + + + + 8 + + + + + 9 + + + + + 10 + + + + + + + + + Height (m) + + + + + Measurement Step + + + + + Sat Nov 05 20:48:29 2022 + + + Measurement Output Z + + + Measurement Output Z + + + + + + + + + + + + + + + + + + Estimated State X + + + Estimated State X + + + + + + + + + + + + + + + + + + True State + + + True State + + + + + + + + + + + + + + + + Sample 1x1x0 Building Height + + + States + + + + + + + + + + + + + + + 0 + + + + + + + + + + + + + 50 + + + + + + + + + + + + + 100 + + + + + + + + + + + + + 150 + + + + + + + + + + + + + 200 + + + + + + + + + + + + + 250 + + + + + 0 + + + + + 1 + + + + + 2 + + + + + 3 + + + + + 4 + + + + + 5 + + + + + 6 + + + + + 7 + + + + + 8 + + + + + 9 + + + + + 10 + + + + + + + + + Height (m) + + + + + Measurement Step + + + + + Sat Nov 05 20:48:29 2022 + + + Measurement Uncertainty R + + + Measurement Uncertainty R + + + + + + + + + + + + + + + + + + Estimation Uncertainty P + + + Estimation Uncertainty P + + + + + + + + + + + + + + + + + + + + + + + + + + + + Uncertainties + + + + + + + + + + + + + + + 0 + + + + + + + + + + + + + 0.1 + + + + + + + + + + + + + 0.2 + + + + + + + + + + + + + 0.3 + + + + + + + + + + + + + 0.4 + + + + + + + + + + + + + 0.5 + + + + + + + + + + + + + 0.6 + + + + + + + + + + + + + 0.7 + + + + + + + + + + + + + 0.8 + + + + + + + + + + + + + 0.9 + + + + + + + + + + + + + 1 + + + + + 0 + + + + + 1 + + + + + 2 + + + + + 3 + + + + + 4 + + + + + 5 + + + + + 6 + + + + + 7 + + + + + 8 + + + + + 9 + + + + + 10 + + + + + + + + + Height (m) + + + + + Measurement Step + + + + + Sat Nov 05 20:48:29 2022 + + + Gain K + + + Gain K + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gain + + + + + + + diff --git a/sample/kf_1x1x0_building_height.cpp b/sample/kf_1x1x0_building_height.cpp index c979422f6f..a9ac6d5840 100644 --- a/sample/kf_1x1x0_building_height.cpp +++ b/sample/kf_1x1x0_building_height.cpp @@ -18,6 +18,8 @@ namespace { //! 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 image/kf_1x1x0_building_height.svg +//! //! @example kf_1x1x0_building_height.cpp [[maybe_unused]] auto kf_1x1x0_building_height{[] { // A one-dimensional filter, constant system dynamic model. diff --git a/sample/result/kf_1x1x0_building_height.json b/sample/result/kf_1x1x0_building_height.json new file mode 100644 index 0000000000..12bffac917 --- /dev/null +++ b/sample/result/kf_1x1x0_building_height.json @@ -0,0 +1,134 @@ +[ + { + "f": 1, + "h": 1, + "k": 1, + "p": 225, + "q": 0, + "r": 25, + "s": 1, + "x": 60, + "y": 0, + "z": 0 + }, + { + "f": 1, + "h": 1, + "k": 0.9, + "p": 22.5, + "q": 0, + "r": 25, + "s": 250, + "x": 49.686, + "y": -11.46, + "z": 48.54 + }, + { + "f": 1, + "h": 1, + "k": 0.47368421052631576, + "p": 11.842105263157897, + "q": 0, + "r": 25, + "s": 47.5, + "x": 48.46578947368421, + "y": -2.5760000000000005, + "z": 47.11 + }, + { + "f": 1, + "h": 1, + "k": 0.32142857142857145, + "p": 8.035714285714288, + "q": 0, + "r": 25, + "s": 36.8421052631579, + "x": 50.56928571428571, + "y": 6.544210526315787, + "z": 55.01 + }, + { + "f": 1, + "h": 1, + "k": 0.24324324324324328, + "p": 6.0810810810810825, + "q": 0, + "r": 25, + "s": 33.03571428571429, + "x": 51.68351351351351, + "y": 4.580714285714286, + "z": 55.15 + }, + { + "f": 1, + "h": 1, + "k": 0.19565217391304351, + "p": 4.891304347826088, + "q": 0, + "r": 25, + "s": 31.08108108108108, + "x": 51.33260869565217, + "y": -1.7935135135135098, + "z": 49.89 + }, + { + "f": 1, + "h": 1, + "k": 0.16363636363636366, + "p": 4.090909090909092, + "q": 0, + "r": 25, + "s": 29.891304347826086, + "x": 49.61727272727272, + "y": -10.482608695652168, + "z": 40.85 + }, + { + "f": 1, + "h": 1, + "k": 0.14062500000000003, + "p": 3.515625000000001, + "q": 0, + "r": 25, + "s": 29.090909090909093, + "x": 49.20984374999999, + "y": -2.8972727272727212, + "z": 46.72 + }, + { + "f": 1, + "h": 1, + "k": 0.12328767123287675, + "p": 3.0821917808219186, + "q": 0, + "r": 25, + "s": 28.515625, + "x": 49.313424657534235, + "y": 0.8401562500000068, + "z": 50.05 + }, + { + "f": 1, + "h": 1, + "k": 0.10975609756097564, + "p": 2.7439024390243913, + "q": 0, + "r": 25, + "s": 28.08219178082192, + "x": 49.528170731707306, + "y": 1.9565753424657686, + "z": 51.27 + }, + { + "f": 1, + "h": 1, + "k": 0.09890109890109894, + "p": 2.4725274725274726, + "q": 0, + "r": 25, + "s": 27.743902439024392, + "x": 49.5698901098901, + "y": 0.4218292682926972, + "z": 49.95 + } +] \ No newline at end of file diff --git a/sample/script/kf_1x1x0_building_height.plt b/sample/script/kf_1x1x0_building_height.plt new file mode 100644 index 0000000000..346baa1e30 --- /dev/null +++ b/sample/script/kf_1x1x0_building_height.plt @@ -0,0 +1,61 @@ +#!/usr/bin/gnuplot +# _ __ _ __ __ _ _ +# | |/ / /\ | | | \/ | /\ | \ | | +# | ' / / \ | | | \ / | / \ | \| | +# | < / /\ \ | | | |\/| | / /\ \ | . ` | +# | . \ / ____ \| |____| | | |/ ____ \| |\ | +# |_|\_\/_/ \_\______|_| |_/_/ \_\_| \_| + +# 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 + +set terminal svg enhanced background rgb "white" size 720,1080 +set datafile separator "," +set output "kalman/sample/image/kf_1x1x0_building_height.svg" +set timestamp +set ylabel "Height (m)" +set xlabel "Measurement Step" +set grid ytics +set xtics 1 + +set multiplot layout 3,1 + +set title "{/:Bold Sample 1x1x0 Building Height}\nStates" +plot "/tmp/kalman/kf_1x1x0_building_height.csv" using ($0):10 with linespoints title "Measurement Output Z", \ + "/tmp/kalman/kf_1x1x0_building_height.csv" using ($0):8 with linespoints title "Estimated State X", \ + 50 with lines title "True State" + +set title "Uncertainties" +plot "/tmp/kalman/kf_1x1x0_building_height.csv" using ($0):6 with linespoints title "Measurement Uncertainty R", \ + "/tmp/kalman/kf_1x1x0_building_height.csv" using ($0):4 with linespoints title "Estimation Uncertainty P" + +set title "Gain" +plot "/tmp/kalman/kf_1x1x0_building_height.csv" using ($0):3 with linespoints title "Gain K" diff --git a/sample/script/plot.sh b/sample/script/plot.sh new file mode 100644 index 0000000000..3181552dbd --- /dev/null +++ b/sample/script/plot.sh @@ -0,0 +1,57 @@ +#!/bin/bash +# _ __ _ __ __ _ _ +# | |/ / /\ | | | \/ | /\ | \ | | +# | ' / / \ | | | \ / | / \ | \| | +# | < / /\ \ | | | |\/| | / /\ \ | . ` | +# | . \ / ____ \| |____| | | |/ ____ \| |\ | +# |_|\_\/_/ \_\______|_| |_/_/ \_\_| \_| + +# 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 + +set -e + +rm -rf /tmp/kalman +mkdir /tmp/kalman + +RESULTS=`find "kalman/sample/result" -iname "*.json"` +for RESULT in ${RESULTS}; do + NAME=$(basename ${RESULT} .json) + jq --compact-output '.[]' ${RESULT} > /tmp/kalman/${NAME}.json + sed -E 's#\{(.*)\}#\1#' /tmp/kalman/${NAME}.json | + sed -E 's#"##g' | + sed -E 's#[a-z]:##g' > /tmp/kalman/${NAME}.csv +done + +PLOTS=`find "kalman/sample/script" -iname "*.plt"` +for PLOT in ${PLOTS}; do + gnuplot ${PLOT} +done