| |Pierre Proulx, ing, professeur|
|:---|:---|
|Département de génie chimique et de génie biotechnologique |** GCH200-Phénomènes d'échanges I **|

# Chapitre 6: Facteurs de friction:

### 1 Définition

>> ### Le facteur de friction est facteur de proportionnalité défini comme le rapport entre la force de friction exercée sur un objet par un fluide qui s'écoule sur cet objet d'une part, et le produit de l'énergie cinétique du fluide multipliée par la surface.

 $$ \boxed { { f  } = \frac {F_k}{A \frac {1}{2} \rho V^2}}$$

>> ### a) Le facteur de friction défini pour l'écoulement dans une conduite devient ainsi:

$$
  \boxed{ f = \frac {F_k}{2 \pi R L \big( \frac {1}{2} \rho V^2\big)} }
  $$

$ V $ est la vitesse moyenne de l'écoulement dans la conduite.

Par un bilan des forces qui s'exercent sur la conduite, on peut relier la force de friction à la différence de pression $\mathscr{P}_0 - \mathscr{P}_L$, et on obtient:

$$
\boxed {F_k = (\mathscr{P}_0 - \mathscr{P}_L)\pi R^2}
$$

En substituant $F_k$ dans la définition de $f$, on obtient la définition générale du coefficient de friction pour une conduite circulaire:

$$
  \boxed{ f =  \frac {1}{4} \frac {D}{L}\frac {\mathscr{P}_0 - \mathscr{P}_L} {(\frac {1}{2} \rho V^2)}}
  $$

>> ### b) Le facteur de friction défini pour l'écoulement autour d'une sphère en chute libre :
Le facteur de friction est toujours défini de la même façon

$$ \boxed {f = \frac {F_k}{A \frac {1}{2} \rho v_\infty ^2}}$$

mais ici, ** attention **, la surface A est la surface pprojetée de la sphère, donc la surface d'un disque, et $v_\infty $ est la vitesse relative entre le fluide et la sphère.

$$
  \boxed{ f = \frac {F_k}{\pi R^2 \frac {1}{2} \rho v_\infty^2}}
  $$

Par un bilan des forces qui s'exercent sur la sphère (gravité - poussée d'Archimède), on obtient:

$$
\boxed {F_k = \frac {4}{3}\pi R^3 \rho_{sph} g - \frac {4}{3}\pi R^3 \rho g  }
$$

En substituant $F_k$ dans la définition de $f$, on obtient la définition générale du coefficient de friction pour une sphère en chute libre:

$$
  \boxed{ f =  \frac {4}{3} \frac {gD}{v_\infty ^2} \frac {\rho_{sph} -\rho} {\rho}}
  $$

### 2. Facteur de friction pour une conduite

Le résultat obtenu à la section 1a)  est la définition du facteur de friction, ce facteur peut maintenant être déterminé si on connait le profil de vitesse. Il y a deux cas:

####  * Cas laminaire
$$
\boxed {f = \frac {16}{Re}}
$$

####  * Cas turbulent

Malheureusement dans le cas turbulent le facteur de friction ne peut pas être déterminé en général de façon aussi simple que dans le cas laminaire et on doit utiliser souvent des corrélations empiriques telles que celles présentées à la section 6.2. Cependant, la relation de Blasius, bien qu'elle soit limitée à des écoulements à des nombres de Reynolds inférieurs à 100,000 et seulement pour des conduites lisses, est souvent utilisée:
$$
\boxed {f = \frac {0.0791}{Re^{0.25}}}
$$

Pour des conduites rugueuses, la formule de Haaland est souvent proposée pour des écoulements fortement turbulents (Re>40,000)
$$
\boxed {\frac {1}{\sqrt{f}} = -3.6 log_{10} \bigg [ \frac {6} {Re} +\big (\frac {k/D}{3.7} \big)^{10/9} \bigg ] }
$$

```python
#
# Utilisons Sympy pour tracer et voir l'influence de Re et k/D, ainsi que la cohérence entre les 2 corrélations
# turbulente ainsi que la partie laminaire
#
import sympy as sp
from IPython.display import *
sp.init_printing(use_latex=True)
%matplotlib inline
Re,ksurD,f=sp.symbols('Re,ksurD,f')
f=sp.Eq(1/(-3.6*sp.log(6/Re+(ksurD/3.7)**(10/9),10))**2).lhs
display(f)
dico={'ksurD':0.0001}
f1=f.subs(dico)
dico={'ksurD':0.0005}                     # Turbulent avec rugosité
f2=f.subs(dico)
dico={'ksurD':0.001}
f3=f.subs(dico)
fBlasius=sp.Eq(0.0791/Re**0.25).lhs       # Turbulent jusqu'à 50,000 conduite lisse
fLam=sp.Eq(16/Re).lhs                     # laminaire, Re < 2000
import matplotlib.pyplot as plt
plt.rcParams['figure.figsize'] = 10,8
p=sp.plot((fLam,(Re,100,2000)),
        (f1,(Re,50000,200000)),
        (f2,(Re,50000,200000)),
        (f3,(Re,50000,200000)),
        (fBlasius,(Re,2000,50000)),legend=True,xscale='log',yscale='log',show=False);
p[0].line_color = 'red'
p[0].label='16/Re , laminaire'
p[1].line_color = 'black'
p[1].label='Haaland k=0.0001'
p[2].line_color = 'blue'
p[2].label='Haaland k=0.0005'
p[3].line_color = 'cyan'
p[3].label='Haaland k=0.001'
p[4].line_color = 'green'
p[4].label='Blasius'
p.show()
```

## 3. Facteur de friction pour une sphère en chute libre

Le résultat obtenu dans la section 1b) est la définition du facteur de friction, ce facteur peut maintenant être déterminé si on connait le profil de vitesse autour de la sphère. La solution n'est pas unique, encore une fois, mais dépends du régime d'écoulement.

####  * Régime de Stokes, pour des nombres de Reynolds très faibles ( < 1 )

Dans ce cas, le facteur de friction peut être obtenu:
$$
\boxed {f = \frac {24}{Re}}
$$

####  * Régime intermédiaire,  ( 500 > Re  > 1  )

Dans ce cas, le facteur de friction peut être obtenu par:
$$
\boxed {f = \bigg ( \sqrt{\frac {24}{Re}} + 0.5407 \bigg )^2}
$$

####  * Régime de Newton,  ( 100000> Re  >500 )

Expérimentalement on a trouvé que dans cette région le facteur de friction varie très peu:
$$
\boxed {f = 0.44}
$$

```python
#
# Utilisons Sympy pour tracer et voir l'influence de Re et k/D, ainsi que la cohérence entre les 2 corrélations
# turbulente ainsi que la partie laminaire
#
import sympy as sp
from IPython.display import *
sp.init_printing(use_latex=True)
%matplotlib inline
Re,f=sp.symbols('Re,f')
f2=0.44
f1=((24/Re)**0.5+0.5407)**2
f=24/Re
plt.rcParams['figure.figsize'] = 10,8
p=sp.plot((f,(Re,0.001,1)),
        (f1,(Re,0.1,2000)),
        (f2,(Re,2000,10000)),
          legend=True,xlabel='Reynolds',ylabel='f',xscale='log',yscale='log',show=False);
p[0].line_color = 'red'
p[0].label='24/Re ,Stokes '
p[1].line_color = 'black'
p[1].label='équation 6.3-16'
p[2].line_color = 'green'
p[2].label='0.44, Newton'
p.show()
```

## 4. Facteur de friction dans un lit fixe

Le développement par Ergun (1952) d'une relation entre la différence de pression appliquée sur un lit fixe et le débit nous donne un outil assez facile à utiliser, en effet:

####  * Régime d'Ergun,  ( 1000 > Re  >10 )

$$
  \boxed{
  ( \frac  {(\mathscr{P}_0 - \mathscr{P}_L) \rho} {G_0^2}  )( \frac  {D_p} {L}  )( \frac  {\epsilon^3} {1- \epsilon}  )= 150  ( \frac  {1-\epsilon} {D_p G_0 / \mu}  )+\frac {7}{4}
  }
 $$

Cette expression représente bien les résultats expérimentaux pour des valeurs de $\frac {D_p G_0} {\mu(1-\epsilon)}$ (le nombre de Reynolds corrigé pour la présence des particules dans le lit) entre 10 et 1000.

####  * Régime de Blake-Kozeny,  ( 10 > Re  )

Si le nombre de Reynolds est inférieur à 10, on peut négliger le dernier terme à droite de l'équation d'Ergun:
$$
  \boxed{
  ( \frac  {(\mathscr{P}_0 - \mathscr{P}_L) \rho} {G_0^2} )( \frac  {D_p} {L}  )
 ( \frac  {\epsilon^3} {1-\epsilon}  )= 150   ( \frac  {1-\epsilon} {D_p G_0/\mu} )
    }
 $$

####  * Régime de Burke-Plummer,  (  Re > 1000 )

Si le nombre de Reynolds est supérieur à 1000, on peut négliger le premier terme à droite de l'équation d'Ergun:

$$
  \boxed{  ( \frac  {(\mathscr{P}_0 - \mathscr{P}_L) \rho} {G_0^2}  )
   ( \frac  {D_p} {L} g )
   ( \frac  {\epsilon^3} {1-\epsilon}  )
  =
 \frac {7}{4}
    }
 $$

```python
#
# Utilisons Sympy pour tracer sous la même forme que dans Transport Phenomena Fig 6.4-2
#
import sympy as sp
from IPython.display import *
sp.init_printing(use_latex=True)
%matplotlib inline
DP,rho,G_0,D_p,epsilon,mu,L=sp.symbols('DP,rho,G_0,D_p,epsilon,mu,L')
Ergun=((DP)*rho/G_0**2)*(D_p/L)*(epsilon**3/(1-epsilon))-150*(1-epsilon)/(D_p*G_0/mu)+7/4
display(Ergun)
#
# C'est l'équation d'Ergun qu'on change de forme pour le graphe, Fig 6.4-2
#
Re=(D_p*G_0/mu)/(1-epsilon)
graphe=150/Re  +1.75                                       # voir 6.4-13
dico={'mu':0.001,'epsilon':0.5,'rho':1000,'D_p':0.001}
graphe=graphe.subs(dico)
#
# DP_s et Re_s sont les abscisse et ordonnées de la figure 6.4-2 respectivement
#
plt.rcParams['figure.figsize'] = 10,8
sp.plot(graphe,(G_0,1,1000
               ),xscale='log',yscale='log',xlabel='Re_s',ylabel='DP_s');
#
# On imprime ici pour vérification avec la figure 6.4.2 la valeur de l'ordonnée
# pour une abscisse de 1, ce qui correspond à l'extrême gauche du graphe.
print(graphe.subs(G_0,.5))
Re=Re.subs(dico)
print(Re.subs(G_0,.5))
#
# On varie G_0 pour avoir les abscisses voulues
```


