pid_design: add model-based PID tuning law based on GMVC

src/lib/CMakeLists.txt

@@ -58,6 +58,7 @@ add_subdirectory(motion_planning)
 add_subdirectory(output_limit)
 add_subdirectory(perf)
 add_subdirectory(pid)
+add_subdirectory(pid_design)
 add_subdirectory(pwm)
 add_subdirectory(rc)
 add_subdirectory(sensor_calibration)

src/lib/pid_design/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   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(PidDesign
+	pid_design.hpp
+)
+
+px4_add_unit_gtest(SRC pid_design_test.cpp)

src/lib/pid_design/pid_design.hpp

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   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 pid_design.hpp
+ * @brief Helper functions to design PID controllers using identified models
+ *
+ * @author Mathieu Bresciani <mathieu@auterion.com>
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+#include <mathlib/mathlib.h>
+
+namespace pid_design
+{
+
+/*
+* Compute a set of PID gains using General Minimum Variance Control law design
+*
+* @param num: the coefficients of the numerator of the discrete-time plant transfer function
+* @param den: the coefficients of the denominator of the discrete-time plant transfer function
+* @param dt: the sampling time in seconds
+* @param sigma: the desired closed-loop rise time in seconds
+* @param delta: the damping index (between 0 and 2). 0 is critical damping, 1 is Butterworth
+* @param lbda: the "detuning" coefficients. This affects the gain of the controller kc only (increase to detune the controller)
+*
+* @return The PID gains in standard form: u = kc*[1 + ki*dt + kd/dt]*e
+*         kc: the controller gain
+*         ki: the integral gain (= 1/Ti)
+*         kd: the derivative gain (= Td)
+*
+* Reference:
+* T.Yamatoto, K.Fujii and M.Kaneda, Design and implementation of a self-tuning pid controller, 1998
+*/
+
+inline matrix::Vector3f computePidGmvc(const matrix::Vector3f &num, const matrix::Vector3f &den, float dt,
+				       float sigma = 0.1f, float delta = 1.f, float lbda = 0.5f)
+{
+	sigma = math::constrain(sigma, 0.01f, 1.f);
+	delta = math::constrain(delta, 0.f, 1.f);
+	lbda = math::constrain(lbda, 0.f, 10.f);
+
+	const float a1 = den(1);
+	const float a2 = den(2);
+	const float b0 = num(0);
+	const float b1 = num(1);
+	const float b2 = num(2);
+
+	// Solve GMVC law (see derivation in pid_synthesis_symbolic.py)
+	const float rho = dt / sigma;
+	const float mu = 0.25f * (1.f - delta) + 0.51f * delta; // mu is in the interval [0.25 0.51]
+	const float p1 = -2.f * expf(-rho / (2.f * mu)) * cosf(sqrtf(4.f * mu - 1.f) * rho / (2.f * mu));
+	const float p2 = exp(-rho / mu);
+	const float e1 = -a1 + p1 + 1.f;
+	const float f0 = -a1 * e1 + a1 - a2 + e1 + p2;
+	const float f1 = a1 * e1 - a2 * e1 + a2;
+	const float f2 = a2 * e1;
+
+	// Translate to PID gains
+	const float nu = lbda + (e1 + 1.f) * (b0 + b1 + b2);
+
+	if (fabsf(nu) < FLT_EPSILON) {
+		return matrix::Vector3f();
+	}
+
+	const float kc = -(f1 + 2.f * f2) / nu;
+	float ki = -(f0 + f1 + f2) / (dt * (f1 + 2.f * f2));
+	ki /= 5.f; // This is not part of the original implementation but is required to produce reasonable gains
+	const float kd = -dt * f2 / (f1 + 2.f * f2);
+
+	return matrix::Vector3f(kc, ki, kd);
+}
+} // namespace pid_design

src/lib/pid_design/pid_design_test.cpp

@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   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 pid_design functions
+ * Run this test only using make tests TESTFILTER=pid_design
+ */
+
+#include <gtest/gtest.h>
+#include <matrix/matrix/math.hpp>
+
+#include "pid_design.hpp"
+
+using namespace pid_design;
+using namespace matrix;
+
+TEST(PidDesignTest, testImpl)
+{
+	const Vector3f num(.0098f, -0.162f, .147f);
+	const Vector3f den(1.f, -1.814f, .822f);
+	const float dt = 0.004f;
+	const Vector3f kid = computePidGmvc(num, den, dt, 0.1f, 1.f, 0.5f);
+
+	// Compare with values generated from the python implementation
+	EXPECT_NEAR(kid(0), 0.129f, .001f);
+	EXPECT_NEAR(kid(1), 11.911f / 5.f, .001f);
+	EXPECT_NEAR(kid(2), 0.0463f, .0001f);
+}