sysID: add system identification library

src/lib/CMakeLists.txt

@@ -63,6 +63,7 @@ add_subdirectory(rc)
 add_subdirectory(sensor_calibration)
 add_subdirectory(slew_rate)
 add_subdirectory(systemlib)
+add_subdirectory(system_identification)
 add_subdirectory(tecs)
 add_subdirectory(terrain_estimation)
 add_subdirectory(tunes)

src/lib/system_identification/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(SystemIdentification
+	system_identification.cpp
+	system_identification.hpp
+	arx_rls.hpp
+)
+
+px4_add_unit_gtest(SRC arx_rls_test.cpp LINKLIBS SystemIdentification)
+px4_add_unit_gtest(SRC system_identification_test.cpp LINKLIBS SystemIdentification mathlib)

src/lib/system_identification/arx_rls.hpp

@@ -0,0 +1,208 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file arx_rls.hpp
+ * @brief Efficient recursive weighted least-squares algorithm without matrix inversion
+ *
+ * Assumes an ARX (autoregressive) model:
+ * A(q^-1)y(k) = q^-d * B(q^-1)u(k) + A(q^-1)e(k)
+ *
+ * with:
+ * q^-i  backward shift operator
+ * A(q^-1) = 1 + a_1*q^-1 +...+ a_n*q^-n
+ * B(q^-1) = b_0 + b_1*q^-1...+ b_m*q^-m
+ * n  order of A(q^-1)
+ * m  order of B(q^-1)
+ * d  delay
+ * u  input of the system
+ * y  output of the system
+ * e  white noise input
+ *
+ * References:
+ * - Identification de systemes dynamiques, D.Bonvin and A.Karimi, epfl, 2011
+ *
+ * @author Mathieu Bresciani <mathieu@auterion.com>
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+
+template<size_t N, size_t M, size_t D>
+class ArxRls final
+{
+public:
+	ArxRls()
+	{
+		static_assert(N >= M, "The transfer function needs to be proper");
+
+		reset();
+	}
+
+	~ArxRls() = default;
+
+	void setForgettingFactor(float time_constant, float dt) { _lambda = 1.f - dt / time_constant; }
+	void setForgettingFactor(float lambda) { _lambda = lambda; }
+
+	/*
+	 * return the vector of estimated parameters
+	 * [a_1 .. a_n b_0 .. b_m]'
+	 */
+	const matrix::Vector < float, N + M + 1 > &getCoefficients() const { return _theta_hat; }
+	const matrix::Vector < float, N + M + 1 > getVariances() const { return _P.diag(); }
+	float getInnovation() const { return _innovation; }
+	const matrix::Vector < float, N + M + 1 > &getDiffEstimate() const { return _diff_theta_hat; }
+
+	void reset(const matrix::Vector < float, N + M + 1 > &theta_init = {})
+	{
+		/* _P.uncorrelateCovarianceSetVariance<N + M + 1>(0, 10e3f); // does not work */
+		_P.setZero();
+
+		for (size_t i = 0; i < (N + M + 1); i++) {
+			_P(i, i) = 10e3f;
+		}
+
+		_diff_theta_hat.setZero();
+
+		_theta_hat = theta_init;
+
+		for (size_t i = 0; i < M + D + 1; i++) {
+			_u[i] = 0.f;
+		}
+
+		for (size_t i = 0; i < N + 1; i++) {
+			_y[i] = 0.f;
+		}
+
+		_nb_samples = 0;
+		_innovation = 0.f;
+	}
+
+	void update(float u, float y)
+	{
+		const matrix::Vector < float, N + M + 1 > theta_prev = _theta_hat;
+
+		addInputOutput(u, y);
+
+		if (!isBufferFull()) {
+			// Do not start to update the RLS algorithm when the
+			// buffer still contains zeros
+			return;
+		}
+
+		const matrix::Vector < float, N + M + 1 > phi = constructDesignVector();
+		const matrix::Matrix < float, 1, N + M + 1 > phi_t = phi.transpose();
+
+		_P = (_P - _P * phi * phi_t * _P / (_lambda + (phi_t * _P * phi)(0, 0))) / _lambda;
+		_innovation = _y[N] - (phi_t * _theta_hat)(0, 0);
+		_theta_hat = _theta_hat + _P * phi * _innovation;
+
+		for (size_t i = 0; i < N + M + 1; i++) {
+			_diff_theta_hat(i) = fabsf(_theta_hat(i) - theta_prev(i));
+		}
+
+		/* fixCovarianceErrors(); // TODO: this could help against ill-conditioned matrix but needs more testing*/
+	}
+
+private:
+	void addInputOutput(float u, float y)
+	{
+		shiftRegisters();
+		_u[M + D] = u;
+		_y[N] = y;
+
+		if (!isBufferFull()) {
+			_nb_samples++;
+		}
+	}
+
+	void shiftRegisters()
+	{
+		for (size_t i = 0; i < N; i++) {
+			_y[i] = _y[i + 1];
+		}
+
+		for (size_t i = 0; i < (M + D); i++) {
+			_u[i] = _u[i + 1];
+		}
+	}
+
+	bool isBufferFull() const { return _nb_samples > (M + N + D); }
+
+	matrix::Vector < float, N + M + 1 > constructDesignVector() const
+	{
+		matrix::Vector < float, N + M + 1 > phi;
+
+		for (size_t i = 0; i < N; i++) {
+			phi(i) = -_y[N - i - 1];
+		}
+
+		int j = 0;
+
+		for (size_t i = N; i < (N + M + 1); i++) {
+			phi(i) = _u[M - j];
+			j++;
+		}
+
+		return phi;
+	}
+
+	void fixCovarianceErrors()
+	{
+		float max_var = 0.f;
+
+		for (size_t i = 0; i < (N + M + 1); i++) {
+			if (_P(i, i) > max_var) {
+				max_var = _P(i, i);
+			}
+		}
+
+		const float min_var_allowed = max_var * 0.1f;
+
+		for (size_t i = 0; i < (N + M + 1); i++) {
+			if (_P(i, i) < min_var_allowed) {
+				_P(i, i) = min_var_allowed;
+			}
+		}
+	}
+
+	matrix::SquareMatrix < float, N + M + 1 > _P;
+	matrix::Vector < float, N + M + 1 > _theta_hat;
+	matrix::Vector < float, N + M + 1 > _diff_theta_hat;
+	float _innovation{};
+	float _u[M + D + 1] {};
+	float _y[N + 1] {};
+	unsigned _nb_samples{0};
+	float _lambda{1.f};
+};