## Chapter 6

*Notes*
* see "utilities.py" for implementations and unit-testing of methods used throughout this notebook.

```python
import sys
sys.path.append("..")
```

```python
from sympy import *
import numpy as np
import matplotlib.pyplot as plt
init_printing(use_latex='mathjax')
from importlib import reload
import Ch3.utilities as ch3_utils
reload(ch3_utils)
```

    <module 'Ch3.utilities' from '../Ch3/utilities.py'>

```python
import Ch6.utilities as ch6_utils
reload(ch6_utils)
```

    <module 'Ch6.utilities' from '../Ch6/utilities.py'>

### Problem 6.1

#### a)

The desired poles are at $s = -25, -4, $ and $-2 \pm j 2\sqrt{3}$.

```python
# using constants from Problem 3.6
m, M, l, g, k, R, r = .1, 1., 1., 9.8, 1., 100., .02
A = Matrix([[0., 1., 0., 0.],
            [0., -k**2 / (M*r**2*R), -(m*g)/M, 0.],
            [0., 0., 0., 1.],
            [0, k**2 / (M*r**2*R*l), ((M+m)*g)/(M*l), 0.]])
B = Matrix([[0.],[k/(M*R*r)],[0.],[-k/(M*R*r*l)]])
C = Matrix([[1., 0., 0., 0.]])
D = zeros(1, 1)
Phi = ch3_utils.computeResolvent(A, imag=False, smplfy=True)
pprint(Phi)
```

    ⎡                 ⎛                    2                            ⎞
    ⎢1.0     0.001936⋅⎝0.0927643784786641⋅s  + 0.210828132906055⋅s - 1.0⎠   0.044
    ⎢───  - ───────────────────────────────────────────────────────────── + ─────
    ⎢ s                          3                      2                     s
    ⎢       0.00408163265306122⋅s  + 0.102040816326531⋅s  - 0.044⋅s - 1.0
    ⎢
    ⎢                             ⎛                    2      ⎞
    ⎢                       0.044⋅⎝0.0927643784786642⋅s  - 1.0⎠
    ⎢ 0        ─────────────────────────────────────────────────────────────
    ⎢                               3                      2
    ⎢          0.00408163265306122⋅s  + 0.102040816326531⋅s  - 0.044⋅s - 1.0
    ⎢
    ⎢                                0.102040816326531
    ⎢ 0        ─────────────────────────────────────────────────────────────
    ⎢                               3                      2
    ⎢          0.00408163265306122⋅s  + 0.102040816326531⋅s  - 0.044⋅s - 1.0
    ⎢
    ⎢                               0.102040816326531⋅s
    ⎢ 0        ─────────────────────────────────────────────────────────────
    ⎢                               3                      2
    ⎣          0.00408163265306122⋅s  + 0.102040816326531⋅s  - 0.044⋅s - 1.0

                                -0.004                                  0.00040816
     ─────────────────────────────────────────────────────────────  - ────────────
                          3                      2
     0.00408163265306122⋅s  + 0.102040816326531⋅s  - 0.044⋅s - 1.0    0.0040816326

                               -0.004⋅s
     ─────────────────────────────────────────────────────────────       ─────────
                          3                      2
     0.00408163265306122⋅s  + 0.102040816326531⋅s  - 0.044⋅s - 1.0       0.0040816

                   0.102040816326531⋅s⋅(0.04⋅s + 1.0)
     ─────────────────────────────────────────────────────────────       ─────────
                          3                      2
     0.00408163265306122⋅s  + 0.102040816326531⋅s  - 0.044⋅s - 1.0       0.0040816

                          1.0⋅(0.044⋅s + 1.0)
     ─────────────────────────────────────────────────────────────       ─────────
                          3                      2
     0.00408163265306122⋅s  + 0.102040816326531⋅s  - 0.044⋅s - 1.0       0.0040816

               ⎛      2                            ⎞         ⎤
    3265306122⋅⎝0.04⋅s  + 1.0⋅s - 0.431200000000004⎠    0.004⎥
    ───────────────────────────────────────────────── + ─────⎥
             3                      2                     s  ⎥
    5306122⋅s  + 0.102040816326531⋅s  - 0.044⋅s - 1.0        ⎥
                                                             ⎥
                                                             ⎥
                      -0.004                                 ⎥
    ────────────────────────────────────────────────────     ⎥
                3                      2                     ⎥
    3265306122⋅s  + 0.102040816326531⋅s  - 0.044⋅s - 1.0     ⎥
                                                             ⎥
          0.102040816326531⋅(0.04⋅s + 1.0)                   ⎥
    ────────────────────────────────────────────────────     ⎥
                3                      2                     ⎥
    3265306122⋅s  + 0.102040816326531⋅s  - 0.044⋅s - 1.0     ⎥
                                                             ⎥
         0.102040816326531⋅s⋅(0.04⋅s + 1.0)                  ⎥
    ────────────────────────────────────────────────────     ⎥
                3                      2                     ⎥
    3265306122⋅s  + 0.102040816326531⋅s  - 0.044⋅s - 1.0     ⎦

Now, provided the matrix $Q$ is full-rank, the gain matrix $G$ is found to be $[QW]^{-1}(\mathbf{\bar a}-\mathbf{a})$ (see `bassGura` function implemented in "utilities.py"):

```python
desiredPoles = np.array([-25., -4., np.complex(-2., 2.*np.sqrt(3)), np.complex(-2., -2.*np.sqrt(3))])
G = ch6_utils.bassGura(np.array(A), np.array(B), desiredPoles)
pprint('G =')
pprint(G)
```

    G =
    [[-326.53061224 -226.32653061 -812.09061224 -242.32653061]]

So, the matrix $G$, where
\begin{eqnarray}
G = \begin{pmatrix}
-326.53061224, \
-226.32653061, \
-812.09061224, \
-242.32653061
\end{pmatrix}
\end{eqnarray}

transforms the open-loop plant into the closed-loop plant at the desired poles.

#### b)
This part requires looking at $\theta_r = \frac{\pi}{2}$ as an exogenous input to the system.  This system enters via the pendulum angle $\theta$.  Following Example 6F, we can change $x_3 = \theta$ to be the error signal $x_3 = \theta_r - \theta$.  $\theta_r$ is constant, so $\dot \theta_r = 0$.  The modified open-loop system with $\theta_r$ as an exogeneous input is:

\begin{eqnarray}
\mathbf{\dot x} &=& \begin{pmatrix}
0 & 1 & 0 & 0 \\
0 & -\frac{k^2}{Mr^2R} & \frac{mg}{M} & 0 \\
0 & 0 & 0 & 1 \\
0 & -\frac{k^2}{Mr^2R \ell} & \frac{M+m}{M \ell} g & 0
\end{pmatrix} \mathbf{x} + \frac{k}{MRr}\begin{pmatrix}
0 \\
1 \\
0 \\
\frac{1}{\ell}
\end{pmatrix} u +
\begin{pmatrix}
0 \\
-\frac{mg}{M} \\
0 \\
-\frac{M+m}{M \ell}g
\end{pmatrix} \theta_r
\end{eqnarray}

where the state vector $\mathbf{x}$ is $\mathbf{x} = [x_1, \ x_2, \ e_\theta, \ \dot e_\theta]^T$.

The design process of Part a) can now be followed to find the gain matrix that transforms the characteristic polynomial to the desired form.  Now, the scalar feedforward gain $G_0$ needs to be found via the method from Section 6.4; this gain will eliminate error from a step input to the pendulum angle error.  The question did not explicitly say to follow these steps I have outlined, so I will stop here.

### Problem 6.2

```python
Kv, Lv, J, Km, Omm, Kdp = 94.3, 1.0, 7900., 8.46e6, 45.9, 6.33e-6
A = np.array([[0., 1., 0., 0.], [0., 0., 1., 0.], [0., -Km/J, -(Km/J)*(Omm*J/Km), (Km/J)], [0., 0., -Kv*Kdp*J, -Lv*Kv]])
B = np.array([[0.], [0.], [0.], [Kv]])
C = np.array([[1., 0., 0., 0.]])
D = np.zeros((1, 1))
```

#### a)

```python
desiredPoles = np.array([-10.*np.sqrt(2)*np.complex(1., 1.), -10.*np.sqrt(2)*np.complex(1., -1.), np.complex(-64.5, 69.6), np.complex(-64.5, -69.6)])
```

```python
G = ch6_utils.bassGura(A, B, desiredPoles)
pprint('G =')
pprint(G)
```

    G =
    [[3.56664832e+01 1.85180106e+00 1.80198060e-02 1.81169366e-01]]

#### b)

The objective for $g_A$ becomes to find the desired coefficients $\bar a_i$, $i = 1, \dots, 4$ s.t.:
\begin{eqnarray}
\mathbf{qw}_3^{-1} \Delta \mathbf{a} &=& 0 \\
\mathbf{qw}_4^{-1} \Delta \mathbf{a} &=& 0
\end{eqnarray}

Where $\mathbf{qw}_i^{-1}$ is the $i$th row of $[(QW)^T]^{-1}$ and $\mathbf{\Delta a} = \mathbf{\bar a} - \mathbf{a}$, where $\mathbf{a}$ is the coefficient array of the open-loop process and $\mathbf{\bar a}$ is the unknown coefficient array.

Similarly, for $g_B$:

\begin{eqnarray}
\mathbf{qw}_2^{-1} \Delta \mathbf{a} &=& 0 \\
\mathbf{qw}_4^{-1} \Delta \mathbf{a} &=& 0
\end{eqnarray}

The design goal is to keep the two poles in the desired locations and then see how close the remaining two poles can be brought to the fast pole locations with zero $g_3, g_4$.  First, factor out a quadratic with the desired pole locations at $-10 \sqrt{2} (1 \pm j)$:

```python
coeffsFromFixedRoots = np.poly([-10.*np.sqrt(2)*np.complex(1., 1.), -10.*np.sqrt(2)*np.complex(1., -1.)])
pprint(coeffsFromFixedRoots)
```

    [  1.          28.28427125 400.        ]

The desired closed-loop characteristic polynomial now has the form $(s^2 + 28.284s + 400)(s^2 + \chi_1 s + \chi_2)$

The desired characteristic polynomial expanded is:

