Attempted loop shaping

Author: sachinumans

SysReg_crashcourse_FreqDomain_BlockB.py

@@ -940,12 +940,16 @@ def animate(t):
 # 
 # | Frequencies | $C(s)$  | $\mid S(s)\mid$ |
 # | --------    | ------- | --------        |
-# | Low         | -       | Low gain ensures disturbance rejection            |
+# | Low         | High gain ensures unscaled tracking       | Low gain ensures disturbance rejection            |
 # | Cross-over  | Ensure good GM and PM     | Ensure good SM             |
 # | High        | Low gain ensures measurement noise rejection      | -            |
 # 
+# That unscaled tracking cell confused me slightly, so briefly why: the transfer function $G_{yr} = TF = \frac{L}{1+L}F$. Ignoring $F$ for a second, $\lim_{|L|\rightarrow\infty}\frac{L}{1+L} = 1$, meaning there 
+# is unitary gain for constant input, i.e. unscaled reference tracking.
+# 
 # ## Easier said...
-# So lets run an example and you'll understand better (I hope). We start with a unitary feedback controller and see how we're doing and what plant we have.
+# So lets run an example and you'll understand better (I hope). This will be very much a Plato-style of explanation (coincidentally my favourite style). We start with a unitary 
+# feedback controller and see what plant we're dealing with.
 # 
 # %%
 %matplotlib notebook
@@ -973,18 +977,99 @@ def animate(t):
 
 ###############################################
 SYS = loopShaper()
-fig = plt.figure(figsize=[15, 6])
-gs = GridSpec(3,2, figure=fig)
-ax = [fig.add_subplot(a) for a in [gs[0,0], gs[1, 0], gs[2, 0], gs[:, 1]]]
+fig = plt.figure(figsize=[15, 8])
+gs = GridSpec(4,2, figure=fig)
+ax = [fig.add_subplot(a) for a in [gs[0,0], gs[1, 0], gs[2, 0], gs[:3, 1], gs[3,:]]]
+
+SYS.plot_LS(ax)
+display(fig)
+
+# %% [markdown]
+# What should you get from this plot? Well, lets start with the crossover frequencies: they're around 30 or 300 rad/s. The loop transfers below and above have a low gain. Our margins are looking good however 
+# and we can gain some performance for sure. Lets work low to high frequencies. For the lower frequencies our sensitivity needs low gains, so our loop needs high gains, also ensuring good tracking. 
+# It doesn't, so lets add an integrator:
+
+# %%
+SYS.Cpoles = [0]
+SYS.OM = np.logspace(-5, 5, 1000)
 
+[a.cla() for a in ax]
 SYS.plot_LS(ax)
 display(fig)
 
+# %% [markdown]
+# We need a bit more downslope around 1E-3 rad/s, i.e. more poles:
+
+# %%
+SYS.Cpoles = [0, -1e-3]
+
+[a.cla() for a in ax]
+SYS.plot_LS(ax)
+display(fig)
+
+# %% [markdown]
+# Okaayyy, lower frequencies looking good. Zooming in on the crossover frequency next:
+
+# %%
+[a.set_xlim([1e-2, 1e2]) for a in ax[:3]]
+ax[0].set_ylim([1e-3, 5e1])
+display(fig)
+
+# %% [markdown]
+# Notes: our PM is very large, as well as our GM. In this case we can decrease both of these with the gain. Lets aim for $35^\circ$ PM, looking at the phase plot that happens at 50 or so rad/s, the 
+# magnitude there now is $5E-4$. *Therefore*, our gain can be $2E3$:
+
+# %%
+SYS.Cgain = 2e2
+
+[a.cla() for a in ax]
+SYS.plot_LS(ax)
+display(fig, np.pi/3)
+
+# %% [markdown]
+# Honestly I suck at loop shaping.
+# 
+# ## Feedforward control
+# We've been ignoring the feedforward block up to now, but really it's very powerful when you can measure the output noise. 
+# Consider when there's no input noise $d$, which is often the case, and we add an extra feedforward control $u_\text{ff}$ 
+# such that $u = Ce + u_\text{ff}$. Now append/recall $y = TFr + PSu_\text{ff} + Sn$
+# Then with the measurement of the output noise $n$, define $u_\text{ff} = P^{-1}Fr - P^{-1}n$
+# $$ y = (1- S)Fr + PSu_\text{ff} + Sn  = Fr - SFr + PS(P^{-1}Fr - P^{-1}) + Sn = Fr - SFr + PSP^{-1}Fr - PSP^{-1}n + Sn $$
+# $$ = Fr - SFr + SFr - Sn + Sn = Fr.$$
+# This means you have perfect tracking. However, inverting the plant might not always be possible (non minimum phase systems for example). 
+# There are more tricks like this when you have knowledge of your disturbances.
 
 # %% [markdown]
 # # Fundamental Limitations