src/lib/system_identification/arx_rls_test.cpp

@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020-2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Test code for the ArxRls class
+ * Run this test only using make tests TESTFILTER=arx_rls
+ */
+
+#include <gtest/gtest.h>
+#include <matrix/matrix/math.hpp>
+
+#include "arx_rls.hpp"
+
+using namespace matrix;
+
+class ArxRlsTest : public ::testing::Test
+{
+public:
+	ArxRlsTest() {};
+};
+
+TEST_F(ArxRlsTest, test211)
+{
+	ArxRls<2, 1, 1> _rls;
+
+	for (int i = 0; i < (2 + 1 + 1); i++) {
+		// Fill the buffers with zeros
+		_rls.update(0.f, 0.f);
+	}
+
+	_rls.update(1, 2);
+	_rls.update(3, 4);
+	_rls.update(5, 6);
+	const Vector<float, 4> coefficients = _rls.getCoefficients();
+	float data_check[] = {-1.79f, 0.97f, 0.42f, -0.48f}; // generated from Python script
+	const Vector<float, 4> coefficients_check(data_check);
+	float eps = 1e-2;
+	EXPECT_TRUE((coefficients - coefficients_check).abs().max() < eps);
+}
+
+TEST_F(ArxRlsTest, test221)
+{
+	ArxRls<2, 2, 1> _rls;
+
+	for (int i = 0; i < (2 + 2 + 1); i++) {
+		// Fill the buffers with zeros
+		_rls.update(0.f, 0.f);
+	}
+
+	_rls.update(1, 2);
+	_rls.update(3, 4);
+	_rls.update(5, 6);
+	_rls.update(7, 8);
+	const Vector<float, 5> coefficients = _rls.getCoefficients();
+	float data_check[] = {-1.81, 1.06f, 0.38f, -0.27f, 0.26f};
+	const Vector<float, 5> coefficients_check(data_check);
+	float eps = 1e-2;
+	EXPECT_TRUE((coefficients - coefficients_check).abs().max() < eps);
+	coefficients.print();
+	coefficients_check.print();
+}
+
+TEST_F(ArxRlsTest, resetTest)
+{
+	ArxRls<2, 2, 1> _rls;
+	_rls.update(1, 2);
+	_rls.update(3, 4);
+	_rls.update(5, 6);
+	_rls.update(7, 8);
+	const Vector<float, 5> coefficients = _rls.getCoefficients();
+
+	// WHEN: resetting
+	_rls.reset();
+
+	// THEN: the variances and coefficients should be properly reset
+	EXPECT_TRUE(_rls.getVariances().min() > 5000.f);
+	EXPECT_TRUE(_rls.getCoefficients().abs().max() < 1e-8f);
+
+	// AND WHEN: running the same sequence of inputs-outputs
+	_rls.update(1, 2);
+	_rls.update(3, 4);
+	_rls.update(5, 6);
+	_rls.update(7, 8);
+
+	// THEN: the result should be exactly the same
+	EXPECT_TRUE((coefficients - _rls.getCoefficients()).abs().max() < 1e-8f);
+}