\begin{eqnarray}
s^4 + (\chi_1 + 28.284) s^3 + (\chi_2 + 28.284 \chi_1 + 400) s^2 + (28.284 \chi_2 + 400 \chi_1) s + 400 \chi_2
&=& 0
\end{eqnarray}

The desired coefficients are:

\begin{eqnarray}
\mathbf{\bar a} = \begin{pmatrix}
\chi_1 + 28.284, \
28.284 \chi_1 + \chi_2 + 400, \
400 \chi_1 + 28.284 \chi_2, \
400 \chi_2
\end{pmatrix}
\end{eqnarray}

Follow the steps from Example 6F to find $\chi_1, \chi_2$: this means setting up a linear system $M \Delta \mathbf{a} = \mathbf{0}$.  The matrix $M$ are the rows of the $[(QW)^T]^{-1}$ discussed above.

```python
Q = control.ctrb(A, B).astype('float')
W = np.eye(A.shape[0]).astype('float')
openLoopCoeffs = ch6_utils.computeCoeffs(ch6_utils.computePoles(A))
for i in range(W.shape[0]):
    W[i, i+1:] = openLoopCoeffs[1:-(i+1)]
M = Matrix(np.linalg.inv((Q@W).T))
c1, c2 = symbols('c1 c2')
da = Matrix([[c1 + 28.284 - 140.2], [28.284*c1 + c2 + 400. - 10449.], [28.284*c2 + 400.*c1 - 100980.], [400.*c2]])
pprint('linear system to solve:')
pprint(M[2:, :] * da)
pprint(' = ')
pprint(Matrix([[0], [0]]))
```

    linear system to solve:
    ⎡-0.000174442514082865⋅c₁ + 9.90250420543058e-6⋅c₂ - 0.0486416512363089⎤
    ⎢                                                                      ⎥
    ⎣               0.0106044538706257⋅c₁ - 1.18680805938494               ⎦
     =
    ⎡0⎤
    ⎢ ⎥
    ⎣0⎦

```python
Asys = np.array([[-1.7444e-4, 9.9025e-6], [0.010604, 0.]])
bsys = np.array([[4.864e-2], [1.1868]])
sol = np.linalg.solve(Asys, bsys)
gACharPolyCoeffs = np.array([1., sol[0] + 28.284, 28.284*sol[0] + sol[1] + 400., 28.284*sol[1] + 400.*sol[0], 400.*sol[1]])
gACharPolyRoots = np.roots(gACharPolyCoeffs)
print('Roots of characteristic polynomial given g3, g4=0 - {}'.format(gACharPolyRoots))
```

    Roots of characteristic polynomial given g3, g4=0 - [-55.96001509+61.25294542j -55.96001509-61.25294542j
     -14.142     +14.14227125j -14.142     -14.14227125j]

This is pretty good, let's find the gain matrix $G = [g_1, g_2, 0, 0]$ that results.

```python
da = gACharPolyCoeffs - openLoopCoeffs
gA = np.array(M).dot(da[1:]).astype('float').reshape((1, 4))
pprint('gA = {}'.format(gA))
```

    gA = [[ 2.72653436e+01  1.37120514e+00 -3.79322042e-06  4.27378292e-05]]

Close enough given rounding.  Let's try $g_B$.

```python
da = Matrix([[c1 + 28.284 - 140.2], [28.284*c1 + c2 + 400. - 10449.], [28.284*c2 + 400.*c1 - 100980.], [400.*c2]])
Mslice = Matrix([M[1, :], M[3, :]])
pprint('linear system to solve:')
pprint(Mslice * da)
pprint(' = ')
pprint(Matrix([[0], [0]]))
```

    linear system to solve:
    ⎡-0.00664345218845344⋅c₁ + 0.000280082428946399⋅c₂ + 0.186853184720562⎤
    ⎢                                                                     ⎥
    ⎣              0.0106044538706257⋅c₁ - 1.18680805938494               ⎦
     =
    ⎡0⎤
    ⎢ ⎥
    ⎣0⎦

```python
Asys = np.array([[-6.6435e-3, 2.80082e-4], [0.010604, 0.]])
bsys = np.array([[-.18685], [1.1868]])
sol = np.linalg.solve(Asys, bsys)
gBCharPolyCoeffs = np.array([1., sol[0] + 28.284, 28.284*sol[0] + sol[1] + 400., 28.284*sol[1] + 400.*sol[0], 400.*sol[1]])
gBCharPolyRoots = np.roots(gBCharPolyCoeffs)
print('Roots of characteristic polynomial given g2, g4=0 - {}'.format(gBCharPolyRoots))
da = gBCharPolyCoeffs - openLoopCoeffs
gB = np.array(M).dot(da[1:]).astype('float').reshape((1, 4))
pprint('gB = {}'.format(gB))
```

    Roots of characteristic polynomial given g2, g4=0 - [-89.78196398 +0.j         -22.1380662  +0.j
     -14.142     +14.14227125j -14.142     -14.14227125j]
    gB = [[ 7.87288323e+00 -3.57369719e-05 -4.84849442e-02  4.27378292e-05]]

This doesn't work so well; the fast oscillatory poles are no longer oscillatory.  The design goal was to minimize the movement of the fast poles via placement, so the first option is better.

#### c)

Following the signal flow of the block diagram (with $G_r$ being the feedforward gain on the reference angle $\theta_r$):

\begin{eqnarray}
e(s) &=& \frac{1 - G_r H(s)}{1 + g H(s)} \theta_r(s) \\
&=& \frac{1}{1 + g H(s)} \theta_r(s) - \frac{G_r H(s)}{1 + g H(s)} \theta_r(s)
\end{eqnarray}

By linearity of limits, i.e. $\lim f + g = \lim f + \lim g$, we can consider the limit as $s \to 0$ of $s$ times each term above.  If $\lim_{s \to 0} \frac{s}{1 + gH(s)} \theta_r(s)$ is non-zero, we would need a feedforward gain from the second term above to eliminate the difference.

$\theta_r(s) = \frac{\theta_r}{s}$ is the Laplace transform of a zeroth order polynomial in time ($m=0$).  As was mentioned in the problem setup, $H(s)$ is a type-1 system ($p=1$), so it has a proper pole at $s=0$ with multiplicity 1.  From Eqn. 4.51 from the book, we see that $p > m$ and so $\lim_{s \to 0} \frac{s}{1 + gH(s)} \theta_r(s) = 0$.  Therefore, no feedforward gain is needed because there is no steady-state error introduced by the feedback: $G_r = 0$.

#### d)

```python
C = np.array([1., 0., 0., 0.]).reshape((1, 4))
D = np.zeros(1)
E = np.zeros((4,3))
E[1, 0] = 1.
E[2, 1] = 1.
E[3, 2] = 1.
gainMatrices = {'Full': G, 'gA': gA, 'gB': gB}
for g in gainMatrices:
    try:
        # follow steps of Feedforward Gain stage of Example 4F
        print('############## Finding G0 for {} ##################'.format(g))
        G0 = ch6_utils.bassGuraExo(A, B, C, gainMatrices[g], E)
        pprint('G0 = {}'.format(G0))
        print('\n')
    except Exception as e:
        print(e)
        print('G0 is undefined')
        print('\n')
        continue
```

    ############## Finding G0 for Full ##################
    G0 = [[0.11865374 0.00110298 0.01060445]]

    ############## Finding G0 for gA ##################
    G0 = [[0.09286674 0.00093385 0.01060445]]

    ############## Finding G0 for gB ##################
    G0 = [[0.04438559 0.00093385 0.01060445]]

$G_0$ is defined in all cases, so we can minimize complexity of the design by going with gain $g_A$.  This is even more true because of the feedforward gain matrices $G_0$ having the same number of non-zero elements.

### Problem 6.3

We know from Problem 5.2 that the system is controllable with either motor only.  The problem explicitly says to use motor 1, so that's cool.  We want to bring the system to rest at the origin, so the states _are_ the errors.

```python
# with K, M, k, R, r = 40, 1, 2, 100, 0.02
A = np.array([[0, 0, 1, 0], [0, 0, 0, 1], [-40, 40, -100, 0], [40, -40, 0, -100]])
B = np.array([[0], [0], [1], [0]])
C = np.array([[1, 0, 0, 0]])
closedLoopPoles = np.array([np.complex(-1., -1.), np.complex(-1., 1.), np.complex(-100., -100.), np.complex(-100., 100.)])
G = ch6_utils.bassGura(A, B, closedLoopPoles)
pprint('G =')
pprint(G)
```

    G =
    [[ 1.0122e+04 -9.1220e+03  2.0000e+00 -2.4497e+04]]

The gain is $G = [10122, \ -9122, \ 2, \ -24497]$ and the feedback law is $u = -10122 z_1 + 9122 z_2 - 2 \dot z_1 + 24497 \dot z_2$.

### Problem 6.4

This part requires looking at $V$ as an exogenous input to the system.  Following Example 6F, we can change $z_3 = \dot z_1$, $z_4 = \dot z_2$ to be the error signals $e_3 = z_3 - V$ and $e_4 = z_4 - V$.  The modified open-loop system with $V$ as an exogeneous input is:

\begin{eqnarray}
\mathbf{\dot x} &=& \begin{pmatrix}
0 & 0 & -1 & 0 \\
0 & 0 & 0 & -1 \\
-40 & 40 & 100 & 0 \\
40 & -40 & 0 & 100
\end{pmatrix} \mathbf{x} + \begin{pmatrix}
0 \\
0 \\
1 \\
0
\end{pmatrix} u +
\begin{pmatrix}
1 \\
1 \\
-100 \\
-100
\end{pmatrix} V
\end{eqnarray}

where the state vector $\mathbf{x}$ is $\mathbf{x} = [z_1, \ z_2, \ e_3, \ e_4]^T$ and the control $u$ is the motor voltage.

```python
# with K, M, k, R, r = 40, 1, 2, 100, 0.02
A = np.array([[0, 0, -1, 0], [0, 0, 0, -1], [-40, 40, 100, 0], [40, -40, 0, 100]])
B = np.array([[0], [0], [1], [0]])
C = np.array([[1, 0, 0, 0]])
E = np.array([[1], [1], [-100], [-100]])

sys = control.StateSpace(A, B, C, D)
closedLoopPoles =np.array([np.complex(-1., -1.), np.complex(-1., 1.), np.complex(-100., -100.), np.complex(-100., 100.)])
G = ch6_utils.bassGura(A, B, closedLoopPoles, C, E)
pprint('[G, G0] =')
pprint(G)
```

    [G, G0] =
    [[ -50682.   51682.     402. -127917.  127515.]]

The new feedback law is $u = -G \mathbf{x} - G_0 V$, which for the $G, G_0$ found above is:

\begin{eqnarray}
u = 50682 z_1 - 51682 z_2 - 402 e_3 + 127917 e_4 - 127515 V
\end{eqnarray}

### Problem 6.5

```python
A = np.array([[-1, 1, 0], [-5, -.5, 0], [0, 1, 0]])
B = np.array([[-.1], [-9], [0]])
C = np.eye(3)
D = np.zeros((3,1))
closedLoopPoles = np.array([np.complex(-2., 0.), np.complex(-1., np.sqrt(3.)), np.complex(-1., -np.sqrt(3.))])
G = ch6_utils.bassGura(A, B, closedLoopPoles)
pprint('G =')
pprint(G)
```

    G =
    [[ 0.93033864 -0.28811487 -0.94117647]]

The gain is $G = [ 0.93033864, \ -0.28811487, \ -0.94117647]$.

### Problem 6.6

#### a)

#### b)

```python
A = np.array([[-1, 1, 0, 0], [-5, -.5, 0, 0], [0, 1, 0, 0], [-1, 0, 1, 0]])
B = np.array([[-.1], [-9], [0], [0]])
C = np.array([[1, 0, 0, 0]])
D = np.zeros((4,1))
closedLoopPoles = np.array(2.5*np.array([np.complex(-0.5, 0.5*np.sqrt(3.)), np.complex(-0.5, -0.5*np.sqrt(3.)),
                                         np.complex(-0.5*np.sqrt(3.), 0.5), np.complex(-0.5*np.sqrt(3.), -0.5)]))
G = ch6_utils.bassGura(A, B, closedLoopPoles)
pprint('G =')
pprint(G)
```

    G =
    [[ 3.59461768 -0.63217653 -5.04918509 -4.59558824]]

The gain is $G = [ 3.59461768, \ -0.63217653, \ -5.04918509, \ -4.59558824]$.  If we knew the desired speed, $V$, we could find the feedforward gain on $h_0$ as well.

### Problem 6.7

#### a)

```python
dr, YRoverV, YaOverV, da, gOverV, phi, YrOverV, r, YpOverV, p, YbOverV, T, b = symbols('dr YRoverV YaOverV da gOverV phi YrOverV r YpOverV p YbOverV T b', real=True)

dr = (1 / YRoverV) * (-YaOverV * da - gOverV * phi + (1 - YrOverV) * r - YpOverV * p - (YbOverV + 1 / T) * b)
Lb, Lp, Lr, La, LR, Nb, Np, Nr, Na, NR = symbols('Lb Lp Lr La LR Nb Np Nr Na NR', real=True)

bdot = YbOverV * b + YpOverV * p + (YrOverV - 1) * r + gOverV * phi + YaOverV * da + YRoverV * dr
pdot = Lb*b + Lp*p + Lr*r + La*da + LR*dr
rdot = Nb*b + Np*p + Nr*r + Na*da + NR*dr
phidot = p
pprint(simplify(bdot))  # just for verification, should = -b / T
pprint(simplify(pdot))
pprint(simplify(rdot))
pprint(simplify(phidot))
```

    -b
    ───
     T
    -LR⋅(T⋅(YaOverV⋅da + YpOverV⋅p + gOverV⋅φ + r⋅(YrOverV - 1)) + b⋅(T⋅YbOverV +
    ──────────────────────────────────────────────────────────────────────────────
                                                            T⋅YRoverV

    1)) + T⋅YRoverV⋅(La⋅da + Lb⋅b + Lp⋅p + Lr⋅r)
    ────────────────────────────────────────────

    -NR⋅(T⋅(YaOverV⋅da + YpOverV⋅p + gOverV⋅φ + r⋅(YrOverV - 1)) + b⋅(T⋅YbOverV +
    ──────────────────────────────────────────────────────────────────────────────
                                                            T⋅YRoverV

    1)) + T⋅YRoverV⋅(Na⋅da + Nb⋅b + Np⋅p + Nr⋅r)
    ────────────────────────────────────────────

    p

```python
YbOverV = -.746
YpOverV = .006
YrOverV = .001
gOverV = .0369
YaOverV = .0012
YRoverV = .0092
Lb = -12.9
Lp = -.746
Lr = .387
La = 6.05
LR = .952
Nb = 4.31
Np = .024
Nr = -.174
Na = -.416
NR = -1.76
T = 0.2
A = np.array([[-LR*(YpOverV / YRoverV + Lp), -LR*((YrOverV - 1.) / YRoverV + Lr), -LR*gOverV / YRoverV],
              [-NR*(YpOverV / YRoverV + Np), -NR*((YrOverV - 1.) / YRoverV + Nr), -NR*gOverV / YRoverV],
              [1., 0., 0.]])
B = np.array([[-LR*(YaOverV / YRoverV + La)],
              [-NR*(YaOverV / YRoverV + Na)],
              [0.]])
C = np.array([[0, 0, 1]])
E = np.array([[-LR*(T*YbOverV + 1.) / (T*YRoverV)],
              [-NR*(T*YbOverV + 1.) / (T*YRoverV)],
              [0.]])
closedLoopPoles = np.array([-1., np.complex(-1., 3.), np.complex(-1., -3.)])
G = ch6_utils.bassGura(A, B, closedLoopPoles, C, E)
pprint('[G, G0] =')
pprint(G)
```

    [G, G0] =
    [[   -2.43234778   403.19030857   -14.87746553 -1714.16334596]]

So the gain matrix $\mathbf{G} \triangleq [ G \ | \ G_0 ] = [-2.4323, 403.1903, -14.8775, -1714.1633]$ leads to a closed-loop system with the desired poles/response.

#### b)
Replacing $\phi$ with $e_\phi \triangleq \phi - \phi_0$ in the plant, where $\phi_0$ is a constant exogenous input, results in the following (slightly) modified problem:

```python
A = np.array([[-LR*(YpOverV / YRoverV + Lp), -LR*((YrOverV - 1.) / YRoverV + Lr), -LR*gOverV / YRoverV],
              [-NR*(YpOverV / YRoverV + Np), -NR*((YrOverV - 1.) / YRoverV + Nr), -NR*gOverV / YRoverV],
              [1., 0., 0.]])
B = np.array([[-LR*(YaOverV / YRoverV + La)],
              [-NR*(YaOverV / YRoverV + Na)],
              [0.]])
# add neg. of third column of A to E because of \phi -> \phi - \phi_0 translation
E = np.array([[-LR*(T*YbOverV + 1.) / (T*YRoverV), LR*gOverV / YRoverV],
              [-NR*(T*YbOverV + 1.) / (T*YRoverV), NR*gOverV / YRoverV],
              [0., 0.]])
C = np.array([[0, 0, 1]])

closedLoopCoeffs = np.array([-1., np.complex(-1., 3.), np.complex(-1., -3.)])
G = ch6_utils.bassGura(A, B, closedLoopPoles, C, E)
pprint('[G, G0] =')
pprint(G)
```

    [G, G0] =
    [[   -2.43234778   403.19030857   -14.87746553 -1714.16334596
                               14.86897684]]

So the gain matrix $\mathbf{G} \triangleq [ G \ | \ G_0 ] = [-2.4323, 403.1903, -14.8775, -1714.1633, 14.8690]$ leads to a closed-loop system with the desired poles/response.

It is worth noting that the _magnitude_ of the feedforward gain $G_{\phi_0} \approx 14.8690$ is approximately equal to the feedback gain on the error signal $e_\phi$.

### Problem 6.8

#### a)
Ignore any exogenous input here.  The data from Problem 3.7 is:

```python
_R, _C = 2., 1.
A = np.array([[-3./(_R*_C), 1./(_R*_C), 0.],[1./(_R*_C), -2./(_R*_C), 1./(_R*_C)], [0., 1./(_R*_C), -3./(_R*_C)]])
B = np.array([[2. / (_R*_C)], [0.], [0.]])
closedLoopPoles = np.roots(np.array([1., 4., 8., 8.]))
G = ch6_utils.bassGura(A, B, closedLoopPoles)
pprint('G =')
pprint(G)
```

    G =
    [[2.22044605e-15 6.50000000e+00 6.50000000e+00]]

The gain matrix is $G = [0, \ 6.5, \ 6.5]$.  It is interesting that the gain on $v_1$ is 0.

#### b)

Replacing $v_3$ with $e = v_3 - \bar v$ and using $\mathbf{x}_0 = [v_0, \ \bar v]^T$ results in the following state space model:

```python
E = np.array([[0., 0.], [0., 1./(_R*_C)], [-3./(_R*_C), 1./(_R*_C)]])
# assume observability of the final signal only
C=np.array([[0., 0., 1.]])
G = ch6_utils.bassGura(A, B, closedLoopPoles, C, E)
pprint('[G, G0] =')
pprint(G)
```

    [G, G0] =
    [[ 2.22044605e-15  6.50000000e+00  6.50000000e+00 -2.70000000e+01
                              1.05000000e+01]]

So the gain matrix $\mathbf{G} \triangleq [ G \ | \ G_0 ] = [0, 6.5, 38.5, -27.0, 10.5]$ leads to a closed-loop system with the desired poles/response.

#### c)

The structure that jumps out is the fact that the feedback and feedforward terms are applied to a signal that is not part of the observed error dynamics.  What I mean is that the only non-zero entry in the $B$ matrix applies to the _only_ signal that is not present in the dynamics equation for $e$.  It is also interesting that the desired temperature/voltage feeds forward to both $v_2$ and $e$.

### Problem 6.9

#### a)

$\omega_{xE}, \omega_{yE}$ are disturbances, and $\delta_x, \delta_y$ are the signals to be regulated to 0.

From Example 3G:

```python
HoverJd, KQoverJd, KDoverJd = symbols('HoverJd KQoverJd KDoverJd', real=True)
g1, g2, g3, g4 = symbols('g1 g2 g3 g4', real=True)
A = Matrix([[0., 0., 1., 0.],
            [0., 0., 0., 1.],
            [-KDoverJd, -KQoverJd, 0., -HoverJd],
            [KQoverJd, -KDoverJd, HoverJd, 0.]])
B = Matrix([[0., 0.],
            [0., 0.],
            [1., 0.],
            [0., 1.]])
G = Matrix([[g1, g2, g3, g4],
            [-g2, g1, -g4, g3]])
pprint((A-B*G))
```

    ⎡        0                   0                 1.0                 0        ⎤
    ⎢                                                                           ⎥
    ⎢        0                   0                  0                 1.0       ⎥
    ⎢                                                                           ⎥
    ⎢-KDoverJd - 1.0⋅g₁  -KQoverJd - 1.0⋅g₂      -1.0⋅g₃       -HoverJd - 1.0⋅g₄⎥
    ⎢                                                                           ⎥
    ⎣KQoverJd + 1.0⋅g₂   -KDoverJd - 1.0⋅g₁  HoverJd + 1.0⋅g₄       -1.0⋅g₃     ⎦

