spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Plots 

Usage: "plot" ["noise|"override"|"ramp"]
last_actuators
ClockeNessMnstr 2022-02-18 19:35:43 -05:00 committed by Harald Schafer
parent 196ec00f1d
commit 079ee5bccd
1 changed files with 94 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
selfdrive/controls/lib/tests/test_pid.py
View File

@ -1,6 +1,7 @@
#!/usr/bin/env python3
import random
import unittest
import sys
from common.numpy_fast import clip, interp
from common.filter_simple import FirstOrderFilter
@ -31,8 +32,40 @@ noise_intensity = 10 # noise = rand() * noise_intensity
def ratelimit(new, last):
return clip(new, last - rate_limit, last + rate_limit)
if __name__ == "__main__":
debug = False
plot = False
debug_noise = False
debug_override = False
debug_ramp = False
while len(sys.argv) > 1:
if "noise" in sys.argv:
debug_noise = True
if "override" in sys.argv:
debug_override = True
if "ramp" in sys.argv:
debug_ramp = True
if "plot" in sys.argv:
plot = True
if "debug" in sys.argv:
debug = True
if plot and not (debug_noise or debug_override or debug_ramp):
debug_noise = True
debug_override = True
debug_ramp = True
if (plot):
num_divs = 4 # (2*n + 1)**2 graphs (81 graphs)
_p = [[0, num_divs, 2*num_divs],[1/gain_range, 1, gain_range]]
#remove args so unittest is not freaked out
sys.argv.pop()
if plot:
import matplotlib.pyplot as plt
class TestPID(unittest.TestCase):
def test_error_noise(self):
self.assertFalse((debug_ramp or debug_override) and not debug_noise, msg="Test skipped for debugging")
self.assertFalse(noise_intensity < 5, msg="Noise not disastrous enough")
for i in range(0, 2*num_divs + 1):
for j in range (0, 2*num_divs + 1):
@ -52,7 +85,17 @@ class TestPID(unittest.TestCase):
pid_c_control = 0
pid_r_control = 0
last_pid_r_control = 0
if plot and debug_noise:
x = []
y = []
y2 = []
y3 = []
for t in range(0, int(4*sec*rate)):
if plot and debug_noise:
x.append(t/rate)
y.append(output_c.x)
y2.append(output_n.x)
y3.append(output_r.x)
target = 0
if (t > start_offset*rate):
target = step_size * limit
@ -76,11 +119,21 @@ class TestPID(unittest.TestCase):
ss_error += ((output_c.x - output_n.x) - ss_error) / (t+1)
ss_error += ((output_r.x - output_n.x) - ss_error) / (t+1)
# assert that moving average of SS error is falling to zero
# threshold base on time and error_intensity. multiply runtime by intensity or divide error by intensity
self.assertFalse(abs(ss_error) > 0.1 * noise_intensity * limit / rate, msg=f"Possible SS_error: {ss_error} detected in controller due to noise")
if plot and debug_noise and not debug or (plot and abs(ss_error) > 0.1 * noise_intensity * limit / rate):
plt.plot(x, y, 'k', label="No Noise")
plt.plot(x, y2, label="Compensating")
plt.plot(x, y3, label="Not Compensating")
plt.title(f"Noise Test: P = {kp:5.3}, I = {ki:5.3}")
plt.legend()
plt.show()
if not debug:
# assert that moving average of SS error is falling to zero
# threshold base on time and error_intensity. multiply runtime by intensity or divide error by intensity
self.assertFalse(abs(ss_error) > 0.1 * noise_intensity * limit / rate, msg=f"Possible SS_error: {ss_error} detected in controller due to noise")
def test_override(self):
self.assertFalse((debug_ramp or debug_noise) and not debug_override, msg="Test skipped for debugging")
for i in range(0, 2*num_divs + 1):
for j in range (0, 2*num_divs + 1):
@ -95,7 +148,15 @@ class TestPID(unittest.TestCase):
pid_n_control = 0
pid_r_control = 0
sum_overshoot_error = 0
if plot and debug_override:
x = []
y = []
y2 = []
for t in range(0, int(sec*rate)):
if plot and debug_override:
x.append(t/rate)
y.append(output_n.x)
y2.append(output_r.x)
target = 0
if (t > start_offset*rate):
target = step_size * limit
@ -113,11 +174,20 @@ class TestPID(unittest.TestCase):
# only sum up overshoot in excess of the allowance of both the unclamped signal and the target.
if output_n.x > target or output_r.x > target:
sum_overshoot_error += min(max(abs(output_r.x - output_n.x)-output_r.x*(overshoot_allowance), 0), max(abs(target - output_n.x)-target*(overshoot_allowance), 0)) / rate
# assert that the overshoot characteristics of a new input response and releasing the controller on an overriden input are the same
self.assertFalse(sum_overshoot_error/target > overshoot_allowance, msg=f"Exessive Overshoot measured after override. Exeeded {100*overshoot_allowance}% : {100*sum_overshoot_error/target}% ")
if plot and debug_override and not debug or (plot and sum_overshoot_error/target > overshoot_allowance):
plt.plot(x, y, label="override")
plt.plot(x, y2, label="new input")
plt.title(f"Override Test: P = {kp:5.3}, I = {ki:5.3}")
plt.legend()
plt.show()
if not debug:
# assert that the overshoot characteristics of a new input response and releasing the controller on an overriden input are the same
self.assertFalse(sum_overshoot_error/target > overshoot_allowance, msg=f"Exessive Overshoot measured after override. Exeeded {100*overshoot_allowance}% : {100*sum_overshoot_error/target}% ")
def test_ramp_up(self):
self.assertFalse((debug_override or debug_noise) and not debug_ramp, msg="Test skipped for debugging")
for i in range(0, 2*num_divs + 1):
for j in range (0, 2*num_divs + 1):
@ -133,16 +203,22 @@ class TestPID(unittest.TestCase):
pid_r_control = 0
last_pid_r_control = 0
sum_overshoot_error = 0
if plot and debug_ramp:
x = []
y = []
y2 = []
for t in range(0, int(sec*rate)):
if plot and debug_ramp:
x.append(t/rate)
y.append(output_n.x)
y2.append(output_r.x)
target = 0
if (t > start_offset * rate):
target = step_size * limit
pid_n_control = ratelimit(pid_n.update(target, output_n.x, last_output=pid_n_control), pid_n_control)
pid_r_control = pid_r.update(target, output_r.x, last_output=pid_r_control)
last_pid_r_control = ratelimit(pid_r_control, last_pid_r_control)
output_n.update(pid_n_control)
@ -152,8 +228,16 @@ class TestPID(unittest.TestCase):
if output_r.x > target or output_n.x > target:
sum_overshoot_error += ((output_n.x - output_r.x) + max(target - output_n.x, 0)) / rate
# assert that the overshoot characteristics of a slew rate limit are improved or equal with compensation on
self.assertFalse(sum_overshoot_error/target > overshoot_allowance, msg=f"Rate limit compensation caused excessive damping or instability. {100*sum_overshoot_error/target}% additional error from target")
if plot and debug_ramp and not debug or (plot and sum_overshoot_error/target > overshoot_allowance):
plt.plot(x, y, label="Compensating")
plt.plot(x, y2, label="Not Compensating")
plt.title(f"Slew Rate Limit Test: P = {kp:5.3}, I = {ki:5.3}")
plt.legend()
plt.show()
if not debug:
# assert that the overshoot characteristics of a slew rate limit are improved or equal with compensation on
self.assertFalse(sum_overshoot_error/target > overshoot_allowance, msg=f"Rate limit compensation caused excessive damping or instability. {100*sum_overshoot_error/target}% additional error from target")
if __name__ == "__main__":
unittest.main()