From 1cad84dfa42b34d64dc325a3b11bfc7902c3c8db Mon Sep 17 00:00:00 2001 From: Divyansh Agrawal Date: Mon, 31 Mar 2025 21:39:15 +0530 Subject: [PATCH 01/11] Added the Active Disturbance Rejection Control (ADRC) Algorithm --- control_algorithms/adrc.py | 64 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 control_algorithms/adrc.py diff --git a/control_algorithms/adrc.py b/control_algorithms/adrc.py new file mode 100644 index 000000000000..3ef4dbd73960 --- /dev/null +++ b/control_algorithms/adrc.py @@ -0,0 +1,64 @@ +""" +Active Disturbance Rejection Control (ADRC) is a robust control strategy +that estimates and compensates for disturbances in real-time without needing +an explicit mathematical model of the system. + +It consists of: +1. Tracking Differentiator (TD) - Smooths the reference signal +2. Extended State Observer (ESO) - Estimates system states and disturbances +3. Nonlinear State Error Feedback (NLSEF) - Generates the control signal + +Refer - https://en.wikipedia.org/wiki/Active_disturbance_rejection_control +""" + + +class ADRC: + def __init__(self, beta1: float, beta2: float, beta3: float, setpoint: float = 0.0): + """ + Initialize the ADRC controller. + + :param beta1: Gain for error correction in ESO + :param beta2: Gain for disturbance estimation in ESO + :param beta3: Gain for acceleration estimation in ESO + :param setpoint: Desired target value + """ + self.beta1 = beta1 + self.beta2 = beta2 + self.beta3 = beta3 + self.setpoint = setpoint + + self.z1 = 0.0 # Estimated system output + self.z2 = 0.0 # Estimated system velocity + self.z3 = 0.0 # Estimated total disturbance + + def compute(self, measured_value: float, dt: float) -> float: + """ + Compute the control signal based on error estimation and disturbance rejection. + + :param measured_value: The current process variable + :param dt: Time difference since the last update + :return: Control output + """ + error = self.setpoint - measured_value + + # Extended State Observer (ESO) Update + self.z1 += dt * (self.z2 - self.beta1 * (self.z1 - measured_value)) + self.z2 += dt * (self.z3 - self.beta2 * (self.z1 - measured_value)) + self.z3 -= self.beta3 * (self.z1 - measured_value) + + # Control Law (Nonlinear State Error Feedback - NLSEF) + control_output = self.z2 - self.z3 + return control_output + + def reset(self): + """Reset the estimated states.""" + self.z1 = 0.0 + self.z2 = 0.0 + self.z3 = 0.0 + + +if __name__ == "__main__": + import doctest + doctest.testmod() + + From 9934f06f672ea90a8cd0809e67673ed00ccc866a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 31 Mar 2025 16:19:13 +0000 Subject: [PATCH 02/11] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- control_algorithms/adrc.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/control_algorithms/adrc.py b/control_algorithms/adrc.py index 3ef4dbd73960..392cd2655178 100644 --- a/control_algorithms/adrc.py +++ b/control_algorithms/adrc.py @@ -59,6 +59,5 @@ def reset(self): if __name__ == "__main__": import doctest - doctest.testmod() - + doctest.testmod() From 5ec73ec5d4294c92db0af832981408dbab483117 Mon Sep 17 00:00:00 2001 From: Divyansh Agrawal Date: Mon, 31 Mar 2025 22:03:40 +0530 Subject: [PATCH 03/11] Added the Active Disturbance Rejection Control (ADRC) Algorithm --- control_algorithms/__init__.py | 0 control_algorithms/adrc.py | 1 - 2 files changed, 1 deletion(-) create mode 100644 control_algorithms/__init__.py diff --git a/control_algorithms/__init__.py b/control_algorithms/__init__.py new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/control_algorithms/adrc.py b/control_algorithms/adrc.py index 392cd2655178..6408a83089cb 100644 --- a/control_algorithms/adrc.py +++ b/control_algorithms/adrc.py @@ -39,7 +39,6 @@ def compute(self, measured_value: float, dt: float) -> float: :param dt: Time difference since the last update :return: Control output """ - error = self.setpoint - measured_value # Extended State Observer (ESO) Update self.z1 += dt * (self.z2 - self.beta1 * (self.z1 - measured_value)) From ceac158b6f5a2d3b537745ab64f0dc1f576e1cd3 Mon Sep 17 00:00:00 2001 From: Divyansh Agrawal <154041893+div-dev123@users.noreply.github.com> Date: Wed, 2 Apr 2025 12:29:45 +0530 Subject: [PATCH 04/11] Update control_algorithms/adrc.py Co-authored-by: Christian Clauss --- control_algorithms/adrc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_algorithms/adrc.py b/control_algorithms/adrc.py index 6408a83089cb..fa90d73182b3 100644 --- a/control_algorithms/adrc.py +++ b/control_algorithms/adrc.py @@ -13,7 +13,7 @@ class ADRC: - def __init__(self, beta1: float, beta2: float, beta3: float, setpoint: float = 0.0): + def __init__(self, error_correction: float, disturbance: float, acceleration: float, target: float = 0.0): """ Initialize the ADRC controller. From 1c5dfe2555f34c6ba1d9292980aaebbf401ce874 Mon Sep 17 00:00:00 2001 From: Divyansh Agrawal <154041893+div-dev123@users.noreply.github.com> Date: Wed, 2 Apr 2025 12:30:00 +0530 Subject: [PATCH 05/11] Update control_algorithms/adrc.py Co-authored-by: Christian Clauss --- control_algorithms/adrc.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/control_algorithms/adrc.py b/control_algorithms/adrc.py index fa90d73182b3..1a856e160b05 100644 --- a/control_algorithms/adrc.py +++ b/control_algorithms/adrc.py @@ -27,9 +27,9 @@ def __init__(self, error_correction: float, disturbance: float, acceleration: fl self.beta3 = beta3 self.setpoint = setpoint - self.z1 = 0.0 # Estimated system output - self.z2 = 0.0 # Estimated system velocity - self.z3 = 0.0 # Estimated total disturbance + self.system_output = 0.0 # Estimated system output + self.system_velocity = 0.0 # Estimated system velocity + self.total_disturbance = 0.0 # Estimated total disturbance def compute(self, measured_value: float, dt: float) -> float: """ From 9f64fc5ef8a11bb2946f68ace42393fe3ca2b094 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 2 Apr 2025 07:00:08 +0000 Subject: [PATCH 06/11] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- control_algorithms/adrc.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/control_algorithms/adrc.py b/control_algorithms/adrc.py index 1a856e160b05..e8148753fc5f 100644 --- a/control_algorithms/adrc.py +++ b/control_algorithms/adrc.py @@ -13,7 +13,13 @@ class ADRC: - def __init__(self, error_correction: float, disturbance: float, acceleration: float, target: float = 0.0): + def __init__( + self, + error_correction: float, + disturbance: float, + acceleration: float, + target: float = 0.0, + ): """ Initialize the ADRC controller. From 7884c0e0cba9b75ac76e91bbe3948452407b5f30 Mon Sep 17 00:00:00 2001 From: Divyansh Agrawal <154041893+div-dev123@users.noreply.github.com> Date: Wed, 2 Apr 2025 12:30:39 +0530 Subject: [PATCH 07/11] Update control_algorithms/adrc.py Co-authored-by: Christian Clauss --- control_algorithms/adrc.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/control_algorithms/adrc.py b/control_algorithms/adrc.py index e8148753fc5f..4c158e2d3c2e 100644 --- a/control_algorithms/adrc.py +++ b/control_algorithms/adrc.py @@ -52,8 +52,7 @@ def compute(self, measured_value: float, dt: float) -> float: self.z3 -= self.beta3 * (self.z1 - measured_value) # Control Law (Nonlinear State Error Feedback - NLSEF) - control_output = self.z2 - self.z3 - return control_output + return self.z2 - self.z3 def reset(self): """Reset the estimated states.""" From c09256db7eaebb2d23f4fa14d7620c2dc24cbf09 Mon Sep 17 00:00:00 2001 From: Divyansh Agrawal Date: Wed, 2 Apr 2025 12:46:25 +0530 Subject: [PATCH 08/11] Refactor ADRC class with readable names, type hints, and doctests --- control_algorithms/adrc.py | 62 ++++++++++++++++++++++++++------------ 1 file changed, 42 insertions(+), 20 deletions(-) diff --git a/control_algorithms/adrc.py b/control_algorithms/adrc.py index 4c158e2d3c2e..940516170faa 100644 --- a/control_algorithms/adrc.py +++ b/control_algorithms/adrc.py @@ -19,46 +19,68 @@ def __init__( disturbance: float, acceleration: float, target: float = 0.0, - ): + ) -> None: """ Initialize the ADRC controller. - :param beta1: Gain for error correction in ESO - :param beta2: Gain for disturbance estimation in ESO - :param beta3: Gain for acceleration estimation in ESO - :param setpoint: Desired target value + :param error_correction: Gain for error correction in ESO + :param disturbance: Gain for disturbance estimation in ESO + :param acceleration: Gain for acceleration estimation in ESO + :param target: Desired target value (default: 0.0) + >>> adrc = ADRC(1.0, 2.0, 3.0, 5.0) + >>> adrc.error_correction, adrc.disturbance, adrc.acceleration, adrc.target + (1.0, 2.0, 3.0, 5.0) + >>> adrc.system_output, adrc.system_velocity, adrc.total_disturbance + (0.0, 0.0, 0.0) """ - self.beta1 = beta1 - self.beta2 = beta2 - self.beta3 = beta3 - self.setpoint = setpoint + self.error_correction = error_correction + self.disturbance = disturbance + self.acceleration = acceleration + self.target = target self.system_output = 0.0 # Estimated system output self.system_velocity = 0.0 # Estimated system velocity self.total_disturbance = 0.0 # Estimated total disturbance - def compute(self, measured_value: float, dt: float) -> float: + def calculate_control_output(self, measured_value: float, dt: float) -> float: """ Compute the control signal based on error estimation and disturbance rejection. :param measured_value: The current process variable :param dt: Time difference since the last update :return: Control output + >>> adrc = ADRC(10.0, 5.0, 2.0) + >>> (adrc.system_output, adrc.system_velocity, + ... adrc.total_disturbance) = (1.0, 2.0, 3.0) + >>> adrc.calculate_control_output(0.5, 0.1) # Simple test with dt=0.1 + 0.8 """ - # Extended State Observer (ESO) Update - self.z1 += dt * (self.z2 - self.beta1 * (self.z1 - measured_value)) - self.z2 += dt * (self.z3 - self.beta2 * (self.z1 - measured_value)) - self.z3 -= self.beta3 * (self.z1 - measured_value) + error = self.system_output - measured_value + self.system_output += dt * ( + self.system_velocity - self.error_correction * error + ) + self.system_velocity += dt * (self.total_disturbance - self.disturbance * error) + self.total_disturbance -= self.acceleration * error # Control Law (Nonlinear State Error Feedback - NLSEF) - return self.z2 - self.z3 + control_output = self.system_velocity - self.total_disturbance + return control_output + + def reset(self) -> None: + """ + Reset the estimated states to zero. - def reset(self): - """Reset the estimated states.""" - self.z1 = 0.0 - self.z2 = 0.0 - self.z3 = 0.0 + >>> adrc = ADRC(1.0, 2.0, 3.0) + >>> (adrc.system_output, adrc.system_velocity, + ... adrc.total_disturbance) = (1.1, 2.2, 3.3) + >>> adrc.reset() + >>> adrc.system_output, adrc.system_velocity, adrc.total_disturbance + (0.0, 0.0, 0.0) + """ + self.system_output = 0.0 + self.system_velocity = 0.0 + self.total_disturbance = 0.0 if __name__ == "__main__": From fca1fe95502706fc3599ed392f9b0a7c3c406d21 Mon Sep 17 00:00:00 2001 From: Divyansh Agrawal Date: Wed, 2 Apr 2025 12:54:10 +0530 Subject: [PATCH 09/11] Fixed Doctests --- control_algorithms/adrc.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/control_algorithms/adrc.py b/control_algorithms/adrc.py index 940516170faa..e819542dc40a 100644 --- a/control_algorithms/adrc.py +++ b/control_algorithms/adrc.py @@ -50,10 +50,13 @@ def calculate_control_output(self, measured_value: float, dt: float) -> float: :param dt: Time difference since the last update :return: Control output >>> adrc = ADRC(10.0, 5.0, 2.0) - >>> (adrc.system_output, adrc.system_velocity, - ... adrc.total_disturbance) = (1.0, 2.0, 3.0) + >>> ( + ... adrc.system_output, + ... adrc.system_velocity, + ... adrc.total_disturbance, + ... ) = (1.0, 2.0, 3.0) >>> adrc.calculate_control_output(0.5, 0.1) # Simple test with dt=0.1 - 0.8 + 0.05 """ # Extended State Observer (ESO) Update error = self.system_output - measured_value @@ -72,8 +75,11 @@ def reset(self) -> None: Reset the estimated states to zero. >>> adrc = ADRC(1.0, 2.0, 3.0) - >>> (adrc.system_output, adrc.system_velocity, - ... adrc.total_disturbance) = (1.1, 2.2, 3.3) + >>> ( + ... adrc.system_output, + ... adrc.system_velocity, + ... adrc.total_disturbance, + ... ) = (1.1, 2.2, 3.3) >>> adrc.reset() >>> adrc.system_output, adrc.system_velocity, adrc.total_disturbance (0.0, 0.0, 0.0) From 7e2da79488e71a9df8da293c62a53c4c24b65c99 Mon Sep 17 00:00:00 2001 From: Divyansh Agrawal Date: Wed, 2 Apr 2025 12:57:41 +0530 Subject: [PATCH 10/11] Fixed Doctests --- control_algorithms/adrc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_algorithms/adrc.py b/control_algorithms/adrc.py index e819542dc40a..1037ec9a73e7 100644 --- a/control_algorithms/adrc.py +++ b/control_algorithms/adrc.py @@ -56,7 +56,7 @@ def calculate_control_output(self, measured_value: float, dt: float) -> float: ... adrc.total_disturbance, ... ) = (1.0, 2.0, 3.0) >>> adrc.calculate_control_output(0.5, 0.1) # Simple test with dt=0.1 - 0.05 + 0.0.04999999999999982 """ # Extended State Observer (ESO) Update error = self.system_output - measured_value From 68f54f6f917dbf29d1df7c4f9ac78196a49ee6f5 Mon Sep 17 00:00:00 2001 From: Divyansh Agrawal Date: Wed, 2 Apr 2025 12:59:35 +0530 Subject: [PATCH 11/11] Fixed Doctests --- control_algorithms/adrc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control_algorithms/adrc.py b/control_algorithms/adrc.py index 1037ec9a73e7..0e03dffb57f8 100644 --- a/control_algorithms/adrc.py +++ b/control_algorithms/adrc.py @@ -56,7 +56,7 @@ def calculate_control_output(self, measured_value: float, dt: float) -> float: ... adrc.total_disturbance, ... ) = (1.0, 2.0, 3.0) >>> adrc.calculate_control_output(0.5, 0.1) # Simple test with dt=0.1 - 0.0.04999999999999982 + 0.04999999999999982 """ # Extended State Observer (ESO) Update error = self.system_output - measured_value