From 6f73c6568542fb2026a9ae4aa64956d9faee5bff Mon Sep 17 00:00:00 2001
From: Max Ahlberg <maxahlberg@users.noreply.github.com>
Date: Thu, 1 Jun 2023 09:11:23 +0200
Subject: [PATCH] fix(TrackingController): Introducing NaN prevention

With faulty inputs, an anti windup controller can accumulate values that reach beyond min/max of a float64, using the inf values in addition or subtraction then creates a NaN.

To prevent the controller to give NaN as ControlSignal output we prevent the ControlError, ControlErrorIntegrand, ControlErrorIntegral and ControlErrorDerivative from reaching inf by clamping it to +/- MaxFloat64.

This gives a more expected behavior and let the user have the possibility to handle the state as the user prefers.
---
 trackingcontroller.go      | 10 ++++---
 trackingcontroller_test.go | 54 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 61 insertions(+), 3 deletions(-)

diff --git a/trackingcontroller.go b/trackingcontroller.go
index 3892f0d..a7cc443 100644
--- a/trackingcontroller.go
+++ b/trackingcontroller.go
@@ -12,6 +12,9 @@ import (
 // Chapter 6 of Åström and Murray, Feedback Systems:
 // An Introduction to Scientists and Engineers, 2008
 // (http://www.cds.caltech.edu/~murray/amwiki)
+//
+// The ControlError, ControlErrorIntegrand, ControlErrorIntegral and ControlErrorDerivative are prevented
+// from reaching +/- inf by clamping them to [-math.MaxFloat64, math.MaxFloat64].
 type TrackingController struct {
 	// Config for the TrackingController.
 	Config TrackingControllerConfig
@@ -86,9 +89,10 @@ func (c *TrackingController) Update(input TrackingControllerInput) {
 	c.State.ControlSignal = math.Max(c.Config.MinOutput, math.Min(c.Config.MaxOutput, c.State.UnsaturatedControlSignal))
 	c.State.ControlErrorIntegrand = e + c.Config.AntiWindUpGain*(input.AppliedControlSignal-
 		c.State.UnsaturatedControlSignal)
-	c.State.ControlErrorIntegral = controlErrorIntegral
-	c.State.ControlErrorDerivative = controlErrorDerivative
-	c.State.ControlError = e
+	c.State.ControlErrorIntegrand = math.Max(-math.MaxFloat64, math.Min(math.MaxFloat64, c.State.ControlErrorIntegrand))
+	c.State.ControlErrorIntegral = math.Max(-math.MaxFloat64, math.Min(math.MaxFloat64, controlErrorIntegral))
+	c.State.ControlErrorDerivative = math.Max(-math.MaxFloat64, math.Min(math.MaxFloat64, controlErrorDerivative))
+	c.State.ControlError = math.Max(-math.MaxFloat64, math.Min(math.MaxFloat64, e))
 }
 
 // DischargeIntegral provides the ability to discharge the controller integral state
diff --git a/trackingcontroller_test.go b/trackingcontroller_test.go
index 7d05154..dab2de6 100644
--- a/trackingcontroller_test.go
+++ b/trackingcontroller_test.go
@@ -1,6 +1,7 @@
 package pid
 
 import (
+	"math"
 	"testing"
 	"time"
 
@@ -78,6 +79,59 @@ func TestTrackingController_PControllerUpdate(t *testing.T) {
 	}
 }
 
+func TestTrackingController_NaN(t *testing.T) {
+	// Given a saturated I controller with a low AntiWindUpGain
+	c := &TrackingController{
+		Config: TrackingControllerConfig{
+			LowPassTimeConstant:           1 * time.Second,
+			IntegralGain:                  10,
+			IntegralDischargeTimeConstant: 10,
+			MinOutput:                     -10,
+			MaxOutput:                     10,
+			AntiWindUpGain:                0.01,
+		},
+	}
+	for _, tt := range []struct {
+		input         TrackingControllerInput
+		expectedState TrackingControllerState
+	}{
+		{
+			// Negative faulty measurement
+			input: TrackingControllerInput{
+				ReferenceSignal:   5.0,
+				ActualSignal:      -math.MaxFloat64,
+				FeedForwardSignal: 2.0,
+				SamplingInterval:  dtTest,
+			},
+		},
+		{
+			// Positive faulty measurement
+			input: TrackingControllerInput{
+				ReferenceSignal:   5.0,
+				ActualSignal:      math.MaxFloat64,
+				FeedForwardSignal: 2.0,
+				SamplingInterval:  dtTest,
+			},
+		},
+	} {
+		tt := tt
+		// When enough iterations have passed
+		c.Reset()
+		for i := 0; i < 220; i++ {
+			c.Update(TrackingControllerInput{
+				ReferenceSignal:   tt.input.ReferenceSignal,
+				ActualSignal:      tt.input.ActualSignal,
+				FeedForwardSignal: tt.input.FeedForwardSignal,
+				SamplingInterval:  tt.input.SamplingInterval,
+			})
+		}
+		// Then
+		assert.Assert(t, !math.IsNaN(c.State.UnsaturatedControlSignal))
+		assert.Assert(t, !math.IsNaN(c.State.ControlSignal))
+		assert.Assert(t, !math.IsNaN(c.State.ControlErrorIntegral))
+	}
+}
+
 func TestTrackingController_Reset(t *testing.T) {
 	// Given a SaturatedPIDController with stored values not equal to 0
 	c := &TrackingController{}