-# I'm so sorry, but everything we've done is technically speaking bachelor level control engineering. Now that we're nearing the end of the course content, we run into problems that we ignored before.
+# ## Waterbed effect
+# The funny thing about control is, the most important proofs are the proofs that say that something 
+# is **impossible**. These are the bounds you cannot circumvent. Bode's integral formula, otherwise known as the waterbed effedct,
+#  is one of these. It is defined as: assume L has relative degree $geq$ 2 and $N_p$ RHP-poles $p_i$, then
+# $$ \int_0^\infty\log|S(i\omega)|\text{d}\omega = \pi\sum_{i=1}^{N_p} \mathfrak{Re}(p_i).$$
+# This is somewhat reminiscent of the Nyquist plot. This integral is basically the integral of the vector going from -1 to the Nyquist plot. 
+# Iff this integral makes a half ellips on the onesided Nyquist contour, there is an encirclement, there is a RHP-pole.
+# 
+# What it's also saying is: you can never change the value of that integral (assuming you don't add RHP poles). Therefore, if you change C 
+# to take some magnitude in S away, it has to come back at other frequencies.
+# 
+# ## Time delays
+# Time delays are annoying, because they put a hard limit on your bandwidth. Why? Well, a time delay $e^{-\theta s}$, has an idealized $T(s)=e^{-\theta s}$. 
+#  The bandwidth is defined as $|T(i\omega_\text{B})| = \frac{1}{\sqrt2}$. Also 
+# $$|S + T| = 1 \leq |S| + |T| \rightarrow |S(i\omega_\text{B})| + \frac{1}{\sqrt2} \geq 1 \rightarrow |S(i\omega_\text{B})|  \geq 1 - \frac12\sqrt2$$
+# $$ S = 1 - e^{-\theta s} = 1 - \cos(-\theta\omega) - i\sin(-\theta\omega) \rightarrow |S| = \sqrt{ (1 - \cos(-\theta\omega)^2 +  \sin(-\theta\omega)^2}$$
+# $$ |S| = 1 \rightarrow  (1 - \cos(-\theta\omega)^2 +  \sin(-\theta\omega)^2 = 1$$
+# $$ (1 - \cos(-\theta\omega)^2 +  \sin(-\theta\omega)^2 = 1 = 1 - 2\cos(-\theta\omega) + \cos(-\theta\omega)^2 +  \sin(-\theta\omega)^2 $$
+# $$ = 1 - 2\cos(-\theta\omega) + 1 = 1 \rightarrow 1 - 2\cos(-\theta\omega) = 0 \rightarrow \cos(-\theta\omega) = \frac12 \rightarrow -\theta\omega = 
+# \pm\frac13\pi \approx \pm 1 \rightarrow \omega \approx \frac{1}{\theta} $$
+
+# %%
+w = np.logspace(-2, 2, 800)
+fig = plt.figure()
+plt.loglog(w, np.abs(1 - np.exp(-1 * w * 1j)))
+plt.loglog(w, np.abs( np.exp(-1 * w * 1j)))
+display(fig)
 
 # %% [markdown]
 # <div style="text-align:center;background-color:tomato;">End of lecture "Frequency Domain Design I & II"</div>