src/lib/system_identification/system_identification.cpp

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file system_identification.cpp
+ *
+ * @author Mathieu Bresciani <mathieu@auterion.com>
+ */
+
+#include "system_identification.hpp"
+
+void SystemIdentification::reset(const matrix::Vector<float, 5> &id_state_init)
+{
+	_rls.reset(id_state_init);
+	_u_lpf.reset(0.f);
+	_u_lpf.reset(0.f);
+	_u_hpf = 0.f;
+	_y_hpf = 0.f;
+	_u_prev = 0.f;
+	_y_prev = 0.f;
+	_fitness_lpf.reset(10.f);
+	_are_filters_initialized = false;
+}
+
+void SystemIdentification::update(float u, float y)
+{
+	updateFilters(u, y);
+	update();
+}
+
+void SystemIdentification::update()
+{
+	_rls.update(_u_hpf, _y_hpf);
+	updateFitness();
+}
+
+void SystemIdentification::updateFilters(float u, float y)
+{
+	if (!_are_filters_initialized) {
+		_u_lpf.reset(u);
+		_y_lpf.reset(y);
+		_u_hpf = 0.f;
+		_y_hpf = 0.f;
+		_u_prev = u;
+		_y_prev = y;
+		_are_filters_initialized = true;
+		return;
+	}
+
+	const float u_lpf = _u_lpf.apply(u);
+	const float y_lpf = _y_lpf.apply(y);
+	_u_hpf = _alpha_hpf * _u_hpf + _alpha_hpf * (u_lpf - _u_prev);
+	_y_hpf = _alpha_hpf * _y_hpf + _alpha_hpf * (y_lpf - _y_prev);
+
+	_u_prev = u_lpf;
+	_y_prev = y_lpf;
+}
+
+void SystemIdentification::updateFitness()
+{
+	const matrix::Vector<float, 5> &diff = _rls.getDiffEstimate();
+	float sum = 0.f;
+
+	for (size_t i = 0; i < 5; i++) {
+		sum += diff(i);
+	}
+
+	if (_dt > FLT_EPSILON) {
+		_fitness_lpf.update(sum / _dt);
+	}
+}

src/lib/system_identification/system_identification.hpp

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file system_identification.hpp
+ *
+ * @author Mathieu Bresciani <mathieu@auterion.com>
+ */
+
+#pragma once
+
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <matrix/matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <px4_platform_common/defines.h>
+
+#include "arx_rls.hpp"
+
+class SystemIdentification final
+{
+public:
+	SystemIdentification() = default;
+	~SystemIdentification() = default;
+
+	void reset(const matrix::Vector<float, 5> &id_state_init = {});
+	void update(float u, float y); // update filters and model
+	void update(); // update model only (to be called after updateFilters)
+	void updateFilters(float u, float y);
+	bool areFiltersInitialized() const { return _are_filters_initialized; }
+	void updateFitness();
+	const matrix::Vector<float, 5> &getCoefficients() const { return _rls.getCoefficients(); }
+	const matrix::Vector<float, 5> getVariances() const { return _rls.getVariances(); }
+	const matrix::Vector<float, 5> &getDiffEstimate() const { return _rls.getDiffEstimate(); }
+	float getFitness() const { return _fitness_lpf.getState(); }
+	float getInnovation() const { return _rls.getInnovation(); }
+
+	void setLpfCutoffFrequency(float sample_freq, float cutoff)
+	{
+		_u_lpf.set_cutoff_frequency(sample_freq, cutoff);
+		_y_lpf.set_cutoff_frequency(sample_freq, cutoff);
+	}
+	void setHpfCutoffFrequency(float sample_freq, float cutoff) { _alpha_hpf = sample_freq / (sample_freq + 2.f * M_PI_F * cutoff); }
+
+	void setForgettingFactor(float time_constant, float dt) { _rls.setForgettingFactor(time_constant, dt); }
+	void setFitnessLpfTimeConstant(float time_constant, float dt)
+	{
+		_fitness_lpf.setParameters(dt, time_constant);
+		_dt = dt;
+	}
+
+	float getFilteredInputData() const { return _u_hpf; }
+	float getFilteredOutputData() const { return _y_hpf; }
+
+private:
+	ArxRls<2, 2, 1> _rls;
+	math::LowPassFilter2p<float> _u_lpf{400.f, 30.f};
+	math::LowPassFilter2p<float> _y_lpf{400.f, 30.f};
+
+	//TODO: replace by HighPassFilter class
+	float _alpha_hpf{0.f};
+	float _u_hpf{0.f};
+	float _y_hpf{0.f};
+
+	float _u_prev{0.f};
+	float _y_prev{0.f};
+
+	bool _are_filters_initialized{false};
+
+	AlphaFilter<float> _fitness_lpf;
+	float _dt{0.1f};
+};