The closed-loop equation needs to have all eigenvalues have real part $< 0$ to be asymptotically stable.  The matrix also needs to be invertible (i.e. have full rank).  $g_3$ cannot be $0$ when $g_1 = -\frac{K_D}{J_d}, g_2 = -\frac{K_Q}{J_d}$ and $g_4 = -\frac{H}{J_d}$, simultaneously.  This doesn't seem like a huge limitation.

#### b)

```python
C = Matrix([[1., 0., 0., 0.],
            [0., 1., 0., 0.]])
E = np.array([[-1., 0.],
              [0., -1.],
              [0., 0.],
              [0., 0.]])
term1 = C*((A-B*G)**(-1))
Bpound = (term1*B)**(-1)*term1
G0 = Bpound*E
pprint(G0.shape)
pprint(G0)
```

    (2, 2)
    ⎡
    ⎢
    ⎢         - ──────────────────────────────────────────────────────────────────
    ⎢           ⎛
    ⎢           ⎜                                              1.0⋅(-KQoverJd - 1.
    ⎢           ⎜─────────────────────────────────────────────────────────────────
    ⎢           ⎜⎛                    2
    ⎢           ⎝⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)
    ⎢
    ⎢
    ⎢
    ⎢─────────────────────────────────────────────────────────────────────────────
    ⎢                     ⎛
    ⎢                     ⎜                                              1.0⋅(-KQo
    ⎢(-KDoverJd - 1.0⋅g₁)⋅⎜───────────────────────────────────────────────────────
    ⎢                     ⎜⎛                    2
    ⎣                     ⎝⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(KQoverJd

    ──────────────────────────────────────────────────────────────────────────────

    0⋅g₂)⋅(-1.0⋅KQoverJd - 1.0⋅g₂)
    ──────────────────────────────────────────────────────────────────────────── +
    ⎞ ⎛                        2                                               ⎞
    ⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠

                                                              1.0⋅(-1.0⋅KQoverJd -
    ──────────────────────────────────────────────────────────────────────────────

    verJd - 1.0⋅g₂)⋅(-1.0⋅KQoverJd - 1.0⋅g₂)
    ──────────────────────────────────────────────────────────────────────────────
              ⎞ ⎛                        2
     + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJd +

                                   1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(-1.0⋅g₃⋅(KQoverJd + 1
    ──────────────────────────────────────────────────────────────────────────────
                                                 ⎛                        2
                    1.0⋅(-1.0⋅KDoverJd - 1.0⋅g₁)⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅
     ─────────────────────────────────────────────────────────────────────────────
                          ⎛                    2
     (-KDoverJd - 1.0⋅g₁)⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(KQoverJd

             ⎛       ⎛                    2
     1.0⋅g₂)⋅⎝1.0⋅g₃⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0
    ──────────────────────────────────────────────────────────────────────────────
                                                           ⎛
                              1.0⋅(-1.0⋅KDoverJd - 1.0⋅g₁)⋅⎝1.0⋅(-KDoverJd - 1.0⋅g
    ──────── + ───────────────────────────────────────────────────────────────────
           ⎞                        ⎛                    2
    1.0⋅g₂)⎠   (-KDoverJd - 1.0⋅g₁)⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅

    .0⋅g₂) - 1.0⋅(HoverJd + 1.0⋅g₄)⋅(-KDoverJd - 1.0⋅g₁))
    ──────────────────────────────────────────────────────────────────────────────
                        2                                               ⎞
    (-KQoverJd - 1.0⋅g₂)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠
    ──────────────────────────────────────────────────────────────────────────────
             ⎞ ⎛                        2
    + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1

        ⎞
    ⋅g₂)⎠ + 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(1.0⋅g₃⋅(KQoverJd + 1.0⋅g₂) + (HoverJd + 1.0⋅
    ──────────────────────────────────────────────────────────────────────────────
      2                           2
    ₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)
    ──────────────────────────────────────────────────────────────────────────────
                       ⎞ ⎛                        2
    (KQoverJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQ

    ──────────────────────────────────────────────────────────────────────────────
           ⎞
           ⎟ ⎛                    2                                           ⎞ ⎛
    ───────⎟⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠⋅⎝1
          ⎞⎟
    .0⋅g₂)⎠⎠

                             ⎞
    g₄)⋅(-KDoverJd - 1.0⋅g₁))⎠
    ──────────────────────────────────────────────────────────────────────────────
    ⎞                ⎞
    ⎠                ⎟ ⎛                    2
    ─────────────────⎟⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1
                    ⎞⎟
    overJd + 1.0⋅g₂)⎠⎠

    ──────────────────────────────────────────────────────────────────────── - ───

                           2                                               ⎞
    .0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠   (-K

    ──────────────────────────────────────────────────────────────────────────────

          ⎞ ⎛                        2
    .0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅

    ──────────────────────────────────────────────────────────────────────────────
                      ⎛
                      ⎜                                              1.0⋅(-KQoverJ
    DoverJd - 1.0⋅g₁)⋅⎜───────────────────────────────────────────────────────────
                      ⎜⎛                    2
                      ⎝⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1

    ──── - ───────────────────────────────────────────────────────────────────────
                                ⎛
       ⎞                        ⎜                                              1.0
    g₂)⎠   (-KDoverJd - 1.0⋅g₁)⋅⎜─────────────────────────────────────────────────
                                ⎜⎛                    2
                                ⎝⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(KQ

                                                          1.0⋅(-1.0⋅KDoverJd - 1.0
    ──────────────────────────────────────────────────────────────────────────────

    d - 1.0⋅g₂)⋅(-1.0⋅KQoverJd - 1.0⋅g₂)
    ──────────────────────────────────────────────────────────────────────────────
          ⎞ ⎛                        2
    .0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅

                                                                              1.0⋅
    ──────────────────────────────────────────────────────────────────────────────

    ⋅(-KQoverJd - 1.0⋅g₂)⋅(-1.0⋅KQoverJd - 1.0⋅g₂)
    ──────────────────────────────────────────────────────────────────────────────
                    ⎞ ⎛                        2
    overJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQove

         ⎛       ⎛                    2
    ⋅g₁)⋅⎝1.0⋅g₃⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)
    ──────────────────────────────────────────────────────────────────────────────
                                                       ⎛                        2
                          1.0⋅(-1.0⋅KDoverJd - 1.0⋅g₁)⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)
    ──── + ───────────────────────────────────────────────────────────────────────
       ⎞                        ⎛                    2
    g₂)⎠   (-KDoverJd - 1.0⋅g₁)⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(KQo

                                                                                ⎛
    (-1.0⋅g₃⋅(KQoverJd + 1.0⋅g₂) - 1.0⋅(HoverJd + 1.0⋅g₄)⋅(-KDoverJd - 1.0⋅g₁))⋅⎝1
    ──────────────────────────────────────────────────────────────────────────────
                                                                 ⎛
                                    1.0⋅(-1.0⋅KDoverJd - 1.0⋅g₁)⋅⎝1.0⋅(-KDoverJd -
    ────────────── + ─────────────────────────────────────────────────────────────
                 ⎞                        ⎛                    2
    rJd + 1.0⋅g₂)⎠   (-KDoverJd - 1.0⋅g₁)⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.

    ⎞
    ⎠ + 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(1.0⋅g₃⋅(KQoverJd + 1.0⋅g₂) + (HoverJd + 1.0⋅g₄)⋅
    ──────────────────────────────────────────────────────────────────────────────
                              2                                               ⎞
    - 1.0⋅(-KQoverJd - 1.0⋅g₂)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠
    ──────────────────────────────────────────────────────────────────────────────
                   ⎞ ⎛                        2
    verJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQover

                           2                           2
    .0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)  - 1.0⋅(-KQoverJd - 1.0⋅g₂
    ──────────────────────────────────────────────────────────────────────────────
            2                           2
     1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1
    ──────────────────────────────────────────────────────────────────────────────
                             ⎞ ⎛                        2
    0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g

                         ⎞
    (-KDoverJd - 1.0⋅g₁))⎠
    ──────────────────────────────────────────────────────────────────────────────
                 ⎞
                 ⎟ ⎛                    2
    ─────────────⎟⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g
                ⎞⎟
    Jd + 1.0⋅g₂)⎠⎠

                         ⎞
    )⋅(KQoverJd + 1.0⋅g₂)⎠
    ──────────────────────────────────────────────────────────────────────────────
          ⎞                ⎞
    .0⋅g₂)⎠                ⎟ ⎛                    2
    ───────────────────────⎟⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(KQover
                          ⎞⎟
    ₂)⋅(KQoverJd + 1.0⋅g₂)⎠⎠

    ──────────────────────────────────────────────────────────────────────────────

      ⎞ ⎛                        2                                               ⎞
    ₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠

    ──────────────────────────────────────────────────────────────────────────────

                ⎞ ⎛                        2
    Jd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJd

                         - ───────────────────────────────────────────────────────
                                                ⎛
                                                ⎜
                           (-KDoverJd - 1.0⋅g₁)⋅⎜─────────────────────────────────
                                                ⎜⎛                    2
                                                ⎝⎝(-KDoverJd - 1.0⋅g₁)  - (-KQover

    ──────────  ──────────────────────────────────────────────────────────────────
                                     ⎛
             ⎞                       ⎜
    + 1.0⋅g₂)⎠  (-KDoverJd - 1.0⋅g₁)⋅⎜────────────────────────────────────────────
                                     ⎜⎛                    2
                                     ⎝⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂

                                                                         1.0⋅(-1.0
    ──────────────────────────────────────────────────────────────────────────────

                 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(-1.0⋅KQoverJd - 1.0⋅g₂)
    ──────────────────────────────────────────────────────────────────────────────
                                    ⎞ ⎛                        2
    Jd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd

                                                              1.0⋅(-1.0⋅KQoverJd -
    ──────────────────────────────────────────────────────────────────────────────

      1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(-1.0⋅KQoverJd - 1.0⋅g₂)
    ──────────────────────────────────────────────────────────────────────────────
                         ⎞ ⎛                        2
    )⋅(KQoverJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(

                        ⎛                          ⎛                    2
    ⋅KDoverJd - 1.0⋅g₁)⋅⎝- 1.0⋅(-HoverJd - 1.0⋅g₄)⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQov
    ──────────────────────────────────────────────────────────────────────────────
                                                                                ⎛
                                                    1.0⋅(-1.0⋅KDoverJd - 1.0⋅g₁)⋅⎝
    ────────────────────────────── + ─────────────────────────────────────────────
                                 ⎞                        ⎛                    2
    - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠   (-KDoverJd - 1.0⋅g₁)⋅⎝(-KDoverJd - 1.0⋅g₁)  -

             ⎛                          ⎛                    2
     1.0⋅g₂)⋅⎝- 1.0⋅(-HoverJd - 1.0⋅g₄)⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅
    ──────────────────────────────────────────────────────────────────────────────
                                                                      ⎛
                                         1.0⋅(-1.0⋅KDoverJd - 1.0⋅g₁)⋅⎝1.0⋅(-KDove
    ─────────────────── + ────────────────────────────────────────────────────────
                      ⎞                        ⎛                    2
    KQoverJd + 1.0⋅g₂)⎠   (-KDoverJd - 1.0⋅g₁)⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd

                                      ⎞
    erJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠ + 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(-1.0⋅g₃⋅(-KDov
    ──────────────────────────────────────────────────────────────────────────────
                            2                           2
    1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)  - 1.0⋅(-KQoverJd - 1.0⋅g
    ──────────────────────────────────────────────────────────────────────────────
                                             ⎞ ⎛                        2
     (-KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-

                           ⎞
    g₂)⋅(KQoverJd + 1.0⋅g₂)⎠ + 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(-1.0⋅g₃⋅(-KDoverJd - 1.0⋅
    ──────────────────────────────────────────────────────────────────────────────
                 2                           2
    rJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJ
    ──────────────────────────────────────────────────────────────────────────────
                                  ⎞ ⎛                        2
     - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd -

                                                             ⎞
    erJd - 1.0⋅g₁) - (-HoverJd - 1.0⋅g₄)⋅(KQoverJd + 1.0⋅g₂))⎠
    ──────────────────────────────────────────────────────────────────────────────
                          ⎞                ⎞
    ₂)⋅(KQoverJd + 1.0⋅g₂)⎠                ⎟ ⎛                    2
    ───────────────────────────────────────⎟⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd -
                                          ⎞⎟
    KQoverJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠⎠

                                                  ⎞
    g₁) - (-HoverJd - 1.0⋅g₄)⋅(KQoverJd + 1.0⋅g₂))⎠
    ──────────────────────────────────────────────────────────────────────────────
               ⎞                ⎞
    d + 1.0⋅g₂)⎠                ⎟ ⎛                    2
    ────────────────────────────⎟⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(K
                               ⎞⎟
    1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠⎠

    ──────────────────────────────────────────────────────────────────────────────

                                ⎞ ⎛                        2
     1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.

    ──────────────────────────────────────────────────────────────────────────────

                     ⎞ ⎛                        2
    QoverJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQov

    ────────────────────────── - ─────────────────────────────────────────────────
                                 ⎛
                             ⎞   ⎜                                              1.
    0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠   ⎜────────────────────────────────────────────────
                                 ⎜⎛                    2
                                 ⎝⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(K

    ─────────────── - ────────────────────────────────────────────────────────────
                                           ⎛
                  ⎞                        ⎜
    erJd + 1.0⋅g₂)⎠   (-KDoverJd - 1.0⋅g₁)⋅⎜──────────────────────────────────────
                                           ⎜⎛                    2
                                           ⎝⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd -

    ──────────────────────────────────────────────────────────────────────────────

    0⋅(-KQoverJd - 1.0⋅g₂)⋅(-1.0⋅KQoverJd - 1.0⋅g₂)
    ──────────────────────────────────────────────────────────────────────────────
                     ⎞ ⎛                        2
    QoverJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQov

    ──────────────────────────────────────────────────────────────────────────────

            1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(-1.0⋅KQoverJd - 1.0⋅g₂)
    ──────────────────────────────────────────────────────────────────────────────
                               ⎞ ⎛                        2
    1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0

                                                    1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(1.0⋅
    ──────────────────────────────────────────────────────────────────────────────
                                                                  ⎛
                                     1.0⋅(-1.0⋅KDoverJd - 1.0⋅g₁)⋅⎝1.0⋅(-KDoverJd
    ─────────────── + ────────────────────────────────────────────────────────────
                  ⎞                        ⎛                    2
    erJd + 1.0⋅g₂)⎠   (-KDoverJd - 1.0⋅g₁)⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1

           1.0⋅(1.0⋅g₃⋅(-KDoverJd - 1.0⋅g₁) + 1.0⋅(-HoverJd - 1.0⋅g₄)⋅(KQoverJd +
    ──────────────────────────────────────────────────────────────────────────────
                                                                            ⎛
                                               1.0⋅(-1.0⋅KDoverJd - 1.0⋅g₁)⋅⎝1.0⋅(
    ───────────────────────── + ──────────────────────────────────────────────────
                            ⎞                        ⎛                    2
    ⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠   (-KDoverJd - 1.0⋅g₁)⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQ

    g₃⋅(-KDoverJd - 1.0⋅g₁) + 1.0⋅(-HoverJd - 1.0⋅g₄)⋅(KQoverJd + 1.0⋅g₂))
    ──────────────────────────────────────────────────────────────────────────────
             2                           2
    - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJd +
    ──────────────────────────────────────────────────────────────────────────────
                              ⎞ ⎛                        2
    .0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅

             ⎛                        2                           2
    1.0⋅g₂))⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)  - 1.0⋅(-KQover
    ──────────────────────────────────────────────────────────────────────────────
                       2                           2
    -KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(K
    ──────────────────────────────────────────────────────────────────────────────
                                        ⎞ ⎛                        2
    overJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQove

    ──────────────────────────────────────────────────────────────────────────────
           ⎞                ⎞
    1.0⋅g₂)⎠                ⎟ ⎛                    2
    ────────────────────────⎟⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅g₂)⋅(KQove
                           ⎞⎟
    g₂)⋅(KQoverJd + 1.0⋅g₂)⎠⎠

                                    ⎞
    Jd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠
    ──────────────────────────────────────────────────────────────────────────────
                     ⎞                ⎞
    QoverJd + 1.0⋅g₂)⎠                ⎟ ⎛                    2
    ──────────────────────────────────⎟⋅⎝(-KDoverJd - 1.0⋅g₁)  - (-KQoverJd - 1.0⋅
                                     ⎞⎟
    rJd - 1.0⋅g₂)⋅(KQoverJd + 1.0⋅g₂)⎠⎠

    ──────────────────────────────────────────────────────────────────────────────

                 ⎞ ⎛                        2
    rJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)⋅(KQoverJd

    ──────────────────────────────────────────────────────────────────────────────

                           ⎞ ⎛                        2
    g₂)⋅(KQoverJd + 1.0⋅g₂)⎠⋅⎝1.0⋅(-KDoverJd - 1.0⋅g₁)  - 1.0⋅(-KQoverJd - 1.0⋅g₂)

                         ⎤
                         ⎥
    ───────────          ⎥
                         ⎥
              ⎞          ⎥
     + 1.0⋅g₂)⎠          ⎥
                         ⎥
                         ⎥
                         ⎥
                         ⎥
                         ⎥
    ─────────────────────⎥
                         ⎥
                        ⎞⎥
    ⋅(KQoverJd + 1.0⋅g₂)⎠⎥
                         ⎥
                         ⎦