-
+# # Closing remark
+# I'm so sorry, but everything we've done is, technically speaking, bachelor level control engineering.

__pycache__/helperFunctions.cpython-314.pyc

@@ -53,23 +53,21 @@ def unwrap_angle(ang):
 
 import matplotlib.pyplot as plt
 import numpy.random as rng
+import scipy as sp
+import control as cm
 from matplotlib.gridspec import GridSpec
 from matplotlib.ticker import MultipleLocator
-from IPython.display import display
+from IPython.display import display, Markdown
 
 class loopShaper():
     def __init__(self, inputNoise=False, outputNoise=False):
         self.Czeros = []
         self.Cpoles = []
         self.Cgain = 1.
-        # self.Pzeros = [1., self.gen_pair(1e-3, 1.5)]
-        # self.Ppoles = [1e2, self.gen_pair(1e-1, .2), self.gen_pair(5e2, 3)]
-        self.Pzeros = [self.gen_pair(-1e-2, 1.5)]
-        self.Ppoles = [-1e2, self.gen_pair(-1e-1, .2)]
-        self.Pgain = 1e3
-        self.omRange = [-4, 4]
-        self.OM = np.logspace(self.omRange[0], self.omRange[1], 500)
-        self.S = self.OM*1j
+        self.Pzeros = [-1., self.gen_pair(1e-3, 1.5)]
+        self.Ppoles = [-1e2, self.gen_pair(1e-1, .2), self.gen_pair(5e2, 3)]
+        self.Pgain = 1e6
+        self.OM = np.logspace(-4, 4, 500)
 
     def gen_pair(self, om0, zeta):
         q1 = -zeta*om0
@@ -79,29 +77,63 @@ def gen_pair(self, om0, zeta):
     def get_response(self, Z, P, K):
         response = np.ones_like(self.S) * K
         for z in Z:
+            if z == []: continue
             if type(z) == tuple: # zero pair
                 response *= (self.S - z[0]) * (self.S - z[1])
             else:
                 response *= (self.S - z)
         for p in P:
+            if p == []: continue
             if type(p) == tuple: # pole pair
                 response /= (self.S - p[0]) 
                 response /= (self.S - p[1])
             else:
                 response /= (self.S - p)
         return response
 
+    def get_CLsteadygain(self):
+        Lzeros, Lpoles = self.Pzeros + self.Czeros, self.Ppoles + self.Cpoles
+        Lgain = self.Pgain * self.Cgain
+
+        NL, DL, = 1, 1
+        for z in Lzeros:
+            if z == []: continue
+            if type(z) == tuple: # zero pair
+                NL *= ( - z[0]) * ( - z[1])
+            else:
+                NL *= ( - z)
+        for p in Lpoles:
+            if p == []: continue
+            if type(p) == tuple: # pole pair
+                DL *= ( - p[0]) * ( - p[1])
+            else:
+                DL *= ( - p)
+
+        return np.abs(Lgain * NL / (DL + Lgain*NL))
+    
+    def unpack(self, Q):
+        out = []
+        for q in Q:
+            if q == []: continue
+            if type(q) == tuple: # pair
+                out.append(q[0])
+                out.append(q[1])
+            else:
+                out.append(q)
+        return out
+
 
     def plot_LS(self, ax):
-        [a.set_title(b) for a, b in zip([ax[-1], ax[0]], ["Nyquist plot", "Bode plots"])]
-        [a.set_ylabel(b) for a, b in zip(ax[:3], ["$|L(s)|$", r"$\angle L(s)$", "$|S(s)|$" ])]
+        [a.set_title(b) for a, b in zip([ax[3], ax[0], ax[4]], ["Nyquist plot", "Bode plots", "Step Response"])]
+        [a.set_ylabel(b) for a, b in zip([ax[0], ax[1], ax[2], ax[4]],
+                                         ["$|L(s)|$", r"$\angle L(s)$", "$|S(s)|$" , "$y(t)$" ])]
         ax[1].yaxis.set_major_locator(MultipleLocator(90))
         ax[2].set_xlabel(r"$\omega$")
