add limits calculation and helper script to plot the frequency response

py/stabilizer/iir_coefficients.py

@@ -112,14 +112,22 @@ def get_filters():
                       arguments=[
                           add_argument("--Kii", default=0, type=float,
                                        help="Double Integrator (I^2) gain"),
+                          add_argument("--Kii_limit", default=float('inf'), type=float,
+                                       help="Integral gain limit"),
                           add_argument("--Ki", default=0, type=float,
                                        help="Integrator (I) gain"),
+                          add_argument("--Ki_limit", default=float('inf'), type=float,
+                                       help="Integral gain limit"),
                           add_argument("--Kp", default=0, type=float,
                                        help="Proportional (P) gain"),
                           add_argument("--Kd", default=0, type=float,
                                        help="Derivative (D) gain"),
+                          add_argument("--Kd_limit", default=float('inf'), type=float,
+                                       help="Derivative gain limit"),
                           add_argument("--Kdd", default=0, type=float,
                                        help="Double Derivative (D^2) gain"),
+                          add_argument("--Kdd_limit", default=float('inf'), type=float,
+                                       help="Derivative gain limit"),
                       ],
                       coefficients=pid_coefficients),
     }
@@ -179,12 +187,12 @@ def pid_coefficients(args):
 
     # Determine filter order
     if args.Kii != 0:
-        assert (args.Kdd, args.Kd) == (0, 0), \
-                "IIR filters I^2 and D or D^2 gain are unsupported"
+        assert (args.Kdd, args.Kd, args.Kdd_limit, args.Kd_limit) == (0, 0, float('inf'), float('inf')), \
+            "IIR filters I^2 and D or D^2 gain/limit are unsupported"
         order = 2
     elif args.Ki != 0:
-        assert args.Kdd == 0, \
-                "IIR filters with I and D^2 gain are unsupported"
+        assert (args.Kdd, args.Kdd_limit) == (0, float('inf')), \
+            "IIR filters with I and D^2 gain/limit are unsupported"
         order = 1
     else:
         order = 0
@@ -196,12 +204,16 @@ def pid_coefficients(args):
     ]
 
     gains = [args.Kii, args.Ki, args.Kp, args.Kd, args.Kdd]
+    limits = [args.Kii/args.Kii_limit, args.Ki/args.Ki_limit,
+              1, args.Kd / args.Kd_limit, args.Kdd / args.Kdd_limit]
     w = 2*pi*args.sample_period
     b = [sum(gains[2 - order + i] * w**(order - i) * kernels[i][j]
              for i in range(3)) for j in range(3)]
 
-    # Normalization is redundant because a0 is 1 in all cases.
-    a = kernels[order]
+    a = [sum(limits[2 - order + i] * w**(order - i) * kernels[i][j]
+             for i in range(3)) for j in range(3)]
+    b = [i/a[0] for i in b]
+    a = [i/a[0] for i in a]
     assert a[0] == 1
     return b + [-ai for ai in a[1:]]
 

py/stabilizer/plot_iir_frequency_response.py

@@ -0,0 +1,70 @@
+#!/usr/bin/python3
+"""
+Small tool to show the frequency response of biquad IIR filters.
+"""
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy import signal
+import argparse
+
+from iir_coefficients import get_filters
+import stabilizer
+
+
+def _main():
+    parser = argparse.ArgumentParser(
+        description="Plot frequency response for filter parameters"
+    )
+    parser.add_argument(
+        "--sample-period",
+        "-s",
+        type=float,
+        default=stabilizer.SAMPLE_PERIOD,
+        help="Sample period in seconds (%(default)s s)",
+    )
+
+    # Next, add subparsers and their arguments.
+    subparsers = parser.add_subparsers(
+        help="Filter-specific design parameters", dest="filter_type", required=True
+    )
+
+    filters = get_filters()
+
+    for (filter_name, filt) in filters.items():
+        subparser = subparsers.add_parser(filter_name, help=filt.help)
+        for arg in filt.arguments:
+            subparser.add_argument(*arg.positionals, **arg.keywords)
+
+    args = parser.parse_args()
+
+    # Calculate the IIR coefficients for the filter.
+    coefficients = filters[args.filter_type].coefficients(args)
+
+    print(coefficients)
+
+    # The feed-forward gain of the IIR filter is the summation
+    # of the "b" components of the filter.
+    forward_gain = sum(coefficients[:3])
+    if forward_gain == 0 and args.x_offset != 0:
+        print("Filter has no DC gain but x_offset is non-zero")
+
+    f = np.logspace(-7, np.log10(0.5 / args.sample_period), 1024, endpoint=False)
+    f, h = signal.freqz(
+        coefficients[:3],
+        [1] + [-c for c in coefficients[3:]],
+        worN=f,
+        fs=1 / args.sample_period,
+    )
+    fig, ax = plt.subplots()
+    ax.margins(0, 0.1)
+    ax.plot(f, 20 * np.log10(abs(h)))
+    ax.set_xscale("log")
+    ax.grid()
+    ax.set_xlabel("Frequency (Hz)")
+    ax.set_ylabel("Magnitude (dB)")
+    ax.set_title("Filter response")
+    plt.show()
+
+
+if __name__ == "__main__":
+    _main()