$G_0$ is the correct size, so I'm going to trust that the mess above is correct:  I am sure of the form of the equation for $B^*$ above, so I'm going with it.

#### c)
The control law is tuned to zero out angular error in the steady-state.  What this means is that, when there is an external disturbance $\omega_{xE}, \omega_{yE}$, the feedback command generated will serve to spin the rotors at the same rate as the external disturbances.  The measured signals $\omega_{xB}, \omega_{yB}$ then represent a measurement of the disturbances; i.e. how the gyro's exterior is rotating.

### Problem 6.10

#### a)

Butterworth polynomials are deliberately constructed to have the frequency response discussed.

\begin{eqnarray}
|H(j \omega)| &=& \sqrt{H(j \omega)H(-j \omega)} \\
&=& \frac{1}{\sqrt{1 + \bigl ( \frac{\omega}{\omega_0} \bigr )^{2k}}}
\end{eqnarray}

The idea as to why to use this form will become clear in the next part of the problem.

#### b)

```python
K = np.array([1, 10, 100])
# normalize omega_0 = 1
omega = np.linspace(0, 1, 40)
plt.figure()
for k in K:
    H = 1. / np.sqrt(1. + np.power(omega, 2.*k))
    lbl='k={}'.format(k)
    plt.plot(omega, control.mag2db(H), label=lbl)
plt.legend()
plt.xlabel('ratio of omega/omega_0')
plt.ylabel('magnitude of frequency response')
plt.show()
```

You can see that, for increasing $k$, the frequency response magnitude doesn't drop until the cutoff frequency, $\omega_0$.  The dropoff is so fast for $k=100$, as can be seen above, that this filter would serve as a great tool for accepting all signals with frequency $\le \omega_0$ and rejecting all frequencies $\ge \omega_0$.

#### c)