-        ax[-1].set(aspect='equal',
+        ax[4].set_xlabel(r"$t$")
+        ax[3].set(aspect='equal',
                     xlabel=r"$\mathfrak{Re}\{L(\Gamma_s)\}$", ylabel=r"$\mathfrak{Im}\{L(\Gamma_s)\}$",
                     xlim=[-2,1], ylim=[-2, 2])
 
-        self.OM = np.logspace(self.omRange[0], self.omRange[1], 500)
         self.S = self.OM*1j
 
         Presponse = self.get_response(self.Pzeros, self.Ppoles, self.Pgain)
@@ -121,14 +153,33 @@ def plot_LS(self, ax):
         ax[1].legend(handles=[llM, lpM, lcM])
         [a.autoscale(enable=True, axis='x', tight=True) for a in ax[:3]]
 
+
         # Plot sensitivity
         ax[2].loglog(self.OM, np.abs(1. / (1. + Lresponse)), 'k')
 
+        [a.axhline(1, color='r', lw=.7) for a in [ax[0], ax[2]]]
+
         # Plots Nyquist
         ax[3].plot(Lresponse.real, Lresponse.imag, 'k')
         ax[3].plot(Lresponse.real, -Lresponse.imag, 'k--')
 
-        UnitCirc = np.exp(np.linspace(0,2*np.pi,100))
+        UnitCirc = np.exp(np.linspace(0,2*np.pi,100)*1j)
         ax[3].plot(UnitCirc.real, UnitCirc.imag, 'k:')
         ax[3].plot([-1], [0], 'rx')
 
+        # Plot step response
+        P = cm.zpk(self.unpack(self.Pzeros), self.unpack(self.Ppoles), self.Pgain)
+        C = cm.zpk(self.unpack(self.Czeros), self.unpack(self.Cpoles), self.Cgain)
+        L = cm.series(C, P)
+        T = cm.feedback(L)
+        t, yout = cm.step_response(L)
+        ax[4].plot(t, yout, 'k')
+        ax[4].autoscale(enable=True, axis='x', tight=True)
+        
+        # Plot steady response
+        Tss = self.get_CLsteadygain()
+        ax[4].axhline(Tss, color='k', ls=':')
+
+        print(f"Steady gain is {Tss:.3E}")
+        display(Markdown(f"$M_S = {np.abs(1. / (1. + Lresponse)).max():.3f}$"))
+

helperFunctions.py

@@ -0,0 +1,28 @@
+import numpy as np
+import matplotlib.pyplot as plt
+plt.rcParams.update({ 'mathtext.fontset':         'cm',
+                      'font.size':          12.0,               'axes.labelsize':           'medium',
+                      'xtick.labelsize':    'x-small',          'ytick.labelsize':          'x-small',
+                      'axes.grid':          True,               'axes.formatter.limits':    [-3, 6],
+                      'grid.alpha':         0.5,                'figure.figsize':           [11.0, 4],
+                      'figure.constrained_layout.use': True,    'scatter.marker':           'x',
+                      'animation.html':     'jshtml'
+                    })
+
+from matplotlib.ticker import MultipleLocator
+from matplotlib.gridspec import GridSpec
+
+import warnings
+warnings.filterwarnings("ignore")
+
+import control as cm
+from helperFunctions import *
+
+###############################################
+SYS = loopShaper()
+fig = plt.figure(figsize=[15, 8])
+gs = GridSpec(4,2, figure=fig)
+ax = [fig.add_subplot(a) for a in [gs[0,0], gs[1, 0], gs[2, 0], gs[:3, 1], gs[3,:]]]
+
+SYS.plot_LS(ax)
+plt.show()