From 803aae0e6fed9c05f53c9cdb1f87017f7cb0d3d9 Mon Sep 17 00:00:00 2001 From: maximusron Date: Fri, 15 Sep 2023 19:44:47 +0530 Subject: [PATCH 1/5] Extended KF input measurements based on motion equations - plot estimates in Python --- notebooks/kalman_CUDA_demo/run_kf.ipynb | 122 ++- notebooks/kalman_CUDA_demo/run_kf_plot.ipynb | 814 +++++++++++++++++++ 2 files changed, 868 insertions(+), 68 deletions(-) create mode 100644 notebooks/kalman_CUDA_demo/run_kf_plot.ipynb diff --git a/notebooks/kalman_CUDA_demo/run_kf.ipynb b/notebooks/kalman_CUDA_demo/run_kf.ipynb index ab5a9757..1c3ade7e 100644 --- a/notebooks/kalman_CUDA_demo/run_kf.ipynb +++ b/notebooks/kalman_CUDA_demo/run_kf.ipynb @@ -431,7 +431,7 @@ " \n", " std::vector expandedMeasurements = generateExpandedData(measurements, expansion_factor);\n", " \n", - " std::vector x0 = {expandedMeasurements[0], 0, -9.81};\n", + " std::vector x0 = {expandedMeasurements[0], 0, -9.5};\n", " kf.init(0, x0);\n", "\n", " // Feed measurements into filter, output estimated states\n", @@ -499,64 +499,64 @@ "name": "stdout", "output_type": "stream", "text": [ - "t = 0, x_hat[0]: 1.49273 0 -9.81 \n", - "t = 0.0333333, y[0] = 1.04203, x_hat[0] = 1.18055 -9.56663 -9.82201 \n", - "t = 0.0666667, y[1] = 1.10727, x_hat[1] = 1.04216 -7.18534 -9.81834 \n", - "t = 0.1, y[2] = 1.29135, x_hat[2] = 1.10856 -4.4342 -9.80991 \n", - "t = 0.133333, y[3] = 1.48485, x_hat[3] = 1.23897 -2.65176 -9.79492 \n", - "t = 0.166667, y[4] = 1.72826, x_hat[4] = 1.41541 -1.36441 -9.7679 \n", - "t = 0.2, y[5] = 1.74216, x_hat[5] = 1.5201 -0.923214 -9.74154 \n", - "t = 0.233333, y[6] = 2.11672, x_hat[6] = 1.71571 -0.249681 -9.67866 \n", - "t = 0.266667, y[7] = 2.14529, x_hat[7] = 1.85071 -0.0135839 -9.61971 \n", - "t = 0.3, y[8] = 2.1603, x_hat[8] = 1.94342 -0.0067291 -9.56589 \n", - "t = 0.333333, y[9] = 2.21269, x_hat[9] = 2.01834 -0.0830709 -9.50747 \n", - "t = 0.366667, y[10] = 2.57709, x_hat[10] = 2.16226 0.0419556 -9.35945 \n", - "t = 0.4, y[11] = 2.66822, x_hat[11] = 2.28825 0.0859778 -9.20125 \n", - "t = 0.433333, y[12] = 2.51642, x_hat[12] = 2.34414 -0.0748605 -9.11883 \n", - "t = 0.466667, y[13] = 2.76034, x_hat[13] = 2.43631 -0.124624 -8.94316 \n", - "t = 0.5, y[14] = 2.88132, x_hat[14] = 2.53044 -0.161939 -8.73035 \n", - "t = 0.533333, y[15] = 2.88374, x_hat[15] = 2.60153 -0.250322 -8.54117 \n", - "t = 0.566667, y[16] = 2.94485, x_hat[16] = 2.66672 -0.338958 -8.33748 \n", - "t = 0.6, y[17] = 2.82867, x_hat[17] = 2.69116 -0.520459 -8.22869 \n", - "t = 0.633333, y[18] = 3.00066, x_hat[18] = 2.74064 -0.611671 -8.00887 \n", - "t = 0.666667, y[19] = 3.12921, x_hat[19] = 2.80348 -0.646838 -7.71773 \n", - "t = 0.7, y[20] = 2.85836, x_hat[20] = 2.79746 -0.860126 -7.66077 \n", - "t = 0.733333, y[21] = 2.83808, x_hat[21] = 2.78289 -1.075 -7.6073 \n", - "t = 0.766667, y[22] = 2.68975, x_hat[22] = 2.73536 -1.36255 -7.65263 \n", - "t = 0.8, y[23] = 2.66533, x_hat[23] = 2.6849 -1.63242 -7.67241 \n", - "t = 0.833333, y[24] = 2.81613, x_hat[24] = 2.66865 -1.7756 -7.52222 \n", - "t = 0.866667, y[25] = 2.81004, x_hat[25] = 2.65085 -1.90394 -7.36013 \n", - "t = 0.9, y[26] = 2.88322, x_hat[26] = 2.6486 -1.96825 -7.12303 \n", - "t = 0.933333, y[27] = 2.69789, x_hat[27] = 2.60682 -2.13543 -7.0323 \n", - "t = 0.966667, y[28] = 2.43422, x_hat[28] = 2.51459 -2.43157 -7.11077 \n", - "t = 1, y[29] = 2.23465, x_hat[29] = 2.39226 -2.78874 -7.2608 \n", - "t = 1.03333, y[30] = 2.30279, x_hat[30] = 2.30003 -3.02869 -7.25825 \n", - "t = 1.06667, y[31] = 2.0207, x_hat[31] = 2.16219 -3.37581 -7.38461 \n", - "t = 1.1, y[32] = 1.94394, x_hat[32] = 2.02788 -3.68333 -7.45684 \n", - "t = 1.13333, y[33] = 1.82498, x_hat[33] = 1.88867 -3.97757 -7.50949 \n", - "t = 1.16667, y[34] = 1.52526, x_hat[34] = 1.709 -4.35693 -7.65511 \n", - "t = 1.2, y[35] = 1.86968, x_hat[35] = 1.6258 -4.44465 -7.47015 \n", - "t = 1.23333, y[36] = 1.18073, x_hat[36] = 1.41784 -4.8526 -7.64198 \n", - "t = 1.26667, y[37] = 1.1073, x_hat[37] = 1.22633 -5.18515 -7.72431 \n", - "t = 1.3, y[38] = 0.916168, x_hat[38] = 1.02624 -5.51275 -7.79693 \n", - "t = 1.33333, y[39] = 0.678548, x_hat[39] = 0.810209 -5.85433 -7.87974 \n", - "t = 1.36667, y[40] = 0.562382, x_hat[40] = 0.604779 -6.14259 -7.90515 \n", - "t = 1.4, y[41] = 0.355468, x_hat[41] = 0.391401 -6.42721 -7.92567 \n", - "t = 1.43333, y[42] = -0.155607, x_hat[42] = 0.113297 -6.84513 -8.072 \n", - "t = 1.46667, y[43] = -0.287199, x_hat[43] = -0.147659 -7.19181 -8.14437 \n", - "t = 1.5, y[44] = -0.602973, x_hat[44] = -0.428043 -7.55795 -8.23086 \n", + "t = 0, x_hat[0]: 0.859101 0 -9.5 \n", + "t = 0.0333333, y[0] = 1.04203, x_hat[0] = 0.985804 3.4334 -9.49513 \n", + "t = 0.0666667, y[1] = 1.10727, x_hat[1] = 1.10541 3.19429 -9.49502 \n", + "t = 0.1, y[2] = 1.29135, x_hat[2] = 1.26163 3.37837 -9.49365 \n", + "t = 0.133333, y[3] = 1.48485, x_hat[3] = 1.43296 3.50712 -9.49049 \n", + "t = 0.166667, y[4] = 1.72826, x_hat[4] = 1.63165 3.68915 -9.48214 \n", + "t = 0.2, y[5] = 1.74216, x_hat[5] = 1.74959 3.34742 -9.48303 \n", + "t = 0.233333, y[6] = 2.11672, x_hat[6] = 1.95339 3.43792 -9.45741 \n", + "t = 0.266667, y[7] = 2.14529, x_hat[7] = 2.09329 3.22131 -9.44701 \n", + "t = 0.3, y[8] = 2.1603, x_hat[8] = 2.18853 2.86377 -9.45401 \n", + "t = 0.333333, y[9] = 2.21269, x_hat[9] = 2.26411 2.48447 -9.46947 \n", + "t = 0.366667, y[10] = 2.57709, x_hat[10] = 2.40705 2.34997 -9.40879 \n", + "t = 0.4, y[11] = 2.66822, x_hat[11] = 2.53053 2.16534 -9.35147 \n", + "t = 0.433333, y[12] = 2.51642, x_hat[12] = 2.5824 1.79776 -9.38304 \n", + "t = 0.466667, y[13] = 2.76034, x_hat[13] = 2.66901 1.55664 -9.33352 \n", + "t = 0.5, y[14] = 2.88132, x_hat[14] = 2.756 1.33866 -9.25752 \n", + "t = 0.533333, y[15] = 2.88374, x_hat[15] = 2.81835 1.07703 -9.21368 \n", + "t = 0.566667, y[16] = 2.94485, x_hat[16] = 2.87319 0.820425 -9.1612 \n", + "t = 0.6, y[17] = 2.82867, x_hat[17] = 2.88572 0.475051 -9.20634 \n", + "t = 0.633333, y[18] = 3.00066, x_hat[18] = 2.92181 0.223687 -9.13968 \n", + "t = 0.666667, y[19] = 3.12921, x_hat[19] = 2.96996 0.0323539 -8.99735 \n", + "t = 0.7, y[20] = 2.85836, x_hat[20] = 2.94814 -0.332372 -9.08131 \n", + "t = 0.733333, y[21] = 2.83808, x_hat[21] = 2.91692 -0.692906 -9.15769 \n", + "t = 0.766667, y[22] = 2.68975, x_hat[22] = 2.85218 -1.11915 -9.31914 \n", + "t = 0.8, y[23] = 2.66533, x_hat[23] = 2.78425 -1.51958 -9.4393 \n", + "t = 0.833333, y[24] = 2.81613, x_hat[24] = 2.75057 -1.78418 -9.37253 \n", + "t = 0.866667, y[25] = 2.81004, x_hat[25] = 2.71563 -2.02402 -9.27641 \n", + "t = 0.9, y[26] = 2.88322, x_hat[26] = 2.69681 -2.18939 -9.08803 \n", + "t = 0.933333, y[27] = 2.69789, x_hat[27] = 2.63919 -2.44704 -9.02954 \n", + "t = 0.966667, y[28] = 2.43422, x_hat[28] = 2.53201 -2.82314 -9.12502 \n", + "t = 1, y[29] = 2.23465, x_hat[29] = 2.39572 -3.25009 -9.27835 \n", + "t = 1.03333, y[30] = 2.30279, x_hat[30] = 2.29058 -3.55016 -9.26707 \n", + "t = 1.06667, y[31] = 2.0207, x_hat[31] = 2.14091 -3.94842 -9.37442 \n", + "t = 1.1, y[32] = 1.94394, x_hat[32] = 1.99583 -4.29884 -9.41907 \n", + "t = 1.13333, y[33] = 1.82498, x_hat[33] = 1.84688 -4.62851 -9.43717 \n", + "t = 1.16667, y[34] = 1.52526, x_hat[34] = 1.65846 -5.03664 -9.54274 \n", + "t = 1.2, y[35] = 1.86968, x_hat[35] = 1.56745 -5.14721 -9.31353 \n", + "t = 1.23333, y[36] = 1.18073, x_hat[36] = 1.35254 -5.57283 -9.43803 \n", + "t = 1.26667, y[37] = 1.1073, x_hat[37] = 1.15488 -5.91855 -9.47095 \n", + "t = 1.3, y[38] = 0.916168, x_hat[38] = 0.949377 -6.2554 -9.49286 \n", + "t = 1.33333, y[39] = 0.678548, x_hat[39] = 0.728596 -6.60288 -9.52434 \n", + "t = 1.36667, y[40] = 0.562382, x_hat[40] = 0.51902 -6.89417 -9.49835 \n", + "t = 1.4, y[41] = 0.355468, x_hat[41] = 0.30204 -7.17939 -9.46784 \n", + "t = 1.43333, y[42] = -0.155607, x_hat[42] = 0.0208246 -7.59585 -9.56385 \n", + "t = 1.46667, y[43] = -0.287199, x_hat[43] = -0.242801 -7.93934 -9.58687 \n", + "t = 1.5, y[44] = -0.602973, x_hat[44] = -0.525461 -8.30085 -9.6252 \n", "\n", - "Exec Success, Final kf states:-0.428043 -7.55795 -8.23086 \n" + "Exec Success, Final kf states:-0.525461 -8.30085 -9.6252 \n" ] } ], "source": [ - "pyrun_sim(10, true)" + "pyrun_sim(1, true)" ] }, { "cell_type": "code", - "execution_count": 23, + "execution_count": null, "id": "d0152901-3f6d-49e5-9138-126d1b072912", "metadata": {}, "outputs": [ @@ -565,15 +565,9 @@ "output_type": "stream", "text": [ "\n", - "Exec Success, Final kf states:-0.454554 -7.75636 -8.59785 \n", - "\n", - "Exec Success, Final kf states:-0.505705 -8.13916 -9.30595 \n", - "\n", - "Exec Success, Final kf states:-0.488794 -8.0126 -9.07184 \n", - "\n", - "Exec Success, Final kf states:-0.536246 -8.36772 -9.72873 \n", + "Exec Success, Final kf states:-0.459225 -7.80515 -8.70828 \n", "\n", - "Exec Success, Final kf states:-0.560802 -8.55149 -10.0687 \n" + "Exec Success, Final kf states:-0.482229 -7.97731 -9.02673 \n" ] } ], @@ -590,18 +584,10 @@ }, { "cell_type": "code", - "execution_count": 24, + "execution_count": null, "id": "aa8b6916-eb9e-449b-b11a-255609a94420", "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[706, 1404, 13545, 27020, 53801]\n" - ] - } - ], + "outputs": [], "source": [ "%%python\n", "\n", diff --git a/notebooks/kalman_CUDA_demo/run_kf_plot.ipynb b/notebooks/kalman_CUDA_demo/run_kf_plot.ipynb new file mode 100644 index 00000000..7c2cd9a4 --- /dev/null +++ b/notebooks/kalman_CUDA_demo/run_kf_plot.ipynb @@ -0,0 +1,814 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 1, + "id": "afdccef0-6aaa-43d6-ac29-7ea950356e19", + "metadata": {}, + "outputs": [], + "source": [ + "#include \"kalman.hpp\"" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "id": "6feb6a99-9580-4bf7-aa48-0a7d175494a4", + "metadata": {}, + "outputs": [], + "source": [ + "#include \n", + "#include \n", + "#include \n" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "ebcc8af4-32b2-418b-bfbe-9ffb4ff9b0ff", + "metadata": {}, + "outputs": [], + "source": [ + "// helper function: matrix addition\n", + "std::vector> matadd(const std::vector>& a, const std::vector>& b) {\n", + " int rows = a.size();\n", + " int cols = a[0].size();\n", + " \n", + " std::vector> result(rows, std::vector(cols));\n", + "\n", + " for (int i = 0; i < rows; i++) {\n", + " for (int j = 0; j < cols; j++) {\n", + " result[i][j] = a[i][j] + b[i][j];\n", + " }\n", + " }\n", + "\n", + " return result;\n", + "}" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "b70f7f32-b7c2-4a1c-96a0-aea42b15f2aa", + "metadata": {}, + "outputs": [], + "source": [ + "// helper function: matrix subtraction\n", + "std::vector> matsub(const std::vector>& a, const std::vector>& b) {\n", + " int rows = a.size();\n", + " int cols = a[0].size();\n", + " \n", + " std::vector> result(rows, std::vector(cols));\n", + "\n", + " for (int i = 0; i < rows; i++) {\n", + " for (int j = 0; j < cols; j++) {\n", + " result[i][j] = a[i][j] - b[i][j];\n", + " }\n", + " }\n", + "\n", + " return result;\n", + "}" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "86b4a452-f04c-45a4-b69b-d87bcb2e5954", + "metadata": {}, + "outputs": [], + "source": [ + "// helper function: matrix transpose\n", + "std::vector> mattranspose(const std::vector>& a) {\n", + " int rows = a.size();\n", + " int cols = a[0].size();\n", + " \n", + " std::vector> result(cols, std::vector(rows));\n", + "\n", + " for (int i = 0; i < rows; i++) {\n", + " for (int j = 0; j < cols; j++) {\n", + " result[j][i] = a[i][j];\n", + " }\n", + " }\n", + "\n", + " return result;\n", + "}\n" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "bf413652-f71a-4b91-9e6e-5520812dc3cb", + "metadata": {}, + "outputs": [], + "source": [ + "// helper function: matrix inversion\n", + "std::vector> matinverse(const std::vector>& a) {\n", + " size_t n = a.size();\n", + " \n", + " if (n != a[0].size()) {\n", + " std::cout<<\" Shape of a : \"<> result(2, std::vector(2));\n", + " result[0][0] = a[1][1] / determinant;\n", + " result[0][1] = -a[0][1] / determinant;\n", + " result[1][0] = -a[1][0] / determinant;\n", + " result[1][1] = a[0][0] / determinant;\n", + "\n", + " return result;\n", + " }\n", + "\n", + " if (n == 3) {\n", + " double determinant = a[0][0]*(a[1][1]*a[2][2]-a[2][1]*a[1][2]) \n", + " - a[0][1]*(a[1][0]*a[2][2]-a[1][2]*a[2][0]) \n", + " + a[0][2]*(a[1][0]*a[2][1]-a[1][1]*a[2][0]);\n", + " \n", + " if (determinant == 0) {\n", + " throw std::runtime_error(\"Matrix is singular and cannot be inverted.\");\n", + " }\n", + "\n", + " std::vector> result(3, std::vector(3));\n", + "\n", + " result[0][0] = (a[1][1] * a[2][2] - a[2][1] * a[1][2]) / determinant;\n", + " result[0][1] = (a[0][2] * a[2][1] - a[0][1] * a[2][2]) / determinant;\n", + " result[0][2] = (a[0][1] * a[1][2] - a[0][2] * a[1][1]) / determinant;\n", + " result[1][0] = (a[1][2] * a[2][0] - a[1][0] * a[2][2]) / determinant;\n", + " result[1][1] = (a[0][0] * a[2][2] - a[0][2] * a[2][0]) / determinant;\n", + " result[1][2] = (a[1][0] * a[0][2] - a[0][0] * a[1][2]) / determinant;\n", + " result[2][0] = (a[1][0] * a[2][1] - a[2][0] * a[1][1]) / determinant;\n", + " result[2][1] = (a[2][0] * a[0][1] - a[0][0] * a[2][1]) / determinant;\n", + " result[2][2] = (a[0][0] * a[1][1] - a[1][0] * a[0][1]) / determinant;\n", + "\n", + " return result;\n", + " }\n", + "\n", + " throw std::runtime_error(\"Only 2x2 and 3x3 matrices supported for inversion in this function.\");\n", + "}\n", + "\n" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "id": "8c4b2db6-0847-4c0f-a8f7-88339d8257d7", + "metadata": {}, + "outputs": [], + "source": [ + "// helper function: matrix-vector subtraction\n", + "std::vector> matvecsub(const std::vector>& mat, const std::vector& vec) {\n", + " std::vector> result(mat.size(), std::vector(vec.size()));\n", + "\n", + " for (size_t i = 0; i < mat.size(); i++) {\n", + " for (size_t j = 0; j < vec.size(); j++) {\n", + " result[i][j] = mat[i][j] - vec[j];\n", + " }\n", + " }\n", + "\n", + " return result;\n", + "}" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "id": "0d925dce-377c-4d23-96d6-9c4fead297cb", + "metadata": {}, + "outputs": [], + "source": [ + "// helper function: matrix-vector multiplication\n", + "std::vector matvecmul(const std::vector>& a, const std::vector& b) {\n", + " int rows = a.size();\n", + " int cols = a[0].size();\n", + "\n", + " if (cols != b.size()) {\n", + " throw std::runtime_error(\"Matrix dimensions do not match for multiplication.\");\n", + " }\n", + "\n", + " std::vector result(rows, 0.0);\n", + "\n", + " for (int i = 0; i < rows; i++) {\n", + " for (int j = 0; j < cols; j++) {\n", + " result[i] += a[i][j] * b[j];\n", + " }\n", + " }\n", + "\n", + " return result;\n", + "}" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "id": "69714460-9a3e-4966-9de7-5e4111fc40af", + "metadata": {}, + "outputs": [], + "source": [ + "// helper function: matrix-matrix multiplication\n", + "std::vector> matmul(const std::vector>& a, const std::vector>& b) {\n", + " int rowsA = a.size();\n", + " int colsA = a[0].size();\n", + " int rowsB = b.size();\n", + " int colsB = b[0].size();\n", + "\n", + " if (colsA != rowsB) {\n", + " throw std::runtime_error(\"Matrix dimensions do not match for multiplication.\");\n", + " }\n", + "\n", + " std::vector> result(rowsA, std::vector(colsB, 0));\n", + "\n", + " for (int i = 0; i < rowsA; i++) {\n", + " for (int j = 0; j < colsB; j++) {\n", + " for (int k = 0; k < colsA; k++) {\n", + " result[i][j] += a[i][k] * b[k][j];\n", + " }\n", + " }\n", + " }\n", + "\n", + " return result;\n", + "}" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "id": "7295e929-dcfc-43be-9b3b-82e9f9fbf9db", + "metadata": {}, + "outputs": [], + "source": [ + "// helper function: vector subtraction\n", + "std::vector vecsub(const std::vector& a, const std::vector& b) {\n", + " size_t len = a.size();\n", + " std::vector result(len);\n", + " for (size_t i = 0; i < len; i++) {\n", + " result[i] = a[i] - b[i];\n", + " }\n", + " return result;\n", + "}" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "id": "f4c9238f-674a-4cf8-8128-840cc7f61e0f", + "metadata": {}, + "outputs": [], + "source": [ + "//set up class and constructor\n", + "KalmanFilter::KalmanFilter(\n", + " double dt,\n", + " const std::vector>& A,\n", + " const std::vector>& C,\n", + " const std::vector>& Q,\n", + " const std::vector>& R,\n", + " const std::vector>& P)\n", + " : A(A), C(C), Q(Q), R(R), P0(P),\n", + " m(C.size()), n(A.size()), dt(dt), initialized(false),\n", + " I(n, std::vector(n)), x_hat(n), x_hat_new(n)\n", + "{\n", + " for (int i = 0; i < n; i++) {\n", + " I[i][i] = 1.0;\n", + " }\n", + "}\n", + "\n", + "KalmanFilter::KalmanFilter() {}" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "id": "bd953ec6-ceed-489c-8e33-8eb7ae6bb32f", + "metadata": {}, + "outputs": [], + "source": [ + "// init the kf states + measurements \n", + "void KalmanFilter::init(double t0, const std::vector& x0) {\n", + " x_hat = x0;\n", + " P = P0;\n", + " this->t0 = t0;\n", + " t = t0;\n", + " initialized = true;\n", + "}" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "id": "2840f0f7-52de-4f47-a9dc-e257235df002", + "metadata": {}, + "outputs": [], + "source": [ + "void KalmanFilter::init() {\n", + " std::fill(x_hat.begin(), x_hat.end(), 0.0);\n", + " P = P0;\n", + " t0 = 0;\n", + " t = t0;\n", + " initialized = true;\n", + "}" + ] + }, + { + "cell_type": "code", + "execution_count": 14, + "id": "ea057f20-9097-4dd3-946e-ab7a383afe69", + "metadata": {}, + "outputs": [], + "source": [ + "\n", + "\n", + "void KalmanFilter::update(const std::vector& y) {\n", + " if (!initialized)\n", + " throw std::runtime_error(\"Filter is not initialized!\");\n", + "\n", + " x_hat_new = matvecmul(A, x_hat);\n", + "\n", + " P = matadd(matmul(matmul(A, P), mattranspose(A)), Q);\n", + "\n", + " std::vector> inv = matinverse(matadd(matmul(matmul(C, P), mattranspose(C)), R));\n", + "\n", + " K = matmul(matmul(P, mattranspose(C)), inv);\n", + " std::vector temp = matvecmul(C, x_hat_new);\n", + " std::vector difference = vecsub(y, temp);\n", + "\n", + " for (size_t i = 0; i < x_hat_new.size(); i++) {\n", + " x_hat_new[i] += matvecmul(K, difference)[i];\n", + " }\n", + "\n", + " P = matmul(matsub(I, matmul(K, C)), P);\n", + "\n", + " x_hat = x_hat_new;\n", + " t += dt;\n", + "}\n" + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "id": "08d28163-a086-46a9-bf41-044f48530929", + "metadata": {}, + "outputs": [], + "source": [ + "void KalmanFilter::update(const std::vector& y, double dt, const std::vector>& A) {\n", + " this->A = A;\n", + " this->dt = dt;\n", + " update(y);\n", + "}\n" + ] + }, + { + "cell_type": "code", + "execution_count": 16, + "id": "80d35cbe-7baa-4bc2-a232-4b5631af59b2", + "metadata": {}, + "outputs": [], + "source": [ + "std::vector generateExpandedData(const std::vector& baseData, int repetitions) {\n", + " std::vector expandedData;\n", + " for (int r = 0; r < repetitions; r++) {\n", + " for (auto val : baseData) {\n", + " // Add slight random perturbation to the data.\n", + " double perturbedValue = val + ((double)rand() / RAND_MAX - 0.5);\n", + " expandedData.push_back(perturbedValue);\n", + " }\n", + " }\n", + " return expandedData;\n", + "}" + ] + }, + { + "cell_type": "code", + "execution_count": 17, + "id": "8938a288-fc2c-4104-82c6-added4466c8c", + "metadata": {}, + "outputs": [], + "source": [ + "std::vector> run_kf(int expansion_factor, bool verbose) {\n", + " \n", + " int n = 3; // Number of states\n", + " int m = 1; // Number of measurements\n", + "\n", + " double dt = 1.0 / 40; // Time step\n", + " \n", + " std::vector g_preds;\n", + " std::vector g_est;\n", + " \n", + " std::vector> A(n, std::vector(n));\n", + " std::vector> C(m, std::vector(n));\n", + " std::vector> Q(n, std::vector(n));\n", + " std::vector> R(m, std::vector(m));\n", + " std::vector> P(n, std::vector(n));\n", + " \n", + " A = {{1, dt, 0}, {0, 1, dt}, {0, 0, 1}};\n", + " C = {{1, 0, 0}};\n", + " Q = {{.05, .05, .0}, {.05, .05, .0}, {.0, .0, .0}};\n", + " R = {{5}};\n", + " P = {{.1, .1, .1}, {.1, 10000, 10}, {.1, 10, 100}};\n", + " \n", + " KalmanFilter kf(dt, A, C, Q, R, P);\n", + " \n", + " std::vector measurements = {\n", + " 1.04202710058, 1.10726790452, 1.2913511148, 1.48485250951, 1.72825901034,\n", + " 1.74216489744, 2.11672039768, 2.14529225112, 2.16029641405, 2.21269371128,\n", + " 2.57709350237, 2.6682215744, 2.51641839428, 2.76034056782, 2.88131780617,\n", + " 2.88373786518, 2.9448468727, 2.82866600131, 3.0006601946, 3.12920591669,\n", + " 2.858361783, 2.83808170354, 2.68975330958, 2.66533185589, 2.81613499531,\n", + " 2.81003612051, 2.88321849354, 2.69789264832, 2.4342229249, 2.23464791825,\n", + " 2.30278776224, 2.02069770395, 1.94393985809, 1.82498398739, 1.52526230354,\n", + " 1.86967808173, 1.18073207847, 1.10729605087, 0.916168349913, 0.678547664519,\n", + " 0.562381751596, 0.355468474885, -0.155607486619, -0.287198661013, -0.602973173813,\n", + " -0.873817307503, -1.144661441193, -1.415505574883, -1.686349708573, -1.957193842263, \n", + " -2.228037975953, -2.498882109643, -2.769726243333, -3.040570377023, -3.311414510713, \n", + " -3.582258644403, -3.853102778093, -4.123946911783, -4.394791045473, -4.665635179163, \n", + " -4.936479312853, -5.207323446543, -5.478167580233, -5.749011713923, -6.019855847613,\n", + " -6.290699981303, -6.561544114993, -6.832388248683, -7.103232382373, -7.374076516063,\n", + " -7.644920649753, -7.915764783443, -8.186608917133, -8.457453050823, -8.728297184513, \n", + " -8.999141318203, -9.269985451893, -9.540829585583, -9.811673719273, -10.082517852963,\n", + " -10.353361986653, -10.624206120343, -10.895050254033, -11.165894387723, -11.436738521413,\n", + " -11.707582655103, -11.978426788793, -12.249270922483, -12.520115056173\n", + " };\n", + " \n", + " // std::vector expandedMeasurements = generateExpandedData(measurements, expansion_factor);\n", + " \n", + " std::vector x0 = {measurements[0], 0, 0};\n", + " kf.init(0, x0);\n", + "\n", + " // Feed measurements into filter, output estimated states\n", + " std::vector y(m);\n", + " if(verbose) {\n", + " std::cout << \"t = \" << 0 << \", \" << \"x_hat[0]: \";\n", + " for (auto& val : kf.state()){\n", + " std::cout << val << \" \";\n", + " }\n", + " g_est.push_back(kf.state()[2]);\n", + " \n", + " std::cout << std::endl;\n", + " }\n", + " \n", + " int i;\n", + " for (i = 0; i < measurements.size(); i++) {\n", + " y[0] = measurements[i];\n", + " kf.update(y);\n", + " if(verbose) {\n", + " std::cout << \"t = \" << (i + 1) * dt << \", y[\" << i << \"] = \" << y[0] << \", x_hat[\" << i << \"] = \";\n", + " for (auto& val : kf.state()) {\n", + " std::cout << val << \" \";\n", + " }\n", + " g_preds.push_back(kf.state()[2]);\n", + " std::cout << std::endl;\n", + " }\n", + " }\n", + " std::cout << std::endl;\n", + " std::cout<<\"Exec Success, Final kf states:\";\n", + " for (auto& val : kf.state()) std::cout << val << \" \";\n", + " std::cout << std::endl;\n", + " \n", + " std::vector> g_res;\n", + " for (size_t i = 0; i < g_preds.size(); ++i) {\n", + " std::vector pair = {g_preds[i], g_est[i]};\n", + " g_res.push_back(pair);\n", + " }\n", + " \n", + " return g_res;\n", + "}" + ] + }, + { + "cell_type": "code", + "execution_count": 18, + "id": "1e77f6cc-410a-4cd4-acbc-94677b77c968", + "metadata": {}, + "outputs": [], + "source": [ + "#include \n", + "auto start = std::chrono::high_resolution_clock::now();\n", + "auto stop = std::chrono::high_resolution_clock::now();\n", + "auto duration = std::chrono::duration_cast(stop - start);" + ] + }, + { + "cell_type": "code", + "execution_count": 19, + "id": "7ea068c2-fc1f-4ad4-a2c9-aecd66cbbf71", + "metadata": {}, + "outputs": [], + "source": [ + "std::vector> pyrun_sim(int exp_factor, bool verbose) {\n", + " start = std::chrono::high_resolution_clock::now();\n", + " std::vector> g_res = run_kf(exp_factor, verbose);\n", + " stop = std::chrono::high_resolution_clock::now();\n", + " duration = std::chrono::duration_cast(stop - start);\n", + " return g_res;\n", + "}" + ] + }, + { + "cell_type": "code", + "execution_count": 20, + "id": "6026ea4e-c42b-42e5-97a7-12af37e121a9", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "t = 0, x_hat[0]: 1.04203 0 0 \n", + "t = 0.025, y[0] = 1.04203, x_hat[0] = 1.04203 0 0 \n", + "t = 0.05, y[1] = 1.10727, x_hat[1] = 1.08709 0.898405 0.00110609 \n", + "t = 0.075, y[2] = 1.29135, x_hat[2] = 1.22062 2.38572 0.00356059 \n", + "t = 0.1, y[3] = 1.48485, x_hat[3] = 1.3876 3.46905 0.00712492 \n", + "t = 0.125, y[4] = 1.72826, x_hat[4] = 1.58997 4.40683 0.013793 \n", + "t = 0.15, y[5] = 1.74216, x_hat[5] = 1.71702 4.52169 0.0154301 \n", + "t = 0.175, y[6] = 2.11672, x_hat[6] = 1.93313 5.12414 0.031192 \n", + "t = 0.2, y[7] = 2.14529, x_hat[7] = 2.08865 5.26575 0.0374157 \n", + "t = 0.225, y[8] = 2.1603, x_hat[8] = 2.20235 5.18421 0.0316618 \n", + "t = 0.25, y[9] = 2.21269, x_hat[9] = 2.29893 5.04726 0.0173069 \n", + "t = 0.275, y[10] = 2.57709, x_hat[10] = 2.46442 5.19829 0.039685 \n", + "t = 0.3, y[11] = 2.66822, x_hat[11] = 2.61234 5.26324 0.0527055 \n", + "t = 0.325, y[12] = 2.51642, x_hat[12] = 2.69148 5.08935 0.00552738 \n", + "t = 0.35, y[13] = 2.76034, x_hat[13] = 2.80588 5.04887 -0.00849014 \n", + "t = 0.375, y[14] = 2.88132, x_hat[14] = 2.92141 5.01625 -0.0224249 \n", + "t = 0.4, y[15] = 2.88374, x_hat[15] = 3.0137 4.91887 -0.0729322 \n", + "t = 0.425, y[16] = 2.94485, x_hat[16] = 3.09895 4.80966 -0.139272 \n", + "t = 0.45, y[17] = 2.82867, x_hat[17] = 3.14447 4.59753 -0.288577 \n", + "t = 0.475, y[18] = 3.00066, x_hat[18] = 3.21103 4.45686 -0.396929 \n", + "t = 0.5, y[19] = 3.12921, x_hat[19] = 3.287 4.34976 -0.484796 \n", + "t = 0.525, y[20] = 2.85836, x_hat[20] = 3.29866 4.07181 -0.747944 \n", + "t = 0.55, y[21] = 2.83808, x_hat[21] = 3.30007 3.77743 -1.04221 \n", + "t = 0.575, y[22] = 2.68975, x_hat[22] = 3.26982 3.40694 -1.43333 \n", + "t = 0.6, y[23] = 2.66533, x_hat[23] = 3.23376 3.03344 -1.83638 \n", + "t = 0.625, y[24] = 2.81613, x_hat[24] = 3.2232 2.74466 -2.13797 \n", + "t = 0.65, y[25] = 2.81004, x_hat[25] = 3.20765 2.45227 -2.44388 \n", + "t = 0.675, y[26] = 2.88322, x_hat[26] = 3.20161 2.19814 -2.69673 \n", + "t = 0.7, y[27] = 2.69789, x_hat[27] = 3.15898 1.84844 -3.07251 \n", + "t = 0.725, y[28] = 2.43422, x_hat[28] = 3.07035 1.3784 -3.6016 \n", + "t = 0.75, y[29] = 2.23465, x_hat[29] = 2.95232 0.840664 -4.20755 \n", + "t = 0.775, y[30] = 2.30279, x_hat[30] = 2.85557 0.387859 -4.67894 \n", + "t = 0.8, y[31] = 2.0207, x_hat[31] = 2.7166 -0.169629 -5.27548 \n", + "t = 0.825, y[32] = 1.94394, x_hat[32] = 2.57681 -0.704099 -5.81837 \n", + "t = 0.85, y[33] = 1.82498, x_hat[33] = 2.42945 -1.23525 -6.3351 \n", + "t = 0.875, y[34] = 1.52526, x_hat[34] = 2.24401 -1.85275 -6.94502 \n", + "t = 0.9, y[35] = 1.86968, x_hat[35] = 2.13957 -2.19864 -7.17158 \n", + "t = 0.925, y[36] = 1.18073, x_hat[36] = 1.92439 -2.85128 -7.78709 \n", + "t = 0.95, y[37] = 1.1073, x_hat[37] = 1.72092 -3.43475 -8.28638 \n", + "t = 0.975, y[38] = 0.916168, x_hat[38] = 1.50771 -4.01434 -8.75833 \n", + "t = 1, y[39] = 0.678548, x_hat[39] = 1.2784 -4.60797 -9.22649 \n", + "t = 1.025, y[40] = 0.562382, x_hat[40] = 1.05708 -5.14471 -9.60338 \n", + "t = 1.05, y[41] = 0.355468, x_hat[41] = 0.827473 -5.67368 -9.95378 \n", + "t = 1.075, y[42] = -0.155607, x_hat[42] = 0.537759 -6.3418 -10.4545 \n", + "t = 1.1, y[43] = -0.287199, x_hat[43] = 0.262434 -6.93117 -10.8401 \n", + "t = 1.125, y[44] = -0.602973, x_hat[44] = -0.0317108 -7.53829 -11.2291 \n", + "t = 1.15, y[45] = -0.873817, x_hat[45] = -0.333878 -8.13198 -11.5854 \n", + "t = 1.175, y[46] = -1.14466, x_hat[46] = -0.64242 -8.70817 -11.9065 \n", + "t = 1.2, y[47] = -1.41551, x_hat[47] = -0.955917 -9.2638 -12.1908 \n", + "t = 1.225, y[48] = -1.68635, x_hat[48] = -1.27315 -9.79661 -12.4381 \n", + "t = 1.25, y[49] = -1.95719, x_hat[49] = -1.5931 -10.3051 -12.6488 \n", + "t = 1.275, y[50] = -2.22804, x_hat[50] = -1.91487 -10.7882 -12.8241 \n", + "t = 1.3, y[51] = -2.49888, x_hat[51] = -2.23773 -11.2455 -12.9653 \n", + "t = 1.325, y[52] = -2.76973, x_hat[52] = -2.56107 -11.6768 -13.0743 \n", + "t = 1.35, y[53] = -3.04057, x_hat[53] = -2.88438 -12.0825 -13.1531 \n", + "t = 1.375, y[54] = -3.31141, x_hat[54] = -3.20725 -12.4629 -13.2039 \n", + "t = 1.4, y[55] = -3.58226, x_hat[55] = -3.52932 -12.8188 -13.2289 \n", + "t = 1.425, y[56] = -3.8531, x_hat[56] = -3.85034 -13.1508 -13.2301 \n", + "t = 1.45, y[57] = -4.12395, x_hat[57] = -4.17008 -13.46 -13.2098 \n", + "t = 1.475, y[58] = -4.39479, x_hat[58] = -4.48837 -13.7472 -13.17 \n", + "t = 1.5, y[59] = -4.66564, x_hat[59] = -4.8051 -14.0134 -13.1126 \n", + "t = 1.525, y[60] = -4.93648, x_hat[60] = -5.12016 -14.2597 -13.0395 \n", + "t = 1.55, y[61] = -5.20732, x_hat[61] = -5.4335 -14.4872 -12.9525 \n", + "t = 1.575, y[62] = -5.47817, x_hat[62] = -5.74508 -14.6968 -12.853 \n", + "t = 1.6, y[63] = -5.74901, x_hat[63] = -6.05487 -14.8895 -12.7428 \n", + "t = 1.625, y[64] = -6.01986, x_hat[64] = -6.36288 -15.0664 -12.623 \n", + "t = 1.65, y[65] = -6.2907, x_hat[65] = -6.66912 -15.2283 -12.4951 \n", + "t = 1.675, y[66] = -6.56154, x_hat[66] = -6.97361 -15.3763 -12.3601 \n", + "t = 1.7, y[67] = -6.83239, x_hat[67] = -7.27637 -15.5111 -12.2191 \n", + "t = 1.725, y[68] = -7.10323, x_hat[68] = -7.57746 -15.6336 -12.0731 \n", + "t = 1.75, y[69] = -7.37408, x_hat[69] = -7.87691 -15.7446 -11.9229 \n", + "t = 1.775, y[70] = -7.64492, x_hat[70] = -8.17476 -15.8449 -11.7694 \n", + "t = 1.8, y[71] = -7.91576, x_hat[71] = -8.47108 -15.9351 -11.6132 \n", + "t = 1.825, y[72] = -8.18661, x_hat[72] = -8.7659 -16.0159 -11.4551 \n", + "t = 1.85, y[73] = -8.45745, x_hat[73] = -9.05929 -16.0881 -11.2954 \n", + "t = 1.875, y[74] = -8.7283, x_hat[74] = -9.3513 -16.152 -11.1349 \n", + "t = 1.9, y[75] = -8.99914, x_hat[75] = -9.64199 -16.2084 -10.9739 \n", + "t = 1.925, y[76] = -9.26999, x_hat[76] = -9.9314 -16.2578 -10.8128 \n", + "t = 1.95, y[77] = -9.54083, x_hat[77] = -10.2196 -16.3005 -10.652 \n", + "t = 1.975, y[78] = -9.81167, x_hat[78] = -10.5066 -16.3372 -10.4918 \n", + "t = 2, y[79] = -10.0825, x_hat[79] = -10.7925 -16.3682 -10.3325 \n", + "t = 2.025, y[80] = -10.3534, x_hat[80] = -11.0774 -16.3939 -10.1743 \n", + "t = 2.05, y[81] = -10.6242, x_hat[81] = -11.3612 -16.4148 -10.0175 \n", + "t = 2.075, y[82] = -10.8951, x_hat[82] = -11.6441 -16.4311 -9.86215 \n", + "t = 2.1, y[83] = -11.1659, x_hat[83] = -11.9261 -16.4432 -9.70853 \n", + "t = 2.125, y[84] = -11.4367, x_hat[84] = -12.2072 -16.4514 -9.55674 \n", + "t = 2.15, y[85] = -11.7076, x_hat[85] = -12.4874 -16.4559 -9.4069 \n", + "t = 2.175, y[86] = -11.9784, x_hat[86] = -12.7669 -16.4572 -9.25909 \n", + "t = 2.2, y[87] = -12.2493, x_hat[87] = -13.0456 -16.4553 -9.1134 \n", + "t = 2.225, y[88] = -12.5201, x_hat[88] = -13.3236 -16.4505 -8.96991 \n", + "\n", + "Exec Success, Final kf states:-13.3236 -16.4505 -8.96991 \n" + ] + } + ], + "source": [ + "std::vector> g_res = pyrun_sim(1, true);" + ] + }, + { + "cell_type": "code", + "execution_count": 21, + "id": "5e8f3f24-b003-4416-bb16-eabc480e437e", + "metadata": {}, + "outputs": [], + "source": [ + "std::vector py_g_pred;\n", + "std::vector py_g_est;" + ] + }, + { + "cell_type": "code", + "execution_count": 22, + "id": "107f53c0-0f2d-47b9-a218-67c29e16b87b", + "metadata": {}, + "outputs": [], + "source": [ + "int k = g_res.size();" + ] + }, + { + "cell_type": "code", + "execution_count": 23, + "id": "63f5ba73-393b-4a0a-9c83-6829b56fc526", + "metadata": {}, + "outputs": [], + "source": [ + "std::vector ret_1d_vector(std::vector> res, int axis) {\n", + " std::vector ret;\n", + " for (int i = 0; i < res.size(); i++) {\n", + " ret.push_back(res[i][axis]);\n", + " }\n", + " return ret;\n", + "}\n", + " " + ] + }, + { + "cell_type": "code", + "execution_count": 24, + "id": "a6bc89d0-026f-4f52-b8c9-9a8fca6948f2", + "metadata": {}, + "outputs": [], + "source": [ + "py_g_pred = ret_1d_vector(g_res, 0);\n", + "py_g_est = ret_1d_vector(g_res, 1);" + ] + }, + { + "cell_type": "code", + "execution_count": 25, + "id": "1b22451b-0bd7-48d6-8590-ff59b56a8031", + "metadata": {}, + "outputs": [], + "source": [ + "void printMatrix(const std::vector& vec) {\n", + " for (size_t i = 0; i < vec.size(); i++) {\n", + " std::cout << vec[i] << \" \";\n", + " }\n", + " std::cout << std::endl;\n", + " }\n" + ] + }, + { + "cell_type": "code", + "execution_count": 26, + "id": "47fd1710-59a8-4d80-96de-04ef86e233fd", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "0 0.00110609 0.00356059 0.00712492 0.013793 0.0154301 0.031192 0.0374157 0.0316618 0.0173069 0.039685 0.0527055 0.00552738 -0.00849014 -0.0224249 -0.0729322 -0.139272 -0.288577 -0.396929 -0.484796 -0.747944 -1.04221 -1.43333 -1.83638 -2.13797 -2.44388 -2.69673 -3.07251 -3.6016 -4.20755 -4.67894 -5.27548 -5.81837 -6.3351 -6.94502 -7.17158 -7.78709 -8.28638 -8.75833 -9.22649 -9.60338 -9.95378 -10.4545 -10.8401 -11.2291 -11.5854 -11.9065 -12.1908 -12.4381 -12.6488 -12.8241 -12.9653 -13.0743 -13.1531 -13.2039 -13.2289 -13.2301 -13.2098 -13.17 -13.1126 -13.0395 -12.9525 -12.853 -12.7428 -12.623 -12.4951 -12.3601 -12.2191 -12.0731 -11.9229 -11.7694 -11.6132 -11.4551 -11.2954 -11.1349 -10.9739 -10.8128 -10.652 -10.4918 -10.3325 -10.1743 -10.0175 -9.86215 -9.70853 -9.55674 -9.4069 -9.25909 -9.1134 -8.96991 \n" + ] + } + ], + "source": [ + "printMatrix(py_g_pred);" + ] + }, + { + "cell_type": "code", + "execution_count": 27, + "id": "d0152901-3f6d-49e5-9138-126d1b072912", + "metadata": {}, + "outputs": [], + "source": [ + "// %%python\n", + "\n", + "// expand_factor = [1, 2, 3, 4, 5]\n", + "// benchmarks = []\n", + "\n", + "// for i in expand_factor:\n", + "// time_kf = cppyy.gbl.pyrun_sim(int(i), 0)\n", + "// benchmarks.append(time_kf)\n", + "// " + ] + }, + { + "cell_type": "code", + "execution_count": 28, + "id": "2c0b5c86-7875-45d9-8113-00febd1be3d5", + "metadata": {}, + "outputs": [], + "source": [ + "%%python\n", + "\n", + "import matplotlib.pyplot as plt" + ] + }, + { + "cell_type": "code", + "execution_count": 29, + "id": "e0d86e62-207f-4e14-80c1-2e04c8984fd6", + "metadata": {}, + "outputs": [], + "source": [ + "%%python\n", + "\n", + "true_val = 9.81\n", + "g_pred = list(cppyy.gbl.py_g_pred)\n", + "g_pred = list(-x for x in g_pred)\n", + "x = range(len(g_pred))\n", + "\n", + "\n", + "# Plot the constant green line\n", + "plt.axhline(y=true_val, color='green', linestyle='-')\n", + "\n", + "# Plot g_pred in orange\n", + "plt.plot(x, g_pred, color='orange', marker='o', label='KF Estimates')\n", + "plt.annotate(f'{true_val}', xy=(-0.5, true_val), color='green',\n", + " verticalalignment='center', horizontalalignment = 'left')\n", + "# Add labels and title\n", + "plt.xlabel('Index')\n", + "plt.ylabel('Acceleration (m/s₂)')\n", + "plt.title('True Value vs. g_pred')\n", + "plt.legend()\n", + "plt.savefig(\"kf_plot2.jpg\")\n", + " \n", + "plt.yscale('symlog')\n", + "# Show the plot\n", + "plt.show()\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c8d9db0e-a46b-4de0-af1d-0e6aec92abcc", + "metadata": {}, + "outputs": [], + "source": [] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3ac6b7dd-7b1b-4fe8-8d9a-c8c3c29326b6", + "metadata": {}, + "outputs": [], + "source": [] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "34c14f7b-0a0a-43ff-9401-2637f6c7f3ea", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "CUDA (C++17)", + "language": "CUDA", + "name": "cuda-xcpp17" + }, + "language_info": { + "codemirror_mode": "text/x-c++src", + "file_extension": ".cpp", + "mimetype": "text/x-c++src", + "name": "c++", + "version": "17" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} From 9942cb24582dba84593fbc97d0a40e17a95d297d Mon Sep 17 00:00:00 2001 From: maximusron Date: Fri, 15 Sep 2023 19:45:19 +0530 Subject: [PATCH 2/5] Add gravity estimates as plot --- notebooks/kalman_CUDA_demo/1D_kf_plot.jpg | Bin 0 -> 21810 bytes .../kalman_CUDA_demo/{kalman.h++ => kalman.hpp} | 0 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 notebooks/kalman_CUDA_demo/1D_kf_plot.jpg rename notebooks/kalman_CUDA_demo/{kalman.h++ => kalman.hpp} (100%) diff --git a/notebooks/kalman_CUDA_demo/1D_kf_plot.jpg b/notebooks/kalman_CUDA_demo/1D_kf_plot.jpg new file mode 100644 index 0000000000000000000000000000000000000000..8013ad2a64cfe320405fa9e61acf9066da5dc79c GIT binary patch literal 21810 zcmeHv1yojB*Y3ub4haEiln|s-8U&=f8>G9trS+u*6p&775b2hbknZm8Zn&@dedj;N zbIy0ZaNPUkB(1H&(1Ht(gg*8 zzLWI}Wk1k`4x#G~G&Ber?kinTcbp*&gboe!kP#MxUk*;!_8tk74?LzoL~3y(0x7fn z9+sY6KO#06%RKr1SJJ*w_RkUK`)^V93t|67*93qJf`SMSgbwflrx%oIFA)E?{om&S zc*Cf?G|tgiZGv*Vg%QvefORSq#pQt&B|4_Xglb8CnPJwInGEj|v)v6>B5#2X)6!d@*va~u5?ony_sm>| z82XsU;FHlX*`Oo2SYj~F(0N`ltWA2DUk+IxTuHySbCITigLTkz(TuKKH73Ao??5MF zyZx%wK6zX{Ik+tOo)oW-VNS1gj+8OaH2yMhR9AAe>f|DH42`byv z!C31zb24SIt>I)*$W|kD(96$$tE;16Cv z*WlzhHgd@GH?ng(WM2su-U81Xbt%ScZ-KH6>=V`53gftWHV2M2`>>m|N-aVDL3vNM zChVm9ewx(-NA2S?J4chvco>RkfJF^cal1B;IkR+K(&7AQ5cYx$!=y{$%nb7?uFWgw z0Q?>#fB|X;NI^ABtL#PYCTs}4avJ1J`|JuGrn+c$9nC9K0e4Wu`_X-~7tVvd|6wWk zpyy=5Ic~(fygY0^9Y|x&jTE7owDqf)c)>FOUf@Pb*|@+?&!ue*eV>@oQ(RXTP`9Z` z!-O0}U8Q1T9plp`B(@N!b+65fa3B5QgLm;_wR$e&6y4~&O+p~P?LIhTAy=)Wplp|t zM)l|-lXg1CZYgVzcNr!Dtr-^B?6{+EQSgjOFNf>qR0g&Rk1eTCQ3*k^<>+FL7LPEV zbvBlBGO%xr&k?Hw+u12-!u;*53x_5XRwa9ftuqwEgttJ2((cgIyI7uv=$;(-)qn|1 z%iWrp7d&a;uD#JKa5dkxtKcn=g1A`gm^Hr9fPRIp>$QDinmz<-A;^wLKpzubg(C<% zYX#hBu@dDtQT@42;X)QB-QCg#C#4uxodif9X^vieM2KS8uh=PcNjtZzc z47K$39;FRxb3Zzdq%aUOY$ks9(KgIBEbs-7cJcAG^Cjvf{WX5?rBfF~S-OUsd%yrQh=qK4D z--+I;xUXK9^^PLpi#1bbNO;sC!Q9SvW!Mqa=8>EbfcaCJl!w3G-Mf@fLn^5Ap==r> z9*;B3Vu+YVoZvlUErCT6%RPz>Sc;bDf_kxp?I^lO` z!_S1vbS?-TdoK@-Z-F*SPrbQ#9(~Zg1!N?g%G8`I(6NzK za6c=-vG-!eS%qe;0ts5d2<)(!hcS}mL=s}u3#w6i`SYv#<&t`|GB3*-FFfzGk zq|Fl}DWj*=u58k&2GO`n{_Sv2FLA6s*PABqg_<=Sac(A`uJqmlA#%3>{7Bv{AXr#+ z3wT-F0+5MhHKES0YmAbaFM(AHU)+j!qyz#`o3b~w2?kg_Wu%6zw<6u^SieL^V(Ts! zw@E`au5qqr%KlCfe$<{jZ6)(4NGji;HOoEMSxQrAx{(kvx0xu3G~mZh!FJRC3{IM&d$HRR=P?_yeoW>^*_eT@qH z@h{)I#&y1cShnBt$UUB61bZHuk@};nJM4Y)$H)xqJ!W-9e%yS9nPzwvcwV$+(2UbN zaUzk14+`-FT6>{=dT|%h{*v(^w}}aAE>+9tQm>nW3CtsHQl29xxgj3X-**vSRBag6 z{jjGfuyM&4s&ouIz>l#`=nGsPx)=UB;f1}H_Z6vn(e>Tx)$8+;TVN5>yNNaT7UeS@=NbyYO7Zp32higYd6l*mez zsaTr?uXEI=RgQ&HRw-iXk)xHtV`|YIOAY9@#Z*Fg9)6!3(kGL*7Br8;&ph?mALsgAKQGk3pwlx$1IU&qHZ3;Tq8lEVeaks$6}7sMa8mPGoEzc^%2OD{%{W<6oJ=l+uWzJ|rz9=k_WK&FLs?&5}I8fBM3s z4Q2AvNw>YaL517~L%iVS%5#7Rb#$WELfIi(Ih5Y$_?$0t!&&YV=WF{Fb%?h#PI`0H zaJt{n7XDG>dfj_>(K{4k!(ZJ3*MbfE7X!gx4itJOSGMs_9C|(6RSLu_N}ZO?hY9Sn zx`z1#N_~5%Us^6zjkAm&q?@*zGB7!XEX~>3Di%6w!FF;i)r?p^kZ^@oV5vxbCM_&b zOxsABfVb>L_Bmf7|IC)&k;Bo^4wvWoi$0rNl6lX>+}yC8jV4a0yF^o#QKk%U)1+1o#l#I{#Y2I~t&;-jfQwOxrg2+u08TPS{G z^IzT zG;9akjtQ{Uggf!vxqpq}9iO$+%c}J@0{r1&5Hxyw{wUf)YJs}hEbEOScVS390$*cN z3Jyc(Ymw~CA|~+40@TZ_NXCSD)i86ru2yG8rh);QVjo{c-G-)`y(rdJNw!^a*Seer(j| z;2vC6xH@2ASd~PIT*6b$+<}Wj`9)Yfq74-%kN58n{e?<^7`r#n`Q9%Y7$3@kJx-S9l=rl*jK(xgEoi*T8b$0hY?Jj$tk z#kabm)8X9|{`X;15S3@}`YaZcxU}v``Ov|RM^h`xQ64>q;|o8OaA`Ae#=Z*^_r_ALO{zS*Qp{xUkA zwoCoh&WaF{Rf;N#snOnfyDe#bg~^3@xy$6P_M=U`99`Y5wT)Y#;7b=Vikf=p zAjoFv`R--<8nqTl7z4Cz?hA$|A+|DU^@V}hq3{O-V3APyGV!oOXT&fK7h`2LJrR^4 zCcn5y@N}z~+Pn87*_~B;JMlK`ew`N2aeDZq-}<=Do3~@K)*$&LJ;fKj>UMgg691}W zFE$|G;;BwwL~VA?hq*=o+4NV7;EoW`S!DQU4XDeX{LLc^M)0pj>DL z$!EqT@w^_-Yx9U97V0isftM2|y&|DvSE!jD%aTM~RLP}jyTr{a4Mhq=p&qW{E;w6sK$#9zDp?TAX5?ckS1p`hIG?BADh_%GoF4`$_s#<&SSUkHH+#Y46QsY>W)YI*v&Ip7%uw6D zaJpU0D@)`80$mVc5Pq19dglB!6`{165hL1w?fPC*}d`2!DsHOalr^e@1E z$q9LamHUa%eps=ff^*L7Cib;OF$HZtbF7;3E>vDla^%ygBYe8c;IbxC=BA`v_o1S} zKVv_S0IeWQWeRVvgdj5dp?$w*$rbVO)R9e<|2nVQMwp85{yjC}=us+d+9_UF{-Yi0 zLf;jnj9KU$k|T0yTTQym)amuS72a`@9=0s&qD=m*4HZ0tHdVyLX5@S(6jxR5NpzpM z=7o{qU5R56OAjm>fs<1020tD=aO(Sw@{17o&W#fE=wN43h+RC**^eN?kUW#8$`2*EBXd=zG=w|j$261?LQFp&znwvHqCxHukD!E#Zq1H zIZ1li4&VuNX?5_Ts;?>!gMaQ6YS3Y%;A%@?re1?R zzu4vFoPN5DS6uqor^fz_eN;s&7rP18f%(J~p?5y#C-X0Ru}Yo-$WSjRi_)596(0-7 zt<~QG%w2hML-Fw&5XZ*qt9SD|&=mR(gDX~-xi-X%Xt|oS+ydzS^sC`}h3~;jZO?P` z)(;f5!;9;XpRGy6!NoRmM~@<~h*-QFhpi!`Xpz%u>2lc@qf2?dmbwvh^Ay2D}bi{UXL7W+0bu#f`}uC$ORFC0U9NDvEAoyzffk}xR}Cq`-c z4lkIp@G$n*9@cL?s@@;Hu~iSyEkL&0taE}?ehc(VXkq=1A-`eLcN;7HzTu+$0uNZd z`tff2dZ=3<(s{n<5@ynSEwd2(@1>@|D~@I5(as&JL&czaD)rV;)_4uo9;+dKF*oVBXAltGC&nB(E|7`Vg_4GG$HzDSKsy7Gs+<-KKr-u_cif zl@d&Cd#vpPP2>wauh9JRH$4Tok&ofqQ9|6oyM$PMd4OUFnz{dQDeh6*-a`0>WZt0QoCFQh+~)x~FtnT#IQ!0m|A@Eu37Fc4s4Uq4r(4;3gj%!TRUTl#IM`f9w z%Dv&$*V3+qk@=vS%om%2sQrWLRn0;%K%8;s`9hJa?~9&bm8Zz+QzRjy&Fa(v7S|$D zMQ7z_vfBYL36wPMP&E`|N>XepRh&x`ys5Mq6-ZVUkI6q!J&C5`mzRcTU*-0j$n2)j zBNWZCD{%rZJI-Qgu1~xGNHl~jYsCv=T^y(6PwR5Pf5BP z+)#5)pjFh&hg|j%zYdRm^>fdM8OaS^|JO20RMuz8yl9X-wb(82>7Dmk?q+uG31-7v z&sG{%;$4&ooK^UbTC3Eucn+yZwDZ-Irp!nAN*iF1`HzP$b$p;J-_x)-27k9q-} ze1#Bm3xLyq!?)@U%5Q@=9=q|i*Du-e?d5wsyak>@Kye=?NYYTzyQz3D==zS#4RqHn z(5j*(MAQ^>P*llVByxU#Doo@zgN6U_W}DgRWAYFwzzwh60wk9a?T{45uSb)EfE}Nk zh>sBX&hF9Q0$5s(nC^%=o%M4Ub;L{m0;R+%=~ux!A`p-HH^YfsQ4_;^_d$S_Imw1A zCC4GTt;gO}mwp>Z-VvVdOWwcbTlNopDy#D1LEU`Z5lKz#EdD#|a5)6#(4bl`4Ntw%gauHt2jy10Z?@)cX^ z`eLR4!$lqCIPGwL{Z1A{h5j)ERr(Hw9vPM#`?RyW8o)f*bcyym3yj0BI%?o+$2OIh0 zTrZ%-Z$Y1Z9JZbgws}f0$2Z8cX;)u^!u{k?T#neII#k#~Sj@~WcIu1YSuX-(k0^Q0haV7&uSp81o*Cl%(^H-0eB*eQ zxg3g-%i1eq0QZw&14L9!!|*~>EZ!LUmy0!*yv5k@w&?Og!%-{-M2%@4i@d4f18MtY z>Ppum@mXd`8m-kDZN4Uctu(d|m1j_KU=?)}4qyyjur-k4x5XElEX8z9Xrrm61W1aI zIo?J@ydqDx+zvT-5-|t>PNiRCD zYYi0o8w}VJvXC&hg#$enj$j{%0?D8xfA|d@bjOCh=0MZehnduwopa$03Deb zcDWfi?>zgMZT;i&RXKCIc7IN}KoCjGvjewh< zyJ)g1cz*l%vhG1QCF}$j!`c^E=Or!-@1QJcd&!|%{1mfMOK1D6fNJjsp3)*~h#yzm z|1tVXN_BvnHQhw=z;pz?nZ*y48RMYxh_0oWSm7yN*}DZ4JU%oNXFQ82ZX#1*HQ76I z{eC%(bMtR(XG+&n?s|*wT_53XZmO5(5FXE!Uy=zR>5#C5e9&)OA%6u*SPX-)YEMZw z05xtrKKgFl{*%1HA1Z~Wcd$Y7cE51T%O{2+diK4G12u@P)nPmmI^gMHZG4liN**LO zfOu6~+gfO(`I54RgC*v8y{LVwQA#1NXXF)AvOig3TBR^qQ-)>lQ`a8q_wR*}ZG=g| ztj`FsgkgBAZP<+io7+MXCm36ll+3C!%^|?BmeIpsE9Sp?{2QwR;o`p)DS}?@Pj$wH z1!K*P%!)_#UOsaZm)5LA_)sHw2t&+W9!_Qmlh5|9QB(EL~e?@Z-(weDm*MM zMHUn5$G4TPti3GhO0%t5H4-U=V!KXiChbdupgc;Mo}ZhI+b8j^;jV;3>%`X-DC6@e zuPXQJGI~ay%o6|IU_sR0ioH|mi2!m`=>#ceo*Qt@#1 zgGlg0JPx3Gc}jGM@o>I0>Vl9z2Ps5N<*ca ziC$8A5-uq*t`%UG5RTUXvZqW(Br1Oep*d&)r7+mgffaq8kW_g%u4iRKjGk1sUXm)H z%cG#KVK{fmHmV)BC&nkq+`T|XB3Y>4A^lDxr$|N!SzG{h1RW43hc?_nxhkgsoJ>0 zstReE>cnwO6*G!xX%C$^8A|8nb3;xZ?F}!np--XAuFE`(ek!6rNHkohIU2-bp2rna zU8O_6OqG)mm2^BXq9&2MSA5c6a#Kc+m%5!yS`ERd>quY!8ukBoj(<+3z)qWHgr+pq zT+~v!JL_W-b!>*q)>7gk8O1476M7{YKiT$YL76@EGcD3u85E7FXHbG1N%;Qyjqp&h zcb)=scj3y-=@-HZ+%wl7H0N}`*T9#kLis}W*xWw^M~{Yo0%vF4SNsJk_oj=!#?91- zqiOyw%t{o2(>ijypvbE3)vz!D1E{Nqxq}5#zs|f5Ct5S=fs&)Dnpf8Ka_L!UdC69U zj#*Mmz^7BzLAj>{^!{6E;jIc&Yt>RaG3oedQ9|~hPvljzuN#RY1$JPHdqgO(1~x8W zl0GO)yeoq?>3c!zh_Kyw@p78~%3S&TbTR?`o37U%3CZ_hajV>3rouTopmx}5 zLd>0%CL{rd^hH)SQL_5hu|({)!}wB^={Y>2_sb+kwI@XP%Y_2X10!Zlj6xQzJgGl* zn=BM#&C8%tYp5(2hF@&CsdDsZO3UX~^sOoRQR?=E4D<`Rug3iX{{KN1?4Q2&b0PzF zdTcE-bxeCLowG*T?MJ3~)Q0^5&HlK03LfSp`3HlEtk?nd)b2Oq$t`YpZ`IY_aH~Vak2wW|t`%;@A70BTpN* zq_SeTOsq^|Q|wN&(z*-;oQcPo`RfaJ*D@)KacUF5= zg20|4mh>e<&z-Q!(}F$#2L!12y_qvaENQi$7C$*0b-s4wA+UNVj-K9SRB&zqk>P(Y zDRUH|Pl(wTtTUNbCn&p3+4i0oCQZKwn}qeUYTjXBBH@_g_^jDK-z8Epa%{>R>X;38z{%miLR;C)>x7sC zRq`nmS3CrwDN$r_Ryx)O)NxMo*MeTa{^obNr5|$azs<=1+R^^?u{{xkn50t=^O7(B zE#O|U9=BF#tf66Q?C~~K0*z~-vWlCZb|lrB^^559fK&LG^UDbBN6s(P@%U5{@|Ufn z^Hi4Ha6PHQvdX~KI!kJMZ|twD)iBXvzc6k|yIi@A>Pkt~z<#?W@3864- zMYqioUlWWZ=`Gpu?RETM+j4;(zWql4Zr@Z z|Ib~Hz87yeeg^=4g69kUf=L=kfw!_sEq&R+^3tz-rA;Q&^|&q5+0o`vv=I%yUC3Tm zIh=UIg87@OA=y^WF1$8i%nqufTV}qFgsYcnVS}(g=MMMM9rtVV#Y4p75@huzxqj+R zPvZEBIrmC4hrLpsGARwtP+mb+|9ec(rfAFnV+WSei?2(doQ@WJWvful*xu64B)<@* z68s zp*B_--8!->o@&nK67}~p&+4^-8)^%ITU1NJ+?y$wA7+@^7-9QRW%|hRgUf1?!idQ0 zFXMcB#7`Muso#IEvi?tNg0NmKP>%_}5fLI0*`E zW~pf>&7qCkJN#nt;pi=U+tsPc*8*FG{;m%j`RDOx7G0c*%u=};`BV~c(EEMH370Bq zFMv;TN|wA7;3k+@ugfm2)q2~R{E>5M(1t@&h23abNO5JdHBZ6Ub#0jaKaPxl9X0=) zDESw#;Ac+Wyd3SqWPS1utw}ff93Cnv#ZY#Rj&WM?5Y3FV3S*vyM1c+JmU6I27>`fb zU}#I!4Hmt>u`6j3wyZJ}t_DHKDk)!pVW_CtLHQvLDxgqp@!^Rh-8 zx_#7C)Q`z@3&Vzg`Yw|j1d2#F_=g2izh>q?d;H&& z>8aj%yaMVRFQYoa0QxZ zn&-_=R391KW2xm!{mj{%E%BJDAcAOS?v!<8Ncm)cVlo4*aIEh+Vsw2tly4vg*k8HM zV|;nR7Nbb6V|%a+TK|;8r6J^gYRI+e+Q3y(vgDb#uq}%>b^#c5|(OkDvKX1=!%h26{1fQt3PO$ zSPUJ(V3vZ4>d34jt67L$fUAR3#j=`eGhFAaD3gg&TPZEi2CR}Ivsr6Y>Ju#~PTe4MJPYY}BCN1L% z-e>2%(tK?(Uk6Qw3~FP;acvISpzZIiIGOyBP1AG!hNWRI-0U6cR+dk4eZ!)r_k>!* z0pAr&{Iv?`s%w{1)k7gmG^KsunSBV;>bYWii<{c?&1>OVMeb|%LQXBJoBK-cVjrg& zs%{kf-_JhTQkOZn-knZ*oWIK$UPEPe6MogWe5R5cvk=yGgP}!_bb0b^LsI(Je^4$@ z7Ow+Wj|!PDc~6mUUh#Dgo*KvTmF(Kgu0>tdc{{meUe|;}lAq=~-Ydl8W7aF)Zk!eS zhQ3{8^2R#5FvRU~MQkCYfGc*0jOTXvvZ}3#TS8}kc^JrHO5hgYOd>`xv-6ddAmXc^ z_NL-1Wp-;oR&O}<{=%E*6S(cUG~iNr9lWI~OI&lXRA3-Rb6P<@Ra(NA(!tu_ICoel z)jHZ0{!U-poB0MSC(8so&SJ&DI8yTY{m)%T>6gq*t)<0GrPjkYFL^8cQ{pqqj;*rP|EM@xgafeWj2g;6f0SQ5+D)DdFl?S!Z{~W z^3YPBy*CI+(qhfn|HjMwb#V8e9RHMT%r8dy?=v@q?J=-OiV4;GV|mo#Q7EXj`bn^U z=H!{5unEofzzp-Fh=+RFsluX_1aoePH7E4AknP#-g10~;cf(cGQ^@w&*3H4QUctSc z%sg)k6MIkah(tK`wk?#W4#QTimdylW5g81**qkE{3+c-;S9lK!?k05!1P0F(7@{!!~ z0xtPl4Wa`sjYTDbMtT^E&&sSud9Q|1pC6YnAJUY&V5v`+m~4` z??XOau*Q=LDX&Y?xe-hM_xgN~`wQKFxcl|k*%x2Fic((h4b*RQ2viIDVV{E0nYRQd zaS%&4*_TI8l#4aaQ=N9lQtd+$jETxJ|0y&MH*Taja~(V9I_D8-rS)SHw9@X!4m^RZ^lH!X&^g2 zS=W+4#?%lkTf9~Uzeq7kxS<2(N0*#COTsiUYgTdpNzI$WwO0T;TnXA29zo+e!0{XQ z_yfoyH|*$6i#<+9pWz^4iw8gNsut^Og9W8$gUUaVE3&|YxGNg3dTF4e|Iz8t@Esz{ zU08U-=eQauA!w-GyHfiqU>a`Hv_#?SKsps82xL!XpjyA~>y$ zJ~QRG*5*y@xUhojgay@M{YcvWY|Tgv7D87pv0X5*dlc#|IjU^Ebv`};i$r7Bv@iM& z7gq%81!-ysMiBC&ic1ZQ>Gp78_iA`?Fk}Zk$I*Z>E$-Fuh(FW_x%$4 z{2kDcot~Z?Q|oqiq-wQB#3hReM@%t@i>nI9Qr(Ohtv31av7~Nr&YtDu^u8tBJCfJ> z9q9Fn1m)~Y^zY%JcNcb1QJyrbvI))CBA16m4{(3Hn~%)^*$N*-5!TFFl(m5!SK!Y! z!klqZLbtXg?eB1P*A9M5$d$s+5q99=USV-j2DZB1n6|o*?dttd!pzV{J4I+33?V|V zwbcX_=Q&LK)EO;TYr?vnOh-qH!c3Vr2gM40eQYyC-A@Un{$N$d#mjnCnn<)xYqk1j zs@gS4s;)MwQgYE&br@X8zcgoK8khXafd^)AJL+71Dkt7Td6)F#3n(Y}i~tbGcFt`S zR0QuZi|bOjsxWel@-<8$1)p4(!e<}d zw-NiFMbe>3hCYmYcr&6Us-0DC`;^GX+7@NFR0aDW<3p#WvoC1r zXgm-%MavjYQ`1wpD%{CAWj!CjfJ&`C>ckODka=;3R3A0Gv%kjm_LrI!n$~w7fcUr zrh;A41>H_MNhYln!$dh3CV~~j7Q!urQl6I^>(IN^&8%cj5p0m;P>g`Z20I<_9RK}Y zTgZ3PV8MspIGda^Ir9y81^!M*nd>Km1w6@U+**)O z>N4JinH0tEe15dRE7(5$LU_h4#2MRnNv(-q{eCq(_ey|xs)v0khA8fOq{$t#KdZO< ze|VnxxQ=92yMKqJE9>pU1WKcqFPf3@Lwq^I#Z%e*SB_vD>pWy{eYIQco*wwJx z-CzEMD>p=3VBPp}Y&P8S+Lv6uJ9dxw8#^Zsu8{=r|k H+wuPc&agn< literal 0 HcmV?d00001 diff --git a/notebooks/kalman_CUDA_demo/kalman.h++ b/notebooks/kalman_CUDA_demo/kalman.hpp similarity index 100% rename from notebooks/kalman_CUDA_demo/kalman.h++ rename to notebooks/kalman_CUDA_demo/kalman.hpp From dd1fe96a3877cb48fcfdc9cdceb14c4e637e303b Mon Sep 17 00:00:00 2001 From: maximusron Date: Fri, 15 Sep 2023 20:09:49 +0530 Subject: [PATCH 3/5] embed plot image inotebook --- notebooks/kalman_CUDA_demo/run_kf.ipynb | 330 ++++++-- notebooks/kalman_CUDA_demo/run_kf_plot.ipynb | 814 ------------------- 2 files changed, 253 insertions(+), 891 deletions(-) delete mode 100644 notebooks/kalman_CUDA_demo/run_kf_plot.ipynb diff --git a/notebooks/kalman_CUDA_demo/run_kf.ipynb b/notebooks/kalman_CUDA_demo/run_kf.ipynb index 1c3ade7e..5003f79f 100644 --- a/notebooks/kalman_CUDA_demo/run_kf.ipynb +++ b/notebooks/kalman_CUDA_demo/run_kf.ipynb @@ -396,12 +396,15 @@ "metadata": {}, "outputs": [], "source": [ - "int run_kf(int expansion_factor, bool verbose) {\n", + "std::vector> run_kf(int expansion_factor, bool verbose) {\n", " \n", " int n = 3; // Number of states\n", " int m = 1; // Number of measurements\n", "\n", - " double dt = 1.0 / 30; // Time step\n", + " double dt = 1.0 / 40; // Time step\n", + " \n", + " std::vector g_preds;\n", + " std::vector g_est;\n", " \n", " std::vector> A(n, std::vector(n));\n", " std::vector> C(m, std::vector(n));\n", @@ -426,19 +429,32 @@ " 2.81003612051, 2.88321849354, 2.69789264832, 2.4342229249, 2.23464791825,\n", " 2.30278776224, 2.02069770395, 1.94393985809, 1.82498398739, 1.52526230354,\n", " 1.86967808173, 1.18073207847, 1.10729605087, 0.916168349913, 0.678547664519,\n", - " 0.562381751596, 0.355468474885, -0.155607486619, -0.287198661013, -0.602973173813\n", + " 0.562381751596, 0.355468474885, -0.155607486619, -0.287198661013, -0.602973173813,\n", + " -0.873817307503, -1.144661441193, -1.415505574883, -1.686349708573, -1.957193842263, \n", + " -2.228037975953, -2.498882109643, -2.769726243333, -3.040570377023, -3.311414510713, \n", + " -3.582258644403, -3.853102778093, -4.123946911783, -4.394791045473, -4.665635179163, \n", + " -4.936479312853, -5.207323446543, -5.478167580233, -5.749011713923, -6.019855847613,\n", + " -6.290699981303, -6.561544114993, -6.832388248683, -7.103232382373, -7.374076516063,\n", + " -7.644920649753, -7.915764783443, -8.186608917133, -8.457453050823, -8.728297184513, \n", + " -8.999141318203, -9.269985451893, -9.540829585583, -9.811673719273, -10.082517852963,\n", + " -10.353361986653, -10.624206120343, -10.895050254033, -11.165894387723, -11.436738521413,\n", + " -11.707582655103, -11.978426788793, -12.249270922483, -12.520115056173\n", " };\n", " \n", - " std::vector expandedMeasurements = generateExpandedData(measurements, expansion_factor);\n", + " // std::vector expandedMeasurements = generateExpandedData(measurements, expansion_factor);\n", " \n", - " std::vector x0 = {expandedMeasurements[0], 0, -9.5};\n", + " std::vector x0 = {measurements[0], 0, 0};\n", " kf.init(0, x0);\n", "\n", " // Feed measurements into filter, output estimated states\n", " std::vector y(m);\n", " if(verbose) {\n", " std::cout << \"t = \" << 0 << \", \" << \"x_hat[0]: \";\n", - " for (auto& val : kf.state()) std::cout << val << \" \";\n", + " for (auto& val : kf.state()){\n", + " std::cout << val << \" \";\n", + " }\n", + " g_est.push_back(kf.state()[2]);\n", + " \n", " std::cout << std::endl;\n", " }\n", " \n", @@ -448,7 +464,10 @@ " kf.update(y);\n", " if(verbose) {\n", " std::cout << \"t = \" << (i + 1) * dt << \", y[\" << i << \"] = \" << y[0] << \", x_hat[\" << i << \"] = \";\n", - " for (auto& val : kf.state()) std::cout << val << \" \";\n", + " for (auto& val : kf.state()) {\n", + " std::cout << val << \" \";\n", + " }\n", + " g_preds.push_back(kf.state()[2]);\n", " std::cout << std::endl;\n", " }\n", " }\n", @@ -456,7 +475,14 @@ " std::cout<<\"Exec Success, Final kf states:\";\n", " for (auto& val : kf.state()) std::cout << val << \" \";\n", " std::cout << std::endl;\n", - " return 0;\n", + " \n", + " std::vector> g_res;\n", + " for (size_t i = 0; i < g_preds.size(); ++i) {\n", + " std::vector pair = {g_preds[i], g_est[i]};\n", + " g_res.push_back(pair);\n", + " }\n", + " \n", + " return g_res;\n", "}" ] }, @@ -480,12 +506,12 @@ "metadata": {}, "outputs": [], "source": [ - "int pyrun_sim(int exp_factor, bool verbose) {\n", + "std::vector> pyrun_sim(int exp_factor, bool verbose) {\n", " start = std::chrono::high_resolution_clock::now();\n", - " run_kf(exp_factor, verbose);\n", + " std::vector> g_res = run_kf(exp_factor, verbose);\n", " stop = std::chrono::high_resolution_clock::now();\n", " duration = std::chrono::duration_cast(stop - start);\n", - " return duration.count();\n", + " return g_res;\n", "}" ] }, @@ -499,108 +525,258 @@ "name": "stdout", "output_type": "stream", "text": [ - "t = 0, x_hat[0]: 0.859101 0 -9.5 \n", - "t = 0.0333333, y[0] = 1.04203, x_hat[0] = 0.985804 3.4334 -9.49513 \n", - "t = 0.0666667, y[1] = 1.10727, x_hat[1] = 1.10541 3.19429 -9.49502 \n", - "t = 0.1, y[2] = 1.29135, x_hat[2] = 1.26163 3.37837 -9.49365 \n", - "t = 0.133333, y[3] = 1.48485, x_hat[3] = 1.43296 3.50712 -9.49049 \n", - "t = 0.166667, y[4] = 1.72826, x_hat[4] = 1.63165 3.68915 -9.48214 \n", - "t = 0.2, y[5] = 1.74216, x_hat[5] = 1.74959 3.34742 -9.48303 \n", - "t = 0.233333, y[6] = 2.11672, x_hat[6] = 1.95339 3.43792 -9.45741 \n", - "t = 0.266667, y[7] = 2.14529, x_hat[7] = 2.09329 3.22131 -9.44701 \n", - "t = 0.3, y[8] = 2.1603, x_hat[8] = 2.18853 2.86377 -9.45401 \n", - "t = 0.333333, y[9] = 2.21269, x_hat[9] = 2.26411 2.48447 -9.46947 \n", - "t = 0.366667, y[10] = 2.57709, x_hat[10] = 2.40705 2.34997 -9.40879 \n", - "t = 0.4, y[11] = 2.66822, x_hat[11] = 2.53053 2.16534 -9.35147 \n", - "t = 0.433333, y[12] = 2.51642, x_hat[12] = 2.5824 1.79776 -9.38304 \n", - "t = 0.466667, y[13] = 2.76034, x_hat[13] = 2.66901 1.55664 -9.33352 \n", - "t = 0.5, y[14] = 2.88132, x_hat[14] = 2.756 1.33866 -9.25752 \n", - "t = 0.533333, y[15] = 2.88374, x_hat[15] = 2.81835 1.07703 -9.21368 \n", - "t = 0.566667, y[16] = 2.94485, x_hat[16] = 2.87319 0.820425 -9.1612 \n", - "t = 0.6, y[17] = 2.82867, x_hat[17] = 2.88572 0.475051 -9.20634 \n", - "t = 0.633333, y[18] = 3.00066, x_hat[18] = 2.92181 0.223687 -9.13968 \n", - "t = 0.666667, y[19] = 3.12921, x_hat[19] = 2.96996 0.0323539 -8.99735 \n", - "t = 0.7, y[20] = 2.85836, x_hat[20] = 2.94814 -0.332372 -9.08131 \n", - "t = 0.733333, y[21] = 2.83808, x_hat[21] = 2.91692 -0.692906 -9.15769 \n", - "t = 0.766667, y[22] = 2.68975, x_hat[22] = 2.85218 -1.11915 -9.31914 \n", - "t = 0.8, y[23] = 2.66533, x_hat[23] = 2.78425 -1.51958 -9.4393 \n", - "t = 0.833333, y[24] = 2.81613, x_hat[24] = 2.75057 -1.78418 -9.37253 \n", - "t = 0.866667, y[25] = 2.81004, x_hat[25] = 2.71563 -2.02402 -9.27641 \n", - "t = 0.9, y[26] = 2.88322, x_hat[26] = 2.69681 -2.18939 -9.08803 \n", - "t = 0.933333, y[27] = 2.69789, x_hat[27] = 2.63919 -2.44704 -9.02954 \n", - "t = 0.966667, y[28] = 2.43422, x_hat[28] = 2.53201 -2.82314 -9.12502 \n", - "t = 1, y[29] = 2.23465, x_hat[29] = 2.39572 -3.25009 -9.27835 \n", - "t = 1.03333, y[30] = 2.30279, x_hat[30] = 2.29058 -3.55016 -9.26707 \n", - "t = 1.06667, y[31] = 2.0207, x_hat[31] = 2.14091 -3.94842 -9.37442 \n", - "t = 1.1, y[32] = 1.94394, x_hat[32] = 1.99583 -4.29884 -9.41907 \n", - "t = 1.13333, y[33] = 1.82498, x_hat[33] = 1.84688 -4.62851 -9.43717 \n", - "t = 1.16667, y[34] = 1.52526, x_hat[34] = 1.65846 -5.03664 -9.54274 \n", - "t = 1.2, y[35] = 1.86968, x_hat[35] = 1.56745 -5.14721 -9.31353 \n", - "t = 1.23333, y[36] = 1.18073, x_hat[36] = 1.35254 -5.57283 -9.43803 \n", - "t = 1.26667, y[37] = 1.1073, x_hat[37] = 1.15488 -5.91855 -9.47095 \n", - "t = 1.3, y[38] = 0.916168, x_hat[38] = 0.949377 -6.2554 -9.49286 \n", - "t = 1.33333, y[39] = 0.678548, x_hat[39] = 0.728596 -6.60288 -9.52434 \n", - "t = 1.36667, y[40] = 0.562382, x_hat[40] = 0.51902 -6.89417 -9.49835 \n", - "t = 1.4, y[41] = 0.355468, x_hat[41] = 0.30204 -7.17939 -9.46784 \n", - "t = 1.43333, y[42] = -0.155607, x_hat[42] = 0.0208246 -7.59585 -9.56385 \n", - "t = 1.46667, y[43] = -0.287199, x_hat[43] = -0.242801 -7.93934 -9.58687 \n", - "t = 1.5, y[44] = -0.602973, x_hat[44] = -0.525461 -8.30085 -9.6252 \n", + "t = 0, x_hat[0]: 1.04203 0 0 \n", + "t = 0.025, y[0] = 1.04203, x_hat[0] = 1.04203 0 0 \n", + "t = 0.05, y[1] = 1.10727, x_hat[1] = 1.08709 0.898405 0.00110609 \n", + "t = 0.075, y[2] = 1.29135, x_hat[2] = 1.22062 2.38572 0.00356059 \n", + "t = 0.1, y[3] = 1.48485, x_hat[3] = 1.3876 3.46905 0.00712492 \n", + "t = 0.125, y[4] = 1.72826, x_hat[4] = 1.58997 4.40683 0.013793 \n", + "t = 0.15, y[5] = 1.74216, x_hat[5] = 1.71702 4.52169 0.0154301 \n", + "t = 0.175, y[6] = 2.11672, x_hat[6] = 1.93313 5.12414 0.031192 \n", + "t = 0.2, y[7] = 2.14529, x_hat[7] = 2.08865 5.26575 0.0374157 \n", + "t = 0.225, y[8] = 2.1603, x_hat[8] = 2.20235 5.18421 0.0316618 \n", + "t = 0.25, y[9] = 2.21269, x_hat[9] = 2.29893 5.04726 0.0173069 \n", + "t = 0.275, y[10] = 2.57709, x_hat[10] = 2.46442 5.19829 0.039685 \n", + "t = 0.3, y[11] = 2.66822, x_hat[11] = 2.61234 5.26324 0.0527055 \n", + "t = 0.325, y[12] = 2.51642, x_hat[12] = 2.69148 5.08935 0.00552738 \n", + "t = 0.35, y[13] = 2.76034, x_hat[13] = 2.80588 5.04887 -0.00849014 \n", + "t = 0.375, y[14] = 2.88132, x_hat[14] = 2.92141 5.01625 -0.0224249 \n", + "t = 0.4, y[15] = 2.88374, x_hat[15] = 3.0137 4.91887 -0.0729322 \n", + "t = 0.425, y[16] = 2.94485, x_hat[16] = 3.09895 4.80966 -0.139272 \n", + "t = 0.45, y[17] = 2.82867, x_hat[17] = 3.14447 4.59753 -0.288577 \n", + "t = 0.475, y[18] = 3.00066, x_hat[18] = 3.21103 4.45686 -0.396929 \n", + "t = 0.5, y[19] = 3.12921, x_hat[19] = 3.287 4.34976 -0.484796 \n", + "t = 0.525, y[20] = 2.85836, x_hat[20] = 3.29866 4.07181 -0.747944 \n", + "t = 0.55, y[21] = 2.83808, x_hat[21] = 3.30007 3.77743 -1.04221 \n", + "t = 0.575, y[22] = 2.68975, x_hat[22] = 3.26982 3.40694 -1.43333 \n", + "t = 0.6, y[23] = 2.66533, x_hat[23] = 3.23376 3.03344 -1.83638 \n", + "t = 0.625, y[24] = 2.81613, x_hat[24] = 3.2232 2.74466 -2.13797 \n", + "t = 0.65, y[25] = 2.81004, x_hat[25] = 3.20765 2.45227 -2.44388 \n", + "t = 0.675, y[26] = 2.88322, x_hat[26] = 3.20161 2.19814 -2.69673 \n", + "t = 0.7, y[27] = 2.69789, x_hat[27] = 3.15898 1.84844 -3.07251 \n", + "t = 0.725, y[28] = 2.43422, x_hat[28] = 3.07035 1.3784 -3.6016 \n", + "t = 0.75, y[29] = 2.23465, x_hat[29] = 2.95232 0.840664 -4.20755 \n", + "t = 0.775, y[30] = 2.30279, x_hat[30] = 2.85557 0.387859 -4.67894 \n", + "t = 0.8, y[31] = 2.0207, x_hat[31] = 2.7166 -0.169629 -5.27548 \n", + "t = 0.825, y[32] = 1.94394, x_hat[32] = 2.57681 -0.704099 -5.81837 \n", + "t = 0.85, y[33] = 1.82498, x_hat[33] = 2.42945 -1.23525 -6.3351 \n", + "t = 0.875, y[34] = 1.52526, x_hat[34] = 2.24401 -1.85275 -6.94502 \n", + "t = 0.9, y[35] = 1.86968, x_hat[35] = 2.13957 -2.19864 -7.17158 \n", + "t = 0.925, y[36] = 1.18073, x_hat[36] = 1.92439 -2.85128 -7.78709 \n", + "t = 0.95, y[37] = 1.1073, x_hat[37] = 1.72092 -3.43475 -8.28638 \n", + "t = 0.975, y[38] = 0.916168, x_hat[38] = 1.50771 -4.01434 -8.75833 \n", + "t = 1, y[39] = 0.678548, x_hat[39] = 1.2784 -4.60797 -9.22649 \n", + "t = 1.025, y[40] = 0.562382, x_hat[40] = 1.05708 -5.14471 -9.60338 \n", + "t = 1.05, y[41] = 0.355468, x_hat[41] = 0.827473 -5.67368 -9.95378 \n", + "t = 1.075, y[42] = -0.155607, x_hat[42] = 0.537759 -6.3418 -10.4545 \n", + "t = 1.1, y[43] = -0.287199, x_hat[43] = 0.262434 -6.93117 -10.8401 \n", + "t = 1.125, y[44] = -0.602973, x_hat[44] = -0.0317108 -7.53829 -11.2291 \n", + "t = 1.15, y[45] = -0.873817, x_hat[45] = -0.333878 -8.13198 -11.5854 \n", + "t = 1.175, y[46] = -1.14466, x_hat[46] = -0.64242 -8.70817 -11.9065 \n", + "t = 1.2, y[47] = -1.41551, x_hat[47] = -0.955917 -9.2638 -12.1908 \n", + "t = 1.225, y[48] = -1.68635, x_hat[48] = -1.27315 -9.79661 -12.4381 \n", + "t = 1.25, y[49] = -1.95719, x_hat[49] = -1.5931 -10.3051 -12.6488 \n", + "t = 1.275, y[50] = -2.22804, x_hat[50] = -1.91487 -10.7882 -12.8241 \n", + "t = 1.3, y[51] = -2.49888, x_hat[51] = -2.23773 -11.2455 -12.9653 \n", + "t = 1.325, y[52] = -2.76973, x_hat[52] = -2.56107 -11.6768 -13.0743 \n", + "t = 1.35, y[53] = -3.04057, x_hat[53] = -2.88438 -12.0825 -13.1531 \n", + "t = 1.375, y[54] = -3.31141, x_hat[54] = -3.20725 -12.4629 -13.2039 \n", + "t = 1.4, y[55] = -3.58226, x_hat[55] = -3.52932 -12.8188 -13.2289 \n", + "t = 1.425, y[56] = -3.8531, x_hat[56] = -3.85034 -13.1508 -13.2301 \n", + "t = 1.45, y[57] = -4.12395, x_hat[57] = -4.17008 -13.46 -13.2098 \n", + "t = 1.475, y[58] = -4.39479, x_hat[58] = -4.48837 -13.7472 -13.17 \n", + "t = 1.5, y[59] = -4.66564, x_hat[59] = -4.8051 -14.0134 -13.1126 \n", + "t = 1.525, y[60] = -4.93648, x_hat[60] = -5.12016 -14.2597 -13.0395 \n", + "t = 1.55, y[61] = -5.20732, x_hat[61] = -5.4335 -14.4872 -12.9525 \n", + "t = 1.575, y[62] = -5.47817, x_hat[62] = -5.74508 -14.6968 -12.853 \n", + "t = 1.6, y[63] = -5.74901, x_hat[63] = -6.05487 -14.8895 -12.7428 \n", + "t = 1.625, y[64] = -6.01986, x_hat[64] = -6.36288 -15.0664 -12.623 \n", + "t = 1.65, y[65] = -6.2907, x_hat[65] = -6.66912 -15.2283 -12.4951 \n", + "t = 1.675, y[66] = -6.56154, x_hat[66] = -6.97361 -15.3763 -12.3601 \n", + "t = 1.7, y[67] = -6.83239, x_hat[67] = -7.27637 -15.5111 -12.2191 \n", + "t = 1.725, y[68] = -7.10323, x_hat[68] = -7.57746 -15.6336 -12.0731 \n", + "t = 1.75, y[69] = -7.37408, x_hat[69] = -7.87691 -15.7446 -11.9229 \n", + "t = 1.775, y[70] = -7.64492, x_hat[70] = -8.17476 -15.8449 -11.7694 \n", + "t = 1.8, y[71] = -7.91576, x_hat[71] = -8.47108 -15.9351 -11.6132 \n", + "t = 1.825, y[72] = -8.18661, x_hat[72] = -8.7659 -16.0159 -11.4551 \n", + "t = 1.85, y[73] = -8.45745, x_hat[73] = -9.05929 -16.0881 -11.2954 \n", + "t = 1.875, y[74] = -8.7283, x_hat[74] = -9.3513 -16.152 -11.1349 \n", + "t = 1.9, y[75] = -8.99914, x_hat[75] = -9.64199 -16.2084 -10.9739 \n", + "t = 1.925, y[76] = -9.26999, x_hat[76] = -9.9314 -16.2578 -10.8128 \n", + "t = 1.95, y[77] = -9.54083, x_hat[77] = -10.2196 -16.3005 -10.652 \n", + "t = 1.975, y[78] = -9.81167, x_hat[78] = -10.5066 -16.3372 -10.4918 \n", + "t = 2, y[79] = -10.0825, x_hat[79] = -10.7925 -16.3682 -10.3325 \n", + "t = 2.025, y[80] = -10.3534, x_hat[80] = -11.0774 -16.3939 -10.1743 \n", + "t = 2.05, y[81] = -10.6242, x_hat[81] = -11.3612 -16.4148 -10.0175 \n", + "t = 2.075, y[82] = -10.8951, x_hat[82] = -11.6441 -16.4311 -9.86215 \n", + "t = 2.1, y[83] = -11.1659, x_hat[83] = -11.9261 -16.4432 -9.70853 \n", + "t = 2.125, y[84] = -11.4367, x_hat[84] = -12.2072 -16.4514 -9.55674 \n", + "t = 2.15, y[85] = -11.7076, x_hat[85] = -12.4874 -16.4559 -9.4069 \n", + "t = 2.175, y[86] = -11.9784, x_hat[86] = -12.7669 -16.4572 -9.25909 \n", + "t = 2.2, y[87] = -12.2493, x_hat[87] = -13.0456 -16.4553 -9.1134 \n", + "t = 2.225, y[88] = -12.5201, x_hat[88] = -13.3236 -16.4505 -8.96991 \n", "\n", - "Exec Success, Final kf states:-0.525461 -8.30085 -9.6252 \n" + "Exec Success, Final kf states:-13.3236 -16.4505 -8.96991 \n" ] } ], "source": [ - "pyrun_sim(1, true)" + "std::vector> g_res = pyrun_sim(1, true);" ] }, { "cell_type": "code", - "execution_count": null, - "id": "d0152901-3f6d-49e5-9138-126d1b072912", + "execution_count": 21, + "id": "5e8f3f24-b003-4416-bb16-eabc480e437e", + "metadata": {}, + "outputs": [], + "source": [ + "std::vector py_g_pred;\n", + "std::vector py_g_est;" + ] + }, + { + "cell_type": "code", + "execution_count": 22, + "id": "107f53c0-0f2d-47b9-a218-67c29e16b87b", + "metadata": {}, + "outputs": [], + "source": [ + "int k = g_res.size();" + ] + }, + { + "cell_type": "code", + "execution_count": 23, + "id": "63f5ba73-393b-4a0a-9c83-6829b56fc526", + "metadata": {}, + "outputs": [], + "source": [ + "std::vector ret_1d_vector(std::vector> res, int axis) {\n", + " std::vector ret;\n", + " for (int i = 0; i < res.size(); i++) {\n", + " ret.push_back(res[i][axis]);\n", + " }\n", + " return ret;\n", + "}\n", + " " + ] + }, + { + "cell_type": "code", + "execution_count": 24, + "id": "a6bc89d0-026f-4f52-b8c9-9a8fca6948f2", + "metadata": {}, + "outputs": [], + "source": [ + "py_g_pred = ret_1d_vector(g_res, 0);\n", + "py_g_est = ret_1d_vector(g_res, 1);" + ] + }, + { + "cell_type": "code", + "execution_count": 25, + "id": "1b22451b-0bd7-48d6-8590-ff59b56a8031", + "metadata": {}, + "outputs": [], + "source": [ + "void printMatrix(const std::vector& vec) {\n", + " for (size_t i = 0; i < vec.size(); i++) {\n", + " std::cout << vec[i] << \" \";\n", + " }\n", + " std::cout << std::endl;\n", + " }\n" + ] + }, + { + "cell_type": "code", + "execution_count": 26, + "id": "47fd1710-59a8-4d80-96de-04ef86e233fd", "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "\n", - "Exec Success, Final kf states:-0.459225 -7.80515 -8.70828 \n", - "\n", - "Exec Success, Final kf states:-0.482229 -7.97731 -9.02673 \n" + "0 0.00110609 0.00356059 0.00712492 0.013793 0.0154301 0.031192 0.0374157 0.0316618 0.0173069 0.039685 0.0527055 0.00552738 -0.00849014 -0.0224249 -0.0729322 -0.139272 -0.288577 -0.396929 -0.484796 -0.747944 -1.04221 -1.43333 -1.83638 -2.13797 -2.44388 -2.69673 -3.07251 -3.6016 -4.20755 -4.67894 -5.27548 -5.81837 -6.3351 -6.94502 -7.17158 -7.78709 -8.28638 -8.75833 -9.22649 -9.60338 -9.95378 -10.4545 -10.8401 -11.2291 -11.5854 -11.9065 -12.1908 -12.4381 -12.6488 -12.8241 -12.9653 -13.0743 -13.1531 -13.2039 -13.2289 -13.2301 -13.2098 -13.17 -13.1126 -13.0395 -12.9525 -12.853 -12.7428 -12.623 -12.4951 -12.3601 -12.2191 -12.0731 -11.9229 -11.7694 -11.6132 -11.4551 -11.2954 -11.1349 -10.9739 -10.8128 -10.652 -10.4918 -10.3325 -10.1743 -10.0175 -9.86215 -9.70853 -9.55674 -9.4069 -9.25909 -9.1134 -8.96991 \n" ] } ], "source": [ - "%%python\n", + "printMatrix(py_g_pred);" + ] + }, + { + "cell_type": "code", + "execution_count": 27, + "id": "d0152901-3f6d-49e5-9138-126d1b072912", + "metadata": {}, + "outputs": [], + "source": [ + "// %%python\n", "\n", - "expand_factor = [500000, 1000000, 10000000, 20000000, 40000000]\n", - "benchmarks = []\n", + "// expand_factor = [1, 2, 3, 4, 5]\n", + "// benchmarks = []\n", "\n", - "for i in expand_factor:\n", - " time_kf = cppyy.gbl.pyrun_sim(int(i), 0)\n", - " benchmarks.append(time_kf)\n" + "// for i in expand_factor:\n", + "// time_kf = cppyy.gbl.pyrun_sim(int(i), 0)\n", + "// benchmarks.append(time_kf)\n", + "// " ] }, { "cell_type": "code", - "execution_count": null, - "id": "aa8b6916-eb9e-449b-b11a-255609a94420", + "execution_count": 28, + "id": "2c0b5c86-7875-45d9-8113-00febd1be3d5", "metadata": {}, "outputs": [], "source": [ "%%python\n", "\n", - "print(benchmarks)" + "import matplotlib.pyplot as plt" ] }, { "cell_type": "code", - "execution_count": null, - "id": "2c0b5c86-7875-45d9-8113-00febd1be3d5", + "execution_count": 29, + "id": "e0d86e62-207f-4e14-80c1-2e04c8984fd6", "metadata": {}, "outputs": [], - "source": [] + "source": [ + "%%python\n", + "\n", + "true_val = 9.81\n", + "g_pred = list(cppyy.gbl.py_g_pred)\n", + "g_pred = list(-x for x in g_pred)\n", + "x = range(len(g_pred))\n", + "\n", + "\n", + "# Plot the constant green line\n", + "plt.axhline(y=true_val, color='green', linestyle='-')\n", + "\n", + "# Plot g_pred in orange\n", + "plt.plot(x, g_pred, color='orange', marker='o', label='KF Estimates')\n", + "plt.annotate(f'{true_val}', xy=(-0.5, true_val), color='green',\n", + " verticalalignment='center', horizontalalignment = 'left')\n", + "# Add labels and title\n", + "plt.xlabel('Index')\n", + "plt.ylabel('Acceleration (m/s₂)')\n", + "plt.title('True Value vs. g_pred')\n", + "plt.legend()\n", + "plt.savefig(\"kf_plot2.jpg\")\n", + " \n", + "plt.yscale('symlog')\n", + "# Show the plot\n", + "plt.show()\n" + ] + }, + { + "cell_type": "markdown", + "id": "7f478ffc-eafa-4c98-9fcc-cd3d18833987", + "metadata": {}, + "source": [ + "" + ] } ], "metadata": { diff --git a/notebooks/kalman_CUDA_demo/run_kf_plot.ipynb b/notebooks/kalman_CUDA_demo/run_kf_plot.ipynb deleted file mode 100644 index 7c2cd9a4..00000000 --- a/notebooks/kalman_CUDA_demo/run_kf_plot.ipynb +++ /dev/null @@ -1,814 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": 1, - "id": "afdccef0-6aaa-43d6-ac29-7ea950356e19", - "metadata": {}, - "outputs": [], - "source": [ - "#include \"kalman.hpp\"" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "6feb6a99-9580-4bf7-aa48-0a7d175494a4", - "metadata": {}, - "outputs": [], - "source": [ - "#include \n", - "#include \n", - "#include \n" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "ebcc8af4-32b2-418b-bfbe-9ffb4ff9b0ff", - "metadata": {}, - "outputs": [], - "source": [ - "// helper function: matrix addition\n", - "std::vector> matadd(const std::vector>& a, const std::vector>& b) {\n", - " int rows = a.size();\n", - " int cols = a[0].size();\n", - " \n", - " std::vector> result(rows, std::vector(cols));\n", - "\n", - " for (int i = 0; i < rows; i++) {\n", - " for (int j = 0; j < cols; j++) {\n", - " result[i][j] = a[i][j] + b[i][j];\n", - " }\n", - " }\n", - "\n", - " return result;\n", - "}" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "b70f7f32-b7c2-4a1c-96a0-aea42b15f2aa", - "metadata": {}, - "outputs": [], - "source": [ - "// helper function: matrix subtraction\n", - "std::vector> matsub(const std::vector>& a, const std::vector>& b) {\n", - " int rows = a.size();\n", - " int cols = a[0].size();\n", - " \n", - " std::vector> result(rows, std::vector(cols));\n", - "\n", - " for (int i = 0; i < rows; i++) {\n", - " for (int j = 0; j < cols; j++) {\n", - " result[i][j] = a[i][j] - b[i][j];\n", - " }\n", - " }\n", - "\n", - " return result;\n", - "}" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "86b4a452-f04c-45a4-b69b-d87bcb2e5954", - "metadata": {}, - "outputs": [], - "source": [ - "// helper function: matrix transpose\n", - "std::vector> mattranspose(const std::vector>& a) {\n", - " int rows = a.size();\n", - " int cols = a[0].size();\n", - " \n", - " std::vector> result(cols, std::vector(rows));\n", - "\n", - " for (int i = 0; i < rows; i++) {\n", - " for (int j = 0; j < cols; j++) {\n", - " result[j][i] = a[i][j];\n", - " }\n", - " }\n", - "\n", - " return result;\n", - "}\n" - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "bf413652-f71a-4b91-9e6e-5520812dc3cb", - "metadata": {}, - "outputs": [], - "source": [ - "// helper function: matrix inversion\n", - "std::vector> matinverse(const std::vector>& a) {\n", - " size_t n = a.size();\n", - " \n", - " if (n != a[0].size()) {\n", - " std::cout<<\" Shape of a : \"<> result(2, std::vector(2));\n", - " result[0][0] = a[1][1] / determinant;\n", - " result[0][1] = -a[0][1] / determinant;\n", - " result[1][0] = -a[1][0] / determinant;\n", - " result[1][1] = a[0][0] / determinant;\n", - "\n", - " return result;\n", - " }\n", - "\n", - " if (n == 3) {\n", - " double determinant = a[0][0]*(a[1][1]*a[2][2]-a[2][1]*a[1][2]) \n", - " - a[0][1]*(a[1][0]*a[2][2]-a[1][2]*a[2][0]) \n", - " + a[0][2]*(a[1][0]*a[2][1]-a[1][1]*a[2][0]);\n", - " \n", - " if (determinant == 0) {\n", - " throw std::runtime_error(\"Matrix is singular and cannot be inverted.\");\n", - " }\n", - "\n", - " std::vector> result(3, std::vector(3));\n", - "\n", - " result[0][0] = (a[1][1] * a[2][2] - a[2][1] * a[1][2]) / determinant;\n", - " result[0][1] = (a[0][2] * a[2][1] - a[0][1] * a[2][2]) / determinant;\n", - " result[0][2] = (a[0][1] * a[1][2] - a[0][2] * a[1][1]) / determinant;\n", - " result[1][0] = (a[1][2] * a[2][0] - a[1][0] * a[2][2]) / determinant;\n", - " result[1][1] = (a[0][0] * a[2][2] - a[0][2] * a[2][0]) / determinant;\n", - " result[1][2] = (a[1][0] * a[0][2] - a[0][0] * a[1][2]) / determinant;\n", - " result[2][0] = (a[1][0] * a[2][1] - a[2][0] * a[1][1]) / determinant;\n", - " result[2][1] = (a[2][0] * a[0][1] - a[0][0] * a[2][1]) / determinant;\n", - " result[2][2] = (a[0][0] * a[1][1] - a[1][0] * a[0][1]) / determinant;\n", - "\n", - " return result;\n", - " }\n", - "\n", - " throw std::runtime_error(\"Only 2x2 and 3x3 matrices supported for inversion in this function.\");\n", - "}\n", - "\n" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "8c4b2db6-0847-4c0f-a8f7-88339d8257d7", - "metadata": {}, - "outputs": [], - "source": [ - "// helper function: matrix-vector subtraction\n", - "std::vector> matvecsub(const std::vector>& mat, const std::vector& vec) {\n", - " std::vector> result(mat.size(), std::vector(vec.size()));\n", - "\n", - " for (size_t i = 0; i < mat.size(); i++) {\n", - " for (size_t j = 0; j < vec.size(); j++) {\n", - " result[i][j] = mat[i][j] - vec[j];\n", - " }\n", - " }\n", - "\n", - " return result;\n", - "}" - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "id": "0d925dce-377c-4d23-96d6-9c4fead297cb", - "metadata": {}, - "outputs": [], - "source": [ - "// helper function: matrix-vector multiplication\n", - "std::vector matvecmul(const std::vector>& a, const std::vector& b) {\n", - " int rows = a.size();\n", - " int cols = a[0].size();\n", - "\n", - " if (cols != b.size()) {\n", - " throw std::runtime_error(\"Matrix dimensions do not match for multiplication.\");\n", - " }\n", - "\n", - " std::vector result(rows, 0.0);\n", - "\n", - " for (int i = 0; i < rows; i++) {\n", - " for (int j = 0; j < cols; j++) {\n", - " result[i] += a[i][j] * b[j];\n", - " }\n", - " }\n", - "\n", - " return result;\n", - "}" - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "id": "69714460-9a3e-4966-9de7-5e4111fc40af", - "metadata": {}, - "outputs": [], - "source": [ - "// helper function: matrix-matrix multiplication\n", - "std::vector> matmul(const std::vector>& a, const std::vector>& b) {\n", - " int rowsA = a.size();\n", - " int colsA = a[0].size();\n", - " int rowsB = b.size();\n", - " int colsB = b[0].size();\n", - "\n", - " if (colsA != rowsB) {\n", - " throw std::runtime_error(\"Matrix dimensions do not match for multiplication.\");\n", - " }\n", - "\n", - " std::vector> result(rowsA, std::vector(colsB, 0));\n", - "\n", - " for (int i = 0; i < rowsA; i++) {\n", - " for (int j = 0; j < colsB; j++) {\n", - " for (int k = 0; k < colsA; k++) {\n", - " result[i][j] += a[i][k] * b[k][j];\n", - " }\n", - " }\n", - " }\n", - "\n", - " return result;\n", - "}" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "id": "7295e929-dcfc-43be-9b3b-82e9f9fbf9db", - "metadata": {}, - "outputs": [], - "source": [ - "// helper function: vector subtraction\n", - "std::vector vecsub(const std::vector& a, const std::vector& b) {\n", - " size_t len = a.size();\n", - " std::vector result(len);\n", - " for (size_t i = 0; i < len; i++) {\n", - " result[i] = a[i] - b[i];\n", - " }\n", - " return result;\n", - "}" - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "id": "f4c9238f-674a-4cf8-8128-840cc7f61e0f", - "metadata": {}, - "outputs": [], - "source": [ - "//set up class and constructor\n", - "KalmanFilter::KalmanFilter(\n", - " double dt,\n", - " const std::vector>& A,\n", - " const std::vector>& C,\n", - " const std::vector>& Q,\n", - " const std::vector>& R,\n", - " const std::vector>& P)\n", - " : A(A), C(C), Q(Q), R(R), P0(P),\n", - " m(C.size()), n(A.size()), dt(dt), initialized(false),\n", - " I(n, std::vector(n)), x_hat(n), x_hat_new(n)\n", - "{\n", - " for (int i = 0; i < n; i++) {\n", - " I[i][i] = 1.0;\n", - " }\n", - "}\n", - "\n", - "KalmanFilter::KalmanFilter() {}" - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "id": "bd953ec6-ceed-489c-8e33-8eb7ae6bb32f", - "metadata": {}, - "outputs": [], - "source": [ - "// init the kf states + measurements \n", - "void KalmanFilter::init(double t0, const std::vector& x0) {\n", - " x_hat = x0;\n", - " P = P0;\n", - " this->t0 = t0;\n", - " t = t0;\n", - " initialized = true;\n", - "}" - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "id": "2840f0f7-52de-4f47-a9dc-e257235df002", - "metadata": {}, - "outputs": [], - "source": [ - "void KalmanFilter::init() {\n", - " std::fill(x_hat.begin(), x_hat.end(), 0.0);\n", - " P = P0;\n", - " t0 = 0;\n", - " t = t0;\n", - " initialized = true;\n", - "}" - ] - }, - { - "cell_type": "code", - "execution_count": 14, - "id": "ea057f20-9097-4dd3-946e-ab7a383afe69", - "metadata": {}, - "outputs": [], - "source": [ - "\n", - "\n", - "void KalmanFilter::update(const std::vector& y) {\n", - " if (!initialized)\n", - " throw std::runtime_error(\"Filter is not initialized!\");\n", - "\n", - " x_hat_new = matvecmul(A, x_hat);\n", - "\n", - " P = matadd(matmul(matmul(A, P), mattranspose(A)), Q);\n", - "\n", - " std::vector> inv = matinverse(matadd(matmul(matmul(C, P), mattranspose(C)), R));\n", - "\n", - " K = matmul(matmul(P, mattranspose(C)), inv);\n", - " std::vector temp = matvecmul(C, x_hat_new);\n", - " std::vector difference = vecsub(y, temp);\n", - "\n", - " for (size_t i = 0; i < x_hat_new.size(); i++) {\n", - " x_hat_new[i] += matvecmul(K, difference)[i];\n", - " }\n", - "\n", - " P = matmul(matsub(I, matmul(K, C)), P);\n", - "\n", - " x_hat = x_hat_new;\n", - " t += dt;\n", - "}\n" - ] - }, - { - "cell_type": "code", - "execution_count": 15, - "id": "08d28163-a086-46a9-bf41-044f48530929", - "metadata": {}, - "outputs": [], - "source": [ - "void KalmanFilter::update(const std::vector& y, double dt, const std::vector>& A) {\n", - " this->A = A;\n", - " this->dt = dt;\n", - " update(y);\n", - "}\n" - ] - }, - { - "cell_type": "code", - "execution_count": 16, - "id": "80d35cbe-7baa-4bc2-a232-4b5631af59b2", - "metadata": {}, - "outputs": [], - "source": [ - "std::vector generateExpandedData(const std::vector& baseData, int repetitions) {\n", - " std::vector expandedData;\n", - " for (int r = 0; r < repetitions; r++) {\n", - " for (auto val : baseData) {\n", - " // Add slight random perturbation to the data.\n", - " double perturbedValue = val + ((double)rand() / RAND_MAX - 0.5);\n", - " expandedData.push_back(perturbedValue);\n", - " }\n", - " }\n", - " return expandedData;\n", - "}" - ] - }, - { - "cell_type": "code", - "execution_count": 17, - "id": "8938a288-fc2c-4104-82c6-added4466c8c", - "metadata": {}, - "outputs": [], - "source": [ - "std::vector> run_kf(int expansion_factor, bool verbose) {\n", - " \n", - " int n = 3; // Number of states\n", - " int m = 1; // Number of measurements\n", - "\n", - " double dt = 1.0 / 40; // Time step\n", - " \n", - " std::vector g_preds;\n", - " std::vector g_est;\n", - " \n", - " std::vector> A(n, std::vector(n));\n", - " std::vector> C(m, std::vector(n));\n", - " std::vector> Q(n, std::vector(n));\n", - " std::vector> R(m, std::vector(m));\n", - " std::vector> P(n, std::vector(n));\n", - " \n", - " A = {{1, dt, 0}, {0, 1, dt}, {0, 0, 1}};\n", - " C = {{1, 0, 0}};\n", - " Q = {{.05, .05, .0}, {.05, .05, .0}, {.0, .0, .0}};\n", - " R = {{5}};\n", - " P = {{.1, .1, .1}, {.1, 10000, 10}, {.1, 10, 100}};\n", - " \n", - " KalmanFilter kf(dt, A, C, Q, R, P);\n", - " \n", - " std::vector measurements = {\n", - " 1.04202710058, 1.10726790452, 1.2913511148, 1.48485250951, 1.72825901034,\n", - " 1.74216489744, 2.11672039768, 2.14529225112, 2.16029641405, 2.21269371128,\n", - " 2.57709350237, 2.6682215744, 2.51641839428, 2.76034056782, 2.88131780617,\n", - " 2.88373786518, 2.9448468727, 2.82866600131, 3.0006601946, 3.12920591669,\n", - " 2.858361783, 2.83808170354, 2.68975330958, 2.66533185589, 2.81613499531,\n", - " 2.81003612051, 2.88321849354, 2.69789264832, 2.4342229249, 2.23464791825,\n", - " 2.30278776224, 2.02069770395, 1.94393985809, 1.82498398739, 1.52526230354,\n", - " 1.86967808173, 1.18073207847, 1.10729605087, 0.916168349913, 0.678547664519,\n", - " 0.562381751596, 0.355468474885, -0.155607486619, -0.287198661013, -0.602973173813,\n", - " -0.873817307503, -1.144661441193, -1.415505574883, -1.686349708573, -1.957193842263, \n", - " -2.228037975953, -2.498882109643, -2.769726243333, -3.040570377023, -3.311414510713, \n", - " -3.582258644403, -3.853102778093, -4.123946911783, -4.394791045473, -4.665635179163, \n", - " -4.936479312853, -5.207323446543, -5.478167580233, -5.749011713923, -6.019855847613,\n", - " -6.290699981303, -6.561544114993, -6.832388248683, -7.103232382373, -7.374076516063,\n", - " -7.644920649753, -7.915764783443, -8.186608917133, -8.457453050823, -8.728297184513, \n", - " -8.999141318203, -9.269985451893, -9.540829585583, -9.811673719273, -10.082517852963,\n", - " -10.353361986653, -10.624206120343, -10.895050254033, -11.165894387723, -11.436738521413,\n", - " -11.707582655103, -11.978426788793, -12.249270922483, -12.520115056173\n", - " };\n", - " \n", - " // std::vector expandedMeasurements = generateExpandedData(measurements, expansion_factor);\n", - " \n", - " std::vector x0 = {measurements[0], 0, 0};\n", - " kf.init(0, x0);\n", - "\n", - " // Feed measurements into filter, output estimated states\n", - " std::vector y(m);\n", - " if(verbose) {\n", - " std::cout << \"t = \" << 0 << \", \" << \"x_hat[0]: \";\n", - " for (auto& val : kf.state()){\n", - " std::cout << val << \" \";\n", - " }\n", - " g_est.push_back(kf.state()[2]);\n", - " \n", - " std::cout << std::endl;\n", - " }\n", - " \n", - " int i;\n", - " for (i = 0; i < measurements.size(); i++) {\n", - " y[0] = measurements[i];\n", - " kf.update(y);\n", - " if(verbose) {\n", - " std::cout << \"t = \" << (i + 1) * dt << \", y[\" << i << \"] = \" << y[0] << \", x_hat[\" << i << \"] = \";\n", - " for (auto& val : kf.state()) {\n", - " std::cout << val << \" \";\n", - " }\n", - " g_preds.push_back(kf.state()[2]);\n", - " std::cout << std::endl;\n", - " }\n", - " }\n", - " std::cout << std::endl;\n", - " std::cout<<\"Exec Success, Final kf states:\";\n", - " for (auto& val : kf.state()) std::cout << val << \" \";\n", - " std::cout << std::endl;\n", - " \n", - " std::vector> g_res;\n", - " for (size_t i = 0; i < g_preds.size(); ++i) {\n", - " std::vector pair = {g_preds[i], g_est[i]};\n", - " g_res.push_back(pair);\n", - " }\n", - " \n", - " return g_res;\n", - "}" - ] - }, - { - "cell_type": "code", - "execution_count": 18, - "id": "1e77f6cc-410a-4cd4-acbc-94677b77c968", - "metadata": {}, - "outputs": [], - "source": [ - "#include \n", - "auto start = std::chrono::high_resolution_clock::now();\n", - "auto stop = std::chrono::high_resolution_clock::now();\n", - "auto duration = std::chrono::duration_cast(stop - start);" - ] - }, - { - "cell_type": "code", - "execution_count": 19, - "id": "7ea068c2-fc1f-4ad4-a2c9-aecd66cbbf71", - "metadata": {}, - "outputs": [], - "source": [ - "std::vector> pyrun_sim(int exp_factor, bool verbose) {\n", - " start = std::chrono::high_resolution_clock::now();\n", - " std::vector> g_res = run_kf(exp_factor, verbose);\n", - " stop = std::chrono::high_resolution_clock::now();\n", - " duration = std::chrono::duration_cast(stop - start);\n", - " return g_res;\n", - "}" - ] - }, - { - "cell_type": "code", - "execution_count": 20, - "id": "6026ea4e-c42b-42e5-97a7-12af37e121a9", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "t = 0, x_hat[0]: 1.04203 0 0 \n", - "t = 0.025, y[0] = 1.04203, x_hat[0] = 1.04203 0 0 \n", - "t = 0.05, y[1] = 1.10727, x_hat[1] = 1.08709 0.898405 0.00110609 \n", - "t = 0.075, y[2] = 1.29135, x_hat[2] = 1.22062 2.38572 0.00356059 \n", - "t = 0.1, y[3] = 1.48485, x_hat[3] = 1.3876 3.46905 0.00712492 \n", - "t = 0.125, y[4] = 1.72826, x_hat[4] = 1.58997 4.40683 0.013793 \n", - "t = 0.15, y[5] = 1.74216, x_hat[5] = 1.71702 4.52169 0.0154301 \n", - "t = 0.175, y[6] = 2.11672, x_hat[6] = 1.93313 5.12414 0.031192 \n", - "t = 0.2, y[7] = 2.14529, x_hat[7] = 2.08865 5.26575 0.0374157 \n", - "t = 0.225, y[8] = 2.1603, x_hat[8] = 2.20235 5.18421 0.0316618 \n", - "t = 0.25, y[9] = 2.21269, x_hat[9] = 2.29893 5.04726 0.0173069 \n", - "t = 0.275, y[10] = 2.57709, x_hat[10] = 2.46442 5.19829 0.039685 \n", - "t = 0.3, y[11] = 2.66822, x_hat[11] = 2.61234 5.26324 0.0527055 \n", - "t = 0.325, y[12] = 2.51642, x_hat[12] = 2.69148 5.08935 0.00552738 \n", - "t = 0.35, y[13] = 2.76034, x_hat[13] = 2.80588 5.04887 -0.00849014 \n", - "t = 0.375, y[14] = 2.88132, x_hat[14] = 2.92141 5.01625 -0.0224249 \n", - "t = 0.4, y[15] = 2.88374, x_hat[15] = 3.0137 4.91887 -0.0729322 \n", - "t = 0.425, y[16] = 2.94485, x_hat[16] = 3.09895 4.80966 -0.139272 \n", - "t = 0.45, y[17] = 2.82867, x_hat[17] = 3.14447 4.59753 -0.288577 \n", - "t = 0.475, y[18] = 3.00066, x_hat[18] = 3.21103 4.45686 -0.396929 \n", - "t = 0.5, y[19] = 3.12921, x_hat[19] = 3.287 4.34976 -0.484796 \n", - "t = 0.525, y[20] = 2.85836, x_hat[20] = 3.29866 4.07181 -0.747944 \n", - "t = 0.55, y[21] = 2.83808, x_hat[21] = 3.30007 3.77743 -1.04221 \n", - "t = 0.575, y[22] = 2.68975, x_hat[22] = 3.26982 3.40694 -1.43333 \n", - "t = 0.6, y[23] = 2.66533, x_hat[23] = 3.23376 3.03344 -1.83638 \n", - "t = 0.625, y[24] = 2.81613, x_hat[24] = 3.2232 2.74466 -2.13797 \n", - "t = 0.65, y[25] = 2.81004, x_hat[25] = 3.20765 2.45227 -2.44388 \n", - "t = 0.675, y[26] = 2.88322, x_hat[26] = 3.20161 2.19814 -2.69673 \n", - "t = 0.7, y[27] = 2.69789, x_hat[27] = 3.15898 1.84844 -3.07251 \n", - "t = 0.725, y[28] = 2.43422, x_hat[28] = 3.07035 1.3784 -3.6016 \n", - "t = 0.75, y[29] = 2.23465, x_hat[29] = 2.95232 0.840664 -4.20755 \n", - "t = 0.775, y[30] = 2.30279, x_hat[30] = 2.85557 0.387859 -4.67894 \n", - "t = 0.8, y[31] = 2.0207, x_hat[31] = 2.7166 -0.169629 -5.27548 \n", - "t = 0.825, y[32] = 1.94394, x_hat[32] = 2.57681 -0.704099 -5.81837 \n", - "t = 0.85, y[33] = 1.82498, x_hat[33] = 2.42945 -1.23525 -6.3351 \n", - "t = 0.875, y[34] = 1.52526, x_hat[34] = 2.24401 -1.85275 -6.94502 \n", - "t = 0.9, y[35] = 1.86968, x_hat[35] = 2.13957 -2.19864 -7.17158 \n", - "t = 0.925, y[36] = 1.18073, x_hat[36] = 1.92439 -2.85128 -7.78709 \n", - "t = 0.95, y[37] = 1.1073, x_hat[37] = 1.72092 -3.43475 -8.28638 \n", - "t = 0.975, y[38] = 0.916168, x_hat[38] = 1.50771 -4.01434 -8.75833 \n", - "t = 1, y[39] = 0.678548, x_hat[39] = 1.2784 -4.60797 -9.22649 \n", - "t = 1.025, y[40] = 0.562382, x_hat[40] = 1.05708 -5.14471 -9.60338 \n", - "t = 1.05, y[41] = 0.355468, x_hat[41] = 0.827473 -5.67368 -9.95378 \n", - "t = 1.075, y[42] = -0.155607, x_hat[42] = 0.537759 -6.3418 -10.4545 \n", - "t = 1.1, y[43] = -0.287199, x_hat[43] = 0.262434 -6.93117 -10.8401 \n", - "t = 1.125, y[44] = -0.602973, x_hat[44] = -0.0317108 -7.53829 -11.2291 \n", - "t = 1.15, y[45] = -0.873817, x_hat[45] = -0.333878 -8.13198 -11.5854 \n", - "t = 1.175, y[46] = -1.14466, x_hat[46] = -0.64242 -8.70817 -11.9065 \n", - "t = 1.2, y[47] = -1.41551, x_hat[47] = -0.955917 -9.2638 -12.1908 \n", - "t = 1.225, y[48] = -1.68635, x_hat[48] = -1.27315 -9.79661 -12.4381 \n", - "t = 1.25, y[49] = -1.95719, x_hat[49] = -1.5931 -10.3051 -12.6488 \n", - "t = 1.275, y[50] = -2.22804, x_hat[50] = -1.91487 -10.7882 -12.8241 \n", - "t = 1.3, y[51] = -2.49888, x_hat[51] = -2.23773 -11.2455 -12.9653 \n", - "t = 1.325, y[52] = -2.76973, x_hat[52] = -2.56107 -11.6768 -13.0743 \n", - "t = 1.35, y[53] = -3.04057, x_hat[53] = -2.88438 -12.0825 -13.1531 \n", - "t = 1.375, y[54] = -3.31141, x_hat[54] = -3.20725 -12.4629 -13.2039 \n", - "t = 1.4, y[55] = -3.58226, x_hat[55] = -3.52932 -12.8188 -13.2289 \n", - "t = 1.425, y[56] = -3.8531, x_hat[56] = -3.85034 -13.1508 -13.2301 \n", - "t = 1.45, y[57] = -4.12395, x_hat[57] = -4.17008 -13.46 -13.2098 \n", - "t = 1.475, y[58] = -4.39479, x_hat[58] = -4.48837 -13.7472 -13.17 \n", - "t = 1.5, y[59] = -4.66564, x_hat[59] = -4.8051 -14.0134 -13.1126 \n", - "t = 1.525, y[60] = -4.93648, x_hat[60] = -5.12016 -14.2597 -13.0395 \n", - "t = 1.55, y[61] = -5.20732, x_hat[61] = -5.4335 -14.4872 -12.9525 \n", - "t = 1.575, y[62] = -5.47817, x_hat[62] = -5.74508 -14.6968 -12.853 \n", - "t = 1.6, y[63] = -5.74901, x_hat[63] = -6.05487 -14.8895 -12.7428 \n", - "t = 1.625, y[64] = -6.01986, x_hat[64] = -6.36288 -15.0664 -12.623 \n", - "t = 1.65, y[65] = -6.2907, x_hat[65] = -6.66912 -15.2283 -12.4951 \n", - "t = 1.675, y[66] = -6.56154, x_hat[66] = -6.97361 -15.3763 -12.3601 \n", - "t = 1.7, y[67] = -6.83239, x_hat[67] = -7.27637 -15.5111 -12.2191 \n", - "t = 1.725, y[68] = -7.10323, x_hat[68] = -7.57746 -15.6336 -12.0731 \n", - "t = 1.75, y[69] = -7.37408, x_hat[69] = -7.87691 -15.7446 -11.9229 \n", - "t = 1.775, y[70] = -7.64492, x_hat[70] = -8.17476 -15.8449 -11.7694 \n", - "t = 1.8, y[71] = -7.91576, x_hat[71] = -8.47108 -15.9351 -11.6132 \n", - "t = 1.825, y[72] = -8.18661, x_hat[72] = -8.7659 -16.0159 -11.4551 \n", - "t = 1.85, y[73] = -8.45745, x_hat[73] = -9.05929 -16.0881 -11.2954 \n", - "t = 1.875, y[74] = -8.7283, x_hat[74] = -9.3513 -16.152 -11.1349 \n", - "t = 1.9, y[75] = -8.99914, x_hat[75] = -9.64199 -16.2084 -10.9739 \n", - "t = 1.925, y[76] = -9.26999, x_hat[76] = -9.9314 -16.2578 -10.8128 \n", - "t = 1.95, y[77] = -9.54083, x_hat[77] = -10.2196 -16.3005 -10.652 \n", - "t = 1.975, y[78] = -9.81167, x_hat[78] = -10.5066 -16.3372 -10.4918 \n", - "t = 2, y[79] = -10.0825, x_hat[79] = -10.7925 -16.3682 -10.3325 \n", - "t = 2.025, y[80] = -10.3534, x_hat[80] = -11.0774 -16.3939 -10.1743 \n", - "t = 2.05, y[81] = -10.6242, x_hat[81] = -11.3612 -16.4148 -10.0175 \n", - "t = 2.075, y[82] = -10.8951, x_hat[82] = -11.6441 -16.4311 -9.86215 \n", - "t = 2.1, y[83] = -11.1659, x_hat[83] = -11.9261 -16.4432 -9.70853 \n", - "t = 2.125, y[84] = -11.4367, x_hat[84] = -12.2072 -16.4514 -9.55674 \n", - "t = 2.15, y[85] = -11.7076, x_hat[85] = -12.4874 -16.4559 -9.4069 \n", - "t = 2.175, y[86] = -11.9784, x_hat[86] = -12.7669 -16.4572 -9.25909 \n", - "t = 2.2, y[87] = -12.2493, x_hat[87] = -13.0456 -16.4553 -9.1134 \n", - "t = 2.225, y[88] = -12.5201, x_hat[88] = -13.3236 -16.4505 -8.96991 \n", - "\n", - "Exec Success, Final kf states:-13.3236 -16.4505 -8.96991 \n" - ] - } - ], - "source": [ - "std::vector> g_res = pyrun_sim(1, true);" - ] - }, - { - "cell_type": "code", - "execution_count": 21, - "id": "5e8f3f24-b003-4416-bb16-eabc480e437e", - "metadata": {}, - "outputs": [], - "source": [ - "std::vector py_g_pred;\n", - "std::vector py_g_est;" - ] - }, - { - "cell_type": "code", - "execution_count": 22, - "id": "107f53c0-0f2d-47b9-a218-67c29e16b87b", - "metadata": {}, - "outputs": [], - "source": [ - "int k = g_res.size();" - ] - }, - { - "cell_type": "code", - "execution_count": 23, - "id": "63f5ba73-393b-4a0a-9c83-6829b56fc526", - "metadata": {}, - "outputs": [], - "source": [ - "std::vector ret_1d_vector(std::vector> res, int axis) {\n", - " std::vector ret;\n", - " for (int i = 0; i < res.size(); i++) {\n", - " ret.push_back(res[i][axis]);\n", - " }\n", - " return ret;\n", - "}\n", - " " - ] - }, - { - "cell_type": "code", - "execution_count": 24, - "id": "a6bc89d0-026f-4f52-b8c9-9a8fca6948f2", - "metadata": {}, - "outputs": [], - "source": [ - "py_g_pred = ret_1d_vector(g_res, 0);\n", - "py_g_est = ret_1d_vector(g_res, 1);" - ] - }, - { - "cell_type": "code", - "execution_count": 25, - "id": "1b22451b-0bd7-48d6-8590-ff59b56a8031", - "metadata": {}, - "outputs": [], - "source": [ - "void printMatrix(const std::vector& vec) {\n", - " for (size_t i = 0; i < vec.size(); i++) {\n", - " std::cout << vec[i] << \" \";\n", - " }\n", - " std::cout << std::endl;\n", - " }\n" - ] - }, - { - "cell_type": "code", - "execution_count": 26, - "id": "47fd1710-59a8-4d80-96de-04ef86e233fd", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "0 0.00110609 0.00356059 0.00712492 0.013793 0.0154301 0.031192 0.0374157 0.0316618 0.0173069 0.039685 0.0527055 0.00552738 -0.00849014 -0.0224249 -0.0729322 -0.139272 -0.288577 -0.396929 -0.484796 -0.747944 -1.04221 -1.43333 -1.83638 -2.13797 -2.44388 -2.69673 -3.07251 -3.6016 -4.20755 -4.67894 -5.27548 -5.81837 -6.3351 -6.94502 -7.17158 -7.78709 -8.28638 -8.75833 -9.22649 -9.60338 -9.95378 -10.4545 -10.8401 -11.2291 -11.5854 -11.9065 -12.1908 -12.4381 -12.6488 -12.8241 -12.9653 -13.0743 -13.1531 -13.2039 -13.2289 -13.2301 -13.2098 -13.17 -13.1126 -13.0395 -12.9525 -12.853 -12.7428 -12.623 -12.4951 -12.3601 -12.2191 -12.0731 -11.9229 -11.7694 -11.6132 -11.4551 -11.2954 -11.1349 -10.9739 -10.8128 -10.652 -10.4918 -10.3325 -10.1743 -10.0175 -9.86215 -9.70853 -9.55674 -9.4069 -9.25909 -9.1134 -8.96991 \n" - ] - } - ], - "source": [ - "printMatrix(py_g_pred);" - ] - }, - { - "cell_type": "code", - "execution_count": 27, - "id": "d0152901-3f6d-49e5-9138-126d1b072912", - "metadata": {}, - "outputs": [], - "source": [ - "// %%python\n", - "\n", - "// expand_factor = [1, 2, 3, 4, 5]\n", - "// benchmarks = []\n", - "\n", - "// for i in expand_factor:\n", - "// time_kf = cppyy.gbl.pyrun_sim(int(i), 0)\n", - "// benchmarks.append(time_kf)\n", - "// " - ] - }, - { - "cell_type": "code", - "execution_count": 28, - "id": "2c0b5c86-7875-45d9-8113-00febd1be3d5", - "metadata": {}, - "outputs": [], - "source": [ - "%%python\n", - "\n", - "import matplotlib.pyplot as plt" - ] - }, - { - "cell_type": "code", - "execution_count": 29, - "id": "e0d86e62-207f-4e14-80c1-2e04c8984fd6", - "metadata": {}, - "outputs": [], - "source": [ - "%%python\n", - "\n", - "true_val = 9.81\n", - "g_pred = list(cppyy.gbl.py_g_pred)\n", - "g_pred = list(-x for x in g_pred)\n", - "x = range(len(g_pred))\n", - "\n", - "\n", - "# Plot the constant green line\n", - "plt.axhline(y=true_val, color='green', linestyle='-')\n", - "\n", - "# Plot g_pred in orange\n", - "plt.plot(x, g_pred, color='orange', marker='o', label='KF Estimates')\n", - "plt.annotate(f'{true_val}', xy=(-0.5, true_val), color='green',\n", - " verticalalignment='center', horizontalalignment = 'left')\n", - "# Add labels and title\n", - "plt.xlabel('Index')\n", - "plt.ylabel('Acceleration (m/s₂)')\n", - "plt.title('True Value vs. g_pred')\n", - "plt.legend()\n", - "plt.savefig(\"kf_plot2.jpg\")\n", - " \n", - "plt.yscale('symlog')\n", - "# Show the plot\n", - "plt.show()\n" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "c8d9db0e-a46b-4de0-af1d-0e6aec92abcc", - "metadata": {}, - "outputs": [], - "source": [] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "3ac6b7dd-7b1b-4fe8-8d9a-c8c3c29326b6", - "metadata": {}, - "outputs": [], - "source": [] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "34c14f7b-0a0a-43ff-9401-2637f6c7f3ea", - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "CUDA (C++17)", - "language": "CUDA", - "name": "cuda-xcpp17" - }, - "language_info": { - "codemirror_mode": "text/x-c++src", - "file_extension": ".cpp", - "mimetype": "text/x-c++src", - "name": "c++", - "version": "17" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} From 310188f5fec724c40c0527d9861f846a172dddf3 Mon Sep 17 00:00:00 2001 From: Aaron Jomy <75925957+maximusron@users.noreply.github.com> Date: Fri, 15 Sep 2023 20:15:01 +0530 Subject: [PATCH 4/5] update kf plot path --- notebooks/kalman_CUDA_demo/run_kf.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/notebooks/kalman_CUDA_demo/run_kf.ipynb b/notebooks/kalman_CUDA_demo/run_kf.ipynb index 5003f79f..c83b3ca6 100644 --- a/notebooks/kalman_CUDA_demo/run_kf.ipynb +++ b/notebooks/kalman_CUDA_demo/run_kf.ipynb @@ -775,7 +775,7 @@ "id": "7f478ffc-eafa-4c98-9fcc-cd3d18833987", "metadata": {}, "source": [ - "" + "" ] } ], From c2721e49b82d4bf91909196534dfc2fc6e6e914b Mon Sep 17 00:00:00 2001 From: maximusron Date: Sat, 16 Sep 2023 11:32:54 +0530 Subject: [PATCH 5/5] remove presaved kf plot --- notebooks/kalman_CUDA_demo/1D_kf_plot.jpg | Bin 21810 -> 0 bytes notebooks/kalman_CUDA_demo/run_kf.ipynb | 10 +++++++++- 2 files changed, 9 insertions(+), 1 deletion(-) delete mode 100644 notebooks/kalman_CUDA_demo/1D_kf_plot.jpg diff --git a/notebooks/kalman_CUDA_demo/1D_kf_plot.jpg b/notebooks/kalman_CUDA_demo/1D_kf_plot.jpg deleted file mode 100644 index 8013ad2a64cfe320405fa9e61acf9066da5dc79c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 21810 zcmeHv1yojB*Y3ub4haEiln|s-8U&=f8>G9trS+u*6p&775b2hbknZm8Zn&@dedj;N zbIy0ZaNPUkB(1H&(1Ht(gg*8 zzLWI}Wk1k`4x#G~G&Ber?kinTcbp*&gboe!kP#MxUk*;!_8tk74?LzoL~3y(0x7fn z9+sY6KO#06%RKr1SJJ*w_RkUK`)^V93t|67*93qJf`SMSgbwflrx%oIFA)E?{om&S zc*Cf?G|tgiZGv*Vg%QvefORSq#pQt&B|4_Xglb8CnPJwInGEj|v)v6>B5#2X)6!d@*va~u5?ony_sm>| z82XsU;FHlX*`Oo2SYj~F(0N`ltWA2DUk+IxTuHySbCITigLTkz(TuKKH73Ao??5MF zyZx%wK6zX{Ik+tOo)oW-VNS1gj+8OaH2yMhR9AAe>f|DH42`byv z!C31zb24SIt>I)*$W|kD(96$$tE;16Cv z*WlzhHgd@GH?ng(WM2su-U81Xbt%ScZ-KH6>=V`53gftWHV2M2`>>m|N-aVDL3vNM zChVm9ewx(-NA2S?J4chvco>RkfJF^cal1B;IkR+K(&7AQ5cYx$!=y{$%nb7?uFWgw z0Q?>#fB|X;NI^ABtL#PYCTs}4avJ1J`|JuGrn+c$9nC9K0e4Wu`_X-~7tVvd|6wWk zpyy=5Ic~(fygY0^9Y|x&jTE7owDqf)c)>FOUf@Pb*|@+?&!ue*eV>@oQ(RXTP`9Z` z!-O0}U8Q1T9plp`B(@N!b+65fa3B5QgLm;_wR$e&6y4~&O+p~P?LIhTAy=)Wplp|t zM)l|-lXg1CZYgVzcNr!Dtr-^B?6{+EQSgjOFNf>qR0g&Rk1eTCQ3*k^<>+FL7LPEV zbvBlBGO%xr&k?Hw+u12-!u;*53x_5XRwa9ftuqwEgttJ2((cgIyI7uv=$;(-)qn|1 z%iWrp7d&a;uD#JKa5dkxtKcn=g1A`gm^Hr9fPRIp>$QDinmz<-A;^wLKpzubg(C<% zYX#hBu@dDtQT@42;X)QB-QCg#C#4uxodif9X^vieM2KS8uh=PcNjtZzc z47K$39;FRxb3Zzdq%aUOY$ks9(KgIBEbs-7cJcAG^Cjvf{WX5?rBfF~S-OUsd%yrQh=qK4D z--+I;xUXK9^^PLpi#1bbNO;sC!Q9SvW!Mqa=8>EbfcaCJl!w3G-Mf@fLn^5Ap==r> z9*;B3Vu+YVoZvlUErCT6%RPz>Sc;bDf_kxp?I^lO` z!_S1vbS?-TdoK@-Z-F*SPrbQ#9(~Zg1!N?g%G8`I(6NzK za6c=-vG-!eS%qe;0ts5d2<)(!hcS}mL=s}u3#w6i`SYv#<&t`|GB3*-FFfzGk zq|Fl}DWj*=u58k&2GO`n{_Sv2FLA6s*PABqg_<=Sac(A`uJqmlA#%3>{7Bv{AXr#+ z3wT-F0+5MhHKES0YmAbaFM(AHU)+j!qyz#`o3b~w2?kg_Wu%6zw<6u^SieL^V(Ts! zw@E`au5qqr%KlCfe$<{jZ6)(4NGji;HOoEMSxQrAx{(kvx0xu3G~mZh!FJRC3{IM&d$HRR=P?_yeoW>^*_eT@qH z@h{)I#&y1cShnBt$UUB61bZHuk@};nJM4Y)$H)xqJ!W-9e%yS9nPzwvcwV$+(2UbN zaUzk14+`-FT6>{=dT|%h{*v(^w}}aAE>+9tQm>nW3CtsHQl29xxgj3X-**vSRBag6 z{jjGfuyM&4s&ouIz>l#`=nGsPx)=UB;f1}H_Z6vn(e>Tx)$8+;TVN5>yNNaT7UeS@=NbyYO7Zp32higYd6l*mez zsaTr?uXEI=RgQ&HRw-iXk)xHtV`|YIOAY9@#Z*Fg9)6!3(kGL*7Br8;&ph?mALsgAKQGk3pwlx$1IU&qHZ3;Tq8lEVeaks$6}7sMa8mPGoEzc^%2OD{%{W<6oJ=l+uWzJ|rz9=k_WK&FLs?&5}I8fBM3s z4Q2AvNw>YaL517~L%iVS%5#7Rb#$WELfIi(Ih5Y$_?$0t!&&YV=WF{Fb%?h#PI`0H zaJt{n7XDG>dfj_>(K{4k!(ZJ3*MbfE7X!gx4itJOSGMs_9C|(6RSLu_N}ZO?hY9Sn zx`z1#N_~5%Us^6zjkAm&q?@*zGB7!XEX~>3Di%6w!FF;i)r?p^kZ^@oV5vxbCM_&b zOxsABfVb>L_Bmf7|IC)&k;Bo^4wvWoi$0rNl6lX>+}yC8jV4a0yF^o#QKk%U)1+1o#l#I{#Y2I~t&;-jfQwOxrg2+u08TPS{G z^IzT zG;9akjtQ{Uggf!vxqpq}9iO$+%c}J@0{r1&5Hxyw{wUf)YJs}hEbEOScVS390$*cN z3Jyc(Ymw~CA|~+40@TZ_NXCSD)i86ru2yG8rh);QVjo{c-G-)`y(rdJNw!^a*Seer(j| z;2vC6xH@2ASd~PIT*6b$+<}Wj`9)Yfq74-%kN58n{e?<^7`r#n`Q9%Y7$3@kJx-S9l=rl*jK(xgEoi*T8b$0hY?Jj$tk z#kabm)8X9|{`X;15S3@}`YaZcxU}v``Ov|RM^h`xQ64>q;|o8OaA`Ae#=Z*^_r_ALO{zS*Qp{xUkA zwoCoh&WaF{Rf;N#snOnfyDe#bg~^3@xy$6P_M=U`99`Y5wT)Y#;7b=Vikf=p zAjoFv`R--<8nqTl7z4Cz?hA$|A+|DU^@V}hq3{O-V3APyGV!oOXT&fK7h`2LJrR^4 zCcn5y@N}z~+Pn87*_~B;JMlK`ew`N2aeDZq-}<=Do3~@K)*$&LJ;fKj>UMgg691}W zFE$|G;;BwwL~VA?hq*=o+4NV7;EoW`S!DQU4XDeX{LLc^M)0pj>DL z$!EqT@w^_-Yx9U97V0isftM2|y&|DvSE!jD%aTM~RLP}jyTr{a4Mhq=p&qW{E;w6sK$#9zDp?TAX5?ckS1p`hIG?BADh_%GoF4`$_s#<&SSUkHH+#Y46QsY>W)YI*v&Ip7%uw6D zaJpU0D@)`80$mVc5Pq19dglB!6`{165hL1w?fPC*}d`2!DsHOalr^e@1E z$q9LamHUa%eps=ff^*L7Cib;OF$HZtbF7;3E>vDla^%ygBYe8c;IbxC=BA`v_o1S} zKVv_S0IeWQWeRVvgdj5dp?$w*$rbVO)R9e<|2nVQMwp85{yjC}=us+d+9_UF{-Yi0 zLf;jnj9KU$k|T0yTTQym)amuS72a`@9=0s&qD=m*4HZ0tHdVyLX5@S(6jxR5NpzpM z=7o{qU5R56OAjm>fs<1020tD=aO(Sw@{17o&W#fE=wN43h+RC**^eN?kUW#8$`2*EBXd=zG=w|j$261?LQFp&znwvHqCxHukD!E#Zq1H zIZ1li4&VuNX?5_Ts;?>!gMaQ6YS3Y%;A%@?re1?R zzu4vFoPN5DS6uqor^fz_eN;s&7rP18f%(J~p?5y#C-X0Ru}Yo-$WSjRi_)596(0-7 zt<~QG%w2hML-Fw&5XZ*qt9SD|&=mR(gDX~-xi-X%Xt|oS+ydzS^sC`}h3~;jZO?P` z)(;f5!;9;XpRGy6!NoRmM~@<~h*-QFhpi!`Xpz%u>2lc@qf2?dmbwvh^Ay2D}bi{UXL7W+0bu#f`}uC$ORFC0U9NDvEAoyzffk}xR}Cq`-c z4lkIp@G$n*9@cL?s@@;Hu~iSyEkL&0taE}?ehc(VXkq=1A-`eLcN;7HzTu+$0uNZd z`tff2dZ=3<(s{n<5@ynSEwd2(@1>@|D~@I5(as&JL&czaD)rV;)_4uo9;+dKF*oVBXAltGC&nB(E|7`Vg_4GG$HzDSKsy7Gs+<-KKr-u_cif zl@d&Cd#vpPP2>wauh9JRH$4Tok&ofqQ9|6oyM$PMd4OUFnz{dQDeh6*-a`0>WZt0QoCFQh+~)x~FtnT#IQ!0m|A@Eu37Fc4s4Uq4r(4;3gj%!TRUTl#IM`f9w z%Dv&$*V3+qk@=vS%om%2sQrWLRn0;%K%8;s`9hJa?~9&bm8Zz+QzRjy&Fa(v7S|$D zMQ7z_vfBYL36wPMP&E`|N>XepRh&x`ys5Mq6-ZVUkI6q!J&C5`mzRcTU*-0j$n2)j zBNWZCD{%rZJI-Qgu1~xGNHl~jYsCv=T^y(6PwR5Pf5BP z+)#5)pjFh&hg|j%zYdRm^>fdM8OaS^|JO20RMuz8yl9X-wb(82>7Dmk?q+uG31-7v z&sG{%;$4&ooK^UbTC3Eucn+yZwDZ-Irp!nAN*iF1`HzP$b$p;J-_x)-27k9q-} ze1#Bm3xLyq!?)@U%5Q@=9=q|i*Du-e?d5wsyak>@Kye=?NYYTzyQz3D==zS#4RqHn z(5j*(MAQ^>P*llVByxU#Doo@zgN6U_W}DgRWAYFwzzwh60wk9a?T{45uSb)EfE}Nk zh>sBX&hF9Q0$5s(nC^%=o%M4Ub;L{m0;R+%=~ux!A`p-HH^YfsQ4_;^_d$S_Imw1A zCC4GTt;gO}mwp>Z-VvVdOWwcbTlNopDy#D1LEU`Z5lKz#EdD#|a5)6#(4bl`4Ntw%gauHt2jy10Z?@)cX^ z`eLR4!$lqCIPGwL{Z1A{h5j)ERr(Hw9vPM#`?RyW8o)f*bcyym3yj0BI%?o+$2OIh0 zTrZ%-Z$Y1Z9JZbgws}f0$2Z8cX;)u^!u{k?T#neII#k#~Sj@~WcIu1YSuX-(k0^Q0haV7&uSp81o*Cl%(^H-0eB*eQ zxg3g-%i1eq0QZw&14L9!!|*~>EZ!LUmy0!*yv5k@w&?Og!%-{-M2%@4i@d4f18MtY z>Ppum@mXd`8m-kDZN4Uctu(d|m1j_KU=?)}4qyyjur-k4x5XElEX8z9Xrrm61W1aI zIo?J@ydqDx+zvT-5-|t>PNiRCD zYYi0o8w}VJvXC&hg#$enj$j{%0?D8xfA|d@bjOCh=0MZehnduwopa$03Deb zcDWfi?>zgMZT;i&RXKCIc7IN}KoCjGvjewh< zyJ)g1cz*l%vhG1QCF}$j!`c^E=Or!-@1QJcd&!|%{1mfMOK1D6fNJjsp3)*~h#yzm z|1tVXN_BvnHQhw=z;pz?nZ*y48RMYxh_0oWSm7yN*}DZ4JU%oNXFQ82ZX#1*HQ76I z{eC%(bMtR(XG+&n?s|*wT_53XZmO5(5FXE!Uy=zR>5#C5e9&)OA%6u*SPX-)YEMZw z05xtrKKgFl{*%1HA1Z~Wcd$Y7cE51T%O{2+diK4G12u@P)nPmmI^gMHZG4liN**LO zfOu6~+gfO(`I54RgC*v8y{LVwQA#1NXXF)AvOig3TBR^qQ-)>lQ`a8q_wR*}ZG=g| ztj`FsgkgBAZP<+io7+MXCm36ll+3C!%^|?BmeIpsE9Sp?{2QwR;o`p)DS}?@Pj$wH z1!K*P%!)_#UOsaZm)5LA_)sHw2t&+W9!_Qmlh5|9QB(EL~e?@Z-(weDm*MM zMHUn5$G4TPti3GhO0%t5H4-U=V!KXiChbdupgc;Mo}ZhI+b8j^;jV;3>%`X-DC6@e zuPXQJGI~ay%o6|IU_sR0ioH|mi2!m`=>#ceo*Qt@#1 zgGlg0JPx3Gc}jGM@o>I0>Vl9z2Ps5N<*ca ziC$8A5-uq*t`%UG5RTUXvZqW(Br1Oep*d&)r7+mgffaq8kW_g%u4iRKjGk1sUXm)H z%cG#KVK{fmHmV)BC&nkq+`T|XB3Y>4A^lDxr$|N!SzG{h1RW43hc?_nxhkgsoJ>0 zstReE>cnwO6*G!xX%C$^8A|8nb3;xZ?F}!np--XAuFE`(ek!6rNHkohIU2-bp2rna zU8O_6OqG)mm2^BXq9&2MSA5c6a#Kc+m%5!yS`ERd>quY!8ukBoj(<+3z)qWHgr+pq zT+~v!JL_W-b!>*q)>7gk8O1476M7{YKiT$YL76@EGcD3u85E7FXHbG1N%;Qyjqp&h zcb)=scj3y-=@-HZ+%wl7H0N}`*T9#kLis}W*xWw^M~{Yo0%vF4SNsJk_oj=!#?91- zqiOyw%t{o2(>ijypvbE3)vz!D1E{Nqxq}5#zs|f5Ct5S=fs&)Dnpf8Ka_L!UdC69U zj#*Mmz^7BzLAj>{^!{6E;jIc&Yt>RaG3oedQ9|~hPvljzuN#RY1$JPHdqgO(1~x8W zl0GO)yeoq?>3c!zh_Kyw@p78~%3S&TbTR?`o37U%3CZ_hajV>3rouTopmx}5 zLd>0%CL{rd^hH)SQL_5hu|({)!}wB^={Y>2_sb+kwI@XP%Y_2X10!Zlj6xQzJgGl* zn=BM#&C8%tYp5(2hF@&CsdDsZO3UX~^sOoRQR?=E4D<`Rug3iX{{KN1?4Q2&b0PzF zdTcE-bxeCLowG*T?MJ3~)Q0^5&HlK03LfSp`3HlEtk?nd)b2Oq$t`YpZ`IY_aH~Vak2wW|t`%;@A70BTpN* zq_SeTOsq^|Q|wN&(z*-;oQcPo`RfaJ*D@)KacUF5= zg20|4mh>e<&z-Q!(}F$#2L!12y_qvaENQi$7C$*0b-s4wA+UNVj-K9SRB&zqk>P(Y zDRUH|Pl(wTtTUNbCn&p3+4i0oCQZKwn}qeUYTjXBBH@_g_^jDK-z8Epa%{>R>X;38z{%miLR;C)>x7sC zRq`nmS3CrwDN$r_Ryx)O)NxMo*MeTa{^obNr5|$azs<=1+R^^?u{{xkn50t=^O7(B zE#O|U9=BF#tf66Q?C~~K0*z~-vWlCZb|lrB^^559fK&LG^UDbBN6s(P@%U5{@|Ufn z^Hi4Ha6PHQvdX~KI!kJMZ|twD)iBXvzc6k|yIi@A>Pkt~z<#?W@3864- zMYqioUlWWZ=`Gpu?RETM+j4;(zWql4Zr@Z z|Ib~Hz87yeeg^=4g69kUf=L=kfw!_sEq&R+^3tz-rA;Q&^|&q5+0o`vv=I%yUC3Tm zIh=UIg87@OA=y^WF1$8i%nqufTV}qFgsYcnVS}(g=MMMM9rtVV#Y4p75@huzxqj+R zPvZEBIrmC4hrLpsGARwtP+mb+|9ec(rfAFnV+WSei?2(doQ@WJWvful*xu64B)<@* z68s zp*B_--8!->o@&nK67}~p&+4^-8)^%ITU1NJ+?y$wA7+@^7-9QRW%|hRgUf1?!idQ0 zFXMcB#7`Muso#IEvi?tNg0NmKP>%_}5fLI0*`E zW~pf>&7qCkJN#nt;pi=U+tsPc*8*FG{;m%j`RDOx7G0c*%u=};`BV~c(EEMH370Bq zFMv;TN|wA7;3k+@ugfm2)q2~R{E>5M(1t@&h23abNO5JdHBZ6Ub#0jaKaPxl9X0=) zDESw#;Ac+Wyd3SqWPS1utw}ff93Cnv#ZY#Rj&WM?5Y3FV3S*vyM1c+JmU6I27>`fb zU}#I!4Hmt>u`6j3wyZJ}t_DHKDk)!pVW_CtLHQvLDxgqp@!^Rh-8 zx_#7C)Q`z@3&Vzg`Yw|j1d2#F_=g2izh>q?d;H&& z>8aj%yaMVRFQYoa0QxZ zn&-_=R391KW2xm!{mj{%E%BJDAcAOS?v!<8Ncm)cVlo4*aIEh+Vsw2tly4vg*k8HM zV|;nR7Nbb6V|%a+TK|;8r6J^gYRI+e+Q3y(vgDb#uq}%>b^#c5|(OkDvKX1=!%h26{1fQt3PO$ zSPUJ(V3vZ4>d34jt67L$fUAR3#j=`eGhFAaD3gg&TPZEi2CR}Ivsr6Y>Ju#~PTe4MJPYY}BCN1L% z-e>2%(tK?(Uk6Qw3~FP;acvISpzZIiIGOyBP1AG!hNWRI-0U6cR+dk4eZ!)r_k>!* z0pAr&{Iv?`s%w{1)k7gmG^KsunSBV;>bYWii<{c?&1>OVMeb|%LQXBJoBK-cVjrg& zs%{kf-_JhTQkOZn-knZ*oWIK$UPEPe6MogWe5R5cvk=yGgP}!_bb0b^LsI(Je^4$@ z7Ow+Wj|!PDc~6mUUh#Dgo*KvTmF(Kgu0>tdc{{meUe|;}lAq=~-Ydl8W7aF)Zk!eS zhQ3{8^2R#5FvRU~MQkCYfGc*0jOTXvvZ}3#TS8}kc^JrHO5hgYOd>`xv-6ddAmXc^ z_NL-1Wp-;oR&O}<{=%E*6S(cUG~iNr9lWI~OI&lXRA3-Rb6P<@Ra(NA(!tu_ICoel z)jHZ0{!U-poB0MSC(8so&SJ&DI8yTY{m)%T>6gq*t)<0GrPjkYFL^8cQ{pqqj;*rP|EM@xgafeWj2g;6f0SQ5+D)DdFl?S!Z{~W z^3YPBy*CI+(qhfn|HjMwb#V8e9RHMT%r8dy?=v@q?J=-OiV4;GV|mo#Q7EXj`bn^U z=H!{5unEofzzp-Fh=+RFsluX_1aoePH7E4AknP#-g10~;cf(cGQ^@w&*3H4QUctSc z%sg)k6MIkah(tK`wk?#W4#QTimdylW5g81**qkE{3+c-;S9lK!?k05!1P0F(7@{!!~ z0xtPl4Wa`sjYTDbMtT^E&&sSud9Q|1pC6YnAJUY&V5v`+m~4` z??XOau*Q=LDX&Y?xe-hM_xgN~`wQKFxcl|k*%x2Fic((h4b*RQ2viIDVV{E0nYRQd zaS%&4*_TI8l#4aaQ=N9lQtd+$jETxJ|0y&MH*Taja~(V9I_D8-rS)SHw9@X!4m^RZ^lH!X&^g2 zS=W+4#?%lkTf9~Uzeq7kxS<2(N0*#COTsiUYgTdpNzI$WwO0T;TnXA29zo+e!0{XQ z_yfoyH|*$6i#<+9pWz^4iw8gNsut^Og9W8$gUUaVE3&|YxGNg3dTF4e|Iz8t@Esz{ zU08U-=eQauA!w-GyHfiqU>a`Hv_#?SKsps82xL!XpjyA~>y$ zJ~QRG*5*y@xUhojgay@M{YcvWY|Tgv7D87pv0X5*dlc#|IjU^Ebv`};i$r7Bv@iM& z7gq%81!-ysMiBC&ic1ZQ>Gp78_iA`?Fk}Zk$I*Z>E$-Fuh(FW_x%$4 z{2kDcot~Z?Q|oqiq-wQB#3hReM@%t@i>nI9Qr(Ohtv31av7~Nr&YtDu^u8tBJCfJ> z9q9Fn1m)~Y^zY%JcNcb1QJyrbvI))CBA16m4{(3Hn~%)^*$N*-5!TFFl(m5!SK!Y! z!klqZLbtXg?eB1P*A9M5$d$s+5q99=USV-j2DZB1n6|o*?dttd!pzV{J4I+33?V|V zwbcX_=Q&LK)EO;TYr?vnOh-qH!c3Vr2gM40eQYyC-A@Un{$N$d#mjnCnn<)xYqk1j zs@gS4s;)MwQgYE&br@X8zcgoK8khXafd^)AJL+71Dkt7Td6)F#3n(Y}i~tbGcFt`S zR0QuZi|bOjsxWel@-<8$1)p4(!e<}d zw-NiFMbe>3hCYmYcr&6Us-0DC`;^GX+7@NFR0aDW<3p#WvoC1r zXgm-%MavjYQ`1wpD%{CAWj!CjfJ&`C>ckODka=;3R3A0Gv%kjm_LrI!n$~w7fcUr zrh;A41>H_MNhYln!$dh3CV~~j7Q!urQl6I^>(IN^&8%cj5p0m;P>g`Z20I<_9RK}Y zTgZ3PV8MspIGda^Ir9y81^!M*nd>Km1w6@U+**)O z>N4JinH0tEe15dRE7(5$LU_h4#2MRnNv(-q{eCq(_ey|xs)v0khA8fOq{$t#KdZO< ze|VnxxQ=92yMKqJE9>pU1WKcqFPf3@Lwq^I#Z%e*SB_vD>pWy{eYIQco*wwJx z-CzEMD>p=3VBPp}Y&P8S+Lv6uJ9dxw8#^Zsu8{=r|k H+wuPc&agn< diff --git a/notebooks/kalman_CUDA_demo/run_kf.ipynb b/notebooks/kalman_CUDA_demo/run_kf.ipynb index 5003f79f..e1d98020 100644 --- a/notebooks/kalman_CUDA_demo/run_kf.ipynb +++ b/notebooks/kalman_CUDA_demo/run_kf.ipynb @@ -763,7 +763,7 @@ "plt.ylabel('Acceleration (m/s₂)')\n", "plt.title('True Value vs. g_pred')\n", "plt.legend()\n", - "plt.savefig(\"kf_plot2.jpg\")\n", + "plt.savefig(\"1D_kf_plot.jpg\")\n", " \n", "plt.yscale('symlog')\n", "# Show the plot\n", @@ -777,6 +777,14 @@ "source": [ "" ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "145a5f37-15ae-4336-b126-fba8b6760de2", + "metadata": {}, + "outputs": [], + "source": [] } ], "metadata": {