The "maximal-flatness", aside from being graphically-apparent from the chart above, can be seen as a consequence of the form of the frequency response function $|H(j \omega)|$.  There is a nice discussion in the [wikipedia](https://en.wikipedia.org/wiki/Butterworth_filter) page, so I won't bother copying it here.


# 3.2 Model-Based Predictive Controls
Prepared by (C) Vasken Dermardiros

The objective here is not to teach you optimization or even what is model-based predictive controls. There are just too many excellent references out there, I would not want to embarrass myself by producing low grade explanations.

You can start by reading [this webpage](http://www.me.berkeley.edu/~yzhwang/MPC/optimization.html) made by grad students from Berkeley's [MPC lab](http://www.mpc.berkeley.edu), [this document](http://kom.aau.dk/~mju/downloads/otherDocuments/MPCusingCVX.pdf) demonstrates a simple application that uses [CVX](http://cvxr.com/cvx/) which is in part developed by Prof. Stephen Boyd from Stanford. He also has [his lectures on convex optimization](https://lagunita.stanford.edu/courses/Engineering/CVX101/Winter2014/about) posted online -- free -- along with his co-authored [book](http://www.stanford.edu/~boyd/cvxbook/), which is excellent -- also free.
You can also take a look at [this article](http://www.sciencedirect.com/science/article/pii/S0378778811004105) from the OptiControl team of Switzerland.

What is demo-ed in this notebook is not the "state of the art", it, however, provides an easy way to try a few ideas. If your objective is speed, I suggest you look into using compiled solvers tailored to the problem, *e.g.* [CVXGEN](http://cvxgen.com/docs/index.html), for non-commercial use.

I will start by showing what an optimization looks like since too many throw around that word not knowing what they're talking about. Running 10000 simulations with different parameters and returning the best is **not** an optimization; it's more of a design exploration. In that sense, genetic algorithms cannot guarantee an *optimal* result more so than a very good design. Genetic algorithms are explorative algorithms, whereas optimization is exploitative. A hybrid is possible -- see memetic evolutionary algorithms.

Right after, we will go through 2 simple examples to see how MPC can be used to minimize energy use in a zone. The first example is a simple room with an ideal HVAC system that adds/removes heat directly. The second example uses the same room and, additionally, has a radiant slab system.

### General optimization formulation
$\begin{align}
minimize~ & f(x) \\
subject~to~~ & h_i(x) = 0, & i = 1,...,m  \\
            & g_j(x) >= 0, & j = 1,...,n
\end{align}$

The function $f(x)$ will be minimized while the equality constraints of $h_i(x)$ and inequality constraints of $g_j(x)$ must be satisfied. The function $f(x)$ is refered to as the cost function. The constraints can also be included in the cost function as soft constraints. As soft constraints, they may be violated, but at a high cost; whereas as hard constraints, if a given solution violates the constraints, it is completely rejected.

The solution $x^*$ is called the optimal solution *iff* $f(x^*) \leq f(x)~\forall~x$. (If the equality holds, then more than one optimal exists and $x^*$ is refered to as a soft minimum; otherwise, if there is only one optimal, then it is a hard minimum.)

### Optimization formulation for MPC
Taken from [Oldewurtel *et al.* (2012)](http://www.sciencedirect.com/science/article/pii/S0378778811004105)

---
## Example 1: Simple glazed room, MPC to control ideal HVAC system
Here we are considering a south-facing room with a modest window (WWR: 40%):

As we've done in previous chapters, we will model the room as a thermal network:

The HVAC system is the only variable we can control. We could use On-Off controls, Proportional control, PI, PID, or use a schedule, but they don't explicitely consider thermal mass -- which we will see in the next example.

Let's begin!

### MPC Formulation
What I particularly like about MPC is that the controls are explicit to the user. What is the objective? If it's to reduce the energy dispenditure, make that the cost function. Is it to guarantee a tight temperature control? Make that the cost function. Or maybe it's both? No problem, put them both!

Next come the constraints. You don't want to cycle the heat pump? Make it a constraint! Careful though you don't want to over constrain the optimization so that it fails to find feasible solutions. You can always *soften* constraints into penalties towards the cost function and so violating them just a tad won't lead to a failure in the optimization.

**Cost Function**
$\newcommand{\norm}[1]{\left\lVert#1\right\rVert}
J = \norm{ Q_{\text{HVAC}} }_2$

**Constraints: System Dynamics**
${\sum}_{j}{[U_{ij}^{v+t+1} (T_{j}^{v+t+1}-T_{i}^{v+t+1})]}+{\sum}_{k}{[U_{ik}^{v+t+1} (T_{k}^{v+t+1}-T_{i}^{v+t+1})]} - \cfrac{C}{\Delta t} (T_{i}^{v+t+1}-T_{v+i}^{t}) + \dot{Q}_{i}^{v+t+1} = 0, \,\,\, v\in[0,ph-1]$

$t$: simulation timestep
$v$: timestep within prediction horizon
$ph$: prediction horizon

**Constraints**
$T^{v+t+1}_{\text{Room}} - T^{v+t+1}_{\text{Heat, SP}} >= 0, \,\,\, v\in[0,ph-1]  \\
T^{v+t+1}_{\text{Cool, SP}} - T^{v+t+1}_{\text{Room}}  >= 0, \,\,\, v\in[0,ph-1]  \\
Q_{\text{Capacity, Heat}} - Q_{\text{HVAC}}^{v+t+1}  >= 0, \,\,\, v\in[0,ph-1]  \\
- Q_{\text{Capacity, Cool}} - Q_{\text{HVAC}}^{v+t+1}  >= 0, \,\,\, v\in[0,ph-1]  \\$

**Other Constraints that can be Considered**
Limiting the rate of change (slew rate):
$C_{\text{ROC}} = \norm{ T^{t+1}-T^t }_2$

Peak power:
$C_{\text{P}} = \$/kW \cdot \text{max}(\dot{Q}_{\text{A}}^+)$

Energy consumption:
$C_{\text{E}} = \frac{\$/kWh \cdot \Delta t}{3600} \sum_{t=0}^T \lvert \dot{Q} \lvert$

---------

Onto Python!

### Load Dependencies

```python
import numpy as np
import matplotlib.pylab as plt
from scipy.optimize import minimize
import time
# from tqdm import tqdm # progress bar
%matplotlib inline
plt.rcParams['figure.figsize'] = (10, 6)
from IPython import display

# Helper functions to keep this notebook simple and tidy
from code_base import simfun
# from code_base import optfun
# from code_base import load_data
```

### Simulation Setup

```python
# Steps per hour, number of timesteps, timestep and number of days to simulate
st = 4
nt, dt, days = int(st*24), 3600/st, 1

# Prediction horizon
ph = 8*st  # hours * steps/hour
```

### Room Model

```python
# Number of nodes: to solve for, with known temperatures, controllable
nN, nM, nS = 1, 1, 1
nSwhere = 1 # which node receives the controlled variable? 0-indexed according to nN
NS = nN + nS
U = np.zeros((nN,nN)) # K/W
F = np.zeros((nN,nM)) # K/W
C = np.zeros((nN))    # J/K

# Nodel connections: here you define how each node is connected to one another
# Node Number: Object
# 0: room air
# No nodal connections between nodes to solve for (only 1!)
# U[0,n] = ...

# Node Number with known temperatures: Object
# 0: ambient air
# Connection between room air node to ambient air node
A, WWR = 3.*4., 0.4
F[0,0] = (    (26.*WWR*A)**-1 +   1.1/(WWR*A) +     (6.*WWR*A)**-1)**-1 + \
         ((26.*(1-WWR)*A)**-1 + 4/((1-WWR)*A) + (6.*(1-WWR)*A)**-1)**-1

# Nodes with capacitance
C[0] = (A*5.)*1.*1.005*40. # 40x multiplier on room capacitance
```

### Initialize temperature and heat source variables; set initial conditions; set boundary conditions; set limits

```python
T, TK, Q, S = np.zeros((nt*days, nN)), np.zeros((nt*days, nM)), np.zeros((nt*days, nN)), np.zeros((nt*days, nS))
T[:] = np.nan # for clearer plots
S[:] = np.nan # for clearer plots
T[0,] = 17.5
TK[:, 0] = simfun.periodic(-10., 10., 15., 86400., dt, nt, days) # ambient temp
Q[:,0] = simfun.halfperiodic(0.4*A*WWR*800., 12., 86400., dt, nt, days)      # solar gains
Q[:,0] += simfun.linearRamp(450., 400., 17., 9., 0., dt, nt, days).flatten() # internal gains + equip
minS = -5000. # cooling capacity
maxS = 5000.  # heating capacity
#HeatSP = simfun.periodic(16., 5., 15., 86400., dt, nt, days)
#CoolSP = simfun.periodic(20., 5., 15., 86400., dt, nt, days)
HeatSP = simfun.linearRamp(21., 5., 18., 6., 0., dt, nt, days)
CoolSP = simfun.linearRamp(26.,-5., 18., 6., 0., dt, nt, days)
```

### Constraints

```python
def getConstraints(i, cons):
    # Current State
    if i == 0:
        cons.append({'type': 'eq', 'fun': lambda x: x[0] - T[ct,0]},)

    # System Dynamics
    cons.append({'type': 'eq', 'fun': lambda x:
                 F[0,0]*(TK[ct+i+1,0]-x[NS*(i+1)]) -
                 C[0]*(x[NS*(i+1)]-x[NS*i]) +
                 x[NS*i+nN] +
                 Q[ct+i+1,0] },)

    # Constraints
    cons.append({'type': 'ineq', 'fun': lambda x: CoolSP[ct+i+1] - x[NS*(i+1)]},)
    cons.append({'type': 'ineq', 'fun': lambda x: x[NS*(i+1)] - HeatSP[ct+i+1]},)
    cons.append({'type': 'ineq', 'fun': lambda x: maxS - x[NS*i+nN]},)
    cons.append({'type': 'ineq', 'fun': lambda x: x[NS*i+nN] - minS},)
    return cons
```

```python
timer = time.time()
optRng = range(days*nt-ph)
for ct in optRng:
    # Cost function
    costfun = lambda x: np.linalg.norm(x[(nN):-(nN+nS):(nN+nS)]) # minimize heat input

    # Initial guess for ct=0, warm start with previous optimal for rest
    if ct ==0: x0 = np.zeros((ph,nN+nS)).reshape(-1,1)
    else: x0 = np.vstack((res.x[(nN+nS)::].reshape(-1,1), np.zeros((nN+nS,1))))

    # Constraints; loop through prediction steps and get constraints for every timestep
    cons = []
    for i in range(ph-1):
        getConstraints(i, cons)
    cons = tuple(cons)

    # Run optimization
    res = minimize(costfun, x0, method='SLSQP', constraints=cons,
                   options={'ftol': 1e-3, 'disp': False, 'maxiter': 50})

    # Break on error
    if res.status != 0:
        print "Optimization Failed!"
        print "Timestep: %i, Reason: %i"%(ct,res.status)
        break

    # Sort and store results
    T[ct+1,] = res.x[nN+nS:2*nN+nS]
    S[ct+1,] = res.x[nN:nN+nS]
    tempT = res.x.reshape(-1,NS)[2:,0:nN]
    tempS = res.x.reshape(-1,NS)[1:-1,nN:nN+nS]
    del cons

    # Plot
    ax1 = plt.subplot2grid((6,1), (0, 0), rowspan=2)
    ax2 = plt.subplot2grid((6,1), (2, 0), rowspan=2, sharex=ax1)
    ax3 = plt.subplot2grid((6,1), (4, 0), sharex=ax1)
    ax4 = plt.subplot2grid((6,1), (5, 0), sharex=ax1)
    ax1.hold(True)
    ax1.plot(T,'g')
    ax1.plot(range(ct+2,ct+ph),tempT, 'g--')
    ax1.axvline(ct+1, color='crimson') # draw control horizon
    ax1.axvline(ct+ph, color='lime')   # draw prediction horizon
    ax1.plot(HeatSP,'r--')
    ax1.plot(CoolSP,'b--')
    ax1.set_ylim([15,32])
    ax1.set_ylabel('Room')
    ax2.plot(S,'r')
    ax2.plot(range(ct+2,ct+ph),tempS, 'r--')
    ax2.axvline(ct+1, color='crimson') # draw control horizon
    ax2.axvline(ct+ph, color='lime')   # draw prediction horizon
    ax2.set_ylabel('HVAC')
    ax3.plot(TK, color='navy')
    ax3.set_ylabel('TK')
    ax4.plot(Q, color='gold')
    ax4.set_ylabel('Gains')
    plt.subplots_adjust(hspace=0)
    display.clear_output(wait=True)
    display.display(plt.gcf())

print "Elapsed time: %s" % (time.time()-timer)
```

---

3.2 Model-Based Predictive Controls.ipynb


# SERIES DE TAYLOR
Juan celeita - Jessica Gutierrez 161003508-161003518. Adjuntamos dos codigos, ambos cumplen la misma funcion pero los trabajamos de forma diferente cada uno.

Completar la tabla para la función $cos(x)$, alrededor del punto $a = 0$, utilizando el número indicado de términos en la aproximación.

```python
import matplotlib.pyplot as plt
from sympy import Symbol
from sympy import diff
from sympy import sin,cos
import numpy as np
from tabulate import tabulate
x=Symbol('x')
fx=cos(x)
fx=fx.evalf( subs={x:0})
a=0
x=[0,0.00001,0.0001,0.001,0.01,0.1,1.0,10.0]
derivadas=[0,-1,0,1]
yq=[0,0,0,0,0,0,0,0]
yv=[0,0,0,0,0,0,0,0]
yb=[0,0,0,0,0,0,0,0]
yn=[0,0,0,0,0,0,0,0]
ym=[0,0,0,0,0,0,0,0]
yp=[0,0,0,0,0,0,0,0]
yr=[0,0,0,0,0,0,0,0]
for i in range(8):
    yc=cos(x[i])
    yr[i]=yc

for i in range(8):
    y1=cos(a)
    yq[i]=y1

for i in range(8):
    yx=derivadas[0]*x[i]+fx
    yv[i]=yx

for i in range(8):
    y=((derivadas[1]*(x[i]**2))/2)+yx
    yb[i]=y

for i in range(8):
    y3=((derivadas[2]*(x[i]**3))/6)+yb[i]
    yp[i]=y3

for i in range(8):
    y4=((derivadas[3]*(x[i]**4))/24)+yp[i]
    yn[i]=y4

h=0;
t=0;
c=0;
r=0;
f=0;
w=0;
q=0;
valorx=[0,0.00001,0.0001,0.001,0.01,0.1,1,10]
yw=[[0,0,0,0,0,0,0],
    [0,0,0,0,0,0,0],
    [0,0,0,0,0,0,0],
    [0,0,0,0,0,0,0],
    [0,0,0,0,0,0,0],
    [0,0,0,0,0,0,0],
    [0,0,0,0,0,0,0],
    [0,0,0,0,0,0,0]]
for i in range (8):
    yw[i][0]=valorx[t]
    t=t+1;
for i in range (8):
    yw[i][1]=yr[c]
    c=c+1;
for i in range (8):
    yw[i][2]=yq[r]
    r=r+1;
for i in range (8):
    yw[i][3]=yv[h]
    h=h+1;
for i in range (8):
    yw[i][4]=yb[f]
    f=f+1;
for i in range (8):
    yw[i][5]=yp[w]
    w=w+1;
for i in range (8):
    yw[i][6]=yn[q]
    q=q+1;
print(tabulate(yw,stralign='decimal', headers=['x', 'exacto', 'Aprox.1', 'Aprox.2', 'Aprox.3', 'Aprox.4', 'Aprox.5'],floatfmt='.11f',tablefmt='fancy_grid'))
```

    ╒════════════════╤════════════════╤═══════════════╤═══════════════╤═════════════════╤═════════════════╤═════════════════╕
    │              x │         exacto │       Aprox.1 │       Aprox.2 │         Aprox.3 │         Aprox.4 │         Aprox.5 │
    ╞════════════════╪════════════════╪═══════════════╪═══════════════╪═════════════════╪═════════════════╪═════════════════╡
    │  0.00000000000 │  1.00000000000 │ 1.00000000000 │ 1.00000000000 │   1.00000000000 │   1.00000000000 │   1.00000000000 │
    ├────────────────┼────────────────┼───────────────┼───────────────┼─────────────────┼─────────────────┼─────────────────┤
    │  0.00001000000 │  0.99999999995 │ 1.00000000000 │ 1.00000000000 │   0.99999999995 │   0.99999999995 │   0.99999999995 │
    ├────────────────┼────────────────┼───────────────┼───────────────┼─────────────────┼─────────────────┼─────────────────┤
    │  0.00010000000 │  0.99999999500 │ 1.00000000000 │ 1.00000000000 │   0.99999999500 │   0.99999999500 │   0.99999999500 │
    ├────────────────┼────────────────┼───────────────┼───────────────┼─────────────────┼─────────────────┼─────────────────┤
    │  0.00100000000 │  0.99999950000 │ 1.00000000000 │ 1.00000000000 │   0.99999950000 │   0.99999950000 │   0.99999950000 │
    ├────────────────┼────────────────┼───────────────┼───────────────┼─────────────────┼─────────────────┼─────────────────┤
    │  0.01000000000 │  0.99995000042 │ 1.00000000000 │ 1.00000000000 │   0.99995000000 │   0.99995000000 │   0.99995000042 │
    ├────────────────┼────────────────┼───────────────┼───────────────┼─────────────────┼─────────────────┼─────────────────┤
    │  0.10000000000 │  0.99500416528 │ 1.00000000000 │ 1.00000000000 │   0.99500000000 │   0.99500000000 │   0.99500416667 │
    ├────────────────┼────────────────┼───────────────┼───────────────┼─────────────────┼─────────────────┼─────────────────┤
    │  1.00000000000 │  0.54030230587 │ 1.00000000000 │ 1.00000000000 │   0.50000000000 │   0.50000000000 │   0.54166666667 │
    ├────────────────┼────────────────┼───────────────┼───────────────┼─────────────────┼─────────────────┼─────────────────┤
    │ 10.00000000000 │ -0.83907152908 │ 1.00000000000 │ 1.00000000000 │ -49.00000000000 │ -49.00000000000 │ 367.66666666667 │
    ╘════════════════╧════════════════╧═══════════════╧═══════════════╧═════════════════╧═════════════════╧═════════════════╛

```python
from math import factorial
from math import cos
import matplotlib.pyplot as plt
import numpy as np
import pandas as pd

x1=0
x2=0.00001
x3=0.0001
x4=0.001
x5=0.01
x6=0.1
x7=1
x8=10

c0 = 1
c1 = 0
c2 = -1/factorial(2)
c3 = 1/factorial(4)

data = {'Angulo x' : [0,"{0:.5f}".format(0.00001), 0.0001,0.001, 0.01, 0.1, 1, 10],
        'Exacto' : [cos(x1),"{0:.11f}".format(cos(x2)),"{0:.9f}".format(cos(x3)),"{0:.16f}".format(cos(x4)),"{0:.16f}".format(cos(x5)),"{0:.16f}".format(cos(x6)),"{0:.16f}".format(cos(x7)),"{0:.16f}".format(cos(x8))],
        'aprox. 1' : [c0+c1*x1, c0+c1*x2, c0+c1*x3, c0+c1*x4, c0+c1*x5, c0+c1*x6, c0+c1*x7, c0+c1*x8],
        'aprox. 2' : [c0+c1*x1, c0+c1*x2, c0+c1*x3, c0+c1*x4, c0+c1*x5, c0+c1*x6, c0+c1*x7, c0+c1*x8],
        'aprox. 3' : [c0+c1*x1+c2*(x1**2),"{0:.11f}".format(c0+c1*x2+c2*(x2**2)), "{0:.9f}".format(c0+c1*x3+c2*(x3**2)),"{0:.7f}".format(c0+c1*x4+c2*(x4**2)),"{0:.5f}".format(c0+c1*x5+c2*(x5**2)),"{0:.3f}".format(c0+c1*x6+c2*(x6**2)),"{0:.2f}".format(c0+c1*x7+c2*(x7**2)), c0+c1*x8+c2*(x8**2)],
        'aprox. 4' : [c0+c1*x1+c2*(x1**2),"{0:.11f}".format(c0+c1*x2+c2*(x2**2)), "{0:.9f}".format(c0+c1*x3+c2*(x3**2)),"{0:.7f}".format(c0+c1*x4+c2*(x4**2)),"{0:.5f}".format(c0+c1*x5+c2*(x5**2)),"{0:.3f}".format(c0+c1*x6+c2*(x6**2)),"{0:.2f}".format(c0+c1*x7+c2*(x7**2)), c0+c1*x8+c2*(x8**2)],
        'aprox. 5' : [c0+c1*x1+c2*(x1**2)+c3*(x1**4),"{0:.11f}".format(c0+c1*x2+c2*(x2**2)+c3*(x2**4)),"{0:.9f}".format(c0+c1*x3+c2*(x3**2)+c3*(x3**4)),"{0:.17f}".format(c0+c1*x4+c2*(x4**2)+c3*(x4**4)),"{0:.16f}".format(c0+c1*x5+c2*(x5**2)+c3*(x5**4)),"{0:.16f}".format(c0+c1*x6+c2*(x6**2)+c3*(x6**4)),"{0:.16f}".format(c0+c1*x7+c2*(x7**2)+c3*(x7**4)),"{0:.16f}".format(c0+c1*x8+c2*(x8**2)+c3*(x8**4))]}
df = pd.DataFrame(data)
df
```

      Angulo x
      Exacto
      aprox. 1
      aprox. 2
      aprox. 3
      aprox. 4
      aprox. 5

      0
      0
      1
      1.0
      1.0
      1
      1
      1

      1
      0.00001
      0.99999999995
      1.0
      1.0
      0.99999999995
      0.99999999995
      0.99999999995

      2
      0.0001
      0.999999995
      1.0
      1.0
      0.999999995
      0.999999995
      0.999999995

      3
      0.001
      0.9999995000000417
      1.0
      1.0
      0.9999995
      0.9999995
      0.99999950000004167

      4
      0.01
      0.9999500004166653
      1.0
      1.0
      0.99995
      0.99995
      0.9999500004166667

      5
      0.1
      0.9950041652780258
      1.0
      1.0
      0.995
      0.995
      0.9950041666666667

      6
      1
      0.5403023058681398
      1.0
      1.0
      0.50
      0.50
      0.5416666666666666

      7
      10
      -0.8390715290764524
      1.0
      1.0
      -49
      -49
      367.6666666666666288