diff --git a/Makefile b/Makefile
index 9d482efec0516a41f36d9edd8acdfe69b2bfc794..58fa33dadce988c17325a20f4b84b252a09c764c 100644
--- a/Makefile
+++ b/Makefile
@@ -128,6 +128,7 @@ CSRC = $(ALLCSRC) \
$(SRCDIR)/benchmarks/blind_abstraction.c \
$(SRCDIR)/benchmarks/observer_abstraction.c \
$(SRCDIR)/benchmarks/luenberger_observer.c \
+ $(SRCDIR)/benchmarks/kalman_filter.c \
$(SRCDIR)/main.c \
$(SRCDIR)/random.c \
$(SRCDIR)/serial.c \
diff --git a/doc/references.bib b/doc/references.bib
index 62b5c0d4f21eb9849be06931c3f8d3983308a129..0fd73af8600983d0bde23ba4ff94a72e88be6e80 100644
--- a/doc/references.bib
+++ b/doc/references.bib
@@ -18,3 +18,12 @@
volume = {5},
doi = {10/gq5wdm},
}
+
+@inproceedings{MFI15-Noack,
+ author = {Noack, Benjamin and Baum, Marcus and Hanebeck, Uwe D.},
+ booktitle = {Proceedings of the IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI)},
+ title = {{State estimation for ellipsoidally constrained dynamic systems with set-membership pseudo measurements}},
+ year = {2015},
+ pages = {297--302},
+ doi = {10/gq5wfg},
+}
diff --git a/scripts/benchmarks/kalman_filter.py b/scripts/benchmarks/kalman_filter.py
new file mode 100644
index 0000000000000000000000000000000000000000..6f0d05bba5b1d5d32bb62ca0795425eb56b533ec
--- /dev/null
+++ b/scripts/benchmarks/kalman_filter.py
@@ -0,0 +1,123 @@
+## This file is part of the execution-time evaluation for the qronos observer abstractions.
+## Copyright (C) 2022-2023 Tim Rheinfels <tim.rheinfels@fau.de>
+## See https://gitlab.cs.fau.de/qronos-state-abstractions/execution-time
+##
+## This program is free software: you can redistribute it and/or modify
+## it under the terms of the GNU General Public License as published by
+## the Free Software Foundation, either version 3 of the License, or
+## (at your option) any later version.
+##
+## This program is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+## GNU General Public License for more details.
+##
+## You should have received a copy of the GNU General Public License
+## along with this program. If not, see <https://www.gnu.org/licenses/>.
+
+###
+### @file benchmarks/kalman_filter.py
+###
+### @brief Provides @p kalman_filter.KalmanFilter, the @p benchmark.Benchmark
+### subclass for the Kalman filter benchmark
+###
+### @author Tim Rheinfels <tim.rheinfels@fau.de>
+###
+
+import json
+import numpy as np
+import scipy as sp
+import scipy.linalg
+
+from benchmark import Benchmark
+from data import decode
+
+###
+### @brief Encapsulates the blind abstraction execution time benchmark
+###
+class KalmanFilter(Benchmark):
+
+ ###
+ ### @brief Validates the computations performed on the STM32 by running them
+ ### again in numpy and comparing the results.
+ ###
+ ### @see benchmark.Benchmark._validate for parameters
+ ###
+ def _validate(self, benchmark, key):
+ n_y = int(key)
+
+ for run in benchmark['tests']:
+ A = decode(run['setup']['A'])
+ C = decode(run['setup']['C'])
+ W = decode(run['setup']['W'])
+ V = decode(run['setup']['V'])
+ x = decode(run['setup']['x'])
+ X = decode(run['setup']['X'])
+ y = decode(run['setup']['y'])
+
+ if n_y > 0:
+ assert(A.shape == (n_y, n_y))
+ assert(C.shape == (n_y, n_y))
+ assert(W.shape == (n_y, n_y))
+ assert(np.allclose(W, W.T) and np.all(np.linalg.eigvalsh(W) > 0.0))
+ assert(X.shape == (n_y, n_y))
+ assert(np.allclose(X, X.T) and np.all(np.linalg.eigvalsh(X) > 0.0))
+ else:
+ assert(A.size == 0)
+ assert(C.size == 0)
+ assert(W.size == 0)
+ assert(X.size == 0)
+ A = np.zeros((0, 0))
+ C = np.zeros((0, 0))
+ W = np.zeros((0, 0))
+ X = np.zeros((0, 0))
+
+ assert(x.shape == (n_y,))
+ assert(y.shape == (n_y,))
+
+ for i in range(benchmark['repetition_count']):
+ # Predict
+ x = A @ x
+ X = A @ X @ A.T
+ delta = np.sqrt(np.trace(X) / np.trace(W))
+ X = (1.0 + 1.0 / delta) * X + (1.0 + delta) * W
+
+ # Compute gain
+ omega = 0.5
+ S = (1.0 / omega) * C @ X @ C.T + (1.0 / (1.0 - omega)) * V
+ K = (1.0 / omega) * X @ C.T
+ K = sp.linalg.solve(S.T, K.T, assume_a='pos').T
+ IKC = np.eye(K.shape[0]) - K @ C
+
+ # Update
+ x = IKC @ x + K @ y
+ X = (1.0 / omega) * IKC @ X @ IKC.T + (1.0 / (1.0 - omega)) * K @ V @ K.T
+
+ x_p = decode(run['teardown']['x_p'])
+ X_p = decode(run['teardown']['X_p'])
+
+ #
+ # Up until solving the linear system the values identical. The small
+ # floating point error introduced then amplifies over the iterations
+ # as inverses are numerically unstable.
+ # The relatively high value for atol practically disables the check
+ # for very small absolute values. We argue that since the absolute
+ # values of X are ranging roughly from 1e-4 to 3e2, there are plenty
+ # of values validated due to the small rtol of 1e-6.
+ #
+ # See warning and notes on
+ # https://numpy.org/doc/1.24/reference/generated/numpy.isclose.html#numpy.isclose
+ #
+ # TL;DR: The inversion introduces floating point inaccuracies which is
+ # why the comparision with standard values fails here
+ #
+ assert(np.allclose(x, x_p, atol=3e-4, rtol=1e-6))
+ assert(np.allclose(X, X_p, atol=3e-4, rtol=1e-6))
+
+ ###
+ ### @brief Constructor
+ ###
+ ### @see benchmark.Benchmark.__init__ for parameters
+ ###
+ def __init__(self, data):
+ Benchmark.__init__(self, data, 'Kalman Filter', r'kalman_filter_([0-9]+)', lambda m: m.group(1))
diff --git a/src/benchmarks.c b/src/benchmarks.c
index 47f43ef94484d7a9c5c58a603b6e64572370b6a9..8a065bd85a61a32bbaf0e6cbf686fdeba8c3510d 100644
--- a/src/benchmarks.c
+++ b/src/benchmarks.c
@@ -30,6 +30,7 @@
#include <benchmarks/blind_abstraction.h>
#include <benchmarks/observer_abstraction.h>
#include <benchmarks/luenberger_observer.h>
+#include <benchmarks/kalman_filter.h>
void benchmarks_run(void)
{
@@ -45,5 +46,6 @@ void benchmarks_run(void)
benchmarks_run_blind_abstraction();
benchmarks_run_observer_abstraction();
benchmarks_run_luenberger_observer();
+ benchmarks_run_kalman_filter();
}
diff --git a/src/benchmarks/kalman_filter.c b/src/benchmarks/kalman_filter.c
new file mode 100644
index 0000000000000000000000000000000000000000..4395362724dbea55deff9d5fcd74072ffe9d18bc
--- /dev/null
+++ b/src/benchmarks/kalman_filter.c
@@ -0,0 +1,253 @@
+// This file is part of the execution-time evaluation for the qronos observer abstractions.
+// Copyright (C) 2022-2023 Tim Rheinfels <tim.rheinfels@fau.de>
+// See https://gitlab.cs.fau.de/qronos-state-abstractions/execution-time
+//
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <https://www.gnu.org/licenses/>.
+
+///
+/// @file benchmarks/kalman_filter.c
+///
+/// @brief Provides the implementation of the Kalman filter execution time benchmark
+///
+/// @author Tim Rheinfels <tim.rheinfels@fau.de>
+///
+
+#include "kalman_filter.h"
+
+#include <stdlib.h>
+#include <string.h>
+
+#include <lapack.h>
+#include <linalg.h>
+#include <random.h>
+#include <serial.h>
+#include <benchmarks/campaign.h>
+
+static float *A; ///< System dynamics matrix
+static float *C; ///< System measurement matrix
+static float *W; ///< Process disturbance ellipsoid's shape matrix
+static float *V; ///< Measurement disturbance ellipsoid's shape matrix
+static float *S; ///< Intermediate matrix
+static float *K; ///< Intermediate matrix (Kalman gains)
+
+static float *x; ///< Ellipsoid's center vector
+static float *x_p; ///< Memory for transforming the ellipsoid's center vector
+static float *X; ///< Ellipsoid's shape matrix
+static float *X_p; ///< Memory for transforming the ellipsoid's shape matrix
+static float *y; ///< Measurement vector
+
+static float trace_W; ///< Trace of the process disturbance ellipsoid's shape matrix
+
+///
+/// @brief Configures the memory layout and randomly initializes the data
+///
+static void setup_matrices(size_t n_y)
+{
+ A = benchmarks_memory;
+ C = A + n_y*n_y;
+ W = C + n_y*n_y;
+ V = W + n_y*n_y;
+ S = V + n_y*n_y;
+ K = S + n_y*n_y;
+ x = K + n_y*n_y;
+ x_p = x + n_y;
+ X = x_p + n_y;
+ X_p = X + n_y*n_y;
+ y = X_p + n_y*n_y;
+
+ random_matrix(A, n_y, n_y);
+ random_matrix(C, n_y, n_y);
+ random_matrix_spd(W, n_y);
+ random_matrix_spd(V, n_y);
+ random_matrix(x, n_y, 1u);
+ random_matrix_spd(X, n_y);
+ random_matrix(y, n_y, 1u);
+ memset(S, 0x00u, n_y * n_y * sizeof(float));
+ memset(K, 0x00u, n_y * n_y * sizeof(float));
+ memset(x_p, 0x00u, n_y * sizeof(float));
+ memset(X_p, 0x00u, n_y * n_y * sizeof(float));
+
+ LINALG_TRACE(W, trace_W, n_y);
+
+ serial_print_matrix("A", A, n_y, n_y);
+ serial_print_matrix("C", C, n_y, n_y);
+ serial_print_matrix("W", W, n_y, n_y);
+ serial_print_matrix("V", V, n_y, n_y);
+ serial_print_vector("x", x, n_y);
+ serial_print_matrix("X", X, n_y, n_y);
+ serial_print_vector("y", y, n_y);
+}
+
+void benchmarks_run_kalman_filter(void)
+{
+
+ #define SETUP(N_Y) { \
+ setup_matrices(N_Y); \
+ }
+
+ #define TEARDOWN(N_Y) { \
+ serial_print_vector("x_p", x, N_Y); \
+ serial_print_matrix("X_p", X, N_Y, N_Y); \
+ }
+
+ #define CAMPAIGN(N_Y) { \
+ BENCHMARKS_CAMPAIGN("kalman_filter_" #N_Y, \
+ { \
+ /* === Prediction step === */ \
+ \
+ /* x_p <- A x */ \
+ LINALG_MV(A, x, x_p, 1.0f, 0.0f, N_Y, N_Y); \
+ \
+ /* X_p <- A X */ \
+ LINALG_MM(A, X, X_p, 1.0f, 0.0f, N_Y, N_Y, N_Y); \
+ \
+ /* X <- X_p A' */ \
+ LINALG_MM_T(X_p, A, X, 1.0f, 0.0f, N_Y, N_Y, N_Y); \
+ \
+ /* Compute delta optimally */ \
+ float trace_X; \
+ LINALG_TRACE(X, trace_X, N_Y); \
+ float delta = sqrtf(trace_X / trace_W); \
+ \
+ /* X <- (1 + 1/delta) X + (1 + delta) W */ \
+ LINALG_MA(X, W, X, 1.0f + 1.0f/delta, 1.0f + delta, 0.0f, N_Y, N_Y); \
+ \
+ /* === Compute Kalman Gain === */ \
+ \
+ /* Select omega arbitrarily but fixed (should actually be optimised for) */ \
+ float omega = 0.5f; \
+ \
+ /* Copy V to S */ \
+ memcpy(S, V, N_Y*N_Y*sizeof(float)); \
+ \
+ /* X_p <- (1/omega) * C X */ \
+ LINALG_MM(C, X, X_p, 1.0f / omega, 0.0f, N_Y, N_Y, N_Y); \
+ \
+ /* K <- X_p' (transpose since BLAS and LAPACK use column major order) */ \
+ for(size_t i = 0u; i < N_Y; ++i) { \
+ for(size_t j = 0u; j < N_Y; ++j) { \
+ K[i * N_Y + j] = X_p[j * N_Y + i]; \
+ } \
+ } \
+ \
+ /* S <- X_p C' + (1 / (1 - omega)) S */ \
+ LINALG_MM_T(X_p, C, S, 1.0f, 1.0f / (1.0f - omega), N_Y, N_Y, N_Y); \
+ \
+ /* Solve K <- S\K */ \
+ sposv('U', N_Y, N_Y, S, N_Y, K, N_Y); \
+ \
+ /* === Update step === */ \
+ \
+ /* x <- K y */ \
+ LINALG_MV(K, y, x, 1.0f, 0.0f, N_Y, N_Y); \
+ \
+ /* S <- V K */ \
+ LINALG_MM_T(V, K, S, 1.0f, 0.0f, N_Y, N_Y, N_Y); \
+ \
+ /* X_p <- K' S */ \
+ LINALG_MM(K, S, X_p, 1.0f, 0.0f, N_Y, N_Y, N_Y); \
+ \
+ /* Initialise S with identity */ \
+ for(size_t i = 0u; i < N_Y; ++i) { \
+ for(size_t j = 0; j < N_Y; ++j) { \
+ S[i * N_Y + j] = (i == j) ? 1.0f : 0.0f; \
+ } \
+ } \
+ \
+ /* S <- S - K C */ \
+ LINALG_MM(K, C, S, -1.0f, 1.0f, N_Y, N_Y, N_Y); \
+ \
+ /* K <- S X */ \
+ LINALG_MM(S, X, K, 1.0f, 0.0f, N_Y, N_Y, N_Y); \
+ \
+ /* X_p <- (1 / omega) K S' + (1 / (1-omega)) X_p */ \
+ LINALG_MM_T(K, S, X_p, 1.0f / omega, 1.0f / (1.0f - omega), N_Y, N_Y, N_Y); \
+ \
+ /* Swap X and X_p */ \
+ float *tmp = X; \
+ X = X_p; \
+ X_p = tmp; \
+ \
+ /* x <- S x_p + x */ \
+ LINALG_MV(S, x_p, x, 1.0f, 1.0f, N_Y, N_Y); \
+ }, \
+ SETUP(N_Y), \
+ TEARDOWN(N_Y), \
+ 10u, \
+ 10u \
+ ) \
+ }
+
+ CAMPAIGN(1);
+ CAMPAIGN(2);
+ CAMPAIGN(3);
+ CAMPAIGN(4);
+ CAMPAIGN(5);
+ CAMPAIGN(6);
+ CAMPAIGN(7);
+ CAMPAIGN(8);
+ CAMPAIGN(9);
+ CAMPAIGN(10);
+ CAMPAIGN(11);
+ CAMPAIGN(12);
+ CAMPAIGN(13);
+ CAMPAIGN(14);
+ CAMPAIGN(15);
+ CAMPAIGN(16);
+ CAMPAIGN(17);
+ CAMPAIGN(18);
+ CAMPAIGN(19);
+ CAMPAIGN(20);
+ CAMPAIGN(21);
+ CAMPAIGN(22);
+ CAMPAIGN(23);
+ CAMPAIGN(24);
+ CAMPAIGN(25);
+ CAMPAIGN(26);
+ CAMPAIGN(27);
+ CAMPAIGN(28);
+ CAMPAIGN(29);
+ CAMPAIGN(30);
+ CAMPAIGN(31);
+ CAMPAIGN(32);
+ CAMPAIGN(33);
+ CAMPAIGN(34);
+ CAMPAIGN(35);
+ CAMPAIGN(36);
+ CAMPAIGN(37);
+ CAMPAIGN(38);
+ CAMPAIGN(39);
+ CAMPAIGN(40);
+ CAMPAIGN(41);
+ CAMPAIGN(42);
+ CAMPAIGN(43);
+ CAMPAIGN(44);
+ CAMPAIGN(45);
+ CAMPAIGN(46);
+ CAMPAIGN(47);
+ CAMPAIGN(48);
+ CAMPAIGN(49);
+ CAMPAIGN(50);
+ CAMPAIGN(51);
+ CAMPAIGN(52);
+ CAMPAIGN(53);
+ CAMPAIGN(54);
+ CAMPAIGN(55);
+ CAMPAIGN(56);
+ CAMPAIGN(57);
+ CAMPAIGN(58);
+ CAMPAIGN(59);
+ CAMPAIGN(60);
+
+}
diff --git a/src/benchmarks/kalman_filter.h b/src/benchmarks/kalman_filter.h
new file mode 100644
index 0000000000000000000000000000000000000000..4a041e765bf2627395131b756aa1c1d60eda7a43
--- /dev/null
+++ b/src/benchmarks/kalman_filter.h
@@ -0,0 +1,57 @@
+// This file is part of the execution-time evaluation for the qronos observer abstractions.
+// Copyright (C) 2022-2023 Tim Rheinfels <tim.rheinfels@fau.de>
+// See https://gitlab.cs.fau.de/qronos-state-abstractions/execution-time
+//
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <https://www.gnu.org/licenses/>.
+
+///
+/// @file benchmarks/kalman_filter.h
+///
+/// @brief Provides the interface for the Kalman filter execution time benchmark
+///
+/// @author Tim Rheinfels <tim.rheinfels@fau.de>
+///
+
+#ifndef BENCHMARKS_KALMANFILTER_H
+#define BENCHMARKS_KALMANFILTER_H
+
+#include <benchmarks/memory.h>
+
+///
+/// @brief Memory required for running the Kalman filter execution time benchmark
+///
+#define BENCHMARKS_KALMAN_FILTER_WORK_SIZE (8u*N_Y_MAX*N_Y_MAX + 3u*N_Y_MAX)
+
+///
+/// @brief Runs the Kalman filter execution time benchmark
+///
+/// This function measures the execution time needed in order to update the
+/// interval Kalman filter proposed in @cite MFI15-Noack. We omit all
+/// probabilistic terms (the filter can handle both set-valued and probabilistic
+/// constraints) and apply some simplifcations:
+/// - Use fixed value for omega in equations (12) and (13c). This should greatly
+/// reduce the execution time as K needs to be evaluated by equation (12) only
+/// once. Further, this allows for a constant-time algorithm.
+/// - Don't compute the inverse in equation (12) explicitly but solve the linear
+/// system obtained after transposition instead. For the latter, we use sposv
+/// from LAPACK which makes use of the fact that the matrix to be inverted is
+/// s.p.d. thus allowing for a constant-time algorithm (compute Cholesky
+/// factorization and solve triangular system).
+/// - Avoid copying in memory data where possible.
+/// There are surely some further optimizations but we are certain that our results
+/// signify the increased cost as compared to the other benchmarks well.
+///
+void benchmarks_run_kalman_filter(void);
+
+#endif // BENCHMARKS_KALMANFILTER_H
diff --git a/src/benchmarks/memory.c b/src/benchmarks/memory.c
index caaa789f5523b865c17971fe5d0bdfa3e40227a2..aae06236b0f07d4994da9903112caaf467d746b7 100644
--- a/src/benchmarks/memory.c
+++ b/src/benchmarks/memory.c
@@ -31,6 +31,7 @@
#include <benchmarks/blind_abstraction.h>
#include <benchmarks/observer_abstraction.h>
#include <benchmarks/luenberger_observer.h>
+#include <benchmarks/kalman_filter.h>
///
/// @brief Size of the padding in bytes used for checking memory violations
@@ -53,7 +54,8 @@
static float work[2u * PADDING
+ MAX(BENCHMARKS_BLIND_ABSTRACTION_WORK_SIZE,
MAX(BENCHMARKS_OBSERVER_ABSTRACTION_WORK_SIZE,
- BENCHMARKS_LUENBERGER_OBSERVER_WORK_SIZE))
+ MAX(BENCHMARKS_LUENBERGER_OBSERVER_WORK_SIZE,
+ BENCHMARKS_KALMAN_FILTER_WORK_SIZE)))
];
float * const benchmarks_memory = work + PADDING;