## Chapter 4

```python
import sys
from importlib import reload
sys.path.append("..")
```

```python
## INCLUDES
from sympy import *
init_printing(use_latex='mathjax')
s, w = symbols('s w', real=True)
import numpy as np
import matplotlib.pyplot as plt
import control
import Ch3.utilities as ch3_utils
reload(ch3_utils)
```

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

```python
import Ch4.utilities as ch4_utils
reload(ch4_utils)
```

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

### Problem 4.1
#### a)
The difference feedback rule can be used to compose the $\mathbf{\dot x} = A \mathbf{x} + B \mathbf{u}$ and $G$ blocks using a relationship from pg. 117, namely:

\begin{eqnarray}
H(s) = H_1(s)[I + H_2(s)H_1(s)]^{-1}.
\end{eqnarray}

$H_1(s) \equiv (sI-A)^{-1} B \triangleq \Phi(s) B$ comes from the state-space model and $H_2(s) \equiv G$.  It follows that the single-loop feedback transfer function $H(s)$ is

\begin{eqnarray}
H'(s) = \Phi(s) B[I + G \Phi(s) B]^{-1}.
\end{eqnarray}

Finally, use the tandem rule $H(s) = H_2(s) H_1(s)$, with $H_2(s) \equiv C$ and $H_1(s) \equiv H'(s)$ above to get the final reduced transfer function from $\mathbf{u_0}(s)$ to $\mathbf{y}(s)$:

\begin{eqnarray}
H(s) = C \Phi(s) B[I + G \Phi(s) B]^{-1}.
\end{eqnarray}

#### b)
If $z$ is a zero of the open-loop process $\Phi(s)B$, then $\det(\Phi(z)B) = 0$.  It follows that $\det(C \Phi(z) B) = \det(C) \det(\Phi(z)B) = 0$ _regardless of the matrices_ $C$ and $G$!  Therefore, $z$ must also be a zero of the closed-loop process.

