├── .github ├── CODEOWNERS ├── codecov.yml ├── dependabot.yml └── workflows │ ├── ci.yml │ └── release.yml ├── .gitignore ├── .sage ├── .gitignore ├── go.mod ├── go.sum └── main.go ├── CODE_OF_CONDUCT.md ├── LICENSE ├── Makefile ├── README.md ├── SECURITY.md ├── antiwindupcontroller.go ├── antiwindupcontroller_test.go ├── controller.go ├── controller_example_test.go ├── controller_test.go ├── doc.go ├── doc ├── pid-go.png └── pid-go.svg ├── go.mod ├── go.sum ├── trackingcontroller.go └── trackingcontroller_test.go /.github/CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @einride/team-motion-control 2 | -------------------------------------------------------------------------------- /.github/codecov.yml: -------------------------------------------------------------------------------- 1 | codecov: 2 | require_ci_to_pass: yes 3 | 4 | ignore: 5 | - internal/examples/proto/gen 6 | 7 | coverage: 8 | range: "70...100" 9 | 10 | comment: 11 | layout: reach,diff,flags,files 12 | behavior: default 13 | require_changes: no 14 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | 3 | updates: 4 | - package-ecosystem: github-actions 5 | directory: / 6 | schedule: 7 | interval: daily 8 | 9 | - package-ecosystem: gomod 10 | directory: / 11 | schedule: 12 | interval: daily 13 | 14 | - package-ecosystem: gomod 15 | directory: .sage 16 | schedule: 17 | interval: weekly 18 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: ci 2 | 3 | on: 4 | pull_request: 5 | types: [opened, reopened, synchronize] 6 | 7 | jobs: 8 | make: 9 | runs-on: ubuntu-latest 10 | steps: 11 | - name: Setup Sage 12 | uses: einride/sage/actions/setup@master 13 | 14 | - name: Make 15 | run: make 16 | 17 | # Disabling since there is an issue with the token 18 | # - name: Report Code Coverage 19 | # uses: codecov/codecov-action@v4 20 | # with: 21 | # file: .sage/build/go/coverage/go-test.txt 22 | # fail_ci_if_error: true 23 | # token: ${{ secrets.CODECOV_TOKEN }} 24 | -------------------------------------------------------------------------------- /.github/workflows/release.yml: -------------------------------------------------------------------------------- 1 | name: release 2 | 3 | on: 4 | push: 5 | branches: [master] 6 | 7 | permissions: write-all 8 | 9 | jobs: 10 | release: 11 | runs-on: ubuntu-latest 12 | steps: 13 | - name: Setup Sage 14 | uses: einride/sage/actions/setup@master 15 | 16 | - name: Make 17 | run: make 18 | 19 | - name: Release 20 | uses: go-semantic-release/action@v1.23 21 | with: 22 | github-token: ${{ secrets.GITHUB_TOKEN }} 23 | allow-initial-development-versions: true 24 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | -------------------------------------------------------------------------------- /.sage/.gitignore: -------------------------------------------------------------------------------- 1 | .gitignore 2 | tools/ 3 | bin/ 4 | build/ 5 | -------------------------------------------------------------------------------- /.sage/go.mod: -------------------------------------------------------------------------------- 1 | module go.einride.tech/pid/.sage 2 | 3 | go 1.19 4 | 5 | require go.einride.tech/sage v0.334.0 6 | -------------------------------------------------------------------------------- /.sage/go.sum: -------------------------------------------------------------------------------- 1 | go.einride.tech/sage v0.334.0 h1:HY/u8jM2UQ/BerH+vk7JtXf2hg6J0z99Fh0YAQ4QeaE= 2 | go.einride.tech/sage v0.334.0/go.mod h1:EzV5uciFX7/2ho8EKB5K9JghOfXIxlzs694b+Tkl5GQ= 3 | -------------------------------------------------------------------------------- /.sage/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | import ( 4 | "context" 5 | 6 | "go.einride.tech/sage/sg" 7 | "go.einride.tech/sage/tools/sgconvco" 8 | "go.einride.tech/sage/tools/sggit" 9 | "go.einride.tech/sage/tools/sggo" 10 | "go.einride.tech/sage/tools/sggolangcilint" 11 | "go.einride.tech/sage/tools/sggolicenses" 12 | "go.einride.tech/sage/tools/sgmdformat" 13 | "go.einride.tech/sage/tools/sgyamlfmt" 14 | ) 15 | 16 | func main() { 17 | sg.GenerateMakefiles( 18 | sg.Makefile{ 19 | Path: sg.FromGitRoot("Makefile"), 20 | DefaultTarget: Default, 21 | }, 22 | ) 23 | } 24 | 25 | func Default(ctx context.Context) error { 26 | sg.Deps(ctx, ConvcoCheck, FormatMarkdown, FormatYaml) 27 | sg.Deps(ctx, GoLint) 28 | sg.Deps(ctx, GoTest) 29 | sg.Deps(ctx, GoModTidy) 30 | sg.Deps(ctx, GoLicenses, GitVerifyNoDiff) 31 | return nil 32 | } 33 | 34 | func GoModTidy(ctx context.Context) error { 35 | sg.Logger(ctx).Println("tidying Go module files...") 36 | return sg.Command(ctx, "go", "mod", "tidy", "-v").Run() 37 | } 38 | 39 | func GoTest(ctx context.Context) error { 40 | sg.Logger(ctx).Println("running Go tests...") 41 | return sggo.TestCommand(ctx).Run() 42 | } 43 | 44 | func GoLint(ctx context.Context) error { 45 | sg.Logger(ctx).Println("linting Go files...") 46 | return sggolangcilint.Run(ctx) 47 | } 48 | 49 | func GoLicenses(ctx context.Context) error { 50 | sg.Logger(ctx).Println("checking Go licenses...") 51 | return sggolicenses.Check(ctx) 52 | } 53 | 54 | func FormatMarkdown(ctx context.Context) error { 55 | sg.Logger(ctx).Println("formatting Markdown files...") 56 | return sgmdformat.Command(ctx).Run() 57 | } 58 | 59 | func FormatYaml(ctx context.Context) error { 60 | sg.Logger(ctx).Println("formatting Yaml files...") 61 | return sgyamlfmt.Run(ctx) 62 | } 63 | 64 | func ConvcoCheck(ctx context.Context) error { 65 | sg.Logger(ctx).Println("checking git commits...") 66 | return sgconvco.Command(ctx, "check", "origin/master..HEAD").Run() 67 | } 68 | 69 | func GitVerifyNoDiff(ctx context.Context) error { 70 | sg.Logger(ctx).Println("verifying that git has no diff...") 71 | return sggit.VerifyNoDiff(ctx) 72 | } 73 | -------------------------------------------------------------------------------- /CODE_OF_CONDUCT.md: -------------------------------------------------------------------------------- 1 | # Contributor Covenant Code of Conduct 2 | 3 | ## Our Pledge 4 | 5 | We as members, contributors, and leaders pledge to make participation in our 6 | community a harassment-free experience for everyone, regardless of age, body 7 | size, visible or invisible disability, ethnicity, sex characteristics, gender 8 | identity and expression, level of experience, education, socio-economic status, 9 | nationality, personal appearance, race, religion, or sexual identity and 10 | orientation. 11 | 12 | We pledge to act and interact in ways that contribute to an open, welcoming, 13 | diverse, inclusive, and healthy community. 14 | 15 | ## Our Standards 16 | 17 | Examples of behavior that contributes to a positive environment for our 18 | community include: 19 | 20 | - Demonstrating empathy and kindness toward other people 21 | - Being respectful of differing opinions, viewpoints, and experiences 22 | - Giving and gracefully accepting constructive feedback 23 | - Accepting responsibility and apologizing to those affected by our mistakes, 24 | and learning from the experience 25 | - Focusing on what is best not just for us as individuals, but for the overall 26 | community 27 | 28 | Examples of unacceptable behavior include: 29 | 30 | - The use of sexualized language or imagery, and sexual attention or advances of 31 | any kind 32 | - Trolling, insulting or derogatory comments, and personal or political attacks 33 | - Public or private harassment 34 | - Publishing others' private information, such as a physical or email address, 35 | without their explicit permission 36 | - Other conduct which could reasonably be considered inappropriate in a 37 | professional setting 38 | 39 | ## Enforcement Responsibilities 40 | 41 | Community leaders are responsible for clarifying and enforcing our standards of 42 | acceptable behavior and will take appropriate and fair corrective action in 43 | response to any behavior that they deem inappropriate, threatening, offensive, 44 | or harmful. 45 | 46 | Community leaders have the right and responsibility to remove, edit, or reject 47 | comments, commits, code, wiki edits, issues, and other contributions that are 48 | not aligned to this Code of Conduct, and will communicate reasons for moderation 49 | decisions when appropriate. 50 | 51 | ## Scope 52 | 53 | This Code of Conduct applies within all community spaces, and also applies when 54 | an individual is officially representing the community in public spaces. 55 | Examples of representing our community include using an official e-mail address, 56 | posting via an official social media account, or acting as an appointed 57 | representative at an online or offline event. 58 | 59 | ## Enforcement 60 | 61 | Instances of abusive, harassing, or otherwise unacceptable behavior may be 62 | reported to the community leaders responsible for enforcement at 63 | open-source@einride.tech. 64 | 65 | All complaints will be reviewed and investigated promptly and fairly. 66 | 67 | All community leaders are obligated to respect the privacy and security of the 68 | reporter of any incident. 69 | 70 | ## Enforcement Guidelines 71 | 72 | Community leaders will follow these Community Impact Guidelines in determining 73 | the consequences for any action they deem in violation of this Code of Conduct: 74 | 75 | ### 1. Correction 76 | 77 | **Community Impact**: Use of inappropriate language or other behavior deemed 78 | unprofessional or unwelcome in the community. 79 | 80 | **Consequence**: A private, written warning from community leaders, providing 81 | clarity around the nature of the violation and an explanation of why the 82 | behavior was inappropriate. A public apology may be requested. 83 | 84 | ### 2. Warning 85 | 86 | **Community Impact**: A violation through a single incident or series of 87 | actions. 88 | 89 | **Consequence**: A warning with consequences for continued behavior. No 90 | interaction with the people involved, including unsolicited interaction with 91 | those enforcing the Code of Conduct, for a specified period of time. This 92 | includes avoiding interactions in community spaces as well as external channels 93 | like social media. Violating these terms may lead to a temporary or permanent 94 | ban. 95 | 96 | ### 3. Temporary Ban 97 | 98 | **Community Impact**: A serious violation of community standards, including 99 | sustained inappropriate behavior. 100 | 101 | **Consequence**: A temporary ban from any sort of interaction or public 102 | communication with the community for a specified period of time. No public or 103 | private interaction with the people involved, including unsolicited interaction 104 | with those enforcing the Code of Conduct, is allowed during this period. 105 | Violating these terms may lead to a permanent ban. 106 | 107 | ### 4. Permanent Ban 108 | 109 | **Community Impact**: Demonstrating a pattern of violation of community 110 | standards, including sustained inappropriate behavior, harassment of an 111 | individual, or aggression toward or disparagement of classes of individuals. 112 | 113 | **Consequence**: A permanent ban from any sort of public interaction within the 114 | community. 115 | 116 | ## Attribution 117 | 118 | This Code of Conduct is adapted from the 119 | [Contributor Covenant](https://www.contributor-covenant.org), version 2.0, 120 | available at 121 | https://www.contributor-covenant.org/version/2/0/code_of_conduct.html. 122 | 123 | Community Impact Guidelines were inspired by 124 | [Mozilla's code of conduct enforcement ladder](https://github.com/mozilla/diversity). 125 | 126 | For answers to common questions about this code of conduct, see the FAQ at 127 | https://www.contributor-covenant.org/faq. Translations are available at 128 | https://www.contributor-covenant.org/translations. 129 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2020 Einride AB 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of 4 | this software and associated documentation files (the "Software"), to deal in 5 | the Software without restriction, including without limitation the rights to 6 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 7 | of the Software, and to permit persons to whom the Software is furnished to do 8 | so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. 20 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # Code generated by go.einride.tech/sage. DO NOT EDIT. 2 | # To learn more, see .sage/main.go and https://github.com/einride/sage. 3 | 4 | .DEFAULT_GOAL := default 5 | 6 | cwd := $(dir $(realpath $(firstword $(MAKEFILE_LIST)))) 7 | sagefile := $(abspath $(cwd)/.sage/bin/sagefile) 8 | 9 | # Setup Go. 10 | go := $(shell command -v go 2>/dev/null) 11 | export GOWORK ?= off 12 | ifndef go 13 | SAGE_GO_VERSION ?= 1.20.2 14 | export GOROOT := $(abspath $(cwd)/.sage/tools/go/$(SAGE_GO_VERSION)/go) 15 | export PATH := $(PATH):$(GOROOT)/bin 16 | go := $(GOROOT)/bin/go 17 | os := $(shell uname | tr '[:upper:]' '[:lower:]') 18 | arch := $(shell uname -m) 19 | ifeq ($(arch),x86_64) 20 | arch := amd64 21 | endif 22 | $(go): 23 | $(info installing Go $(SAGE_GO_VERSION)...) 24 | @mkdir -p $(dir $(GOROOT)) 25 | @curl -sSL https://go.dev/dl/go$(SAGE_GO_VERSION).$(os)-$(arch).tar.gz | tar xz -C $(dir $(GOROOT)) 26 | @touch $(GOROOT)/go.mod 27 | @chmod +x $(go) 28 | endif 29 | 30 | .PHONY: $(sagefile) 31 | $(sagefile): $(go) 32 | @cd .sage && $(go) mod tidy && $(go) run . 33 | 34 | .PHONY: sage 35 | sage: 36 | @$(MAKE) $(sagefile) 37 | 38 | .PHONY: update-sage 39 | update-sage: $(go) 40 | @cd .sage && $(go) get -d go.einride.tech/sage@latest && $(go) mod tidy && $(go) run . 41 | 42 | .PHONY: clean-sage 43 | clean-sage: 44 | @git clean -fdx .sage/tools .sage/bin .sage/build 45 | 46 | .PHONY: convco-check 47 | convco-check: $(sagefile) 48 | @$(sagefile) ConvcoCheck 49 | 50 | .PHONY: default 51 | default: $(sagefile) 52 | @$(sagefile) Default 53 | 54 | .PHONY: format-markdown 55 | format-markdown: $(sagefile) 56 | @$(sagefile) FormatMarkdown 57 | 58 | .PHONY: format-yaml 59 | format-yaml: $(sagefile) 60 | @$(sagefile) FormatYaml 61 | 62 | .PHONY: git-verify-no-diff 63 | git-verify-no-diff: $(sagefile) 64 | @$(sagefile) GitVerifyNoDiff 65 | 66 | .PHONY: go-licenses 67 | go-licenses: $(sagefile) 68 | @$(sagefile) GoLicenses 69 | 70 | .PHONY: go-lint 71 | go-lint: $(sagefile) 72 | @$(sagefile) GoLint 73 | 74 | .PHONY: go-mod-tidy 75 | go-mod-tidy: $(sagefile) 76 | @$(sagefile) GoModTidy 77 | 78 | .PHONY: go-test 79 | go-test: $(sagefile) 80 | @$(sagefile) GoTest 81 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PID Go 2 | 3 | [![PkgGoDev](https://pkg.go.dev/badge/go.einride.tech/pid)](https://pkg.go.dev/go.einride.tech/pid) 4 | [![GoReportCard](https://goreportcard.com/badge/go.einride.tech/pid)](https://goreportcard.com/report/go.einride.tech/pid) 5 | [![Codecov](https://codecov.io/gh/einride/pid-go/branch/master/graph/badge.svg)](https://codecov.io/gh/einride/pid-go) 6 | 7 |