src/lib/system_identification/system_identification_test.cpp

@@ -0,0 +1,243 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020-2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Test code for the SystemIdentification class
+ * Run this test only using make tests TESTFILTER=system_identification
+ */
+
+#include <gtest/gtest.h>
+#include <matrix/matrix/math.hpp>
+
+#include "system_identification.hpp"
+#include "test_data.h"
+
+using namespace matrix;
+
+class SystemIdentificationTest : public ::testing::Test
+{
+public:
+	SystemIdentificationTest() {};
+	float apply(float sample);
+	void setCoefficients(float a1, float a2, float b0, float b1, float b2)
+	{
+		_a1 = a1;
+		_a2 = a2;
+		_b0 = b0;
+		_b1 = b1;
+		_b2 = b2;
+	}
+
+private:
+	float _a1{};
+	float _a2{};
+	float _b0{};
+	float _b1{};
+	float _b2{};
+	float _delay_element_1{};
+	float _delay_element_2{};
+};
+
+float SystemIdentificationTest::apply(float sample)
+{
+	// Direct Form II implementation
+	const float delay_element_0{sample - _delay_element_1 *_a1 - _delay_element_2 * _a2};
+	const float output{delay_element_0 *_b0 + _delay_element_1 *_b1 + _delay_element_2 * _b2};
+
+	_delay_element_2 = _delay_element_1;
+	_delay_element_1 = delay_element_0;
+
+	return output;
+}
+
+TEST_F(SystemIdentificationTest, basicTest)
+{
+	constexpr float fs = 800.f;
+
+	SystemIdentification _sys_id;
+	_sys_id.setHpfCutoffFrequency(fs, 0.05f);
+	_sys_id.setLpfCutoffFrequency(fs, 30.f);
+	_sys_id.setForgettingFactor(80.f, 1.f / fs);
+
+	for (int i = 0; i < 5; i++) {
+		// Fill the buffers with zeros
+		_sys_id.update(0.f, 0.f);
+	}
+
+	for (int i = 0; i < 10; i += 2) {
+		_sys_id.update(float(i), float(i + 1));
+	}
+
+	const Vector<float, 5> coefficients = _sys_id.getCoefficients();
+	float data_check[] = {-3.96f, 1.30f, -2.28f, -0.33f, 0.369f};
+	const Vector<float, 5> coefficients_check(data_check);
+	float eps = 1e-2;
+	EXPECT_TRUE((coefficients - coefficients_check).abs().max() < eps);
+	coefficients.print();
+	_sys_id.getVariances().print();
+}
+
+TEST_F(SystemIdentificationTest, resetTest)
+{
+	constexpr float fs = 800.f;
+
+	SystemIdentification _sys_id;
+	_sys_id.setHpfCutoffFrequency(fs, 0.05f);
+	_sys_id.setLpfCutoffFrequency(fs, 30.f);
+	_sys_id.setForgettingFactor(80.f, 1.f / fs);
+
+	for (int i = 0; i < 10; i += 2) {
+		_sys_id.update(float(i), float(i + 1));
+	}
+
+	const Vector<float, 5> coefficients = _sys_id.getCoefficients();
+
+	// WHEN: resetting
+	_sys_id.reset();
+
+	// THEN: the variances and coefficients should be properly reset
+	EXPECT_TRUE(_sys_id.getCoefficients().abs().max() < 1e-8f);
+	EXPECT_TRUE(_sys_id.getVariances().min() > 9e3f);
+
+	// AND WHEN: running the same sequence of inputs-outputs
+
+	for (int i = 0; i < 10; i += 2) {
+		_sys_id.update(float(i), float(i + 1));
+	}
+
+	// THEN: the result should be exactly the same
+	EXPECT_TRUE((coefficients - _sys_id.getCoefficients()).abs().max() < 1e-8f);
+	coefficients.print();
+	_sys_id.getVariances().print();
+}
+
+TEST_F(SystemIdentificationTest, simulatedModelTest)
+{
+	constexpr float fs = 200.f;
+	const float gyro_lpf_cutoff = 30.f;
+
+	SystemIdentification _sys_id;
+	_sys_id.setHpfCutoffFrequency(fs, 0.05f);
+	_sys_id.setLpfCutoffFrequency(fs, gyro_lpf_cutoff);
+	_sys_id.setForgettingFactor(60.f, 1.f / fs);
+
+	// Simulated model with integrator
+	const float a1 = -1.77f;
+	const float a2 = 0.77f;
+	const float b0 = 0.3812f;
+	const float b1 = -0.25f;
+	const float b2 = 0.2f;
+	setCoefficients(a1, a2, b0, b1, b2);
+	const float u_bias = -0.1f; // constant control offset
+	const float y_bias = 0.2f; // measurement bias
+
+	const float dt = 1.f / fs;
+	const float duration = 2.f;
+	float u = 0.f;
+	float y = 0.f;
+
+	for (int i = 0; i < static_cast<int>(duration / dt); i++) {
+
+		// Generate square input signal
+		if (_sys_id.areFiltersInitialized()
+		    && (i % 30 == 0)) {
+			if (u > 0.f) {
+				u = -1.f;
+
+			} else {
+				u = 1.f;
+			}
+		}
+
+		_sys_id.update(u + u_bias, y + y_bias); // apply new input and previous output
+
+		y = apply(u);
+#if 0
+		const float t = i * dt;
+		printf("%.6f, %.6f, %.6f\n", (double)t, (double)u, (double)y);
+#endif
+	}
+
+	const Vector<float, 5> coefficients = _sys_id.getCoefficients();
+	float data_check[] = {a1, a2, b0, b1, b2};
+	const Vector<float, 5> coefficients_check(data_check);
+	float eps = 1e-3;
+	EXPECT_TRUE((coefficients - coefficients_check).abs().max() < eps);
+	coefficients.print();
+	_sys_id.getVariances().print();
+}
+
+TEST_F(SystemIdentificationTest, realDataTest)
+{
+	static constexpr int n_samples_dt_avg = 100;
+	static constexpr int n_samples_test_data = sizeof(u_data) / sizeof(u_data[0]);
+	float dt_sum = 0.f;
+
+	for (int i = 1; i <= n_samples_dt_avg; i++) {
+		dt_sum += t_data[i] - t_data[i - 1];
+	}
+
+	const float dt = dt_sum / n_samples_dt_avg;
+	const float fs = 1.f / dt;
+
+	printf("dt = %.6f fs = %.3f\n", (double)dt, (double)(1.f / dt));
+
+	const float gyro_lpf_cutoff = 30.f;
+
+	SystemIdentification _sys_id;
+	_sys_id.setHpfCutoffFrequency(fs, 0.05f);
+	_sys_id.setLpfCutoffFrequency(fs, gyro_lpf_cutoff);
+	_sys_id.setForgettingFactor(60.f, dt);
+
+	float u = 0.f;
+	float y_prev = 0.f;
+
+	for (int i = 0; i < n_samples_test_data; i++) {
+		u = u_data[i];
+		_sys_id.update(u, y_prev); // apply new input and previous output
+
+		y_prev = y_data[i];
+
+#if 0
+		printf("%.6f, %.6f, %.6f\n", (double)t_data[i], (double)u, (double)y);
+#endif
+	}
+
+	const Vector<float, 5> coefficients = _sys_id.getCoefficients();
+	float data_check[] = {-1.916f, 0.916, -0.017f, -0.001f, 0.036f};
+	const Vector<float, 5> coefficients_check(data_check);
+	float eps = 1e-3;
+	EXPECT_TRUE((coefficients - coefficients_check).abs().max() < eps);
+	coefficients.print();
+	_sys_id.getVariances().print();
+}