A nicer approach to showing this equivalence via a generalized eigenvalue problem is shown [here](https://ocw.mit.edu/courses/aeronautics-and-astronautics/16-30-feedback-control-systems-fall-2010/lecture-notes/MIT16_30F10_lec08.pdf).  Basically, it can be shown that the closed-loop system is the same as the open-loop system up to some nonsingular transformation:
\begin{eqnarray}
T = \begin{pmatrix}
I & 0 \\
-G & I
\end{pmatrix}.
\end{eqnarray}

From this fact, it is clear that, if $z$ is a zero of the open-loop process (i.e. it is a singular value of the matrix resulting from the generalized eigenvalue problem) then it will be a zero of the closed-loop process because, again, it will be a singular value of the matrix arising from the generalized eigenvalue problem with state-feedback.

### Problem 4.2

#### a)

```python
g = symbols('g', real=True)
A = Matrix([[-1.5, 0.5, 0], [0.5, -1, 0.5], [0, 0.5, -1.5]])
Phi = ch3_utils.computeResolvent(A)
C = Matrix([[0,0,1]])
B = Matrix([[1],[0],[0]])
E = Matrix([[0],[0],[1]])
H = g*C*Phi*B
Iden = eye(1)
H1 = simplify(H * (Iden + H)**-1)[0]
print('Transfer function H1 (input to v3) is')
pprint(H1)
H2 = simplify(C*Phi*E)[0]
print('Transfer function H2 (disturbance to v3) is')
pprint(H2)
```

    Transfer function H1 (input to v3) is
                                        0.0833333333333333⋅g
    ──────────────────────────────────────────────────────────────────────────────
                                              3                     2
    0.0833333333333333⋅g + 0.333333333333333⋅s  + 1.33333333333333⋅s  + 1.58333333

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

    333333⋅s + 0.5
    Transfer function H2 (disturbance to v3) is
         ⎛                   2                                          ⎞
     1.0⋅⎝0.333333333333333⋅s  + 0.833333333333333⋅s + 0.416666666666667⎠
    ─────────────────────────────────────────────────────────────────────
                       3                     2
    0.333333333333333⋅s  + 1.33333333333333⋅s  + 1.58333333333333⋅s + 0.5

#### b)

```python
# need to use Routh-Hurwitz here
n, d = fraction(H1)
pprint(d)
Dcoeffs = [0.0625, 0.25, 0.296875, 0.09375+0.015625*g]
dets = ch4_utils.hurwitz(Dcoeffs)
pprint(dets)
```

                                              3                     2
    0.0833333333333333⋅g + 0.333333333333333⋅s  + 1.33333333333333⋅s  + 1.58333333

    333333⋅s + 0.5
    ⎡                               2                ⎤
    ⎣4.0, -0.25⋅g + 17.5, - 0.0625⋅g  + 4.0⋅g + 26.25⎦

The Hurwitz conditions (see above) are:
\begin{eqnarray}
4 &>& 0 \\
17.5 - 0.25 g &>& 0 \\
-0.0625g^2 + 4g + 26.25 &>& 0
\end{eqnarray}
The first condition puts no constraints on g, the second requires $g < 70$, and the third requires a bit of work. By the quadratic equation, the two roots are $g = \frac{-4 \pm \sqrt{16 - 4*0.0625*26.25}}{-2*0.0625}$.  The final condition yields a concave-down parabolic equation in $g$, the roots of which are $\approx 24.576$ and $\approx 56.576$.  This condition is the most restrictive of the three, so conclude that $\approx 24.576 < g < \approx 56.576$ is the valid stability range for $g$.

Naively, it makes sense that driving the system with negative feedback would drive the system unstable, since the feedback would continue to drive the error larger and larger.

#### c)
The open loop transfer function is $C \Phi(s) B$.  The [Python Control Systems Library](https://python-control.readthedocs.io/en/latest/index.html) has functions to do frequency domain plots.

```python
H = simplify(C * Phi * B)
n, d = fraction(H[0])
num = np.array([float(n)])
den = np.array([float(p) for p in poly(d).all_coeffs()])
G = control.TransferFunction(num, den)
control.root_locus(G, PrintGain=False)
plt.show()
```

#### d)

```python
plt.figure()
control.bode_plot(G)
plt.figure()
control.nyquist_plot(G)
plt.show()
```

#### e)
Following signal flow around the loop yields the following error equation:
\begin{eqnarray}
e(s) (1 + g C \Phi(s) B) &=& u_r(s) - C \Phi(s) E v_0(s) \\
\implies e(s) = \frac{u_r(s) - C \Phi(s) E v_0(s)}{1 + g C \Phi(s) B}
\end{eqnarray}

The steady-state error is computed by the final-value theorem:
\begin{eqnarray}
e_{ss} &=& \lim_{s \to 0} s e(s)
&=& \lim_{s \to 0} \frac{s \bigl ( e_d(s) - s C \Phi(s) E v_0(s) \bigr )}{1 + g C \Phi(s) B}
\end{eqnarray}

### Problem 4.3

#### a)

```python
h = symbols('h')
A = Matrix([[-1.5, 0.5, 0], [0.5, -1, 0.5], [0, 0.5, -1.5]])
Phi = ch3_utils.computeResolvent(A)
C = Matrix([[0,0,1]])
B = Matrix([[1],[0],[0]])
E = Matrix([[0],[0],[1]])
K = g + h/s
H = K*C*Phi*B
Iden = eye(1)
H1 = simplify(H * (Iden + H)**-1)[0]
#H2 = simplify(C*Phi*E)[0]
#print('Transfer function H2 (disturbance to v3) is')
#pprint(H2)

# need to use Routh-Hurwitz here
n, d = fraction(H1)
pprint(d)
Dcoeffs = [0.0625, 0.25, 0.296875, 0.09375+0.015625*g, 0.015625*h]
dets = ch4_utils.hurwitz(Dcoeffs)
pprint(dets)
```

                                                                         3
    0.0833333333333333⋅g⋅s + 0.0833333333333333⋅h + 0.333333333333333⋅s⋅s  + 1.333

                   2
    33333333333⋅s⋅s  + 1.58333333333333⋅s⋅s + 0.5⋅s
    ⎡                               2                            ⎛            2
    ⎣4.0, -0.25⋅g + 17.5, - 0.0625⋅g  + 4.0⋅g - 4.0⋅h + 26.25, h⋅⎝- 0.015625⋅g  +

                          ⎞⎤
    1.0⋅g - 1.0⋅h + 6.5625⎠⎦

```python
def f(g, h):
    c1 = (-0.25*g + 17.5 > 0)
    c2 = (-0.0625*g**2 + 4.*g - 4.*h + 26.25 > 0)
    c3 = (h*(-0.15625*g**2 + g - h + 6.5625) > 0)
    return int(c1 and c2 and c3)

vf = np.vectorize(f)
gs = np.linspace(-100, 100, 501)
hs = np.linspace(-100, 100, 501)
X = np.meshgrid(gs,hs)
out = vf(X[0], X[1])
plt.imshow(out, origin='lower', extent=(-100, 100, -100, 100))
plt.xlabel('g1')
plt.ylabel('g2')
plt.title('Stability Region (in yellow)')
plt.show()
```

The plot above shows a rough discretization of the stability region vs $g_1$ and $g_2$ using the Hurwitz criteria.

#### b)

```python
H = simplify(C * Phi * B)[0]
PInoGain = 1. + 1./s
A = np.array([[-1.5, 0.5, 0], [0.5, -1, 0.5], [0, 0.5, -1.5]])
C = np.array([[0,0,1]]).astype('float')
B = np.array([[1],[0],[0]]).astype('float')
D = np.zeros((1, 1))
plantSS = control.StateSpace(A, B, C, D)
plantTF = control.ss2tf(plantSS)
PItf = control.TransferFunction(np.array([1]), np.array([1])) + control.TransferFunction(np.array([1]), np.array([0, 1]))
G = PItf * plantTF
control.root_locus(G, PrintGain=False)
plt.show()
```

#### c)

```python
plt.figure()
control.bode_plot(G)
plt.figure()
control.nyquist_plot(G)
plt.show()
```

### Problem 4.4
#### a)
\begin{eqnarray}
A &=& \begin{pmatrix}
-0.746 & 0.006 & 0.001 & 0.0369 \\
-12.9 & -0.746 & 0.387 & 0 \\
4.31 & 0.024 & -0.0174 & 0 \\
0 & 1 & 0 & 0 \\
\end{pmatrix}, \\
B &=& \begin{pmatrix}
0.0012 & 0.0092 \\
6.05 & 0.952 \\
-0.416 & -1.76 \\
0 & 0
\end{pmatrix}
\end{eqnarray}

#### b)

```python
A = np.array([[-.746, .006, .001, .0369], [-12.9, -.746, .387, 0], [4.31, .024, -.0174, 0], [0, 1, 0, 0]])
lam, v = np.linalg.eig(A)
pprint(lam)
```

    [-1.31182557+0.j         -0.14760023+0.62779697j -0.14760023-0.62779697j
                             0.09762603+0.j        ]

The dutch roll mode is described by $s=-0.1477 \pm 0.6278j$.  The roll subsidence mode is described by $s = -1.3118$.  The spiral mode is described by $s = 0.0976$.

#### c)

```python
A = Matrix([[-.746, .006, .001, .0369], [-12.9, -.746, .387, 0], [4.31, .024, -.0174, 0], [0, 1, 0, 0]])
Phi = ch3_utils.computeResolvent(A)
# p(s) / deltaA(s)
C = Matrix([[0, 1, 0, 0]])
B = Matrix([[.0012],[6.05],[-.416],[0]])
H = simplify(C*Phi*B)[0]
print('Transfer function p(s) / deltaA(s)')
pprint(H)
n, d = fraction(H)
num = np.array([float(p) for p in poly(n).all_coeffs()])
den = np.array([float(p) for p in poly(d).all_coeffs()])
G = control.TransferFunction(num, den)
print('    -Zeros: {}'.format(G.zero()))
print('    -Poles: {}'.format(G.pole()))
print('')
# r(s) / deltaA(s)
C = Matrix([[0, 0, 1, 0]])
H = simplify(C*Phi*B)[0]
print('Transfer function r(s) / deltaA(s)')
pprint(H)
n, d = fraction(H)
num = np.array([float(p) for p in poly(n).all_coeffs()])
den = np.array([float(p) for p in poly(d).all_coeffs()])
G = control.TransferFunction(num, den)
print('    -Zeros: {}'.format(G.zero()))
print('    -Poles: {}'.format(G.pole()))
print('')
# p(s) / deltaR(s)
C = Matrix([[0, 1, 0, 0]])
B = Matrix([[.0092],[0.952],[-1.76],[0]])
H = simplify(C*Phi*B)[0]
print('Transfer function p(s) / deltaR(s)')
pprint(H)
n, d = fraction(H)
num = np.array([float(p) for p in poly(n).all_coeffs()])
den = np.array([float(p) for p in poly(d).all_coeffs()])
G = control.TransferFunction(num, den)
print('    -Zeros: {}'.format(G.zero()))
print('    -Poles: {}'.format(G.pole()))
print('')# r(s) / deltaR(s)
C = Matrix([[0, 0, 1, 0]])
H = simplify(C*Phi*B)[0]
print('Transfer function r(s) / deltaR(s)')
pprint(H)
n, d = fraction(H)
num = np.array([float(p) for p in poly(n).all_coeffs()])
den = np.array([float(p) for p in poly(d).all_coeffs()])
G = control.TransferFunction(num, den)
print('    -Zeros: {}'.format(G.zero()))
print('    -Poles: {}'.format(G.pole()))
print('')
```

    Transfer function p(s) / deltaA(s)
                     ⎛                  2
                   s⋅⎝4.00821518484166⋅s  + 2.94295614151318⋅s - 0.040112296276666
    ──────────────────────────────────────────────────────────────────────────────
                       4        3                      2
    0.662514906585398⋅s  + 1.0⋅s  + 0.428169338810123⋅s  + 0.309525513714059⋅s - 0

     ⎞
    2⎠
    ─────────────────

    .0352892003445077
        -Zeros: [-0.74761697  0.01338589  0.        ]
        -Poles: [-1.31182557+0.j         -0.14760023+0.62779697j -0.14760023-0.62779697j
      0.09762603+0.j        ]

    Transfer function r(s) / deltaA(s)
                             4                      3                       2
        - 0.297674418604651⋅s  - 0.303219749552773⋅s  + 0.0409134211091232⋅s  + 0.
    ──────────────────────────────────────────────────────────────────────────────
                       5        4                      3                      2
    0.715563506261181⋅s  + 1.0⋅s  + 0.341593516994633⋅s  + 0.282561153974955⋅s  -

    546444831600429⋅s - 0.0611879441152057
    ──────────────────────────────────────────

    0.0755241173407943⋅s + 0.00426505300615385
        -Zeros: [-1.0371158 +0.93322642j -1.0371158 -0.93322642j  0.94370276+0.j
      0.1119    +0.j        ]
        -Poles: [-1.31182557+0.j         -0.14760023+0.62779697j -0.14760023-0.62779697j
      0.1119    +0.j          0.09762603+0.j        ]

    Transfer function p(s) / deltaR(s)
                    ⎛                   2
                  s⋅⎝0.630714191069299⋅s  - 0.0483922088246985⋅s - 0.3073254320922
    ──────────────────────────────────────────────────────────────────────────────
                       4        3                      2
    0.662514906585398⋅s  + 1.0⋅s  + 0.428169338810123⋅s  + 0.309525513714059⋅s - 0

      ⎞
    22⎠
    ─────────────────

    .0352892003445077
        -Zeros: [ 0.73746065 -0.6607346   0.        ]
        -Poles: [-1.31182557+0.j         -0.14760023+0.62779697j -0.14760023-0.62779697j
      0.09762603+0.j        ]

    Transfer function r(s) / deltaR(s)
                             4                     3                      2
         - 1.25939177101968⋅s  - 1.69336386404293⋅s  - 0.544150312701252⋅s  - 0.40
    ──────────────────────────────────────────────────────────────────────────────
                       5        4                      3                      2
    0.715563506261181⋅s  + 1.0⋅s  + 0.341593516994633⋅s  + 0.282561153974955⋅s  -

    7284411131306⋅s + 0.0549589120692665
    ──────────────────────────────────────────

    0.0755241173407943⋅s + 0.00426505300615385
        -Zeros: [-1.23046487+0.j         -0.11301188+0.55151517j -0.11301188-0.55151517j
      0.1119    +0.j        ]
        -Poles: [-1.31182557+0.j         -0.14760023+0.62779697j -0.14760023-0.62779697j
      0.1119    +0.j          0.09762603+0.j        ]

After computing the zeros and poles for each transfer function, it's clear that the pole-zero combinations for $p(s) / \delta_A(s)$ and $r(s) / \delta_R(s)$ are more amenable for control than the other pair; it requires less movement for pole-zero cancellation.

#### d)
I'd need to find an $s_0$ such that the rank of $H(s)$ is reduced when $s=s_0$.  I can't think of a way to do this easily, so I'll just sketch out an approach.

The rank of the resulting transfer matrix $H(s)$, which has all input-output transfer functions represented as $(i, j)$ entries in the matrix ($i$ corresponds to system output, $j$ to input).  The resulting $H(s)$ is $4 \times 2$, so it has rank 2 _at most_.  I would try and find $s_0$ such that the two columns of $H(s)$ are linearly dependent.  Whenever this condition was met, $s_0$ would be a transmission zero for the process.

I'd find the $4 \times 2$ matrix $H(s)$ via the following cell.  I'd first factor out a common denominator and then I'd look at the resulting scaled matrix.  I would then row-reduce the transpose of that matrix to row-reduced echelon form and then solve for $s_0$ that would turn a row into a linear combination of the others.

```python
numOutputs = 4
numInputs = 2
for i in range(numOutputs):
    C = zeros(1,4)
    C[i] = 1.
    nc = []
    dc = []
    for j in range(numInputs):
        if j == 0:
            B = Matrix([[.0012],[6.05],[-.416],[0]])
        else:
            B = Matrix([[.0092],[0.952],[-1.76],[0]])
        H = simplify(C*Phi*B)[0]
        print('H({}, {})'.format(i,j))
        pprint(H)
        print('')

```

    H(0, 0)
            ⎛                      8                       7
        1.0⋅⎝0.000568885787407855⋅s  + 0.0182408866274625⋅s  + 0.130251581209987⋅s
    ──────────────────────────────────────────────────────────────────────────────
                       9                     8                     7
    0.474071489506546⋅s  + 1.37807841284658⋅s  + 1.53269315035438⋅s  + 1.178448994

    6                      5                       4                       3
      + 0.155153309249951⋅s  + 0.0558774649271001⋅s  + 0.0393957508638441⋅s  - 0.0
    ──────────────────────────────────────────────────────────────────────────────
           6                     5                     4                       3
    20566⋅s  + 0.66305902052578⋅s  + 0.11872827480869⋅s  + 0.0473332659022544⋅s  -

                     2                                               ⎞
    114264117146682⋅s  + 0.000732296736037176⋅s - 5.80996009352629e-6⎠
    ─────────────────────────────────────────────────────────────────────
                         2
     0.0315218334642727⋅s  + 0.00398532843042886⋅s - 0.000150510310014108

    H(0, 1)
           ⎛                     8                       7                       6
       1.0⋅⎝0.00436145770346022⋅s  + 0.0112982044779371⋅s  + 0.0235211273647575⋅s
    ──────────────────────────────────────────────────────────────────────────────
                       9                     8                     7
    0.474071489506546⋅s  + 1.37807841284658⋅s  + 1.53269315035438⋅s  + 1.178448994

                          5                        4                        3
     + 0.012380325641365⋅s  - 0.00788862267262728⋅s  - 0.00047821206980725⋅s  - 0.
    ──────────────────────────────────────────────────────────────────────────────
           6                     5                     4                       3
    20566⋅s  + 0.66305902052578⋅s  + 0.11872827480869⋅s  + 0.0473332659022544⋅s  -

                      2                                              ⎞
    0060548840266216⋅s  + 0.00131142935617842⋅s - 6.92911092915793e-5⎠
    ─────────────────────────────────────────────────────────────────────
                         2
     0.0315218334642727⋅s  + 0.00398532843042886⋅s - 0.000150510310014108

    H(1, 0)
                     ⎛                  2
                   s⋅⎝4.00821518484166⋅s  + 2.94295614151318⋅s - 0.040112296276666
    ──────────────────────────────────────────────────────────────────────────────
                       4        3                      2
    0.662514906585398⋅s  + 1.0⋅s  + 0.428169338810123⋅s  + 0.309525513714059⋅s - 0

     ⎞
    2⎠
    ─────────────────

    .0352892003445077

    H(1, 1)
                    ⎛                   2
                  s⋅⎝0.630714191069299⋅s  - 0.0483922088246985⋅s - 0.3073254320922
    ──────────────────────────────────────────────────────────────────────────────
                       4        3                      2
    0.662514906585398⋅s  + 1.0⋅s  + 0.428169338810123⋅s  + 0.309525513714059⋅s - 0

      ⎞
    22⎠
    ─────────────────

    .0352892003445077

    H(2, 0)
                             4                      3                       2
        - 0.297674418604651⋅s  - 0.303219749552773⋅s  + 0.0409134211091232⋅s  + 0.
    ──────────────────────────────────────────────────────────────────────────────
                       5        4                      3                      2
    0.715563506261181⋅s  + 1.0⋅s  + 0.341593516994633⋅s  + 0.282561153974955⋅s  -

    546444831600429⋅s - 0.0611879441152057
    ──────────────────────────────────────────

    0.0755241173407943⋅s + 0.00426505300615385

    H(2, 1)
                             4                     3                      2
         - 1.25939177101968⋅s  - 1.69336386404293⋅s  - 0.544150312701252⋅s  - 0.40
    ──────────────────────────────────────────────────────────────────────────────
                       5        4                      3                      2
    0.715563506261181⋅s  + 1.0⋅s  + 0.341593516994633⋅s  + 0.282561153974955⋅s  -

    7284411131306⋅s + 0.0549589120692665
    ──────────────────────────────────────────

    0.0755241173407943⋅s + 0.00426505300615385

    H(3, 0)
                                       2
                     4.00821518484166⋅s  + 2.94295614151318⋅s - 0.0401122962766662
    ──────────────────────────────────────────────────────────────────────────────
                       4        3                      2
    0.662514906585398⋅s  + 1.0⋅s  + 0.428169338810123⋅s  + 0.309525513714059⋅s - 0

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

    .0352892003445077

    H(3, 1)
                                       2
                    0.630714191069299⋅s  - 0.0483922088246985⋅s - 0.30732543209222
    ──────────────────────────────────────────────────────────────────────────────
                       4        3                      2
    0.662514906585398⋅s  + 1.0⋅s  + 0.428169338810123⋅s  + 0.309525513714059⋅s - 0

    2
    ─────────────────

    .0352892003445077

### Problem 4.5
#### a)

```python
A = np.array([[0, -14, 0, 0], [0, -1, 1, 0], [0, -5, -.5, 0], [0, 0, 1, 0]])
B = np.array([[-1], [-.1], [-9], [0]])
C = np.eye(4)
D = np.zeros((4,1))
sys = control.StateSpace(A, B, C, D)
pprint(sys.pole())
```

    [ 0.  +0.j         0.  +0.j        -0.75+2.2220486j -0.75-2.2220486j]

The open-loop poles are $s=0$, with multiplicity 2, and a complex conjugate pair $s = -0.75 \pm 2.222 i$.

#### b)

```python
# from deltaE to theta
C = Matrix([[0, 0, 0, 1]])
H1 = simplify(C*Phi*B)
print('theta(s) / deltaE(s):')
pprint(H1)
print('')
# from deltaE to deltau
C = Matrix([[1, 0, 0, 0]])
H2 = simplify(C*Phi*B)
print('deltau(s) / deltaE(s):')
pprint(H2)
print('')
```

    theta(s) / deltaE(s):
    ⎡                                      2
    ⎢                - 0.0662514906585398⋅s  + 6.18832648734597⋅s - 2.601427746124
    ⎢─────────────────────────────────────────────────────────────────────────────
    ⎢                   4        3                      2
    ⎣0.662514906585398⋅s  + 1.0⋅s  + 0.428169338810123⋅s  + 0.309525513714059⋅s -

                      ⎤
    29                ⎥
    ──────────────────⎥
                      ⎥
    0.0352892003445077⎦

    deltau(s) / deltaE(s):
    ⎡                             8                     7                      6
    ⎢        - 0.474071489506546⋅s  - 1.02897216797396⋅s  - 0.755030754202703⋅s  -
    ⎢─────────────────────────────────────────────────────────────────────────────
    ⎢                   9                     8                     7
    ⎣0.474071489506546⋅s  + 1.37807841284658⋅s  + 1.53269315035438⋅s  + 1.17844899

                        5                      4                         3
     0.446291427009702⋅s  - 0.187783759714934⋅s  + 0.000198058901817677⋅s  - 0.024
    ──────────────────────────────────────────────────────────────────────────────
            6                     5                     4                       3
    420566⋅s  + 0.66305902052578⋅s  + 0.11872827480869⋅s  + 0.0473332659022544⋅s

                   2                                                      ⎤
    5042133414924⋅s  + 0.00633503699684281⋅s - 0.000363343030334783       ⎥
    ──────────────────────────────────────────────────────────────────────⎥
                          2                                               ⎥
    - 0.0315218334642727⋅s  + 0.00398532843042886⋅s - 0.000150510310014108⎦

### Problem 4.6

```python
K1, K2, K3 = symbols('K1 K2 K3')
Dcoeffs = [1.0, 36.32+3.04*K1, 182.4+110.4*K1, 554.5*K1 - 206.2*K2, -613.9*K1*K2]  # typo in book, 36.62 vs. 36.32
dets = ch4_utils.hurwitz(Dcoeffs)
pprint(dets)
```

    ⎡                           2
    ⎣3.04⋅K₁ + 36.32, 335.616⋅K₁  + 4009.724⋅K₁ + 206.2⋅K₂ + 6624.768, 613.9⋅K₁⋅K₂

                      2
    ⋅(3.04⋅K₁ + 36.32)  + (3.04⋅K₁ + 36.32)⋅(110.4⋅K₁ + 182.4)⋅(554.5⋅K₁ - 206.2⋅K

                              2        ⎛                   3
    ₂) - (554.5⋅K₁ - 206.2⋅K₂) , K₁⋅K₂⋅⎝- 3482911.457536⋅K₁ ⋅K₂ - 114246220.3008⋅K

     3                     2                        2
    ₁  - 40738905.335296⋅K₁ ⋅K₂ - 1364940323.0162⋅K₁  - 59765823.5307839⋅K₁⋅K₂ - 2

                                       2                     ⎞⎤
    255121044.1984⋅K₁ + 26102070.316⋅K₂  + 838604074.50624⋅K₂⎠⎦

```python
def f(k1, k2):
    c1 = 3.04*k1 + 36.32 > 0
    c2 = 335.616*k1**2 + 4009.724*k1 + 206.2*k2 + 6624.768 > 0
    c3 = 613.9*k1*k2*(3.04*k1 + 36.32)**2 + (3.04*k1 + 36.32)*(110.4*k1 + 182.4)*(554.5*k1 - 206.2*k2) - (554.5*k1-206.2*k2)**2 > 0
    c4 = k1*k2*(-3482911.457536 * k1**3 * k2 - 114246220.3008 * k1**3 - 40738905.335296 * k1**2 * k2 - 1364940323.0162 * k1**2 - 59765823.5307839 * k1 * k2 - 2255121044.1984 * k1 + 26102070.316 * k2**2 + 838604074.50624 * k2) > 0
    return int(c1 and c2 and c3 and c4)

vf = np.vectorize(f)
gs = np.linspace(-10, 50, 301)
hs = np.linspace(-10, 50, 301)
X = np.meshgrid(gs,hs)
out = vf(X[0], -X[1])
plt.imshow(out, origin='lower', extent=(-10, 50, -10, 50))
plt.xlabel('k1')
plt.ylabel('k2')
plt.title('Stability Region (in yellow)')
plt.show()
```

The stability region above is larger than the one predicted by the book; the $K_1$ term appears to continue on past $50$.  The $K_2$ term looks about right though, and the Hurwitz matrix (minus the typos) matches the book's.

### Problem 4.7
#### a)

```python
A = np.array([[0, -.00156, -.0711, 0, 0], [0, -.1419, .0711, 0, 0], [0, -.00875, -1.102, 0, 0], [0, -.00128, -.1489, 0, -.0013], [0, .0605, .1489, 0, -.0591]])
B = np.array([[0, -.143, 0], [0, 0, 0], [.392, 0, 0], [0, .108, -.0592], [0, -.0486, 0]])
C = np.eye(5)
D = np.zeros((5,3))
E = np.array([[.2174, 0, 0], [-.074, .1434, 0], [-.036, 0, 0.1814], [0, 0, 0], [0, 0, 0]])

sys = control.StateSpace(A, B, C, D)
pprint(sys.pole())
```

    [ 0.          0.         -0.0591     -0.14254842 -1.10135158]

The open-loop poles are $s=0$, with multiplicity 2, $s=-0.0591$, $s=-0.1425$, and $s=-1.1014$.  All poles are real and negative $\implies$ stability.

#### b)

```python
A = Matrix([[0, -.00156, -.0711, 0, 0], [0, -.1419, .0711, 0, 0], [0, -.00875, -1.102, 0, 0], [0, -.00128, -.1489, 0, -.0013], [0, .0605, .1489, 0, -.0591]])
B = Matrix([[0, -.143, 0], [0, 0, 0], [.392, 0, 0], [0, .108, -.0592], [0, -.0486, 0]])
Phi = ch3_utils.computeResolvent(A)
obs = Matrix([[1, 0, 0, 0, 0], [0, 0, 0, 1, 0]])
print('Observation Matrix:')
pprint(obs)
print('\n\n')

C = Matrix([[1, 0, 0, 0, 0]])

print('x1(s) / u1(s)')
B = Matrix([[0], [0], [.392], [0], [0]])
pprint(simplify(C*Phi*B)[0])
print('\n\n')
print('x1(s) / u2(s)')
B = Matrix([[-.143], [0], [0], [.108], [-.0486]])
pprint(simplify(C*Phi*B)[0])
print('\n\n')
print('x1(s) / u3(s)')
print('\n\n')
B = Matrix([[0], [0], [0], [-.0592], [0]])
pprint(simplify(C*Phi*B)[0])
print('\n\n\n\n')

C = Matrix([[0, 0, 0, 1, 0]])

print('x4(s) / u1(s)')
B = Matrix([[0], [0], [.392], [0], [0]])
pprint(simplify(C*Phi*B)[0])
print('x4(s) / u2(s)')
print('\n\n')
B = Matrix([[-.143], [0], [0], [.108], [-.0486]])
pprint(simplify(C*Phi*B)[0])
print('\n\n')
print('x4(s) / u3(s)')
print('\n\n')
B = Matrix([[0], [0], [0], [-.0592], [0]])
pprint(simplify(C*Phi*B)[0])
print('\n\n')
```

    Observation Matrix:
    ⎡1  0  0  0  0⎤
    ⎢             ⎥
    ⎣0  0  0  1  0⎦

    x1(s) / u1(s)
     ⎛                      2                                             ⎞
    -⎝3.46944695195361e-18⋅s  + 0.0224063027574564⋅s + 0.00321440819358469⎠
    ────────────────────────────────────────────────────────────────────────
                ⎛                   2                            ⎞
              s⋅⎝0.803923144947343⋅s  + 1.0⋅s + 0.126212657769917⎠

    x1(s) / u2(s)
    -0.143
    ───────
       s

    x1(s) / u3(s)

    0

    x4(s) / u1(s)
        ⎛                      5                       4                       3
    1.0⋅⎝3.46944695195361e-18⋅s  - 0.0229175860850446⋅s  - 0.0331574202740587⋅s  -
    ──────────────────────────────────────────────────────────────────────────────
                  ⎛                   5        4                      3
                s⋅⎝0.392634182731949⋅s  + 1.0⋅s  + 0.788530385959402⋅s  + 0.196543

                          2                                               ⎞
     0.00958050451342888⋅s  - 0.000976496519761018⋅s - 3.11024064661291e-5⎠
    ───────────────────────────────────────────────────────────────────────
               2                                              ⎞
    270117005⋅s  + 0.0187406930713857⋅s + 0.000571942471073223⎠
    x4(s) / u2(s)

        ⎛                    5                      4                       3
    1.0⋅⎝0.0424044917350504⋅s  + 0.108024806627665⋅s  + 0.0852229956119204⋅s  + 0.
    ──────────────────────────────────────────────────────────────────────────────
               ⎛                   5        4                      3
             s⋅⎝0.392634182731949⋅s  + 1.0⋅s  + 0.788530385959402⋅s  + 0.196543270

                      2                                              ⎞
    0212728452292586⋅s  + 0.00203368368696928⋅s + 6.23812137003143e-5⎠
    ──────────────────────────────────────────────────────────────────
            2                                              ⎞
    117005⋅s  + 0.0187406930713857⋅s + 0.000571942471073223⎠

    x4(s) / u3(s)

    -0.0592
    ────────
       s

### Problem 4.8
#### a)

```python
A = Matrix([[0, -.00156, -.0711, 0.143*K2, 0], [0, -.1419, .0711, 0, 0], [-.392*K1, -.00875, -1.102, 0, 0], [0, -.00128, -.1489, -.108*K2, -.0013], [0, .0605, .1489, .0486*K1, -.0591]])
B = Matrix([[0, -.143*K2, 0], [0, 0, 0], [.392*K1, 0, 0], [0, .108*K2, -.0592], [0, -.0486*K2, 0]])
C = Matrix([[1, 0, 0, 0, 0]])
res = ch3_utils.computeResolvent(A, imag=False, smplfy=False)
M = simplify(C*res*B)[0]
C = Matrix([[0, 0, 0, 1, 0]])
n, d = fraction(M)
denCoeffs = [-1.44471785472e-8,
             -1.5602952830976e-9*K2 - 1.88246736470016e-8,
             -3.33022512249417e-9 - 2.03306475387617e-9*K2 + 4.01747429984109e-10*K1,
             2.01948391736579e-28 * K1**2 + 1.6407412184584e-10 * K1 * K2 + 8.0427452651872e-11 * K1 - 1.34047556235777e-10,
             2.54400716081478e-14 * K1**2 + 3.32772044714127e-11 * K1 * K2 + 3.27064729083152e-12 * K1 - 1.44771360734639e-11 * K2,
             3.64963267290489e-15 * K1**2 + 1.41111171520274e-12 * K1 * K2
            ]
```

```python
pprint(denCoeffs)
dets = ch4_utils.hurwitz(denCoeffs)
pprint(dets)
```

    ⎡
    ⎣-1.44471785472e-08, -1.5602952830976e-9⋅K₂ - 1.88246736470016e-8, 4.017474299

    84109e-10⋅K₁ - 2.03306475387617e-9⋅K₂ - 3.33022512249417e-9, 2.01948391736579e

          2
    -28⋅K₁  + 1.6407412184584e-10⋅K₁⋅K₂ + 8.0427452651872e-11⋅K₁ - 1.3404755623577

                                  2
    7e-10, 2.54400716081478e-14⋅K₁  + 3.32772044714127e-11⋅K₁⋅K₂ + 3.2706472908315

                                                               2
    2e-12⋅K₁ - 1.44771360734639e-11⋅K₂, 3.64963267290489e-15⋅K₁  + 1.4111117152027

               ⎤
    4e-12⋅K₁⋅K₂⎦
    ⎡                                         2
    ⎣0.108⋅K₂ + 1.303, 1.39783966174986e-20⋅K₁  + 0.00835356183999998⋅K₁⋅K₂ - 0.03

                                 2
    066684939⋅K₁ + 0.015198192⋅K₂  + 0.20825849682⋅K₂ + 0.2910766115775, - 1.95395

                    4                          3                             3
    571996097e-40⋅K₁  - 2.75519646669034e-22⋅K₁ ⋅K₂ + 3.50855640447375e-22⋅K₁  - 9

                         2   2                          2
    .48494258384628e-5⋅K₁ ⋅K₂  + 0.000302242169068602⋅K₁ ⋅K₂ + 0.00017338288443496

        2                             3                            2
    5⋅K₁  - 0.000145736739267888⋅K₁⋅K₂  - 0.00180939346433065⋅K₁⋅K₂  - 0.000540443

                                                                    3
    895709508⋅K₁⋅K₂ - 0.0015206036345384⋅K₁ - 1.16881863548097e-5⋅K₂  - 0.00014101

                 2
    5803891825⋅K₂  + 0.000230988399112185⋅K₂ + 0.00270074245513609, 3.440725348036

             6                          5                             5
    28e-46⋅K₁  + 4.85163211474968e-28⋅K₁ ⋅K₂ - 7.16018487167673e-28⋅K₁  + 1.670205

                   4   2                         4                             4
    83115162e-10⋅K₁ ⋅K₂  - 5.9085332429748e-10⋅K₁ ⋅K₂ - 8.93649489898064e-11⋅K₁  +

                           3   3                         3   2
     2.18729994952404e-7⋅K₁ ⋅K₂  - 6.93934515719473e-7⋅K₁ ⋅K₂  - 3.84669791996951e

         3                            3                         2   4
    -7⋅K₁ ⋅K₂ - 4.03299416938956e-8⋅K₁  + 3.35685701936201e-7⋅K₁ ⋅K₂  + 4.20404894

                2   3                         2   2                         2
    369815e-6⋅K₁ ⋅K₂  + 1.45274612586103e-6⋅K₁ ⋅K₂  + 2.36809806674641e-6⋅K₁ ⋅K₂ +

                           2                            4
     3.56438283685991e-7⋅K₁  + 8.97832931274887e-8⋅K₁⋅K₂  + 1.7084513281062e-6⋅K₁⋅

      3                            2
    K₂  + 7.52054641074542e-6⋅K₁⋅K₂  - 1.24331582487075e-6⋅K₁⋅K₂ - 6.1141183832302

                                    4                         3
    5e-7⋅K₁ - 1.17124228622051e-8⋅K₂  - 1.41308212865308e-7⋅K₂  + 2.31467374367488

          2                              ⎛                        7
    e-7⋅K₂  + 2.70634268792669e-6⋅K₂, K₁⋅⎝- 8.6919280520133e-53⋅K₁  - 1.2256147472

               6                             6                          5   2
    0175e-34⋅K₁ ⋅K₂ + 1.80879917600072e-34⋅K₁  - 4.21925828073097e-17⋅K₁ ⋅K₂  + 1.

                         5                             5
    49260811736038e-16⋅K₁ ⋅K₂ + 2.25752894643146e-17⋅K₁  - 7.15689111377953e-14⋅K₁

    4   3                          4   2                          4
     ⋅K₂  + 2.33012011190331e-13⋅K₁ ⋅K₂  + 1.05903541137873e-13⋅K₁ ⋅K₂ + 1.0188111

                  4                          3   4                          3   3
    9847389e-14⋅K₁  - 2.14490038201434e-11⋅K₁ ⋅K₂  + 6.67172408219932e-11⋅K₁ ⋅K₂

                             3   2                          3
    + 3.72051925909986e-11⋅K₁ ⋅K₂  + 3.34095442670455e-12⋅K₁ ⋅K₂ - 9.0043104386403

            3                          2   5                          2   4
    3e-14⋅K₁  - 3.27877187286534e-11⋅K₁ ⋅K₂  - 4.10648374864036e-10⋅K₁ ⋅K₂  - 1.42

                       2   3                          2   2
    326911132661e-10⋅K₁ ⋅K₂  - 2.33201115749105e-10⋅K₁ ⋅K₂  - 3.45006182467008e-11

       2                             2                             5
    ⋅K₁ ⋅K₂ + 1.54454284236491e-13⋅K₁  - 8.76948092997961e-12⋅K₁⋅K₂  - 1.668680794

                   4                             3                             2
    68044e-10⋅K₁⋅K₂  - 7.34525110712003e-10⋅K₁⋅K₂  + 1.21380984498162e-10⋅K₁⋅K₂  +

                                                         5
     5.90352814140743e-11⋅K₁⋅K₂ + 1.14399756743293e-12⋅K₂  + 1.38021187996769e-11⋅

      4                         3                          2⎞⎤
    K₂  - 2.2608312245195e-11⋅K₂  - 2.64338940631891e-10⋅K₂ ⎠⎦

```python
def f(k1, k2):
    c1 = 0.108*k2 + 1.303 > 0
    c2 = 1.398e-20*k1**2 + 8.3536e-3*k1*k2 -3.067e-2*k1 + 1.520e-2*k2**2 + .2083*k2 + .2911 > 0
    c3 = -1.954e-40*k1**4 - 2.7552e-22*k1**3*k2 + 3.509e-22*k1**3 - 9.485e-5*k1**2*k2**2 + 3.022e-4*k1**2*k2 + 1.734e-4*k1**2 - 1.457e-4*k1*k2**3 - 1.809e-3*k1*k2**2 - 5.404e-4*k1*k2 - 1.521e-3*k1 - 1.169e-5*k2**3 - 1.4101e-4*k2**2 + 2.310e-4*k2 + 2.701e-3 > 0
    c4 = 3.441e-46*k1**6 + 4.852e-28*k1**5*k2 - 7.160e-28*k1**5 + 1.6702e-10*k1**4*k2**2 - 5.909e-10*k1**4*k2 - 8.936e-11*k1**4 + 2.187e-7*k1**3*k2**3 - 6.939e-7*k1**3*k2**2 - 3.847e-7*k1**3*k2 - 4.033e-8*k1**3 + 3.357e-7*k1**2*k2**4 + 4.204e-6*k1**2*k2**3 + 1.453e-6*k1**2*k2**2 + 2.368e-6*k1**2*k2 + 3.564e-7*k1**2 + 8.978e-8*k1*k2**4 + 1.708e-6*k1*k2**3 + 7.521e-6*k1*k2**2 - 1.243e-6*k1*k2 - 6.114e-7*k1 - 1.171e-8*k2**4 - 1.413e-7*k2**3 + 2.3147e-7*k2**2 + 2.706e-6*k2 > 0
    c5 = k1*(-8.692e-53*k1**7 - 1.226e-34*k1**6*k2 + 1.809e-34*k1**6 - 4.219e-17*k1**5*k2**2 + 1.493e-16*k1**5*k2 + 2.258e-17*k1**5 - 7.157e-14*k1**4*k2**3 + 2.330e-13*k1**4*k2**2 + 1.059e-13*k1**4*k2 + 1.0188e-14*k1**4 - 2.145e-11*k1**3*k2**4 + 6.672e-11*k1**3*k2**3 + 3.721e-11*k1**3*k2**2 + 3.341e-12*k1**3*k2 - 9.003e-14*k1**3 - 3.279e-11*k1**2*k2**5 - 4.106e-10*k1**2*k2**4 - 1.423e-10*k1**2*k2**3 - 2.332e-10*k1**2*k2**2 - 3.450e-11*k1**2*k2 + 1.545e-13*k1**2 - 8.769e-12*k1*k2**5 - 1.667e-10*k1*k2**4 - 7.345e-10*k1*k2**3 + 1.214e-10*k1*k2**2 + 5.904e-11*k1*k2 + 1.44e-12*k2**5 + 1.380e-11*k2**4 - 2.261e-11*k2**3 - 2.643e-10*k2**2) > 0
    return int(c1 and c2 and c3 and c4 and c5)

vf = np.vectorize(f)
gs = np.linspace(-25, 5, 301)
hs = np.linspace(-25, 5, 301)
X = np.meshgrid(gs,hs)
out = vf(X[0], -X[1])
plt.imshow(out, origin='lower', extent=(-10, 50, -10, 50))
plt.xlabel('k1')
plt.ylabel('k2')
plt.title('Stability Region (in yellow)')
plt.show()
```

#### b) and c)

```python
A = Matrix([[0, -.00156, -.0711, 0, 0], [0, -.1419, .0711, 0, 0], [0, -.00875, -1.102, 0, 0], [0, -.00128, -.1489, 0, -.0013], [0, .0605, .1489, 0, -.0591]])
B = Matrix([[0, -.143, 0], [0, 0, 0], [.392, 0, 0], [0, .108, -.0592], [0, -.0486, 0]])
# compute resolvent with complex frequency
Phi = ch3_utils.computeResolvent(A, imag=True, smplfy=False)

C = Matrix([[40., 0, 0, 0, 0]])
B = Matrix([[0], [0], [.392], [0], [0]])
H11 = simplify(C*Phi*B)[0]
C = Matrix([[-40., 0, 0, 0, 0]])
B = Matrix([[-.143], [0], [0], [.108], [-.0486]])
H12 = simplify(C*Phi*B)[0]

C = Matrix([[0, 0, 0, 40, 0]])
B = Matrix([[0], [0], [.392], [0], [0]])
H21 = simplify(C*Phi*B)[0]
C = Matrix([[0, 0, 0, -40, 0]])
B = Matrix([[-.143], [0], [0], [.108], [-.0486]])
H22 = simplify(C*Phi*B)[0]

IplusH = Matrix([[1+H11, H12], [H21, 1+H22]])

# compute product of (I+H)* times (I+H) - (I+H)* is the conjugate transpose
product = Matrix([[simplify(conjugate(1+H11)*(1+H11)) + simplify(conjugate(H21)*H21), simplify(conjugate(1+H11)*H12) + simplify(conjugate(H21)*(1+H22))],
                  [simplify(conjugate(H12)*(1+H11)) + simplify(conjugate(1+H22)*H21), simplify(conjugate(H12)*H12) + simplify(conjugate(1+H22)*(1+H22))]])

# compute determinant of (\lambda*I - product) in terms of w - the roots of this are the singular values
sigma = symbols('sigma', real=True)
charPoly = simplify(det(sigma*eye(2) - product))
pprint(charPoly)
```

     ⎛⎛                                          ⎛     2
     ⎜⎜    ⎛       2                           ⎞ ⎜    _             _
    -⎝⎝1.0⋅⎝- 1.0⋅w  + 1.2439⋅ⅈ⋅w + 0.156995925⎠⋅⎝1.0⋅w  + 1.2439⋅ⅈ⋅w - 0.15699592
    ──────────────────────────────────────────────────────────────────────────────

     ⎞
     ⎟ ⎛              4                    3                         2
    5⎠⋅⎝0.02042908⋅ⅈ⋅w  + 0.0295570217936⋅w  - 0.0085402054308382⋅ⅈ⋅w  - 0.0008704
    ──────────────────────────────────────────────────────────────────────────────

                                           ⎛⎛           5                4
                                         ⎞ ⎜⎜          _                _
    63645162769⋅w + 2.77251516600044e-5⋅ⅈ⎠⋅⎝⎝0.00875⋅ⅈ⋅w  - 0.022285375⋅w  - 0.017
    ──────────────────────────────────────────────────────────────────────────────

                3                        2
               _                        _                           _
    57269535⋅ⅈ⋅w  + 0.00438004047828375⋅w  + 0.000417643372855731⋅ⅈ⋅w - 1.27459524
    ──────────────────────────────────────────────────────────────────────────────

             ⎞             5                  4                    3
             ⎟ _          _                  _                    _
    462934e-5⎠⋅w + 0.0378⋅w  + 0.096294933⋅ⅈ⋅w  - 0.0759690566334⋅w  - 0.018962933
    ──────────────────────────────────────────────────────────────────────────────

               2                                                ⎞   ⎛          2
              _                        _                        ⎟   ⎜         _
    3300396⋅ⅈ⋅w  + 0.00181285614381972⋅w + 5.56075496106657e-5⋅ⅈ⎠ - ⎝- 5.72⋅ⅈ⋅w  +
    ──────────────────────────────────────────────────────────────────────────────

                               ⎞
              _                ⎟ ⎛  ⎛       2                           ⎞
     7.115108⋅w + 0.898016691⋅ⅈ⎠⋅⎝w⋅⎝- 1.0⋅w  + 1.2439⋅ⅈ⋅w + 0.156995925⎠ - 1.1148
    ──────────────────────────────────────────────────────────────────────────────

                          ⎞ ⎛           5                4                    3
    48⋅w + 0.15993609408⋅ⅈ⎠⋅⎝0.00875⋅ⅈ⋅w  + 0.022285375⋅w  - 0.01757269535⋅ⅈ⋅w  -
    ──────────────────────────────────────────────────────────────────────────────

                                                                             ⎛
                         2                                                 ⎞ ⎜
    0.00438004047828375⋅w  + 0.000417643372855731⋅ⅈ⋅w + 1.27459524462934e-5⎠⋅⎝0.00
    ──────────────────────────────────────────────────────────────────────────────

           5                4                    3                        2
          _                _                    _                        _
    875⋅ⅈ⋅w  - 0.022285375⋅w  - 0.01757269535⋅ⅈ⋅w  + 0.00438004047828375⋅w  + 0.00
    ──────────────────────────────────────────────────────────────────────────────

                                              ⎞⎞ ⎛
                       _                      ⎟⎟ ⎜    ⎛       2
    0417643372855731⋅ⅈ⋅w - 1.27459524462934e-5⎠⎠⋅⎝1.0⋅⎝- 1.0⋅w  + 1.2439⋅ⅈ⋅w + 0.1
    ──────────────────────────────────────────────────────────────────────────────

              ⎛     2                           ⎞ ⎛              4
            ⎞ ⎜    _             _              ⎟ ⎜             _
    56995925⎠⋅⎝1.0⋅w  + 1.2439⋅ⅈ⋅w - 0.156995925⎠⋅⎝0.02042908⋅ⅈ⋅w  - 0.02955702179
    ──────────────────────────────────────────────────────────────────────────────

        3                         2
       _                         _                         _
    36⋅w  - 0.0085402054308382⋅ⅈ⋅w  + 0.000870463645162769⋅w + 2.77251516600044e-5
    ──────────────────────────────────────────────────────────────────────────────

      ⎞
      ⎟ ⎛          5                  4                    3
    ⋅ⅈ⎠⋅⎝- 0.0378⋅w  + 0.096294933⋅ⅈ⋅w  + 0.0759690566334⋅w  - 0.0189629333300396⋅
    ──────────────────────────────────────────────────────────────────────────────

       2     ⎛           5                4                    3
    ⅈ⋅w  + w⋅⎝0.00875⋅ⅈ⋅w  + 0.022285375⋅w  - 0.01757269535⋅ⅈ⋅w  - 0.0043800404782
    ──────────────────────────────────────────────────────────────────────────────

          2                                                 ⎞
    8375⋅w  + 0.000417643372855731⋅ⅈ⋅w + 1.27459524462934e-5⎠ - 0.0018128561438197
    ──────────────────────────────────────────────────────────────────────────────

                                                                            ⎛⎛
                               ⎞   ⎛        2                             ⎞ ⎜⎜
    2⋅w + 5.56075496106657e-5⋅ⅈ⎠ + ⎝5.72⋅ⅈ⋅w  + 7.115108⋅w - 0.898016691⋅ⅈ⎠⋅⎝⎝1.0⋅
    ──────────────────────────────────────────────────────────────────────────────

     2                           ⎞                                 ⎞
    _             _              ⎟ _            _                  ⎟ ⎛           5
    w  + 1.2439⋅ⅈ⋅w - 0.156995925⎠⋅w + 1.114848⋅w + 0.15993609408⋅ⅈ⎠⋅⎝0.00875⋅ⅈ⋅w
    ──────────────────────────────────────────────────────────────────────────────

                    4                    3                        2
     + 0.022285375⋅w  - 0.01757269535⋅ⅈ⋅w  - 0.00438004047828375⋅w  + 0.0004176433
    ──────────────────────────────────────────────────────────────────────────────

                                        ⎛           5                4
                                      ⎞ ⎜          _                _
    72855731⋅ⅈ⋅w + 1.27459524462934e-5⎠⋅⎝0.00875⋅ⅈ⋅w  - 0.022285375⋅w  - 0.0175726
    ──────────────────────────────────────────────────────────────────────────────

            3                        2
           _                        _                           _
    9535⋅ⅈ⋅w  + 0.00438004047828375⋅w  + 0.000417643372855731⋅ⅈ⋅w - 1.274595244629
    ──────────────────────────────────────────────────────────────────────────────

         ⎞⎞   ⎛                                            ⎛     2
         ⎟⎟   ⎜      ⎛       2                           ⎞ ⎜    _             _
    34e-5⎠⎠ + ⎝- σ⋅w⋅⎝- 1.0⋅w  + 1.2439⋅ⅈ⋅w + 0.156995925⎠⋅⎝1.0⋅w  + 1.2439⋅ⅈ⋅w -
    ──────────────────────────────────────────────────────────────────────────────

               ⎞
               ⎟ ⎛           5                4                    3
    0.156995925⎠⋅⎝0.00875⋅ⅈ⋅w  + 0.022285375⋅w  - 0.01757269535⋅ⅈ⋅w  - 0.004380040
    ──────────────────────────────────────────────────────────────────────────────

                                                                  ⎛           5
              2                                                 ⎞ ⎜          _
    47828375⋅w  + 0.000417643372855731⋅ⅈ⋅w + 1.27459524462934e-5⎠⋅⎝0.00875⋅ⅈ⋅w  -
    ──────────────────────────────────────────────────────────────────────────────

                                                               2 ⎛     2
                        2 ⎛       2                           ⎞  ⎜    _
                       w ⋅⎝- 1.0⋅w  + 1.2439⋅ⅈ⋅w + 0.156995925⎠ ⋅⎝1.0⋅w  + 1.2439⋅

                 4                    3                        2
                _                    _                        _
    0.022285375⋅w  - 0.01757269535⋅ⅈ⋅w  + 0.00438004047828375⋅w  + 0.0004176433728
    ──────────────────────────────────────────────────────────────────────────────
                      2
                     ⎞
      _              ⎟  ⎛           5                4                    3
    ⅈ⋅w - 0.156995925⎠ ⋅⎝0.00875⋅ⅈ⋅w  + 0.022285375⋅w  - 0.01757269535⋅ⅈ⋅w  - 0.00

                                   ⎞
            _                      ⎟ _       ⎛       2                           ⎞
    55731⋅ⅈ⋅w - 1.27459524462934e-5⎠⋅w + 1.0⋅⎝- 1.0⋅w  + 1.2439⋅ⅈ⋅w + 0.156995925⎠
    ──────────────────────────────────────────────────────────────────────────────

                                                                        2 ⎛
                     2                                                 ⎞  ⎜
    438004047828375⋅w  + 0.000417643372855731⋅ⅈ⋅w + 1.27459524462934e-5⎠ ⋅⎝0.00875

     ⎛     2                           ⎞
     ⎜    _             _              ⎟ ⎛              4                    3
    ⋅⎝1.0⋅w  + 1.2439⋅ⅈ⋅w - 0.156995925⎠⋅⎝0.02042908⋅ⅈ⋅w  + 0.0295570217936⋅w  - 0
    ──────────────────────────────────────────────────────────────────────────────

        5                4                    3                        2
       _                _                    _                        _
    ⋅ⅈ⋅w  - 0.022285375⋅w  - 0.01757269535⋅ⅈ⋅w  + 0.00438004047828375⋅w  + 0.00041

                                                                             ⎛
                         2                                                 ⎞ ⎜
    .0085402054308382⋅ⅈ⋅w  - 0.000870463645162769⋅w + 2.77251516600044e-5⋅ⅈ⎠⋅⎝0.02
    ──────────────────────────────────────────────────────────────────────────────
                                            2
                                           ⎞   2
                    _                      ⎟  _
    7643372855731⋅ⅈ⋅w - 1.27459524462934e-5⎠ ⋅w

              4                    3                         2
             _                    _                         _
    042908⋅ⅈ⋅w  - 0.0295570217936⋅w  - 0.0085402054308382⋅ⅈ⋅w  + 0.000870463645162
    ──────────────────────────────────────────────────────────────────────────────

                                 ⎞
        _                        ⎟   ⎛  ⎛       2                           ⎞
    769⋅w + 2.77251516600044e-5⋅ⅈ⎠ + ⎝w⋅⎝- 1.0⋅w  + 1.2439⋅ⅈ⋅w + 0.156995925⎠ - 1.
    ──────────────────────────────────────────────────────────────────────────────

                                ⎛⎛     2                           ⎞
                              ⎞ ⎜⎜    _             _              ⎟ _
    114848⋅w + 0.15993609408⋅ⅈ⎠⋅⎝⎝1.0⋅w  + 1.2439⋅ⅈ⋅w - 0.156995925⎠⋅w + 1.114848⋅
    ──────────────────────────────────────────────────────────────────────────────

                       ⎞
    _                  ⎟ ⎛           5                4                    3
    w + 0.15993609408⋅ⅈ⎠⋅⎝0.00875⋅ⅈ⋅w  + 0.022285375⋅w  - 0.01757269535⋅ⅈ⋅w  - 0.0
    ──────────────────────────────────────────────────────────────────────────────

                                                                          ⎛
                      2                                                 ⎞ ⎜
    0438004047828375⋅w  + 0.000417643372855731⋅ⅈ⋅w + 1.27459524462934e-5⎠⋅⎝0.00875
    ──────────────────────────────────────────────────────────────────────────────

        5                4                    3                        2
       _                _                    _                        _
    ⋅ⅈ⋅w  - 0.022285375⋅w  - 0.01757269535⋅ⅈ⋅w  + 0.00438004047828375⋅w  + 0.00041
    ──────────────────────────────────────────────────────────────────────────────

                                           ⎞⎞ ⎛
                    _                      ⎟⎟ ⎜    ⎛       2
    7643372855731⋅ⅈ⋅w - 1.27459524462934e-5⎠⎠⋅⎝σ⋅w⋅⎝- 1.0⋅w  + 1.2439⋅ⅈ⋅w + 0.1569
    ──────────────────────────────────────────────────────────────────────────────

           ⎛     2                           ⎞
         ⎞ ⎜    _             _              ⎟ ⎛           5                4
    95925⎠⋅⎝1.0⋅w  + 1.2439⋅ⅈ⋅w - 0.156995925⎠⋅⎝0.00875⋅ⅈ⋅w  + 0.022285375⋅w  - 0.
    ──────────────────────────────────────────────────────────────────────────────

                   3                        2
    01757269535⋅ⅈ⋅w  - 0.00438004047828375⋅w  + 0.000417643372855731⋅ⅈ⋅w + 1.27459
    ──────────────────────────────────────────────────────────────────────────────

                  ⎛           5                4                    3
                ⎞ ⎜          _                _                    _
    524462934e-5⎠⋅⎝0.00875⋅ⅈ⋅w  - 0.022285375⋅w  - 0.01757269535⋅ⅈ⋅w  + 0.00438004
    ──────────────────────────────────────────────────────────────────────────────

               2                                                 ⎞
              _                           _                      ⎟ _   ⎛       2
    047828375⋅w  + 0.000417643372855731⋅ⅈ⋅w - 1.27459524462934e-5⎠⋅w - ⎝- 1.0⋅w  +
    ──────────────────────────────────────────────────────────────────────────────

                               ⎛     2                           ⎞
                             ⎞ ⎜    _             _              ⎟ ⎛          5
     1.2439⋅ⅈ⋅w + 0.156995925⎠⋅⎝1.0⋅w  + 1.2439⋅ⅈ⋅w - 0.156995925⎠⋅⎝- 0.0378⋅w  +
    ──────────────────────────────────────────────────────────────────────────────

                   4                    3                         2     ⎛
    0.096294933⋅ⅈ⋅w  + 0.0759690566334⋅w  - 0.0189629333300396⋅ⅈ⋅w  + w⋅⎝0.00875⋅ⅈ
    ──────────────────────────────────────────────────────────────────────────────

      5                4                    3                        2
    ⋅w  + 0.022285375⋅w  - 0.01757269535⋅ⅈ⋅w  - 0.00438004047828375⋅w  + 0.0004176
    ──────────────────────────────────────────────────────────────────────────────

                                         ⎞
    43372855731⋅ⅈ⋅w + 1.27459524462934e-5⎠ - 0.00181285614381972⋅w + 5.56075496106
    ──────────────────────────────────────────────────────────────────────────────

              ⎛⎛           5                4                    3
            ⎞ ⎜⎜          _                _                    _
    657e-5⋅ⅈ⎠⋅⎝⎝0.00875⋅ⅈ⋅w  - 0.022285375⋅w  - 0.01757269535⋅ⅈ⋅w  + 0.00438004047
    ──────────────────────────────────────────────────────────────────────────────

            2                                                 ⎞             5
           _                           _                      ⎟ _          _
    828375⋅w  + 0.000417643372855731⋅ⅈ⋅w - 1.27459524462934e-5⎠⋅w + 0.0378⋅w  + 0.
    ──────────────────────────────────────────────────────────────────────────────

                 4                    3                         2
                _                    _                         _
    096294933⋅ⅈ⋅w  - 0.0759690566334⋅w  - 0.0189629333300396⋅ⅈ⋅w  + 0.001812856143
    ──────────────────────────────────────────────────────────────────────────────

                                   ⎞                                            ⎛
          _                        ⎟   ⎛        2                             ⎞ ⎜
    81972⋅w + 5.56075496106657e-5⋅ⅈ⎠ + ⎝5.72⋅ⅈ⋅w  + 7.115108⋅w - 0.898016691⋅ⅈ⎠⋅⎝-
    ──────────────────────────────────────────────────────────────────────────────

             2                             ⎞
            _             _                ⎟ ⎛           5                4
     5.72⋅ⅈ⋅w  + 7.115108⋅w + 0.898016691⋅ⅈ⎠⋅⎝0.00875⋅ⅈ⋅w  + 0.022285375⋅w  - 0.01
    ──────────────────────────────────────────────────────────────────────────────

                 3                        2
    757269535⋅ⅈ⋅w  - 0.00438004047828375⋅w  + 0.000417643372855731⋅ⅈ⋅w + 1.2745952
    ──────────────────────────────────────────────────────────────────────────────

                ⎛           5                4                    3
              ⎞ ⎜          _                _                    _
    4462934e-5⎠⋅⎝0.00875⋅ⅈ⋅w  - 0.022285375⋅w  - 0.01757269535⋅ⅈ⋅w  + 0.0043800404
    ──────────────────────────────────────────────────────────────────────────────

             2                                                 ⎞⎞⎞
            _                           _                      ⎟⎟⎟
    7828375⋅w  + 0.000417643372855731⋅ⅈ⋅w - 1.27459524462934e-5⎠⎠⎠
    ───────────────────────────────────────────────────────────────

I am not going to work through the remainder of this problem because the algebraic form above looks very nasty.  If I could reduce all of the terms, I would compute the singular values using the characteristic polynomial of $|\lambda I-M|$ as a function of $\omega$.  Also, a call to the matlab routine [`sigma`](https://www.mathworks.com/help/control/ref/sigma.html) would easily solve the problem given the transfer function $I + GH$.

### Problem 4.9

The characteristic equation comes from the denominator of $G(s)$, where
\begin{eqnarray}
G(s) = \frac{KH(s)}{1+KH(s)}
\end{eqnarray}

```python
K = symbols('K')
H = 100980 / (s*(s**3+140.2*s**2+10.449*s+100980))
G = K*H / (1+K*H)
pprint(simplify(G))
```

                        100980⋅K
    ────────────────────────────────────────────────
                 ⎛ 3          2                    ⎞
    100980⋅K + s⋅⎝s  + 140.2⋅s  + 10.449⋅s + 100980⎠

```python
dets = ch4_utils.hurwitz([1., 140.2, 10.449,100980, 100980*K])
pprint(dets)
```

    [140.2, -99515.0502, -1984866919.2⋅K - 10049029769.196, -K⋅(200431861500816.0⋅
    K + 1.01475102609341e+15)]

The Hurwitz criteria cannot be met for all $K$ because there will always be a negative determinant in the Hurwitz process independent of $K$.

### Problem 4.10

```python
num = -1115.*np.array([1., 0., -2228.])
#to get the denom, uncomment the next two lines
#d = expand((0.01*s+1.)*(s**2 + 3.33*s+248.))
#pprint(d)
den = np.array([.01, 1.0333, 5.81, 248.])
G = control.TransferFunction(num, den)
plt.figure()
control.nyquist_plot(G)
plt.figure()
control.bode_plot(G)
plt.show()
```

### Problem 4.11

Using the feedback law $u = K_1(a_{NC} - Z_\alpha \alpha - Z_\delta \delta) - K_2q$, the closed loop system is:

\begin{eqnarray}
A &=& \begin{pmatrix}
\frac{Z_\alpha}{V} & 1 & \frac{Z_\delta}{V} \\
M_\alpha & 0 & M_\delta \\
-\frac{K_1 Z_\alpha}{\tau} & -\frac{K_2}{\tau} & -\frac{K_1 Z_\delta}{\tau} \\
\end{pmatrix}, \\
B &=& \begin{pmatrix}
0 \\
0 \\
\frac{K_1}{\tau}
\end{pmatrix}, \\
C &=& \begin{pmatrix}
Z_\alpha & 0 & Z_\delta
\end{pmatrix}
\end{eqnarray}

```python
V = 1253.
Za = -4170.
Zd = -1115.
Ma = -248.
Md = -662.
tau = .01
A = Matrix([[-3.33, 1., -0.89], [-248., 0., -662.], [417000.*K1, -100.*K2, 111500.*K1]])
pprint(A)
B = Matrix([[0.], [0.], [100.*K1]])
C = Matrix([[-4170., 0., -1115.]])
Phi = ch3_utils.computeResolvent(A, imag=False, smplfy=False)
pprint(C*B)
H = simplify(C*Phi*B)[0]
pprint(H)
```

    ⎡   -3.33        1.0        -0.89   ⎤
    ⎢                                   ⎥
    ⎢  -248.0        0.0       -662.0   ⎥
    ⎢                                   ⎥
    ⎣417000.0⋅K₁  -100.0⋅K₂  111500.0⋅K₁⎦
    [-111500.0⋅K₁]
               100.0⋅K₁⋅(2484020.0⋅s⋅(s + 3.33) - 4170.0⋅s⋅(662.0⋅s + 1983.74) + 6
    ──────────────────────────────────────────────────────────────────────────────
    (248402000.0⋅K₁ + 248.0⋅s)⋅(s⋅(s + 3.33) + 248.0) - (662.0⋅s + 1983.74)⋅(41700

    16036960.0)
    ──────────────────────
    0.0⋅K₁⋅s + 24800.0⋅K₂)

```python
d = expand((24802000.*K1 + 248.*s)*(s*(s+3.33) + 248.0) - (662.*s + 1983.74)*(417000.*K1*s+24800.*K2))
pprint(d)
```

                      2
    - 251252000.0⋅K₁⋅s  - 744628920.0⋅K₁⋅s + 6150896000.0⋅K₁ - 16417600.0⋅K₂⋅s - 4

                          3           2
    9196752.0⋅K₂ + 248.0⋅s  + 825.84⋅s  + 61504.0⋅s

```python
coeffs = [248., 825.84 - 251252000.*K1, 61504. - 744628920.*K1 - 16417600.*K2+61504., -49196752.*K2 + 6150896000.*K1]
dets = ch4_utils.hurwitz(coeffs)
pprint(dets)
```

    ⎡                                                2
    ⎣-1013112.90322581⋅K₁ + 3.33, 3041907931318.94⋅K₁  + 67068074193.5484⋅K₁⋅K₂ -

    537304444.772581⋅K₁ - 22072.0⋅K₂ + 1651.68, (24802000.0⋅K₁ - 198374.0⋅K₂)⋅(-24

    802000.0⋅K₁ + 198374.0⋅K₂ + (1013112.90322581⋅K₁ - 3.33)⋅(3002535.96774194⋅K₁

                          ⎤
    + 66200.0⋅K₂ - 496.0))⎦

```python
def f(k1, k2):
    c1 = -1013112.903*k1 + 3.33 > 0
    c2 = 3041907931318.94*k1**2 + 67068074193.5484*k1*k2 - 537304444.773*k1 - 22072.*k2 + 1651.68 > 0
    c3 = (24802000.*k1 - 198374.*k2)*(-24802000.*k1 + 198374.*k2 + (1013112.903*k1 - 3.33)*(3002535.968*k1 + 66200.*k2 - 496.)) > 0
    return int(c1 and c2 and c3)

vf = np.vectorize(f)
gs = np.linspace(-100, 500, 501)
hs = np.linspace(-100, 500, 501)
X = np.meshgrid(gs,hs)
out = vf(X[0], -X[1])
plt.imshow(out, origin='lower', extent=(-100, 500, -100, 500))
plt.xlabel('k1')
plt.ylabel('k2')
plt.title('Stability Region (in yellow)')
plt.show()
```

#### b)
It's a little tough to tell from the plot, but it looks like the system cannot be made stable when $K_2$ is 0; i.e. when the feedback uses $\alpha$ and $\delta$ alone.  Only when $K_2$ is increased to around $50$ can the system be made stable.  Moreover, the range of stable $K_1$ (i.e. gain margin) increases with increasing $K_2$.  These two facts make it easy to justify the additional cost of a rate gyro.


```python
from sympy import *
from sympy.plotting import plot
from sympy.physics.continuum_mechanics.beam import Beam
init_printing(use_latex="mathjax")
```

```python
E, I = symbols('E, I')
#E,I,P,a,L = symbols('E,I,P,a,L')
R1, M1 = symbols('R1, M1')
L = 144.
E, I = 29e6, 100
b = Beam(L, E, I)
b.apply_load(R1, L, -1)
b.apply_load(M1, L, -2)
b.apply_load(-15e3, 8*12, -1)
b.apply_load(-2000/12, 0, 0, end=4*12)
b.bc_deflection = [(L, 0),]
b.solve_for_reaction_loads(R1,M1)
```

```python
df = b.deflection()
ss = b.shear_force()
mm = b.bending_moment()
b.reaction_loads
```

$$\left \{ M_{1} : 1681920.0, \quad R_{1} : 23016.0\right \}$$

```python
plot(ss, ("x",0,L))
```

```python
plot(mm, ("x",0,L))
```

```python

```


```
from sympy import symbols
from sympy.physics.mechanics import *

n = 3

q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
u1, u2 = dynamicsymbols('u1 u2')
u1d, u2d = dynamicsymbols('u1 u2', 1)
l, m, g = symbols('l m g')

N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q1, N.z])
B = N.orientnew('B', 'Axis', [q2, N.z])

A.set_ang_vel(N, u1 * N.z)
B.set_ang_vel(N, u2 * N.z)

O = Point('O')
P = O.locatenew('P', l * A.x)
R = P.locatenew('R', l * B.x)

O.set_vel(N, 0)
P.v2pt_theory(O, N, A)
R.v2pt_theory(P, N, B)

ParP = Particle('ParP', P, m)
ParR = Particle('ParR', R, m)

kd = [q1d - u1, q2d - u2]
FL = [(P, m * g * N.x), (R, m * g * N.x)]
BL = [ParP, ParR]

KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)

(fr, frstar) = KM.kanes_equations(FL, BL)
kdd = KM.kindiffdict()
mm = KM.mass_matrix_full
fo = KM.forcing_full
qudots = mm.inv() * fo
qudots = qudots.subs(kdd)
qudots.simplify()
```

```
from sympy import latex
```

```
mlatex(qudots)
```

    '\\left[\\begin{matrix}u_{1}\\\\u_{2}\\\\\\frac{1}{2 l \\left(\\operatorname{cos}^{2}\\left(q_{1} - q_{2}\\right) - 2\\right)} \\left(g \\operatorname{sin}\\left(q_{1} - 2 q_{2}\\right) + 3 g \\operatorname{sin}\\left(q_{1}\\right) + l u^{2}_{1} \\operatorname{sin}\\left(2 q_{1} - 2 q_{2}\\right) + 2 l u^{2}_{2} \\operatorname{sin}\\left(q_{1} - q_{2}\\right)\\right)\\\\\\frac{1}{l \\left(\\operatorname{cos}^{2}\\left(q_{1} - q_{2}\\right) - 2\\right)} \\left(- g \\operatorname{sin}\\left(2 q_{1} - q_{2}\\right) + g \\operatorname{sin}\\left(q_{2}\\right) - 2 l u^{2}_{1} \\operatorname{sin}\\left(q_{1} - q_{2}\\right) - \\frac{l u^{2}_{2}}{2} \\operatorname{sin}\\left(2 q_{1} - 2 q_{2}\\right)\\right)\\end{matrix}\\right]'

```
latex(qudots)
```

    '\\left[\\begin{matrix}\\operatorname{u1}{\\left (t \\right )}\\\\\\operatorname{u2}{\\left (t \\right )}\\\\\\frac{1}{2 l \\left(\\cos^{2}{\\left (\\operatorname{q1}{\\left (t \\right )} - \\operatorname{q2}{\\left (t \\right )} \\right )} - 2\\right)} \\left(g \\sin{\\left (\\operatorname{q1}{\\left (t \\right )} - 2 \\operatorname{q2}{\\left (t \\right )} \\right )} + 3 g \\sin{\\left (\\operatorname{q1}{\\left (t \\right )} \\right )} + l \\operatorname{u1}^{2}{\\left (t \\right )} \\sin{\\left (2 \\operatorname{q1}{\\left (t \\right )} - 2 \\operatorname{q2}{\\left (t \\right )} \\right )} + 2 l \\operatorname{u2}^{2}{\\left (t \\right )} \\sin{\\left (\\operatorname{q1}{\\left (t \\right )} - \\operatorname{q2}{\\left (t \\right )} \\right )}\\right)\\\\\\frac{1}{l \\left(\\cos^{2}{\\left (\\operatorname{q1}{\\left (t \\right )} - \\operatorname{q2}{\\left (t \\right )} \\right )} - 2\\right)} \\left(- g \\sin{\\left (2 \\operatorname{q1}{\\left (t \\right )} - \\operatorname{q2}{\\left (t \\right )} \\right )} + g \\sin{\\left (\\operatorname{q2}{\\left (t \\right )} \\right )} - 2 l \\operatorname{u1}^{2}{\\left (t \\right )} \\sin{\\left (\\operatorname{q1}{\\left (t \\right )} - \\operatorname{q2}{\\left (t \\right )} \\right )} - \\frac{l}{2} \\operatorname{u2}^{2}{\\left (t \\right )} \\sin{\\left (2 \\operatorname{q1}{\\left (t \\right )} - 2 \\operatorname{q2}{\\left (t \\right )} \\right )}\\right)\\end{matrix}\\right]'

```
mlatex(qudots)
```

    '\\left[\\begin{matrix}u_{1}\\\\u_{2}\\\\\\frac{1}{2 l \\left(\\operatorname{cos}^{2}\\left(q_{1} - q_{2}\\right) - 2\\right)} \\left(g \\operatorname{sin}\\left(q_{1} - 2 q_{2}\\right) + 3 g \\operatorname{sin}\\left(q_{1}\\right) + l u^{2}_{1} \\operatorname{sin}\\left(2 q_{1} - 2 q_{2}\\right) + 2 l u^{2}_{2} \\operatorname{sin}\\left(q_{1} - q_{2}\\right)\\right)\\\\\\frac{1}{l \\left(\\operatorname{cos}^{2}\\left(q_{1} - q_{2}\\right) - 2\\right)} \\left(- g \\operatorname{sin}\\left(2 q_{1} - q_{2}\\right) + g \\operatorname{sin}\\left(q_{2}\\right) - 2 l u^{2}_{1} \\operatorname{sin}\\left(q_{1} - q_{2}\\right) - \\frac{l u^{2}_{2}}{2} \\operatorname{sin}\\left(2 q_{1} - 2 q_{2}\\right)\\right)\\end{matrix}\\right]'


Pure Python vs NumPy vs Sklearn Performance Comparison

How much faster does the application run when implemented with NumPy instead of pure Python? What about TensorFlow?

# Technical Background
## Introduction
Linear regression algorithms show a linear relationship between a dependent $y$ and one or more independent $x$ variables, hence called linear regression.

## Usage in Real-Word apps
Linear regression models have many real-world applications in an array of industries such as:
* Economics (e.g. predicting growth)
* Business (e.g. predicting product sales, employee performance)
* Social science (e.g. predicting political leanings from gender or race)
* Healthcare (e.g. predicting blood pressure levels from weight, disease onset from biological factors)

## Important Assumptions in Rgression Aalysis

1. **Linearity**: It is assumed that there is a linear relationship between the dependent $y$ and the independent $X$ variable(s).
2. **Autocorrelation**: There should be no correlation between the residual (error) terms. If some correlation is present then it means that the model is unable to identify some relationship in the data.
3. **Multicollinearity**: There should be no correlation between the independent variables. If the independent variables are moderately or highly correlated then it becomes difficult to find out which variable is contributing to predict the dependent (response) variable.
4. **Homoskedasticity**: The error terms must have constant variance.This non-constant variance arises in the presence of _outliers_. When this phenomenon occurs, the confidence interval for the out of sample prediction tends to be _unrealistically_ wide or narrow.
5. **Normality**: The error terms must be normally distributed. Presence on non-normal distribution suggests that a few unusual data points must be studied closely to make a better model.

> **Note**: Implementation of Univariate Linear Regression in Python without using any machine learning library. The code will be explained step-by-step with provided mathematical background.

## Mathematics behind the model

### Simple Linear Regression
If a single independent variable (x) is used to predict the value of a numerical dependent variable (y).
An estimate of this relationship is given as the linear function:
$$yᵢ = β₀ + β₁Xᵢ + ε$$

**Where:**
* $y$ hat sub i $(ŷᵢ)$ represents the estimated output given the input.
* $Xᵢ$ In this equation, $β₀$ is a bias and $β₁$ is the weight of the model. If the model is charted so that the output is the y-axis and the input is the x-axis, $β₀$ refers to the $y$ - intercept and $β₁$ represents the slope.
* $ε$ represents the random error term. The random error term represents the residual error between an estimated relationship and the actual one. It is subject to variability.

### Multiple Linear Regression
If more than one independent variable (X) is used to predict the value of a numerical dependent variable (y).
An estimate of the relationship can be given as:
$$ŷᵢ = β₀ + β₁Xᵢ₁ + β₂Xᵢ₂ + … βnXn + ε$$

**Where:**

* $Xᵢ₁, … Xᵢn$ represents all of the individual input datapoints where $Xᵢ₁$ represents a singular datapoint of input type $X₁$.
* $n$ is the number of input data types
A multiple linear regression equation can be graphed as an n-dimensional plane.

### Minimize objective function for linear regression

The simple linear regression can be solved using Ordinary Least Squares (OLS), which is a statistical method, to find the model parameters.Our goal is to find statistically significant values of the parameters $β₀$ and $βn$ that minimise the difference between $y$ and $ŷ$.

### Minimize objective function for multiple linear regression

We have more than one predictor variable which makes it hard for us to use that simple OLS method.we can implement a linear regression model for performing OLS regression using one of the following approaches

> **Note**: For very large datasets, computing the matrix inverse is costly or in some cases, the inverse does not exist (the matrix is non-invertible or singular, e.g., in case of perfect multicollinearity)

#### Solving the model parameters **analytically**
**(Normal Equations method)**

This approach treats the data as a matrix and uses linear algebra operations to estimate the optimal values for the model parameters. It means that all of the data must be available and you must have enough memory to fit the data and perform matrix operations. So this method should be preferred for smaller datasets.

The closed-form solution for finding the optimal values for the model parameters is:
$$Y = \beta^T.X$$
Finding the minimum can be achieved through setting the gradient of the loss to zero and solving for ${\beta}$ we get:
$$β = (X^T.X)^{-1}X^T.Y$$

> **Note**: Matrices expressed in upper case letters.

#### Solving the model parameter using an **optimization** algorithm
**(Gradient Descent, Stochastic Gradient Descent, etc.)**

The general idea of Gradient Descent is to tweak parameters iteratively to minimize a cost function. **Error/Cost** here represents the sum of squared error between the predicted and the actual value. This error is defined in terms of a function and is called Mean Squared Error (MSE) cost function. So, the objective of this Gradient Descent optimization algorithm is to minimize the MSE cost function.

It measures the local gradient of the error function with regards to the parameter vector ${\beta}$ and it goes in the direction of the descending gradient. Once the gradient is zero, you have reached a minimum.

An important parameter in the Gradient Descent is the size of the steps, determined by the **learning rate** hyperparameter.
* If the learning rate is too small, the algorithm will have to go through ***many iterations to converge***, which will take a **long time**.
* If the learning rate is too high, the algorithm will jump over the minimum, making it **diverge**. Hence the algorithm won’t reach the minimum.

To implement the Gradient Descent, you need to compute the gradient of the cost function concerning the parameter vector ${\beta}$. To get the gradient, we need to take the partial derivative of the cost function.

The MSE cost function is defined as:
$$MSE(β) =  \frac{1}{m} \sum_{i=1}^{m} (y^{(i)} - β^T.X^{(i)})^2$$
$$\frac{\partial}{\partialβ_{i}}{MSE(β)}=\frac{2}{m}\sum_{i=1}^{m} (y^{(i)} - β^T.X^{(i)})X_{j}^{(i)}$$

**Where:**
* $m$ is the total number of observations (data points) $\frac{1}{m} \sum_{i=1}^{m}$ is the mean
The vectorized form looks like
$$\begin{split}\begin{align}∇_βMSE(β) =
  \begin{bmatrix}
  \frac{\partial}{\partialβ_0}{MSE(β)}\\
  \frac{\partial}{\partialβ_1}{MSE(β)}\\
  \text{.}\\
  \text{.}\\
  \frac{\partial}{\partialβ_n}{MSE(β)}
  \end{bmatrix}
  = \frac{2}{m} X^T.(X.β-Y)\end{align}\end{split}$$

The update rule to get the updated weights/parameters is given below:
 $$β^{(\text{next step})} = β - α\frac{\partial}{\partialβ_n}{MSE(β)} $$
**Where:**
 * α is the learning rate hyperparameter.

---

# Setup

```python
# Python ≥3.5 is required
import sys
assert sys.version_info >= (3,5)
import time
import os
import math
import timeit
import itertools as it
import opendatasets as od
# avoid jupter warning
import warnings
# import all modules and libraries required to load, transform, analyze and plot
import numpy as np
import pandas as pd
from pandas.plotting import table

import seaborn as sns
import matplotlib as mat
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import matplotlib.colors as pltcol
import matplotlib.ticker as ticker
%matplotlib inline

import statsmodels.api as sm
import statsmodels.tools.eval_measures as ev
from statsmodels.formula.api import ols

import scipy
from scipy import stats
from scipy.stats import randint
from haversine import haversine #to calculate mileage

import sklearn
from sklearn import linear_model
assert sklearn.__version__ >= "0.20"

```

```python
mat.rcParams['figure.figsize'] = (9, 6)
mat.rc('axes', labelsize = 16)
mat.rc('xtick', labelsize = 14)
mat.rc('ytick', labelsize = 14)
# set display option to remove scientific notation and
pd.options.display.float_format = '{:,.2f}'.format # remove restrictions on dataframe rows/columns display
pd.set_option('display.max_columns', None)
pd.set_option("max_rows", 20)
warnings.filterwarnings('ignore')   # avoid jupter warning
myseed=42
np.random.seed(myseed)
```

```python
# Create graphs dir to save all created graphs
project_path = os.getcwd()
graphs_path = os.path.join('.',"graphs")

if not os.path.isdir(graphs_path):
    os.makedirs(graphs_path, exist_ok = True)
    print("Graphs directory successfully created (>‿◠)✌")
else:
    print("Graphs directory already exists  (•◡•)")
```

    Graphs directory already exists  (•◡•)

# Proplem Specification
It is technically possible to implement scalar and matrix calculations using `Python` lists. However, this can be unwieldy, and performance is poor when compared to languages suited for numerical computation, such as `MATLAB` or Fortran, or even some general purpose languages, such as `C` or `C++`.

To circumvent this deficiency, several libraries have emerged that maintain Python’s ease of use while lending the ability to perform numerical calculations in an efficient manner. Two such libraries worth mentioning are `NumPy` (one of the pioneer libraries to bring efficient numerical computation to Python) and `Sklrean` (Simple and efficient tools for predictive data analysisSimple and efficient tools for predictive data analysis).

Although it is possible to use this deterministic approach to estimate the coefficients of the linear model, it is not possible for some other models, such as neural networks. In these cases, iterative algorithms are used to
estimate a solution for the parameters of the model.

One of the most-used algorithms is gradient descent, which at a high level consists of updating the parameter coefficients until we converge on a minimized loss (or cost).

# Create Dataset with Gaussian noise
This program creates a set of 10,000 inputs x linearly distributed over the interval from 0 to 2. It then creates a set of desired outputs $y = 3 + 2 * x + \text{noise}$, where noise is taken from a Gaussian (normal) distribution with zero mean and standard deviation sigma = 0.1.

* $X$: A set of 10,000 inputs from 0 to 2.
* $y = 3 + 2 * x + \text{noise}$

```python
n = 10000
sigma = 0.1
noise = sigma * np.random.randn(n)
X = np.linspace(0, 2, n)
y = 3 + 2 * X + noise
y = y.reshape(n,1)
print(f"X shape: {X.shape}\ny shape: {y.shape}")
```

    X shape: (10000,)
    y shape: (10000, 1)

```python
plt.scatter(X[:2000],y[:2000], alpha=0.2)
```

## Gradient Descent in Pure Python
The Python function below estimates the parameters $w_0$ and $w_1$ using gradient descent.

```python
def py_descent(x, y, lr, n_epochs):

    m = len(x)
    # "Empty" predictions, errors, weights, gradients.
    y_pred = [0] * m
    w = [0, 0]
    grad = [0, 0]

    for _ in it.repeat(None, n_epochs):

        err = tuple(i - j for i, j in zip(y, y_pred))
        grad[0] = 2/m * sum(err)
        grad[1] = 2/m * sum(i * j for i, j in zip(err, x))
        w = [i + lr * j for i, j in zip(w, grad)]
        y_pred = (w[0] + w[1] * i for i in x)
    return w
```

>  `itertools.repeat()` faster than `for _ in range(n_epochs)`.because `repeat()` does not need to manufacture a distinct integer for each loop. It just needs to update the reference count to None.

```python
# `lr` is a step size, or scaling factor.
x_list = X.tolist()
y_list = y.squeeze().tolist()
lr = 0.001
epochs = 10000

start = time.time()
py_weights = py_descent(x_list, y_list, lr, epochs)
end = time.time()

print(f"Weights: {py_weights}")
print('Solve time: {:.2f} seconds'.format(round(end-start, 2)))
```

    Weights: [2.965722823133815, 2.0285925159614293]
    Solve time: 27.68 seconds

```python
fig, ax = plt.subplots()
ax.scatter(X[:1000],y[:1000], alpha=0.2, color='g', label = "Data distribution")
ax.plot(X[:1000], py_weights[1]*X[:1000]+ py_weights[0], color='green',linewidth=5, label = "trend line")
ax.legend()
plt.show()
fig.savefig(os.path.join(graphs_path,"list_result.png"))
```

## Gradient Descent Using NumPy

NumPy adds support for large multidimensional arrays and matrices along with a collection of mathematical functions to operate on them. The function below estimates the parameters $w_0$ and $w_1$ using gradient descent.

```python
X = X.reshape(X.shape[0], X.ndim)
```

```python
# import my module
from MultipleLinearRegression import LinearRegression, FeatureScaler

lr = 0.001
epochs = 10000

start = time.time()
np_LR = LinearRegression(X,y)
np_weights,weights_history, costs = np_LR.gradient_descent(lr,epochs)
end = time.time()
```

```python
print(f"Weights: {np.ravel(np_weights)}")
print('Solve time: {} seconds'.format(end-start))
```

    Weights: [2.95424797 2.03831285]
    Solve time: 0.9097013473510742 seconds

```python
# Try to calculate time using timeit module
setup = "from MultipleLinearRegression import LinearRegression, FeatureScaler;from __main__ import X, y, lr, epochs;"
code = "np_LR = LinearRegression(X,y);np_weights,weights_history, costs = np_LR.gradient_descent(lr,epochs)"
np_time = timeit.repeat(code,setup = setup,repeat=5,number = 5)
```

```python
print('Solve time Using (timeit) module: {} seconds'.format(min(np_time)/5))
```

    Solve time Using (timeit) module: 0.9215819600000004 seconds

```python
fig, ax = plt.subplots()
ax.plot(list(range(0,epochs,500)), costs[::500])
ax.set_xlabel('Number of epochs')
ax.set_ylabel('Cost value')
plt.show()
fig.savefig(os.path.join(graphs_path,"numpy_error_vs_epochs.png"))
```

```python
fig, ax = plt.subplots()
ax.scatter(X[:1000],y[:1000], alpha=0.2, color='r', label = "Data distribution")
ax.plot(X[:1000], np_weights[1]*X[:1000]+ np_weights[0], color='r',linewidth=5, label = "trend line")
ax.legend()
plt.show()
fig.savefig(os.path.join(graphs_path,"numpy_result.png"))
```

## Gradient Descent Using Scikit-learn

```python
# import scikit-learn liberary
lr = 0.001
epochs = 10000

start = time.time()
sk_LR = linear_model.LinearRegression()
sk_LR.fit(X,y)
end = time.time()
```

```python
sk_weights = np.array([sk_LR.intercept_,sk_LR.coef_]).ravel()
print(f"Weights: {sk_weights}")
print('Solve time: {}seconds'.format(end-start))
```

    Weights: [3.0015046093655 1.9982817922976575]
    Solve time: 0.0009982585906982422seconds

```python
# Try to calculate time using timeit module
setup = "from __main__ import X, y, lr, epochs, linear_model"
code = "sk_LR = linear_model.LinearRegression();sk_LR.fit(X,y)"
np_time = timeit.repeat(code,setup = setup,repeat=5,number = 5)
print('Solve time Using (timeit) module: {} seconds'.format(min(np_time)/5))
```

    Solve time Using (timeit) module: 0.0003657000000004018 seconds

```python
fig, ax = plt.subplots()
ax.scatter(X[:1000],y[:1000], alpha=0.2, color='purple', label = "Data distribution")
ax.plot(X[:1000], np_weights[1]*X[:1000]+ np_weights[0], color='purple',linewidth=5, label = "trend line")
ax.legend()
plt.show()
fig.savefig(os.path.join(graphs_path,"sklearn_result.png"))
```

# References

* [Least squares approximation](https://en.wikipedia.org/wiki/Least_squares)
* [Linear regression from ML cheatsheet](https://ml-cheatsheet.readthedocs.io/en/latest/linear_regression.html)
* [Regression-metrics from machinelearningmastery](https://machinelearningmastery.com/regression-metrics-for-machine-learning/)
* [Linear regression a complete story](https://medium.com/analytics-vidhya/linear-regression-a-complete-story-c5edd37296c8)
* [Introduction to linear regression in python](https://towardsdatascience.com/introduction-to-linear-regression-in-python-c12a072bedf0)
* [Machine learning multivariate linear regression](https://medium.com/analytics-vidhya/machine-learning-multivariate-linear-regression-8f9878c0f56)
* [Numpy tensorflow performance from realpython](https://realpython.com/numpy-tensorflow-performance/https://realpython.com/numpy-tensorflow-performance/)

# More Resouces

* For more details see the project [repository](https://github.com/heba14101998/Pure-Python-vs-NumPy-vs-Sklearn-Performance-Comparison) on github.
* Linear regression implementation modeule from [here](https://github.com/heba14101998/Pure-Python-vs-NumPy-vs-Sklearn-Performance-Comparison/blob/main/MultipleLinearRegression.py)
* Dispaly the project notebook from [here](https://github.com/heba14101998/Pure-Python-vs-NumPy-vs-Sklearn-Performance-Comparison/blob/main/Pure_Python_vs_NumPy_vs_Sklearn_Performance_Comparison.ipynb)

```python

```


# Expérimentation sur le filtre de Kalman sans parfum

Ces expérimentations sont tirées de : https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python (Chapitre 10).

```python
%matplotlib inline
```

Pour exécuter la cellule suivante il faut que le fichier *book_format.py* et le répertoire *kf_book* soient dans votre répertoire de travail.

```python
import book_format
book_format.set_style()
```

<style>
.output_wrapper, .output {
    height:auto !important;
    max-height:100000px;
}
.output_scroll {
    box-shadow:none !important;
    webkit-box-shadow:none !important;
}
</style>

Pour réaliser les expériences vous devez installer la bibliothèque *FilterPy*.
Voir : https://anaconda.org/conda-forge/filterpy

https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/kf_book/nonlinear_plots.py

```python
from kf_book.book_plots import set_figsize, figsize
import matplotlib.pyplot as plt
from kf_book.nonlinear_plots import plot_nonlinear_func
from scipy.stats import norm
import numpy as np

# create 500, 000 samples with mean 0, std 1
gaussian = (0., 1.)
nbSamples = 500000
data = norm.rvs(loc = gaussian[0], scale = gaussian[1], size = nbSamples)

def f(x):
    return (np.cos(4 * (x / 2 + 0.7))) - 1.3 * x

plot_nonlinear_func(data, f)
```

```python
nbrSamples = 30000
plt.subplot(121)
plt.scatter(data[:nbrSamples], range(nbrSamples), alpha = 0.2, s = 1)
plt.title('Input (before transformation $f(x)$)')
plt.subplot(122)
plt.title('Output (after transformation $f(x)$)')
plt.scatter(f(data[:nbrSamples]), range(nbrSamples), alpha = 0.2, s = 1)
plt.show()
```

Let's consider the 1D-Tracking problem gouverns by the following state space model:
$$
    \begin{bmatrix}
        x_{k+1} \\
        \dot{x}_{k+1}
    \end{bmatrix}
    =
    \begin{bmatrix}
        1 & \Delta t \\
        0 & 1
    \end{bmatrix}
    \begin{bmatrix}
        x_{k} \\
        \dot{x}_{k}
    \end{bmatrix}
    +
    v_k
    \\
    z_k
    =
    \begin{bmatrix}
        1 & 0
    \end{bmatrix}
    \begin{bmatrix}
        x_{k} \\
        \dot{x}_{k}
    \end{bmatrix}
    +
    w_k
$$
The state vector is defined by the position $x_k$ and the velocity $\dot{x}_{k}$ and $\Delta t$ the sampling period.

The following paper described an algorithme to compute the sigma points.

Simon J. Julier and Jeffrey K. Uhlmann. *New extension of the Kalman filter to nonlinear systems*. Proceedings of SPIE, Vol 3068, N° 1, pp. 182-193, 1997.

```python
import os
os.system('open UKF1.pdf')
```

    0

```python
from filterpy.kalman import JulierSigmaPoints
from kf_book.ukf_internal import plot_sigmas
sigmaPoints = JulierSigmaPoints(n = 2, kappa = 1)
plt.title('Sigma points generated with the Julier\'s method')
plot_sigmas(sigmaPoints, x = [3, 17], cov = [[1, 0.5], [0.5, 3]])
```

```python
def fx(x, dt):
    xout = np.empty_like(x)
    xout[0] = x[1] * dt + x[0]
    xout[1] = x[1]
    return xout

def hx(x):
    return x[:1] # return position [x]
```

```python
from scipy.stats import norm
from filterpy.kalman import UnscentedKalmanFilter
from filterpy.common import Q_discrete_white_noise

ukf = UnscentedKalmanFilter(dim_x = 2, dim_z = 1, dt = 1.0, hx = hx, fx = fx, points = sigmaPoints)
ukf.P = ukf.P * 10
ukf.R = ukf.R * 0.5
ukf.Q = Q_discrete_white_noise(dim = 2, dt = 1.0, var = 0.03)

zs, xs = [], []
for i in range(50):
    z = norm.rvs(loc = i, scale = 0.5, size = 1)
    ukf.predict()
    ukf.update(z)
    xs.append(ukf.x[0])
    zs.append(z)

plt.plot(xs, label = 'Position')
plt.plot(zs, marker = 'o', ls = '', color = 'magenta', label = 'Position estimate')
plt.title('Position estimate with UKF')
plt.legend()
plt.show()
```

The following paper described another algorithm to compute the sigma points.

Wan E., Van Der Merwe R. *The unscented Kalman filter for nonlinear estimation*. In: Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium (Cat. No.00EX373). IEEE; 2000:153-158. doi:10.1109/ASSPCC.2000.882463

```python
import os
os.system('open UKF2.pdf')
```

    0

```python
def f_nonlinear_xy(x, y):
    return np.array([x + y, 0.1 * x ** 2 + y * y])
```

```python
from filterpy.kalman import unscented_transform, MerweScaledSigmaPoints
from numpy.random import multivariate_normal
from kf_book.nonlinear_plots import plot_monte_carlo_mean
import scipy.stats as stats

#initial mean and covariance
mean = (0., 0.)
p = np.array([[32., 15], [15., 40.]])

# create sigma points and weights
points = MerweScaledSigmaPoints(n=2, alpha=.3, beta=2., kappa=.1)
sigmaPoints = points.sigma_points(mean, p)

### pass through nonlinear function
sigmas_f = np.empty((5, 2))
for i in range(5):
    sigmas_f[i] = f_nonlinear_xy(sigmaPoints[i, 0], sigmaPoints[i ,1])

### use unscented transform to get new mean and covariance
ukf_mean, ukf_cov = unscented_transform(sigmas_f, points.Wm, points.Wc)

#generate random points
np.random.seed(100)
xs, ys = multivariate_normal(mean=mean, cov=p, size=5000).T

plot_monte_carlo_mean(xs, ys, f_nonlinear_xy, ukf_mean, 'Unscented Mean')
ax = plt.gcf().axes[0]
ax.scatter(sigmaPoints[:,0], sigmaPoints[:,1], c='r', s=30);
```

## Using the UKF

We will consider a linear problem you already know how to solve with the linear Kalman filter. Although the UKF was designed for nonlinear problems, it finds the same optimal result as the linear Kalman filter for linear problems. We will write a filter to track an object in 2D using a constant velocity model.

\begin{equation}
		\left\{
			\begin{array}{l}
				x_{k+1} = F x_k +C u + G v_k \\
				z_k = H x_k + w_k
			\end{array}
		\right.
\end{equation}

$u_t \in \mathbb{R}^n$ : a known input, $v_t \in \mathbb{R}^{\ell}$ : gaussian state noise, $w_t \in \mathbb{R}^p$ : gaussian measurement noise.

$v_t$ et $w_t$ are white noise whose correlation matrix are:
\begin{equation}
	\left\{
		\begin{array}{l}
			\mathbb{E}[v_i v_j^T] = \delta_{ij}Q_i \\
			\mathbb{E}[w_i w_j^T] = \delta_{ij}R_i \\
			\mathbb{E}[v_i w_j^T] = 0
		\end{array}
	\right.
\end{equation}

$x_0 \sim \mathcal{N}(\hat{x}_0,P_{0\vert0})$ is independent from the noises $v_t$ et $w_t$.

We want a constant velocity model, so we define $\bf{x}$ from the Newtonian equations:

$$
\begin{aligned}
    x_k &= x_{k-1} + \dot x_{k-1}\Delta t \\
    y_k &= y_{k-1} + \dot y_{k-1}\Delta t
\end{aligned}
$$

$$
\mathbf{x} = \begin{bmatrix}
                x \\
                \dot{x} \\
                y \\
                \dot{y}
             \end{bmatrix}
$$

With this ordering of state variables the state transition matrix is

$$
\mathbf{F} = \begin{bmatrix}
                1 & \Delta t & 0 & 0 \\
                0 & 1& 0 & 0 \\
                0 & 0 & 1 & \Delta t\\
                0 & 0 & 0 & 1
             \end{bmatrix}
$$

Our sensors provide position but not velocity, so the measurement function is

$$
\mathbf{H} = \begin{bmatrix}
                1 & 0 & 0 & 0 \\
                0 & 0 & 1 & 0
\end{bmatrix}
$$

The sensor readings are in meters with an error of $\sigma=0.3$ meters in both *x* and *y*. This gives us a measurement noise matrix of

$$
\mathbf{R} = \begin{bmatrix}
                0.3 ^ 2 & 0 \\
                0       & 0.3 ^ 2
             \end{bmatrix}
$$

Finally, let's assume that the process noise can be represented by the discrete white noise model - that is, that over each time period the acceleration is constant. We can use `FilterPy`'s `Q_discrete_white_noise()` to create this matrix for us, but for review the matrix is

$$
\mathbf{Q} = \begin{bmatrix}
                \frac{1}{4} \Delta t ^ 4 & \frac{1}{2} \Delta t ^ 3 \\
                \frac{1}{2} \Delta t ^ 3 & \Delta t ^ 2
             \end{bmatrix} \sigma ^ 2
$$

The model is linear, so we can use the **Kalman filter**. An implementation of this filter is proposed below:

```python
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
from scipy.stats import norm

std_x, std_y = 0.3, 0.3
dt = 1.0

np.random.seed(1234)
kf = KalmanFilter(4, 2)
kf.x = np.array([0., 0., 0., 0.])
kf.R = np.diag([std_x ** 2, std_y ** 2])
kf.F = np.array([[1, dt, 0, 0],
                 [0, 1, 0, 0],
                 [0, 0, 1, dt],
                 [0, 0, 0, 1]])
kf.H = np.array([[1, 0, 0, 0],
                 [0, 0, 1, 0]])

kf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt = 1, var = 0.02)
kf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt = 1, var = 0.02)

zs = [np.array([norm.rvs(loc = i, scale = std_x, size = 1),
                norm.rvs(loc = i, scale = std_y, size = 1)]) for i in range(100)]
xs, _, _, _ = kf.batch_filter(zs)
plt.plot(xs[:, 0], xs[:, 2])
plt.xlabel('x')
plt.ylabel('y')
plt.title('Estimation of $x$ and $y$ with a KF')
plt.show()
```

```python
plt.plot(xs[:, 0])
plt.title('$\hat{x}$')
plt.show()
```

```python
plt.plot(xs[:, 1])
plt.title('$\hat{\dot{x}}$')
plt.show()
plt.show()
```

```python
plt.plot(xs[:, 2])
plt.title('$\hat{y}$')
plt.show()
```

```python
plt.plot(xs[:, 3])
plt.title('$\hat{\dot{y}}$')
plt.show()
```

We are going to implement an Unscented Kalman Filter (UKF) to solve the same problem although this is not useful given the linear nature of the model. This is just to illustrate the construction of the filter in the context of the _FilterPy_ library.

```python
def f_cv(x, dt):
    """ state transition function for a
    constant velocity model"""

    F = np.array([[1, dt, 0,  0],
                  [0,  1, 0,  0],
                  [0,  0, 1, dt],
                  [0,  0, 0,  1]])
    return F @ x

def h_cv(x):
    return x[[0, 2]]
```

```python
from filterpy.kalman import MerweScaledSigmaPoints
from filterpy.kalman import UnscentedKalmanFilter as UKF
from filterpy.common import Q_discrete_white_noise

import numpy as np
from numpy.random import randn
import matplotlib.pyplot as plt

std_x, std_y = 0.3, 0.3
dt = 1.0
sigmaPoints = MerweScaledSigmaPoints(4, alpha = 0.1, beta = 2.0, kappa = 1.0)
ukf = UKF(dim_x = 4, dim_z = 2, fx = f_cv, hx = h_cv, dt = dt, points = sigmaPoints)
ukf.x = np.array([0.0, 0.0, 0.0, 0.0])
ukf.R = np.diag([0.09, 0.09])
ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt = 1, var = 0.02)
ukf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt = 1, var = 0.02)

#zs = [np.array([norm.rvs(loc = i, scale = std_x, size = 1),
#                norm.rvs(loc = i, scale = std_y, size = 1)]) for i in range(100)]

zs = [np.array([i + randn() * std_x,
                i + randn() * std_y]) for i in range(100)]

uxs = []
for z in zs:
    ukf.predict()
    ukf.update(z)
    uxs.append(ukf.x.copy())
uxs = np.array(uxs)

plt.plot(uxs[:, 0], uxs[:, 2])
plt.xlabel('x')
plt.ylabel('y')
plt.title('Estimation of $x$ and $y$ with an UKF')
plt.show()
#print(f'UKF standard deviation {np.std(uxs - xs):.3f} meters')
```

### Study the example below

```python
import kf_book.ekf_internal as ekf_internal
ekf_internal.show_radar_chart()
```

The *elevation angle* $\epsilon$ is the angle above the line of sight formed by the ground.

We assume that the aircraft is flying at a constant altitude. The state vector is:

$$
\mathbf{x} = \begin{bmatrix}
                x \\
                \dot{x} \\
                y
             \end{bmatrix}
$$

with $x$ the distance, $\dot{x}$ the velocity and $y$ the altitude.

The matrix $\mathbf{F}$ is:

$$
\mathbf{x}_{k+1} = \begin{bmatrix}
                        1 & \Delta t & 0 \\
                        0 & 1 & 0 \\
                        0 & 0 & 1
                   \end{bmatrix}
                   \mathbf{x}_{k}
$$

The computation of $\mathbf{F}$ can be implemented with the following function:

```python
def f_radar(x, dt):
    """ state transition function for a constant velocity
    aircraft with state vector [x, velocity, altitude]'"""

    F = np.array([[1, dt, 0],
                  [0,  1, 0],
                  [0,  0, 1]], dtype=float)
    return F @ x
```

The measurement function $\mathbf{H}$.

$$
    \begin{bmatrix}
        r \\
        \epsilon
    \end{bmatrix}
    =
    \mathbf{H} \begin{bmatrix}
                x \\
                \dot{x} \\
                y
             \end{bmatrix}
$$

$$
    r = \sqrt{(x_\text{aircraft} - x_\text{radar})^2 + (y_\text{aircraft} - y_\text{radar})^2}
$$

$$
    \epsilon = \tan^{-1} \frac{y}{x}
$$

$$
    \epsilon = \tan^{-1}{\frac{y_\text{aircraft} - y_\text{radar}}{x_\text{aircraft} - x_\text{radar}}}
$$

The computation of $\mathbf{H}$ can be implemented with the following function:

```python
def h_radar(x):
    dx = x[0] - h_radar.radar_pos[0]
    dy = x[2] - h_radar.radar_pos[1]
    slant_range = math.sqrt(dx ** 2 + dy ** 2)
    elevation_angle = math.atan2(dy, dx)
    return [slant_range, elevation_angle]

h_radar.radar_pos = (0, 0)
```

The simulation of the aircraft and the radar are carried out by the following code:

```python
from numpy.linalg import norm
from math import atan2

class RadarStation:

    def __init__(self, pos, range_std, elev_angle_std):
        self.pos = np.asarray(pos)
        self.range_std = range_std
        self.elev_angle_std = elev_angle_std

    def reading_of(self, ac_pos):
        """ Returns (range, elevation angle) to aircraft.
        Elevation angle is in radians.
        """

        diff = np.subtract(ac_pos, self.pos)
        rng = norm(diff)
        brg = atan2(diff[1], diff[0])
        return rng, brg

    def noisy_reading(self, ac_pos):
        """ Compute range and elevation angle to aircraft with
        simulated noise"""

        rng, brg = self.reading_of(ac_pos)
        rng = rng + randn() * self.range_std
        brg = brg + randn() * self.elev_angle_std
        return rng, brg

class ACSim:
    def __init__(self, pos, vel, vel_std):
        self.pos = np.asarray(pos, dtype=float)
        self.vel = np.asarray(vel, dtype=float)
        self.vel_std = vel_std

    def update(self, dt):
        """ Compute and returns next position. Incorporates
        random variation in velocity. """

        dx = self.vel * dt + (randn() * self.vel_std) * dt
        self.pos = self.pos + dx
        return self.pos
```

UKF implementation for aircraft tracking.

```python
import math
from kf_book.ukf_internal import plot_radar

dt = 3. # 12 seconds between readings
range_std = 5 # meters
elevation_angle_std = math.radians(0.5)
ac_pos = (0., 1000.)
ac_vel = (100., 0.)
radar_pos = (0., 0.)
h_radar.radar_pos = radar_pos

points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2., kappa=0.)
kf = UKF(3, 2, dt, fx=f_radar, hx=h_radar, points=points)

kf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1)
kf.Q[2,2] = 0.1

kf.R = np.diag([range_std**2, elevation_angle_std**2])
kf.x = np.array([0., 90., 1100.])
kf.P = np.diag([300**2, 30**2, 150**2])

np.random.seed(200)
pos = (0, 0)
radar = RadarStation(pos, range_std, elevation_angle_std)
ac = ACSim(ac_pos, (100, 0), 0.02)

time = np.arange(0, 360 + dt, dt)
xs = []
for _ in time:
    ac.update(dt)
    r = radar.noisy_reading(ac.pos)
    kf.predict()
    kf.update([r[0], r[1]])
    xs.append(kf.x)
plot_radar(xs, time)
```

```python

```