8 | logo 9 |

10 | 11 | PID controllers for Go. 12 | 13 | ## Examples 14 | 15 | ### `pid.Controller` 16 | 17 | A basic PID controller. 18 | 19 | ```go 20 | import ( 21 | "fmt" 22 | "time" 23 | 24 | "go.einride.tech/pid" 25 | ) 26 | 27 | func ExampleController() { 28 | // Create a PID controller. 29 | c := pid.Controller{ 30 | Config: pid.ControllerConfig{ 31 | ProportionalGain: 2.0, 32 | IntegralGain: 1.0, 33 | DerivativeGain: 1.0, 34 | }, 35 | } 36 | // Update the PID controller. 37 | c.Update(pid.ControllerInput{ 38 | ReferenceSignal: 10, 39 | ActualSignal: 0, 40 | SamplingInterval: 100 * time.Millisecond, 41 | }) 42 | fmt.Printf("%+v\n", c.State) 43 | // Reset the PID controller. 44 | c.Reset() 45 | fmt.Printf("%+v\n", c.State) 46 | // Output: 47 | // {ControlError:10 ControlErrorIntegral:1 ControlErrorDerivative:100 ControlSignal:121} 48 | // {ControlError:0 ControlErrorIntegral:0 ControlErrorDerivative:0 ControlSignal:0} 49 | } 50 | ``` 51 | 52 | \_[Reference ≫](https://en.wikipedia.org/wiki/PID_controller)\_ 53 | 54 | ### `pid.AntiWindupController` 55 | 56 | A PID-controller with low-pass filtering of the derivative term, feed forward 57 | term, a saturated control output and anti-windup. 58 | 59 | *[Reference ≫](http://www.cds.caltech.edu/~murray/amwiki)* 60 | 61 | ### `pid.TrackingController` 62 | 63 | a PID-controller with low-pass filtering of the derivative term, feed forward 64 | term, anti-windup and bumpless transfer using tracking mode control. 65 | 66 | *[Reference ≫](http://www.cds.caltech.edu/~murray/amwiki)* 67 | -------------------------------------------------------------------------------- /SECURITY.md: -------------------------------------------------------------------------------- 1 | # Security Policy 2 | 3 | Einride welcomes feedback from security researchers and the general public to 4 | help improve our security. If you believe you have discovered a vulnerability, 5 | privacy issue, exposed data, or other security issues in relation to this 6 | project, we want to hear from you. This policy outlines steps for reporting 7 | security issues to us, what we expect, and what you can expect from us. 8 | 9 | ## Supported versions 10 | 11 | We release patches for security issues according to semantic versioning. This 12 | project is currently unstable (v0.x) and only the latest version will receive 13 | security patches. 14 | 15 | ## Reporting a vulnerability 16 | 17 | Please do not report security vulnerabilities through public issues, 18 | discussions, or change requests. 19 | 20 | Please report security issues via [oss-security@einride.tech][email]. Provide 21 | all relevant information, including steps to reproduce the issue, any affected 22 | versions, and known mitigations. The more details you provide, the easier it 23 | will be for us to triage and fix the issue. You will receive a response from us 24 | within 2 business days. If the issue is confirmed, a patch will be released as 25 | soon as possible. 26 | 27 | For more information, or security issues not relating to open source code, 28 | please consult our [Vulnerability Disclosure Policy][vdp]. 29 | 30 | ## Preferred languages 31 | 32 | English is our preferred language of communication. 33 | 34 | ## Contributions and recognition 35 | 36 | We appreciate every contribution and will do our best to publicly 37 | [acknowledge][acknowledgments] your contributions. 38 | 39 | [acknowledgments]: https://einride.tech/security-acknowledgments.txt 40 | [email]: mailto:oss-security@einride.tech 41 | [vdp]: https://www.einride.tech/vulnerability-disclosure-policy 42 | -------------------------------------------------------------------------------- /antiwindupcontroller.go: -------------------------------------------------------------------------------- 1 | package pid 2 | 3 | import ( 4 | "math" 5 | "time" 6 | ) 7 | 8 | // AntiWindupController implements a PID-controller with low-pass filter of the derivative term, 9 | // feed forward term, a saturated control output and anti-windup. 10 | // 11 | // The anti-windup mechanism uses an actuator saturation model as defined in Chapter 6 of Åström and Murray, 12 | // Feedback Systems: An Introduction to Scientists and Engineers, 2008 13 | // (http://www.cds.caltech.edu/~murray/amwiki) 14 | // 15 | // The ControlError, ControlErrorIntegrand, ControlErrorIntegral and ControlErrorDerivative are prevented 16 | // from reaching +/- inf by clamping them to [-math.MaxFloat64, math.MaxFloat64]. 17 | type AntiWindupController struct { 18 | // Config for the AntiWindupController. 19 | Config AntiWindupControllerConfig 20 | // State of the AntiWindupController. 21 | State AntiWindupControllerState 22 | } 23 | 24 | // AntiWindupControllerConfig contains config parameters for a AntiWindupController. 25 | type AntiWindupControllerConfig struct { 26 | // ProportionalGain is the P part gain. 27 | ProportionalGain float64 28 | // IntegralGain is the I part gain. 29 | IntegralGain float64 30 | // DerivativeGain is the D part gain. 31 | DerivativeGain float64 32 | // AntiWindUpGain is the anti-windup tracking gain. 33 | AntiWindUpGain float64 34 | // IntegralDischargeTimeConstant is the time constant to discharge the integral state of the PID controller (s) 35 | IntegralDischargeTimeConstant float64 36 | // LowPassTimeConstant is the D part low-pass filter time constant => cut-off frequency 1/LowPassTimeConstant. 37 | LowPassTimeConstant time.Duration 38 | // MaxOutput is the max output from the PID. 39 | MaxOutput float64 40 | // MinOutput is the min output from the PID. 41 | MinOutput float64 42 | } 43 | 44 | // AntiWindupControllerState holds mutable state for a AntiWindupController. 45 | type AntiWindupControllerState struct { 46 | // ControlError is the difference between reference and current value. 47 | ControlError float64 48 | // ControlErrorIntegrand is the control error integrand, which includes the anti-windup correction. 49 | ControlErrorIntegrand float64 50 | // ControlErrorIntegral is the control error integrand integrated over time. 51 | ControlErrorIntegral float64 52 | // ControlErrorDerivative is the low-pass filtered time-derivative of the control error. 53 | ControlErrorDerivative float64 54 | // ControlSignal is the current control signal output of the controller. 55 | ControlSignal float64 56 | // UnsaturatedControlSignal is the control signal before saturation. 57 | UnsaturatedControlSignal float64 58 | } 59 | 60 | // AntiWindupControllerInput holds the input parameters to an AntiWindupController. 61 | type AntiWindupControllerInput struct { 62 | // ReferenceSignal is the reference value for the signal to control. 63 | ReferenceSignal float64 64 | // ActualSignal is the actual value of the signal to control. 65 | ActualSignal float64 66 | // FeedForwardSignal is the contribution of the feed-forward control loop in the controller output. 67 | FeedForwardSignal float64 68 | // SamplingInterval is the time interval elapsed since the previous call of the controller Update method. 69 | SamplingInterval time.Duration 70 | } 71 | 72 | // Reset the controller state. 73 | func (c *AntiWindupController) Reset() { 74 | c.State = AntiWindupControllerState{} 75 | } 76 | 77 | // Update the controller state. 78 | func (c *AntiWindupController) Update(input AntiWindupControllerInput) { 79 | e := input.ReferenceSignal - input.ActualSignal 80 | controlErrorIntegral := c.State.ControlErrorIntegrand*input.SamplingInterval.Seconds() + c.State.ControlErrorIntegral 81 | controlErrorDerivative := ((1/c.Config.LowPassTimeConstant.Seconds())*(e-c.State.ControlError) + 82 | c.State.ControlErrorDerivative) / (input.SamplingInterval.Seconds()/c.Config.LowPassTimeConstant.Seconds() + 1) 83 | c.State.UnsaturatedControlSignal = e*c.Config.ProportionalGain + c.Config.IntegralGain*controlErrorIntegral + 84 | c.Config.DerivativeGain*controlErrorDerivative + input.FeedForwardSignal 85 | c.State.ControlSignal = math.Max(c.Config.MinOutput, math.Min(c.Config.MaxOutput, c.State.UnsaturatedControlSignal)) 86 | c.State.ControlErrorIntegrand = e + c.Config.AntiWindUpGain*(c.State.ControlSignal-c.State.UnsaturatedControlSignal) 87 | c.State.ControlErrorIntegrand = math.Max(-math.MaxFloat64, math.Min(math.MaxFloat64, c.State.ControlErrorIntegrand)) 88 | c.State.ControlErrorIntegral = math.Max(-math.MaxFloat64, math.Min(math.MaxFloat64, controlErrorIntegral)) 89 | c.State.ControlErrorDerivative = math.Max(-math.MaxFloat64, math.Min(math.MaxFloat64, controlErrorDerivative)) 90 | c.State.ControlError = math.Max(-math.MaxFloat64, math.Min(math.MaxFloat64, e)) 91 | } 92 | 93 | // DischargeIntegral provides the ability to discharge the controller integral state 94 | // over a configurable period of time. 95 | func (c *AntiWindupController) DischargeIntegral(dt time.Duration) { 96 | c.State.ControlErrorIntegrand = 0.0 97 | c.State.ControlErrorIntegral = math.Max( 98 | 0, 99 | math.Min(1-dt.Seconds()/c.Config.IntegralDischargeTimeConstant, 1.0), 100 | ) * c.State.ControlErrorIntegral 101 | } 102 | -------------------------------------------------------------------------------- /antiwindupcontroller_test.go: -------------------------------------------------------------------------------- 1 | package pid 2 | 3 | import ( 4 | "math" 5 | "testing" 6 | "time" 7 | 8 | "gotest.tools/v3/assert" 9 | ) 10 | 11 | const ( 12 | dtTest = 10 * time.Millisecond 13 | deltaTest = 1e-3 14 | ) 15 | 16 | func TestAntiWindupController_PControllerUpdate(t *testing.T) { 17 | // Given a saturated P controller 18 | c := &AntiWindupController{ 19 | Config: AntiWindupControllerConfig{ 20 | LowPassTimeConstant: 1 * time.Second, 21 | ProportionalGain: 1, 22 | IntegralDischargeTimeConstant: 10, 23 | MinOutput: -10, 24 | MaxOutput: 10, 25 | }, 26 | } 27 | for _, tt := range []struct { 28 | input AntiWindupControllerInput 29 | expectedControlSignal float64 30 | }{ 31 | { 32 | input: AntiWindupControllerInput{ 33 | ReferenceSignal: 1.0, 34 | ActualSignal: 0.0, 35 | SamplingInterval: dtTest, 36 | }, 37 | expectedControlSignal: c.Config.ProportionalGain * 1.0, 38 | }, 39 | { 40 | input: AntiWindupControllerInput{ 41 | ReferenceSignal: 50.0, 42 | ActualSignal: 0.0, 43 | SamplingInterval: dtTest, 44 | }, 45 | expectedControlSignal: c.Config.MaxOutput, 46 | }, 47 | { 48 | input: AntiWindupControllerInput{ 49 | ReferenceSignal: -50.0, 50 | ActualSignal: 0.0, 51 | SamplingInterval: dtTest, 52 | }, 53 | expectedControlSignal: c.Config.MinOutput, 54 | }, 55 | } { 56 | tt := tt 57 | // When 58 | c.Update(tt.input) 59 | // Then the controller state should be the expected 60 | assert.Equal(t, tt.expectedControlSignal, c.State.ControlSignal) 61 | } 62 | } 63 | 64 | func TestAntiWindupController_PIDUpdate(t *testing.T) { 65 | // Given a saturated PID controller 66 | c := &AntiWindupController{ 67 | Config: AntiWindupControllerConfig{ 68 | LowPassTimeConstant: 1 * time.Second, 69 | DerivativeGain: 0.01, 70 | ProportionalGain: 1, 71 | IntegralGain: 10, 72 | AntiWindUpGain: 10, 73 | IntegralDischargeTimeConstant: 10, 74 | MinOutput: -10, 75 | MaxOutput: 10, 76 | }, 77 | } 78 | for _, tt := range []struct { 79 | input AntiWindupControllerInput 80 | expectedState AntiWindupControllerState 81 | }{ 82 | { 83 | input: AntiWindupControllerInput{ 84 | ReferenceSignal: 5.0, 85 | ActualSignal: 0.0, 86 | SamplingInterval: dtTest, 87 | }, 88 | expectedState: AntiWindupControllerState{ 89 | ControlError: 0.0, 90 | ControlSignal: 5.0, 91 | ControlErrorIntegrand: 0.0, 92 | ControlErrorDerivative: 0.0, 93 | }, 94 | }, 95 | } { 96 | tt := tt 97 | // When enough iterations have passed 98 | c.Reset() 99 | for i := 0; i < 500; i++ { 100 | c.Update(AntiWindupControllerInput{ 101 | ReferenceSignal: tt.input.ReferenceSignal, 102 | ActualSignal: c.State.ControlSignal, 103 | FeedForwardSignal: 0, 104 | SamplingInterval: tt.input.SamplingInterval, 105 | }) 106 | } 107 | // Then the controller I state only should give the expected output 108 | assert.Assert(t, math.Abs(tt.expectedState.ControlError-c.State.ControlError) < deltaTest) 109 | assert.Assert(t, math.Abs(tt.expectedState.ControlSignal-c.State.ControlSignal) < deltaTest) 110 | deltaI := math.Abs(tt.expectedState.ControlSignal - c.State.ControlErrorIntegral*c.Config.IntegralGain) 111 | assert.Assert(t, deltaI < deltaTest) 112 | assert.Assert(t, math.Abs(tt.expectedState.ControlErrorIntegrand-c.State.ControlErrorIntegrand) < deltaTest) 113 | deltaD := math.Abs(tt.expectedState.ControlErrorDerivative - c.State.ControlErrorDerivative*c.Config.DerivativeGain) 114 | assert.Assert(t, deltaD < deltaTest) 115 | } 116 | } 117 | 118 | func TestAntiWindupPID_FFUpdate(t *testing.T) { 119 | // Given a saturated I controller 120 | c := &AntiWindupController{ 121 | Config: AntiWindupControllerConfig{ 122 | LowPassTimeConstant: 1 * time.Second, 123 | IntegralGain: 10, 124 | IntegralDischargeTimeConstant: 10, 125 | MinOutput: -10, 126 | MaxOutput: 10, 127 | }, 128 | } 129 | for _, tt := range []struct { 130 | input AntiWindupControllerInput 131 | expectedState AntiWindupControllerState 132 | }{ 133 | { 134 | input: AntiWindupControllerInput{ 135 | ReferenceSignal: 5.0, 136 | ActualSignal: 0.0, 137 | FeedForwardSignal: 2.0, 138 | SamplingInterval: dtTest, 139 | }, 140 | expectedState: AntiWindupControllerState{ 141 | ControlError: 0.0, 142 | ControlSignal: 5.0, 143 | ControlErrorIntegral: 0.5 - 0.2, 144 | }, 145 | }, 146 | { 147 | input: AntiWindupControllerInput{ 148 | ReferenceSignal: 5.0, 149 | ActualSignal: 0.0, 150 | FeedForwardSignal: 15.0, 151 | SamplingInterval: dtTest, 152 | }, 153 | expectedState: AntiWindupControllerState{ 154 | ControlError: 0.0, 155 | ControlSignal: 5.0, 156 | ControlErrorIntegral: 0.5 - 1.5, 157 | }, 158 | }, 159 | } { 160 | tt := tt 161 | // When enough iterations have passed 162 | c.Reset() 163 | // Then the controller I state should compensate for difference between feed forward without 164 | // violating saturation constraints. 165 | for i := 0; i < 500; i++ { 166 | c.Update(AntiWindupControllerInput{ 167 | ReferenceSignal: tt.input.ReferenceSignal, 168 | ActualSignal: c.State.ControlSignal, 169 | FeedForwardSignal: tt.input.FeedForwardSignal, 170 | SamplingInterval: tt.input.SamplingInterval, 171 | }) 172 | assert.Assert(t, c.State.ControlSignal <= c.Config.MaxOutput) 173 | } 174 | assert.Assert(t, math.Abs(tt.expectedState.ControlError-c.State.ControlError) < deltaTest) 175 | assert.Assert(t, math.Abs(tt.expectedState.ControlSignal-c.State.ControlSignal) < deltaTest) 176 | assert.Assert(t, math.Abs(tt.expectedState.ControlErrorIntegral-c.State.ControlErrorIntegral) < deltaTest) 177 | } 178 | } 179 | 180 | func TestAntiWindupPID_NaN(t *testing.T) { 181 | // Given a saturated I controller with a low AntiWindUpGain 182 | c := &AntiWindupController{ 183 | Config: AntiWindupControllerConfig{ 184 | LowPassTimeConstant: 1 * time.Second, 185 | IntegralGain: 10, 186 | IntegralDischargeTimeConstant: 10, 187 | MinOutput: -10, 188 | MaxOutput: 10, 189 | AntiWindUpGain: 0.01, 190 | }, 191 | } 192 | for _, tt := range []struct { 193 | input AntiWindupControllerInput 194 | expectedState AntiWindupControllerState 195 | }{ 196 | { 197 | // Negative faulty measurement 198 | input: AntiWindupControllerInput{ 199 | ReferenceSignal: 5.0, 200 | ActualSignal: -math.MaxFloat64, 201 | FeedForwardSignal: 2.0, 202 | SamplingInterval: dtTest, 203 | }, 204 | }, 205 | { 206 | // Positive faulty measurement 207 | input: AntiWindupControllerInput{ 208 | ReferenceSignal: 5.0, 209 | ActualSignal: math.MaxFloat64, 210 | FeedForwardSignal: 2.0, 211 | SamplingInterval: dtTest, 212 | }, 213 | }, 214 | } { 215 | tt := tt 216 | // When enough iterations have passed 217 | c.Reset() 218 | for i := 0; i < 220; i++ { 219 | c.Update(AntiWindupControllerInput{ 220 | ReferenceSignal: tt.input.ReferenceSignal, 221 | ActualSignal: tt.input.ActualSignal, 222 | FeedForwardSignal: tt.input.FeedForwardSignal, 223 | SamplingInterval: tt.input.SamplingInterval, 224 | }) 225 | } 226 | // Then 227 | assert.Assert(t, !math.IsNaN(c.State.UnsaturatedControlSignal)) 228 | assert.Assert(t, !math.IsNaN(c.State.ControlSignal)) 229 | assert.Assert(t, !math.IsNaN(c.State.ControlErrorIntegral)) 230 | } 231 | } 232 | 233 | func TestAntiWindupController_Reset(t *testing.T) { 234 | // Given a AntiWindupPIDController with stored values not equal to 0 235 | c := &AntiWindupController{} 236 | c.State = AntiWindupControllerState{ 237 | ControlError: 5, 238 | ControlErrorIntegral: 5, 239 | ControlErrorDerivative: 5, 240 | ControlSignal: 5, 241 | ControlErrorIntegrand: 5, 242 | } 243 | // When resetting stored values 244 | c.Reset() 245 | // Then 246 | assert.Equal(t, AntiWindupControllerState{}, c.State) 247 | } 248 | 249 | func TestAntiWindupController_OffloadIntegralTerm(t *testing.T) { 250 | // Given a saturated PID controller 251 | c := &AntiWindupController{ 252 | Config: AntiWindupControllerConfig{ 253 | LowPassTimeConstant: 1 * time.Second, 254 | ProportionalGain: 1, 255 | DerivativeGain: 10, 256 | IntegralGain: 0.01, 257 | AntiWindUpGain: 0.5, 258 | IntegralDischargeTimeConstant: 10, 259 | MinOutput: -10, 260 | MaxOutput: 10, 261 | }, 262 | } 263 | c.State = AntiWindupControllerState{ 264 | ControlError: 5, 265 | ControlErrorIntegral: 1000, 266 | ControlErrorDerivative: 500, 267 | ControlSignal: 1, 268 | ControlErrorIntegrand: 10, 269 | } 270 | // When offloading the integral term 271 | c.DischargeIntegral(dtTest) 272 | expected := AntiWindupControllerState{ 273 | ControlError: 5, 274 | ControlErrorIntegrand: 0.0, 275 | ControlErrorIntegral: 999.0, 276 | ControlErrorDerivative: 500.0, 277 | ControlSignal: 1.0, 278 | } 279 | // Then 280 | assert.Equal(t, c.State, expected) 281 | } 282 | -------------------------------------------------------------------------------- /controller.go: -------------------------------------------------------------------------------- 1 | package pid 2 | 3 | import "time" 4 | 5 | // Controller implements a basic PID controller. 6 | type Controller struct { 7 | // Config for the Controller. 8 | Config ControllerConfig 9 | // State of the Controller. 10 | State ControllerState 11 | } 12 | 13 | // ControllerConfig contains configurable parameters for a Controller. 14 | type ControllerConfig struct { 15 | // ProportionalGain determines ratio of output response to error signal. 16 | ProportionalGain float64 17 | // IntegralGain determines previous error's affect on output. 18 | IntegralGain float64 19 | // DerivativeGain decreases the sensitivity to large reference changes. 20 | DerivativeGain float64 21 | } 22 | 23 | // ControllerState holds mutable state for a Controller. 24 | type ControllerState struct { 25 | // ControlError is the difference between reference and current value. 26 | ControlError float64 27 | // ControlErrorIntegral is the integrated control error over time. 28 | ControlErrorIntegral float64 29 | // ControlErrorDerivative is the rate of change of the control error. 30 | ControlErrorDerivative float64 31 | // ControlSignal is the current control signal output of the controller. 32 | ControlSignal float64 33 | } 34 | 35 | // ControllerInput holds the input parameters to a Controller. 36 | type ControllerInput struct { 37 | // ReferenceSignal is the reference value for the signal to control. 38 | ReferenceSignal float64 39 | // ActualSignal is the actual value of the signal to control. 40 | ActualSignal float64 41 | // SamplingInterval is the time interval elapsed since the previous call of the controller Update method. 42 | SamplingInterval time.Duration 43 | } 44 | 45 | // Update the controller state. 46 | func (c *Controller) Update(input ControllerInput) { 47 | previousError := c.State.ControlError 48 | c.State.ControlError = input.ReferenceSignal - input.ActualSignal 49 | c.State.ControlErrorDerivative = (c.State.ControlError - previousError) / input.SamplingInterval.Seconds() 50 | c.State.ControlErrorIntegral += c.State.ControlError * input.SamplingInterval.Seconds() 51 | c.State.ControlSignal = c.Config.ProportionalGain*c.State.ControlError + 52 | c.Config.IntegralGain*c.State.ControlErrorIntegral + 53 | c.Config.DerivativeGain*c.State.ControlErrorDerivative 54 | } 55 | 56 | // Reset the controller state. 57 | func (c *Controller) Reset() { 58 | c.State = ControllerState{} 59 | } 60 | -------------------------------------------------------------------------------- /controller_example_test.go: -------------------------------------------------------------------------------- 1 | package pid_test 2 | 3 | import ( 4 | "fmt" 5 | "time" 6 | 7 | "go.einride.tech/pid" 8 | ) 9 | 10 | func ExampleController() { 11 | // Create a PID controller. 12 | c := pid.Controller{ 13 | Config: pid.ControllerConfig{ 14 | ProportionalGain: 2.0, 15 | IntegralGain: 1.0, 16 | DerivativeGain: 1.0, 17 | }, 18 | } 19 | // Update the PID controller. 20 | c.Update(pid.ControllerInput{ 21 | ReferenceSignal: 10, 22 | ActualSignal: 0, 23 | SamplingInterval: 100 * time.Millisecond, 24 | }) 25 | fmt.Printf("%+v\n", c.State) 26 | // Reset the PID controller. 27 | c.Reset() 28 | fmt.Printf("%+v\n", c.State) 29 | // Output: 30 | // {ControlError:10 ControlErrorIntegral:1 ControlErrorDerivative:100 ControlSignal:121} 31 | // {ControlError:0 ControlErrorIntegral:0 ControlErrorDerivative:0 ControlSignal:0} 32 | } 33 | -------------------------------------------------------------------------------- /controller_test.go: -------------------------------------------------------------------------------- 1 | package pid 2 | 3 | import ( 4 | "testing" 5 | "time" 6 | 7 | "gotest.tools/v3/assert" 8 | ) 9 | 10 | func TestSpeedControl_ControlLoop_OutputIncrease(t *testing.T) { 11 | // Given a pidControl with reference value and update interval, dt 12 | pidControl := Controller{ 13 | Config: ControllerConfig{ 14 | ProportionalGain: 2.0, 15 | IntegralGain: 1.0, 16 | DerivativeGain: 1.0, 17 | }, 18 | } 19 | // Check output value when output increase is needed 20 | pidControl.Update(ControllerInput{ 21 | ReferenceSignal: 10, 22 | ActualSignal: 0, 23 | SamplingInterval: 100 * time.Millisecond, 24 | }) 25 | assert.Equal(t, float64(121), pidControl.State.ControlSignal) 26 | assert.Equal(t, float64(10), pidControl.State.ControlError) 27 | } 28 | 29 | func TestSpeedControl_ControlLoop_OutputDecrease(t *testing.T) { 30 | // Given a pidControl with reference output and update interval, dt 31 | pidControl := Controller{ 32 | Config: ControllerConfig{ 33 | ProportionalGain: 2.0, 34 | IntegralGain: 1.0, 35 | DerivativeGain: 1.0, 36 | }, 37 | } 38 | // Check output value when output value decrease is needed 39 | pidControl.Update(ControllerInput{ 40 | ReferenceSignal: 10, 41 | ActualSignal: 10, 42 | SamplingInterval: 100 * time.Millisecond, 43 | }) 44 | assert.Equal(t, float64(0), pidControl.State.ControlSignal) 45 | assert.Equal(t, float64(0), pidControl.State.ControlError) 46 | } 47 | 48 | func TestSimpleController_Reset(t *testing.T) { 49 | // Given a Controller with stored values not equal to 0 50 | c := &Controller{ 51 | Config: ControllerConfig{ 52 | ProportionalGain: 2.0, 53 | IntegralGain: 1.0, 54 | DerivativeGain: 1.0, 55 | }, 56 | State: ControllerState{ 57 | ControlErrorIntegral: 10, 58 | ControlErrorDerivative: 10, 59 | ControlError: 10, 60 | }, 61 | } 62 | // And a duplicate Controller with empty values 63 | expectedController := &Controller{ 64 | Config: ControllerConfig{ 65 | ProportionalGain: 2.0, 66 | IntegralGain: 1.0, 67 | DerivativeGain: 1.0, 68 | }, 69 | } 70 | // When resetting stored values 71 | c.Reset() 72 | // Then 73 | assert.Equal(t, expectedController.State, c.State) 74 | } 75 | -------------------------------------------------------------------------------- /doc.go: -------------------------------------------------------------------------------- 1 | // Package pid provides PID controllers for Go. 2 | package pid 3 | -------------------------------------------------------------------------------- /doc/pid-go.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/einride/pid-go/10e91b14526c38ec58a9224fe12059cace29d53e/doc/pid-go.png -------------------------------------------------------------------------------- /doc/pid-go.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 |
P
P
I
I
D
D
Σ
Σ
Σ
ΣViewer does not support full SVG 1.1 -------------------------------------------------------------------------------- /go.mod: -------------------------------------------------------------------------------- 1 | module go.einride.tech/pid 2 | 3 | go 1.17 4 | 5 | require gotest.tools/v3 v3.5.2 6 | 7 | require github.com/google/go-cmp v0.5.9 // indirect 8 | -------------------------------------------------------------------------------- /go.sum: -------------------------------------------------------------------------------- 1 | github.com/google/go-cmp v0.5.9 h1:O2Tfq5qg4qc4AmwVlvv0oLiVAGB7enBSJ2x2DqQFi38= 2 | github.com/google/go-cmp v0.5.9/go.mod h1:17dUlkBOakJ0+DkrSSNjCkIjxS6bF9zb3elmeNGIjoY= 3 | github.com/yuin/goldmark v1.4.13/go.mod h1:6yULJ656Px+3vBD8DxQVa3kxgyrAnzto9xy5taEt/CY= 4 | golang.org/x/crypto v0.0.0-20190308221718-c2843e01d9a2/go.mod h1:djNgcEr1/C05ACkg1iLfiJU5Ep61QUkGW8qpdssI0+w= 5 | golang.org/x/crypto v0.0.0-20210921155107-089bfa567519/go.mod h1:GvvjBRRGRdwPK5ydBHafDWAxML/pGHZbMvKqRZ5+Abc= 6 | golang.org/x/crypto v0.13.0/go.mod h1:y6Z2r+Rw4iayiXXAIxJIDAJ1zMW4yaTpebo8fPOliYc= 7 | golang.org/x/mod v0.6.0-dev.0.20220419223038-86c51ed26bb4/go.mod h1:jJ57K6gSWd91VN4djpZkiMVwK6gcyfeH4XE8wZrZaV4= 8 | golang.org/x/mod v0.8.0/go.mod h1:iBbtSCu2XBx23ZKBPSOrRkjjQPZFPuis4dIYUhu/chs= 9 | golang.org/x/mod v0.12.0/go.mod h1:iBbtSCu2XBx23ZKBPSOrRkjjQPZFPuis4dIYUhu/chs= 10 | golang.org/x/net v0.0.0-20190620200207-3b0461eec859/go.mod h1:z5CRVTTTmAJ677TzLLGU+0bjPO0LkuOLi4/5GtJWs/s= 11 | golang.org/x/net v0.0.0-20210226172049-e18ecbb05110/go.mod h1:m0MpNAwzfU5UDzcl9v0D8zg8gWTRqZa9RBIspLL5mdg= 12 | golang.org/x/net v0.0.0-20220722155237-a158d28d115b/go.mod h1:XRhObCWvk6IyKnWLug+ECip1KBveYUHfp+8e9klMJ9c= 13 | golang.org/x/net v0.6.0/go.mod h1:2Tu9+aMcznHK/AK1HMvgo6xiTLG5rD5rZLDS+rp2Bjs= 14 | golang.org/x/net v0.10.0/go.mod h1:0qNGK6F8kojg2nk9dLZ2mShWaEBan6FAoqfSigmmuDg= 15 | golang.org/x/net v0.15.0/go.mod h1:idbUs1IY1+zTqbi8yxTbhexhEEk5ur9LInksu6HrEpk= 16 | golang.org/x/sync v0.0.0-20190423024810-112230192c58/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM= 17 | golang.org/x/sync v0.0.0-20220722155255-886fb9371eb4/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM= 18 | golang.org/x/sync v0.1.0/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM= 19 | golang.org/x/sync v0.3.0/go.mod h1:FU7BRWz2tNW+3quACPkgCx/L+uEAv1htQ0V83Z9Rj+Y= 20 | golang.org/x/sys v0.0.0-20190215142949-d0b11bdaac8a/go.mod h1:STP8DvDyc/dI5b8T5hshtkjS+E42TnysNCUPdjciGhY= 21 | golang.org/x/sys v0.0.0-20201119102817-f84b799fce68/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= 22 | golang.org/x/sys v0.0.0-20210615035016-665e8c7367d1/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= 23 | golang.org/x/sys v0.0.0-20220520151302-bc2c85ada10a/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= 24 | golang.org/x/sys v0.0.0-20220722155257-8c9f86f7a55f/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= 25 | golang.org/x/sys v0.5.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= 26 | golang.org/x/sys v0.8.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= 27 | golang.org/x/sys v0.12.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= 28 | golang.org/x/term v0.0.0-20201126162022-7de9c90e9dd1/go.mod h1:bj7SfCRtBDWHUb9snDiAeCFNEtKQo2Wmx5Cou7ajbmo= 29 | golang.org/x/term v0.0.0-20210927222741-03fcf44c2211/go.mod h1:jbD1KX2456YbFQfuXm/mYQcufACuNUgVhRMnK/tPxf8= 30 | golang.org/x/term v0.5.0/go.mod h1:jMB1sMXY+tzblOD4FWmEbocvup2/aLOaQEp7JmGp78k= 31 | golang.org/x/term v0.8.0/go.mod h1:xPskH00ivmX89bAKVGSKKtLOWNx2+17Eiy94tnKShWo= 32 | golang.org/x/term v0.12.0/go.mod h1:owVbMEjm3cBLCHdkQu9b1opXd4ETQWc3BhuQGKgXgvU= 33 | golang.org/x/text v0.3.0/go.mod h1:NqM8EUOU14njkJ3fqMW+pc6Ldnwhi/IjpwHt7yyuwOQ= 34 | golang.org/x/text v0.3.3/go.mod h1:5Zoc/QRtKVWzQhOtBMvqHzDpF6irO9z98xDceosuGiQ= 35 | golang.org/x/text v0.3.7/go.mod h1:u+2+/6zg+i71rQMx5EYifcz6MCKuco9NR6JIITiCfzQ= 36 | golang.org/x/text v0.7.0/go.mod h1:mrYo+phRRbMaCq/xk9113O4dZlRixOauAjOtrjsXDZ8= 37 | golang.org/x/text v0.9.0/go.mod h1:e1OnstbJyHTd6l/uOt8jFFHp6TRDWZR/bV3emEE/zU8= 38 | golang.org/x/text v0.13.0/go.mod h1:TvPlkZtksWOMsz7fbANvkp4WM8x/WCo/om8BMLbz+aE= 39 | golang.org/x/tools v0.0.0-20180917221912-90fa682c2a6e/go.mod h1:n7NCudcB/nEzxVGmLbDWY5pfWTLqBcC2KZ6jyYvM4mQ= 40 | golang.org/x/tools v0.0.0-20191119224855-298f0cb1881e/go.mod h1:b+2E5dAYhXwXZwtnZ6UAqBI28+e2cm9otk0dWdXHAEo= 41 | golang.org/x/tools v0.1.12/go.mod h1:hNGJHUnrk76NpqgfD5Aqm5Crs+Hm0VOH/i9J2+nxYbc= 42 | golang.org/x/tools v0.6.0/go.mod h1:Xwgl3UAJ/d3gWutnCtw505GrjyAbvKui8lOU390QaIU= 43 | golang.org/x/tools v0.13.0/go.mod h1:HvlwmtVNQAhOuCjW7xxvovg8wbNq7LwfXh/k7wXUl58= 44 | golang.org/x/xerrors v0.0.0-20190717185122-a985d3407aa7/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0= 45 | gotest.tools/v3 v3.5.2 h1:7koQfIKdy+I8UTetycgUqXWSDwpgv193Ka+qRsmBY8Q= 46 | gotest.tools/v3 v3.5.2/go.mod h1:LtdLGcnqToBH83WByAAi/wiwSFCArdFIUV/xxN4pcjA= 47 | -------------------------------------------------------------------------------- /trackingcontroller.go: -------------------------------------------------------------------------------- 1 | package pid 2 | 3 | import ( 4 | "math" 5 | "time" 6 | ) 7 | 8 | // TrackingController implements a PID-controller with low-pass filter of the derivative term, 9 | // feed forward term, anti-windup and bumpless transfer using tracking mode control. 10 | // 11 | // The anti-windup and bumpless transfer mechanisms use a tracking mode as defined in 12 | // Chapter 6 of Åström and Murray, Feedback Systems: 13 | // An Introduction to Scientists and Engineers, 2008 14 | // (http://www.cds.caltech.edu/~murray/amwiki) 15 | // 16 | // The ControlError, ControlErrorIntegrand, ControlErrorIntegral and ControlErrorDerivative are prevented 17 | // from reaching +/- inf by clamping them to [-math.MaxFloat64, math.MaxFloat64]. 18 | type TrackingController struct { 19 | // Config for the TrackingController. 20 | Config TrackingControllerConfig 21 | // State of the TrackingController. 22 | State TrackingControllerState 23 | } 24 | 25 | // TrackingControllerConfig contains configurable parameters for a TrackingController. 26 | type TrackingControllerConfig struct { 27 | // ProportionalGain is the P part gain. 28 | ProportionalGain float64 29 | // IntegralGain is the I part gain. 30 | IntegralGain float64 31 | // DerivativeGain is the D part gain. 32 | DerivativeGain float64 33 | // AntiWindUpGain is the anti-windup tracking gain. 34 | AntiWindUpGain float64 35 | // IntegralDischargeTimeConstant is the time constant to discharge the integral state of the PID controller (s) 36 | IntegralDischargeTimeConstant float64 37 | // LowPassTimeConstant is the D part low-pass filter time constant => cut-off frequency 1/LowPassTimeConstant. 38 | LowPassTimeConstant time.Duration 39 | // MaxOutput is the max output from the PID. 40 | MaxOutput float64 41 | // MinOutput is the min output from the PID. 42 | MinOutput float64 43 | } 44 | 45 | // TrackingControllerState holds the mutable state a TrackingController. 46 | type TrackingControllerState struct { 47 | // ControlError is the difference between reference and current value. 48 | ControlError float64 49 | // ControlErrorIntegrand is the integrated control error over time. 50 | ControlErrorIntegrand float64 51 | // ControlErrorIntegral is the control error integrand integrated over time. 52 | ControlErrorIntegral float64 53 | // ControlErrorDerivative is the low-pass filtered time-derivative of the control error. 54 | ControlErrorDerivative float64 55 | // ControlSignal is the current control signal output of the controller. 56 | ControlSignal float64 57 | // UnsaturatedControlSignal is the control signal before saturation used for tracking the 58 | // actual control signal for bumpless transfer or compensation of un-modeled saturations. 59 | UnsaturatedControlSignal float64 60 | } 61 | 62 | // TrackingControllerInput holds the input parameters to a TrackingController. 63 | type TrackingControllerInput struct { 64 | // ReferenceSignal is the reference value for the signal to control. 65 | ReferenceSignal float64 66 | // ActualSignal is the actual value of the signal to control. 67 | ActualSignal float64 68 | // FeedForwardSignal is the contribution of the feed-forward control loop in the controller output. 69 | FeedForwardSignal float64 70 | // AppliedControlSignal is the actual control command applied by the actuator. 71 | AppliedControlSignal float64 72 | // SamplingInterval is the time interval elapsed since the previous call of the controller Update method. 73 | SamplingInterval time.Duration 74 | } 75 | 76 | // Reset the controller state. 77 | func (c *TrackingController) Reset() { 78 | c.State = TrackingControllerState{} 79 | } 80 | 81 | // Update the controller state. 82 | func (c *TrackingController) Update(input TrackingControllerInput) { 83 | e := input.ReferenceSignal - input.ActualSignal 84 | controlErrorIntegral := c.State.ControlErrorIntegrand*input.SamplingInterval.Seconds() + c.State.ControlErrorIntegral 85 | controlErrorDerivative := ((1/c.Config.LowPassTimeConstant.Seconds())*(e-c.State.ControlError) + 86 | c.State.ControlErrorDerivative) / (input.SamplingInterval.Seconds()/c.Config.LowPassTimeConstant.Seconds() + 1) 87 | c.State.UnsaturatedControlSignal = e*c.Config.ProportionalGain + c.Config.IntegralGain*controlErrorIntegral + 88 | c.Config.DerivativeGain*controlErrorDerivative + input.FeedForwardSignal 89 | c.State.ControlSignal = math.Max(c.Config.MinOutput, math.Min(c.Config.MaxOutput, c.State.UnsaturatedControlSignal)) 90 | c.State.ControlErrorIntegrand = e + c.Config.AntiWindUpGain*(input.AppliedControlSignal- 91 | c.State.UnsaturatedControlSignal) 92 | c.State.ControlErrorIntegrand = math.Max(-math.MaxFloat64, math.Min(math.MaxFloat64, c.State.ControlErrorIntegrand)) 93 | c.State.ControlErrorIntegral = math.Max(-math.MaxFloat64, math.Min(math.MaxFloat64, controlErrorIntegral)) 94 | c.State.ControlErrorDerivative = math.Max(-math.MaxFloat64, math.Min(math.MaxFloat64, controlErrorDerivative)) 95 | c.State.ControlError = math.Max(-math.MaxFloat64, math.Min(math.MaxFloat64, e)) 96 | } 97 | 98 | // DischargeIntegral provides the ability to discharge the controller integral state 99 | // over a configurable period of time. 100 | func (c *TrackingController) DischargeIntegral(dt time.Duration) { 101 | c.State.ControlErrorIntegrand = 0.0 102 | c.State.ControlErrorIntegral = math.Max( 103 | 0, 104 | math.Min(1-dt.Seconds()/c.Config.IntegralDischargeTimeConstant, 1.0), 105 | ) * c.State.ControlErrorIntegral 106 | } 107 | -------------------------------------------------------------------------------- /trackingcontroller_test.go: -------------------------------------------------------------------------------- 1 | package pid 2 | 3 | import ( 4 | "math" 5 | "testing" 6 | "time" 7 | 8 | "gotest.tools/v3/assert" 9 | ) 10 | 11 | func TestTrackingController_PControllerUpdate(t *testing.T) { 12 | // Given a tracking P controller 13 | c := &TrackingController{ 14 | Config: TrackingControllerConfig{ 15 | LowPassTimeConstant: 1 * time.Second, 16 | ProportionalGain: 1, 17 | AntiWindUpGain: 0, 18 | IntegralDischargeTimeConstant: 10, 19 | MinOutput: -10, 20 | MaxOutput: 10, 21 | }, 22 | } 23 | for _, tt := range []struct { 24 | input TrackingControllerInput 25 | expectedState TrackingControllerState 26 | }{ 27 | { 28 | input: TrackingControllerInput{ 29 | ReferenceSignal: 1.0, 30 | ActualSignal: 0.0, 31 | SamplingInterval: dtTest, 32 | }, 33 | expectedState: TrackingControllerState{ 34 | ControlError: 1.0, 35 | ControlErrorIntegrand: 1.0, 36 | ControlErrorIntegral: 0.0, 37 | ControlErrorDerivative: 1.0 / (dtTest.Seconds() + c.Config.LowPassTimeConstant.Seconds()), 38 | ControlSignal: 1.0, 39 | UnsaturatedControlSignal: 1.0, 40 | }, 41 | }, 42 | { 43 | input: TrackingControllerInput{ 44 | ReferenceSignal: 50.0, 45 | ActualSignal: 0.0, 46 | SamplingInterval: dtTest, 47 | }, 48 | expectedState: TrackingControllerState{ 49 | ControlError: 50.0, 50 | ControlErrorIntegrand: 50.0, 51 | ControlErrorIntegral: 0.0, 52 | ControlErrorDerivative: 50.0 / (dtTest.Seconds() + c.Config.LowPassTimeConstant.Seconds()), 53 | ControlSignal: 10.0, 54 | UnsaturatedControlSignal: 50.0, 55 | }, 56 | }, 57 | { 58 | input: TrackingControllerInput{ 59 | ReferenceSignal: -50.0, 60 | ActualSignal: 0.0, 61 | SamplingInterval: dtTest, 62 | }, 63 | expectedState: TrackingControllerState{ 64 | ControlError: -50.0, 65 | ControlErrorIntegrand: -50.0, 66 | ControlErrorIntegral: 0.0, 67 | ControlErrorDerivative: -50.0 / (dtTest.Seconds() + c.Config.LowPassTimeConstant.Seconds()), 68 | ControlSignal: -10.0, 69 | UnsaturatedControlSignal: -50.0, 70 | }, 71 | }, 72 | } { 73 | tt := tt 74 | // When 75 | c.Update(tt.input) 76 | // Then the controller state should be the expected 77 | assert.Equal(t, tt.expectedState, c.State) 78 | c.Reset() 79 | } 80 | } 81 | 82 | func TestTrackingController_NaN(t *testing.T) { 83 | // Given a saturated I controller with a low AntiWindUpGain 84 | c := &TrackingController{ 85 | Config: TrackingControllerConfig{ 86 | LowPassTimeConstant: 1 * time.Second, 87 | IntegralGain: 10, 88 | IntegralDischargeTimeConstant: 10, 89 | MinOutput: -10, 90 | MaxOutput: 10, 91 | AntiWindUpGain: 0.01, 92 | }, 93 | } 94 | for _, tt := range []struct { 95 | input TrackingControllerInput 96 | expectedState TrackingControllerState 97 | }{ 98 | { 99 | // Negative faulty measurement 100 | input: TrackingControllerInput{ 101 | ReferenceSignal: 5.0, 102 | ActualSignal: -math.MaxFloat64, 103 | FeedForwardSignal: 2.0, 104 | SamplingInterval: dtTest, 105 | }, 106 | }, 107 | { 108 | // Positive faulty measurement 109 | input: TrackingControllerInput{ 110 | ReferenceSignal: 5.0, 111 | ActualSignal: math.MaxFloat64, 112 | FeedForwardSignal: 2.0, 113 | SamplingInterval: dtTest, 114 | }, 115 | }, 116 | } { 117 | tt := tt 118 | // When enough iterations have passed 119 | c.Reset() 120 | for i := 0; i < 220; i++ { 121 | c.Update(TrackingControllerInput{ 122 | ReferenceSignal: tt.input.ReferenceSignal, 123 | ActualSignal: tt.input.ActualSignal, 124 | FeedForwardSignal: tt.input.FeedForwardSignal, 125 | SamplingInterval: tt.input.SamplingInterval, 126 | }) 127 | } 128 | // Then 129 | assert.Assert(t, !math.IsNaN(c.State.UnsaturatedControlSignal)) 130 | assert.Assert(t, !math.IsNaN(c.State.ControlSignal)) 131 | assert.Assert(t, !math.IsNaN(c.State.ControlErrorIntegral)) 132 | } 133 | } 134 | 135 | func TestTrackingController_Reset(t *testing.T) { 136 | // Given a SaturatedPIDController with stored values not equal to 0 137 | c := &TrackingController{} 138 | c.State = TrackingControllerState{ 139 | ControlError: 5, 140 | ControlErrorIntegral: 5, 141 | ControlErrorDerivative: 5, 142 | ControlSignal: 5, 143 | ControlErrorIntegrand: 5, 144 | } 145 | // When resetting stored values 146 | c.Reset() 147 | // Then 148 | assert.Equal(t, TrackingControllerState{}, c.State) 149 | } 150 | 151 | func TestTrackingController_OffloadIntegralTerm(t *testing.T) { 152 | // Given a tracking PID controller 153 | c := &TrackingController{ 154 | Config: TrackingControllerConfig{ 155 | LowPassTimeConstant: 1 * time.Second, 156 | ProportionalGain: 1, 157 | DerivativeGain: 10, 158 | IntegralGain: 0.01, 159 | AntiWindUpGain: 0.5, 160 | IntegralDischargeTimeConstant: 10, 161 | MinOutput: -10, 162 | MaxOutput: 10, 163 | }, 164 | } 165 | c.State = TrackingControllerState{ 166 | ControlError: 5, 167 | ControlErrorIntegral: 100000, 168 | ControlErrorDerivative: 50, 169 | ControlSignal: 1, 170 | ControlErrorIntegrand: 10, 171 | } 172 | // When offloading the integral term 173 | c.DischargeIntegral(dtTest) 174 | // Then 175 | expected := TrackingControllerState{ 176 | ControlError: 5, 177 | ControlErrorIntegrand: 0.0, 178 | ControlErrorIntegral: 99900.0, 179 | ControlErrorDerivative: 50.0, 180 | ControlSignal: 1.0, 181 | } 182 | assert.Equal(t, expected, c.State) 183 | } 184 | --------------------------------------------------------------------------------