### New KL Upper Bound and Applications

Here we derive an upper bound for the reverse KL divergence and then use it to minimize the reverse KL.

---

First let us write out the reverse KL divergence

$$ \text{KL}(Q || P) = \mathbb{E}_{q}\bigg[\log\frac{q(x)}{p(x)}\bigg] $$

This can be rewritten in information theoretic terms by expanding the expectation

\begin{align}
\mathbb{E}_{q}\bigg[\log\frac{q(x)}{p(x)}\bigg] & = \int q(x) \log \frac{q(x)}{p(x)} dx \\
& = -\int q(x) \log \frac{p(x)}{q(x)} dx \\
& = -\int q(x) \log p(x) dx - \bigg(-\int q(x) \log q(x) \bigg ) dx\\
& = H_p[q] - H[q] \\
\end{align}

Where $H_p[q(x)]$ is the cross entropy and  $H[q(x)]$ is the entropy

We will use the $p(x)$ density function to bound the KL. The term $p(x)$ only appears in the cross entropy

$$\text{KL} = \int q(x)(-\log p(x)) dx - H[q]$$

We will bound $-\log p(x)$ above for certain values of $p(x)$. We know that $p(x)$ is a density function and assume that it is bounded above $p(x) \le 1$. The goal is to use a straight line as the upper bound. Because we assumed $p(x) \le 1$ we make the line pass through $(1,0)$ where $p(x)=1$ and $-\log (p(x))=0$ . The question is, which other point should we use?

```python
import numpy as np
import matplotlib
#matplotlib.use('nbagg')
import matplotlib.pyplot as plt

x = np.linspace(1e-6,1,200)
y1 = -np.log(x)
y2 = -5*x+5

plt.plot(x,y1,label='$-\log p(x)$',lw=2)
plt.plot(x,y2,label='$-5p(x)+5$',lw=2)
plt.xlabel("$p(x)$")
plt.ylim(-0.5,10)
plt.tight_layout()
plt.legend()
plt.show()
```

As we see, the $-\log p(x)$ is a convex function and cannot be bounded above by a line for all $p(x)$. However $p(x)$ is a density function and values of $x$ will appear infrequently from regions where $p(x)$ is small. We will choose the other point of the line to be $(\lambda, -\log \lambda)$ with the intention $\lambda \ll 1$. The line passing through these two points can be expressed as

$$y = \frac{\log \lambda}{1-\lambda}(x-1)$$

We use this line to bound $-\log p(x)$ above for $\lambda \le p(x) \le 1$

$$-\log p(x) \le \frac{\log \lambda}{1-\lambda}\big(p(x)-1\big)$$

For the remainder of this work we define $k=\frac{\log \lambda}{1-\lambda}$

We can now return to the Kullback Leibler divergence between $q(x)$ and $p(x)$. Recall we are bounding with respect to $p(x)$ and the differential entropy term $H[q]$ depends only on $q(x)$, therefore we aren't concerned with the subtraction of a potentially positive quantity while introducing the inequality.

\begin{align}
\text{KL}(q||p) & = \mathbb{E}_q \big[-\log p(x)\big] - H[q] \\
& \le \mathbb{E}_q \big[k\big(p(x)-1\big)\big] - H[q] \\
& \le \int q(x) k\big(p(x)-1\big)dx - H[q] \\
& \le k\bigg(\int p(x)q(x)dx-1\bigg) - H[q] \\
& \le k\big(\mathbb{E}_p \big[q(x)\big] - 1 \big) - H[q] \\
\end{align}

We reiterate that this upper bound only holds for $\lambda \le p(x) \le 1$ and $\lambda \ll 1$. The key observation here, and indeed the entire motivation for this work, was that we completely removed the density $p(x)$ from inside any expectations. Now, the expecatation is over $p(x)$ and we no longer need to explicitly know its functional form. Therefore we can now minimize this upper bound with respect to $q(x)$ without knowing $p(x)$. Before we move onto the final goal of minimization let us examine how tight this bound is on some simple test cases.

### Examining the Upper Bound

The amount of information contained in observation $x$ from $p(x)$ (the universe) is

$$ I(x) = -\log p(x) $$

The expected information contained in $p(x)$ is

$$ H[p(x)] = \int p(x)\big(-\log p(x)\big) dx$$

The cross entropy is essentially a measurement of  $q(x)$ (the neuron's) interpretation of expected information.

$$ H_p[q(x)] = \int q(x)\big(-\log p(x)\big) dx $$

By replacing $-\log p(x)$ with $k\big(p(x)-1\big)$ we're approximating the amount of information in observations from $p(x)$. In particular we're over estimating the information in high probability events and under estimating the information in low probability events $p(x) \lt \lambda$ where the bound no longer holds.

We begin with two univariate gaussians $P \sim N(\frac{1}{4},1)$ and $Q \sim N(\mu,1)$. We will plot both the true KL and the upper bound for $0 \le \mu \le 1$, $\sigma^2=1$ and $\lambda = 10^{-3}$.

```python
from scipy.integrate import quadrature

def reverse_kl_int(q,p):
    def f_i(x):
        global p
        q_i = q(x)
        p_i = p(x)
        return q_i*np.log(q_i/p_i)
    quad_rslt, quad_err = quadrature(f_i, -1, 2.0, tol=1e-8, maxiter=100)
    return (quad_rslt, quad_err)

def kl_upper_bound(q,p,l=1e-3,k=None):
    if not k:
        k = np.log(l)/(1-l)
    def f_i(x):
        global p
        q_i = q(x)
        p_i = p(x)
        return q_i*(k*(p_i-1)-np.log(q_i))
    quad_rslt, quad_err = quadrature(f_i, -1, 2.0, tol=1e-8, maxiter=100)
    return (quad_rslt, quad_err)

from scipy.stats import norm
class gmm: # gaussian mixture model

    def __init__(self, pis, params, normed=False):
        self.params = params # [[mu1, sig1], [mu2, sig2],...]
        self.components = params.shape[0]
        self.pis = pis
        self.max_val = 1
        self.normed = normed
        if self.normed:
            vals = self.__call__(np.linspace(0,1,10000))
            self.max_val = vals.max()

    def __call__(self, x):
        pis = self.pis
        p = self.params
        sz = self.components
        vals = np.array([pis[i]*norm.pdf(x,*(p[i])) for i in range(sz)]).sum(axis=0)
        if self.normed:
            vals /= self.max_val
        return vals

    def sample(self, n_samples=1):
        mode_id = np.random.choice(self.components, size=n_samples, replace=True, p=self.pis)
        return np.array([norm.rvs(*(self.params[i])) for i in mode_id])
```

Below we demonstrate a comparision between KL and the upper bound. We see the upper bound is more sensitive to local extrema and is better at determining the mean of the modes of the distribution. In this particular example we see that the upper bound is able to detect a mode that the true KL cannot.

```python
p = gmm([1], np.array([[0.5,0.1]]), normed=True)
mu = np.linspace(0,1,200)

true_kl = []
upper_bound = []
for m in mu:
    q = gmm([1], np.array([[m,0.1]]))
    true_kl.append(reverse_kl_int(q,p)[0])
    upper_bound.append(kl_upper_bound(q,p,l=1e-5)[0])
max_val = np.max((np.max(true_kl),np.max(upper_bound)))
plt.plot(mu,true_kl/max_val,lw=3,label='KL')
plt.plot(mu,upper_bound/max_val,lw=3,label='bound')
plt.plot(mu,p(mu),ls='dashed',label='$p(x)$')
plt.plot([0,1],[0,0],c='k')
plt.xlabel('$\mu$')
plt.legend()
plt.show()
```

```python
mu[(np.diff(np.sign(np.diff(upper_bound))) > 0).nonzero()[0] + 1]
```

    array([0.50251256])

```python
mu[(np.diff(np.sign(np.diff(true_kl))) > 0).nonzero()[0] + 1]
```

    array([0.49748744, 0.50251256])

We're still building intution, but it is nice to see that the minimum value of $\mu$ in the upper bound corresponds to approximately the same minimum in the true KL.

---

Now let's fix $\mu=0$ and vary the variance $0.01 \le \sigma^2 \le 0.1$ of $q(x)$. As before we fix $\lambda = 10^{-3}$.

```python
p = gmm([1], np.array([[0.5,0.1]]), normed=True)
var = np.linspace(0.05,0.15,500)

true_kl = []
upper_bound = []
for v in var:
    q = gmm([1], np.array([[0.5,v]]))
    true_kl.append(reverse_kl_int(q,p)[0])
    upper_bound.append(kl_upper_bound(q,p,l=1e-3)[0])

plt.plot(var,true_kl,label='KL')
plt.plot(var,upper_bound,label='bound')
plt.xlabel('$\sigma^2$')
plt.legend()
plt.show()
```

```python
var[(np.diff(np.sign(np.diff(upper_bound))) > 0).nonzero()[0] + 1]
```

    array([], dtype=float64)

```python
var[(np.diff(np.sign(np.diff(true_kl))) > 0).nonzero()[0] + 1]
```

    array([0.1001002])

So things are not quite as easy for the variance. However, likely this is only for $\lambda=10^{-3}$ which we held fixed. If we also minimized across lambda we may see the same minimum of $\sigma^2$ around 1. Let us look for a local minima in the upper bound for all lambda.

```python

```

```python
p = gmm([0.1,0.3,0.6], np.array([[-7,1],[0,0.1],[3,0.5]]))
var = np.linspace(0.01,3,500)
ks = np.linspace(-5,-2,10)

true_kl = []
for i,k_val in enumerate(ks):
    print("iteration = ", i)
    upper_bound = []
    for v in var:
        q = gmm([1], np.array([[3,np.sqrt(v)]]))
        true_kl.append(reverse_kl_int(q,p))
        upper_bound.append(kl_upper_bound(q,p,k=k_val))
    out = var[(np.diff(np.sign(np.diff(upper_bound))) > 0).nonzero()[0] + 1]
    if out.size > 0:
        print("Found minima with k = %f", k)
        print(out)

plt.plot(var,true_kl,label='KL')
plt.plot(var,upper_bound,label='bound')
plt.xlabel('$\sigma^2$')
plt.legend()
plt.show()
```

---

To build a little more intuition about the role that $\lambda$ plays in the bound's tightness, let's run the experiment again, but explore $10^{-5} \le \lambda \le 10^{-1}$ while we hold $\mu=2$ and $\sigma^2=1$. In this case we know the true KL should be two. We will only visualize how the upper bound changes.

```python
lmda = np.linspace(1e-5,0.5,1000)

p = gmm([0.1,0.1,0.2,0.2,0.4], np.array([[-8,1],[-4,1],[-2,3],[-5,1],[9,0.1]]))
q = gmm([1], np.array([[0,0.5]]))
true_kl = reverse_kl_int(q,p,-12,12,1000)
upper_bound = []
for l in lmda:
    upper_bound.append(kl_upper_bound(q,p,-12,12,100,l))

plt.plot([lmda[0],lmda[-1]],[true_kl,true_kl],ls='dashed',label='true KL')
plt.plot(lmda,upper_bound,label='bound')
plt.xlabel('$\lambda$')
plt.legend()
plt.show()
```

As suspected in the derivation, for the upper bound to be valid, $\lambda$ must remain small. However, if all we are interested in is minimizing the upper bound with respect to the distribution $q(x)$, then $\lambda$ is irrelevant. In addition, at least as far as one previous experiment showed, the parameter values that minimize the upper bound also minimize the KL.

### 3D Plot of the $\mu$ and $\sigma^2$ surface

```python
from scipy.integrate import quadrature
fig = plt.figure(figsize=(9,5))
#ax2d = fig.add_subplot(121)
ax3d = fig.add_subplot(111, projection='3d')
```

```python
def E(p,q,m,s,b=1):
    def e_i(x,m,s,b):
        global p
        q_i = q(np.array(x).reshape(-1,1),update=False)
        p_i = p(x)
        return p_i*q_i
    quad_rslt, quad_err = quadrature(e_i, 0.0, 1.0, args=(m,s,b), tol=1e-6)
    return (-7*(quad_rslt-1)-b*0.5*np.log(2*np.pi*np.e*s), quad_err)

def f(M, S, b=1):
    global p
    Z = np.ndarray(M.shape)
    for mi,m in enumerate(M[:,0]):
        for si,s in enumerate(S[0]):
            q = Neuron([1,1], np.array([m]), s)
            Z[mi,si] = E(p,q,m,s,b)[0]
    return Z

mu_min = 0
mu_max = 1
mu_step = 0.005
s_min = 0.001
s_max = 0.2
s_step = 0.01

M, S = np.mgrid[mu_min:mu_max:mu_step,s_min:s_max:s_step]
#mf = mu.flatten()
#lf = lamda.flatten()
#locs = np.where(1/lf<(1-mf)**2)[0]

Z = f(M,S,b=0.25)
ax3d.plot_surface(M, S, Z, rstride=5, cstride=5, alpha=0.3)
cset = ax3d.contour(M, S, Z, zdir='z', offset=Z.min(), cmap=cm.coolwarm)
cset = ax3d.contour(M, S, Z, zdir='x', offset=mu_min, cmap=cm.coolwarm)
cset = ax3d.contour(M, S, Z, zdir='y', offset=s_max, cmap=cm.coolwarm)

ax3d.set_xlabel('$\mu$')
ax3d.set_xlim(mu_min, mu_max)
ax3d.set_ylabel('$\sigma^2$')
ax3d.set_ylim(s_min, s_max)
ax3d.set_zlabel('$E_P[q(x)]$')

plt.show()
plt.clf()
```

### Minimizing the Upper Bound

We now explore the minimization of the upper bound.

\begin{align}
\text{KL}(q||p) \le k \big(\mathbb{E}_p \big[q(x)\big] -1\big) - H[q(x)]\\
\end{align}

\begin{align}
\min_{q(x),k} \bigg[ k \big( \mathbb{E}_p \big[q(x)\big] -1\big) - H[q(x)] \bigg ]
\end{align}

We know from information theory the optimal $q(x)$ that minimizes the KL should be $q(x) = p(x)$. This is not the case with the upper bound. We can explore this question with variational methods. We express the objective as a functional and examine the first and second variations.

\begin{align}
J_p[q(x),k] &= k \big( \mathbb{E}_p \big[q(x)\big] -1\big) - H[q(x)] \\
&= \int k q(x)\big(p(x)-1\big) + q(x) \log q(x) dx \\
\end{align}

To simplify notation we drop the parenthesis for the functions, i.e. $q \equiv q(x)$

\begin{align}
J_p[q(x)+\eta(x),k] &= \int k(q+\eta)(p-1) + (q+\eta)\log(q+\eta) dx \\
&=\int kpq + kp\eta -kq -k\eta + (q+\eta)\big[\log(q) + \log(1+\frac{\eta}{q})\big] dx \\
&=\int kpq + kp\eta -kq -k\eta + q\log(q)+\log(q)\eta +(q+\eta)\log(1+\frac{\eta}{q}) dx \\
\end{align}

To find the terms for the second variation we expand the last logarithm in its taylor series

\begin{align}
(q+\eta)\log(1+\frac{\eta}{q}) &= (q+\eta)\big(\frac{\eta}{q} - \frac{\eta^2}{2q^2} + \dots\big) \\
&= \eta - \frac{\eta^2}{2q} + \frac{\eta^2}{q} + \dots \\
&= \eta + \frac{\eta^2}{2q} + \dots
\end{align}

Putting this back in and rearranging the terms yields

\begin{align}
J_p[q(x)+\eta(x),k] = \int kpq -kq +q\log (q)+ (kp-\log q -k-1)\eta + \frac{1}{2q} \eta^2 dx \\
\end{align}

The first variation is

\begin{align}
\frac{\delta J_p[q(x),k]}{\delta q(x)} &= kp -\log (q)-k -1 \\
\end{align}

If we set it to zero we find that

\begin{align}
0 &= kp - \log q -k-1 \\
q^* &= e^{k(p-1)-1} =  \frac{1}{e}e^{k(p-1)} \\
&= \frac{1}{z}e^{k(p-1)}
\end{align}

where the $\frac{1}{z}$ term is the normalizing constant. The second variation is

\begin{align}
\frac{\delta^2 J_p[q(x),k]}{\delta q(x)\delta \bar{q}(x)} &= \frac{1}{q}
\end{align}

And because $q(x) > 0$ we know that our upper bound is convex in $q(x)$ and our extrema $q^*(x)$ is a global minimizer for a given $k$.

Unfortunately it is easy to see that our functional $J_p[q(x),k]$ is linear in $k$

\begin{align}
\frac{\partial J_p[q(x),k]}{\partial k} &= \mathbb{E}_p \big[q(x)\big] -1 \\
\frac{\partial^2 J_p[q(x),k]}{\partial k^2} &= 0 \\
\end{align}

We will need another way to determine k. One way to choose $k$ is such that our minimum upper bound $J_p[q^*(x),k]$ is as close as possible to the true KL at the same distribution $KL(q^*(x)||p(x))$. We solve the equation $J_p[q^*(x),k] - KL(q^*(x)||p(x)) = 0$ for $k\lt-1$.

\begin{align}
J_p[q^*(x),k] - KL(q^*(x)||p(x)) &= k \big( \mathbb{E}_p \big[q^*(x)\big] -1\big) - \mathbb{E}_{q^*}[-\log q^*(x)] - \mathbb{E}_{q^*}\big[-\log p(x) \big] + \mathbb{E}_{q^*}\big[-\log q^*(x)\big]  \\
&= k \big( \mathbb{E}_p \big[q^*(x)\big] -1\big) - \mathbb{E}_{q^*}\big[-\log p(x) \big] \\
\end{align}

By setting this expression to zero and solving for $k$ we find that

\begin{align}
k_1^* = \frac{\mathbb{E}_{q^*}\big[- \log p(x)\big]}{\mathbb{E}_p\big[ q^*(x)\big]-1}
\end{align}

which is a ratio between the true cross entropy and our approximate cross entropy. A subscript was given to distringuish it from a further approximation to $k$. Of course we would need to know the true data generating density $p(x)$ ahead of time. We may have a work around for that which we will explore in the future. For now let us consider another way to determine $k^*$.

Let us choose $k$ such that our upper bound $J[q^*(x),k]$ is $0$ at our given global minimizer. To do this we put $q^*(x)$ back into the upper bound, set it to zero and solve that equation for $k$.

\begin{align}
J_p[q^*(x),k] &= k \big( \mathbb{E}_p \big[q^*(x)\big] -1\big) + \mathbb{E}_{q^*}[\log q^*(x)]
\end{align}

The optimal solution is an exponential so the logarithm of $q^*(x)$ will simply equal the exponent. Recall

\begin{align}
q^*(x) = \frac{1}{z}e^{k(p-1)}
\end{align}

so the logarithm exposes the $p(x)$ density in the expectation

\begin{align}
\log q^*(x) = k(p(x)-1) -\log z
\end{align}

Putting this back into the upper bound yields

\begin{align}
J[q^*(x),k] &= k \big( \mathbb{E}_p \big[q^*(x)\big] -1\big) + \mathbb{E}_{q^*}\big[k(p(x)-1)\big] -\log z \\
&= k \big( \mathbb{E}_p \big[q^*(x)\big] - 1 \big ) + k \big( \mathbb{E}_{p}\big[q^*(x)\big] - 1 \big) - \log z
\end{align}

After we combine the terms if we set it to zero and solve for $k$ we get

\begin{align}
k_2^* =  \frac{ \log z }{2 \big( \mathbb{E}_p \big[q^*(x)\big] - 1 \big )}
\end{align}

The expectation is of the optimal pdf $q^*(x)$ which means it is converged and centered on a local extrema. Recall $q$ is a gaussian with mean $\mu$ and variance $\sigma^2$ and normalizing constant is $z = \sqrt{2 \pi \sigma^2}$. With this in mind we acknowledge that values of $x\sim P$ where $x-\mu$ is large will have little contribution to the expectation so we approximate the expectation as

$$\mathbb{E}_p \big[q^*(x)\big] \approx \mathbb{E}_{q^*} \big[q^*(x)\big] = \frac{1}{2\sigma \sqrt{\pi}}$$

Substituting this into our expression for $k_2^*$ we get

$$k_2^* \approx \frac{1}{2} \frac{\log \sqrt{2 \pi \sigma^2}}{\frac{1}{2 \sigma \sqrt{\pi}}-1} = \frac{\sigma \sqrt{\pi} \log \sqrt{2 \pi \sigma^2}}{1 - 2 \sigma \sqrt{\pi}}$$

Let us breifly examine $k_2^*$ numerically to see if we can simpmlify the expression a bit...

```python
def k2(s):
    root_pi = np.sqrt(np.pi)
    return -(s*root_pi*np.log(np.e*np.sqrt(2)*root_pi*s))/(1-2*s*root_pi)

s_vals = np.linspace(1e-4,0.2,1000)
plt.plot(s_vals,2*s_vals,s_vals,k2(s_vals))
plt.show()
```

```python
np.log(0.001)/(1-0.001)
```

---

### Minimzation with Parameterized Density

Let us now consider the minimization of the upper bound for a parameterized distribution $q(x|\theta)$.

In particular we assume $q(x|\theta)$ is a multivariate gaussian parameterized by the mean $\boldsymbol{\mu}$ and covariance $\Sigma$. In this work we consider $p(x)$ to be the density of any distribution whose functional form is unknown.

Let $q(\boldsymbol{x} | \theta)$ be the pdf for
$\mathcal{N}(\boldsymbol{\mu}, \Sigma)$
where $\theta = \{\boldsymbol{\mu}, \Sigma\}$, $\boldsymbol{x},\boldsymbol{\mu} \in {\rm I\!R}^k$ and $\Sigma \in {\rm I\!R}^{k\times k}$.

\begin{equation}
q(\boldsymbol{x} | \theta) = \frac{1}{(2\pi)^{k/2}
|\Sigma|^{1/2}} \exp \bigg\{ -\frac{1}{2}(\boldsymbol{x}-\boldsymbol{\mu})^T\Sigma^{-1}
(\boldsymbol{x}-\boldsymbol{\mu}) \bigg \}
\end{equation}

With $q(x)$ parameterized, the upper bound can be expressed as a function of $k$ and theta $f(k,\theta)$ because both expectations are over $x$.

\begin{align}
f(k,\theta) &= k \big( \mathbb{E}_p \big[q(\boldsymbol{x} | \theta)\big] -1\big) - H[q(\boldsymbol{x} | \theta)] \\
\end{align}

In addition we can calculate the differential entropy of the multivariate gaussian analytically and substitute it into our objective.

\begin{align}
H[q(\boldsymbol{x} | \theta)] &= \frac{1}{2}\log | 2 \pi e \Sigma | \\
f(k,\theta) &= k \big( \mathbb{E}_p \big[q(\boldsymbol{x} | \theta)\big] -1\big) - \frac{1}{2}\log | 2 \pi e \Sigma| \\
\end{align}

We now proceed with traditional optimization techniques by setting the gradient with respect to $\theta$ equal to zero. Here we use the pathwise derivative (PD) to calculate the gradient of the expectation. This essentially amounts to moving the gradient inside the integral and thus inside the expectations. This procedure is always valid provided the argument of the expectation is continuous and everywhere differentiable.

\begin{equation}
\nabla_{\theta} f = k\mathbb{E}_p \big[ \nabla_{\theta} q(x|\theta)\big] - \big(0,\frac{1}{2}\Sigma^{-1} \big)^T\\
\end{equation}

To allow more transparent analysis we express the gradient in terms of its components

\begin{align}
\frac{\partial f}{\boldsymbol{\mu}} &= k\mathbb{E}_p \big[ \frac{\partial q(x|\boldsymbol{\mu}, \Sigma)}{\partial \boldsymbol{\mu}} \big] \\
\frac{\partial f}{\partial \Sigma} &= k\mathbb{E}_p \big[ \frac{\partial q(x|\boldsymbol{\mu}, \Sigma)}{\partial \Sigma} \big] - \frac{1}{2}\Sigma^{-1} \\
\end{align}

We begin by recalling the partials of the multivariate gaussian pdf for both the mean $\boldsymbol{\mu}$ and the covariance $\Sigma$.

\begin{align}
\frac{\partial q}{\boldsymbol{\mu}} &= q(x|\theta) \Sigma^{-1} (\boldsymbol{x}-\boldsymbol{\mu}) \\
\frac{\partial q}{\partial \Sigma} &= -q(x|\theta) \frac{1}{2}(\Sigma^{-1}-\Sigma^{-1}(\boldsymbol{x}-\boldsymbol{\mu})(\boldsymbol{x}-\boldsymbol{\mu})^T\Sigma^{-1}) \\
\end{align}

(see appendix for derivations)

Substituting these back into our gradient terms we arrive at

\begin{align}
\frac{\partial f}{\boldsymbol{\mu}} &= k\mathbb{E}_p \big[ q(x|\theta) \Sigma^{-1} (\boldsymbol{x}-\boldsymbol{\mu}) \big] \\
\frac{\partial f}{\partial \Sigma} &= k\mathbb{E}_p \big[ -q(x|\theta) \frac{1}{2}(\Sigma^{-1}-\Sigma^{-1}(\boldsymbol{x}-\boldsymbol{\mu})(\boldsymbol{x}-\boldsymbol{\mu})^T\Sigma^{-1}) \big] - \frac{1}{2}\Sigma^{-1} \\
\end{align}

We can now employ the Robbins-Monroe algorithm to determine the optimal parameters for the gaussian $q(x)$. We assume for now that the $x$'s are sampled i.i.d. from $p(x)$ and we update the gaussian in an online manner. To ease notation we introduce a few substitutions. We let $q_n = q(x_n|\boldsymbol{\mu}_n, \Sigma_n)$ and $C_n = (\boldsymbol{x}_n-\boldsymbol{\mu}_n)(\boldsymbol{x}_n-\boldsymbol{\mu}_n)^T$ which is the empirical Fisher matrix that approximates the Hessian giving us second order information in our update. After observing the $n^{\text{th}}$ sample from $p$ we update the gaussian as follows.

\begin{align}
\boldsymbol{\mu}_{n+1} &= \boldsymbol{\mu}_n + \alpha k q_n \Sigma_n^{-1} (\boldsymbol{x}_n-\boldsymbol{\mu}_n)   \\
\Sigma_{n+1} &= \Sigma_n + \beta k q_n \big( \Sigma_n^{-1} C_n \Sigma_n^{-1} - \Sigma_n^{-1} \big) + \gamma\Sigma^{-1} \\
\end{align}

The constants $\alpha$, $\beta$, and $\gamma$ are terms to ensure dimensional consistency. They are often given the misnomer of step size. This is not the case as the gradient terms are not normalized and steps are not of size $\alpha$, $\beta$ or $\gamma$.

This stochastic optimization allows our update to be online and the learning objective is to minimize the reverse KL between a single gaussian and the unknown distribution. Recall that minimizing the reverse Kullback-Leibler divergence is mode seeking, so the gaussian will move towards a nearby local extrema in the unknown distribution $p(x)$.

Now let us take a look at the second derivatives

\begin{align}
\frac{\partial^2 q}{\boldsymbol{\mu}^2} &= q(x|\theta) \big( (\boldsymbol{x}-\boldsymbol{\mu})^T \Sigma^{-2} (\boldsymbol{x}-\boldsymbol{\mu}) - \Sigma^{-1} \big) \\
\frac{\partial q}{\partial \Sigma} &= -q(x|\theta) \frac{1}{2}(\Sigma^{-1}-\Sigma^{-1}(\boldsymbol{x}-\boldsymbol{\mu})(\boldsymbol{x}-\boldsymbol{\mu})^T\Sigma^{-1}) \\
\end{align}

We take a moment now interpret what this learning rule is doing. Reducing the equations to a single dimension allows for an easier interpretation.

\begin{align}
\boldsymbol{\mu}_{n+1} &= \boldsymbol{\mu}_n + \alpha q_n \frac{\boldsymbol{x}_n - \boldsymbol{\mu}_n}{\sigma_n^2}  \\
\sigma_{n+1}^2 &= \sigma_n^2 + \beta \frac{q_n}{2} \bigg( \big(\frac{\boldsymbol{x}_n - \boldsymbol{\mu}_n}{ \sigma_n^2}\big)^2 - \frac{1}{\sigma_n^2} \bigg) + \gamma\frac{1}{\sigma_n^2} \\
\end{align}

Visualizations depiciting the learning process to come...

```python
from ipywidgets import interactive
class neuron():
    def __init__(self, mu_0, var_0, a=1, b=1):
        self.mu = mu_0
        self.var = var_0
        self.a = a
        self.b = b
        self.calls = 0

    def __call__(self, x, update=False):
        self.calls += 1
        z = x-self.mu
        x2 = x + np.random.rand()*self.var
        z2 = x2-self.mu
        norm_const = 1/np.sqrt(2*np.pi*self.var)
        output = norm_const*np.exp(-np.power(z,2)/(2*self.var))
        output2 = norm_const*np.exp(-np.power(z2,2)/(2*self.var))

        if update:
            #self.mu += self.a*output*(z/self.var)
            #self.var += self.b/2*output*(np.power(z/self.var,2)-(1/self.var)) + 1/self.var
            self.mu += self.a*output*z
            #z2 = x-self.mu
            #self.var += self.b*output/2*(np.power(z,2)-self.var) + 1/self.var
            #self.mu += self.a*0.5*(output*(z/self.var)+output2*(z2/self.var))
            #self.var += self.b*0.5*(output*(np.power(z/self.var,2)-(1/self.var))+output2*(np.power(z2/self.var,2)-(1/self.var))) + 1/self.var
            assert self.var > 0, 'ERROR: $\sigma^2 \le 0$'

        return output

p = gmm([0.3,0.7], np.array([[-7,1], [3,1]]))
q = neuron(-2,1,1,2)
t = np.linspace(-11,10,1000)
true_kl = []
upper_bound = []

def f(i=0):
    x = p.sample()
    y = q(x, update=True)
    true_kl.append(reverse_kl_int(q,p,-10,10,100))
    upper_bound.append(kl_upper_bound(q,p,-10,10,100))
    fig, ax = plt.subplots(1, 2, figsize=(12,5), dpi=80)
    ax[0].plot([x,x],[0.0,1.0],c='k', label='$x\sim p(x)$')
    ax[0].plot(t, p(t), c='r', lw=3, label='$p(x)$')
    ax[0].plot(t, q(t), c='b', lw=3, label='$q(x)$')
    ax[0].legend()
    ax[1].plot(true_kl, label='KL')
    ax[1].plot(upper_bound, label='bound')
    ax[1].set_xlabel('$q(x)$')
    ax[1].set_title('KL Divergence and Upper Bound')
    ax[1].legend()
    plt.show()

interactive_plot = interactive(f, i=(0, 1000))
output = interactive_plot.children[-1]
#output.layout.height = '35px'
interactive_plot
```

To build some intuition about the stability issues lets examine closely the cost surface for each iteration

```python
from mpl_toolkits.mplot3d import axes3d
from matplotlib import cm

def f(mu,var,x):
    x = np.array(x)
    N = x.size
    val = 0
    c = 2*var
    for xi in x:
        val += (xi-mu)**2
    return np.exp(-(N/2)*np.log(c*np.pi) - (1/c)*val) + 0.5*np.log(c*np.pi*np.e)

mu_min = 0.5
mu_max = 1.5
mu_step = 0.01
var_min = 0.001
var_max = 2.0
var_step = 0.01

mu,var = np.mgrid[mu_min:mu_max:mu_step,var_min:var_max:var_step]
#mf = mu.flatten()
#sf = sigma.flatten()
#locs = np.where(1/sf<(1-mf)**2)[0]

z = f(mu,var,[1])
fig = plt.figure(figsize=(8,6))
ax = fig.gca(projection='3d')
ax.plot_surface(mu, var, z, rstride=2, cstride=2, alpha=0.3)
cset = ax.contour(mu, var, z, zdir='z', offset=0.04, cmap=cm.coolwarm)
cset = ax.contour(mu, var, z, zdir='x', offset=mu_min, cmap=cm.coolwarm)
cset = ax.contour(mu, var, z, zdir='y', offset=var_max, cmap=cm.coolwarm)

ax.set_xlabel('$\mu$')
ax.set_xlim(mu_min, mu_max)
ax.set_ylabel('$\sigma^2$')
ax.set_ylim(var_min, var_max)
ax.set_ylim(var_min, var_max)
ax.set_zlim(bottom=0)
ax.set_zlabel('$f$')

plt.show()
```

### Cost Surface

A common point of confusion when first computing maximum likelihood solutions is about what function is being optimized. Our probability density function (pdf) $q(x;\mu,\lambda)$ is a function of the events $x$ from our random variable $X$ and is parameterized by $\mu$ and $\lambda$. For example when $\mu=1$ and $\lambda=5$

$$q(x;\mu=1,\lambda=5) = \sqrt{\frac{5}{2 \pi}}\exp\{-\frac{5}{2}(x-1)^2 \}$$

We see clearly $q$ depends only on $x$ and its plot is shown in the figure below

```python
t = np.linspace(0,2,100)
q = gmm([1], np.array([[1,1/5]]))
q_y = q(t)
fig, ax = plt.subplots(figsize=(10,3))
ax.plot(t,q_y)
plt.show()
```

The confusion arises when we apply the optimization definition directly which says we must find the variable $x$ that maximizes the pdf. This is just a little mathematical slight of hand resulting from an imprecise specification of exactly what function is being optimized. When we observe values of the random variable $x$ we view the function $q(x; \mu, \lambda)$ differently. The observed random variable becomes the parameter and $q$ becomes a function of both $\mu$ and $\lambda$. It is no longer required to be a pdf as the integral over both $\mu$ and $\lambda$ is not guaranteed to equal 1. This is made clear by defining a new function for when $x$ is observed.

$$f(\mu,\lambda;\ x_0) =: q(x_0; \mu, \lambda)$$

For example when the random variable $x$ assumes a value $x=1$ the plot of $f(\mu,\lambda;x=1)$ is shown below.

```python
from mpl_toolkits.mplot3d import axes3d
from matplotlib import cm

def f(mu,lamda,x):
    x = np.array(x)
    N = x.size
    val = 0
    c = lamda/2
    for xi in x:
        val += (xi-mu)**2
    return np.exp((N/2)*np.log(c/np.pi) - c*val)

mu_min = -1
mu_max = 3
mu_step = 0.05
l_min = 0.1
l_max = 5
l_step = 0.05

mu,lamda = np.mgrid[mu_min:mu_max:mu_step,l_min:l_max:l_step]
mf = mu.flatten()
lf = lamda.flatten()
locs = np.where(1/lf<(1-mf)**2)[0]

z = f(mu,lamda,[1])
fig = plt.figure(figsize=(8,6))
ax = fig.gca(projection='3d')
ax.plot_surface(mu, lamda, z, rstride=5, cstride=5, alpha=0.3)
cset = ax.contour(mu, lamda, z, zdir='z', offset=0.04, cmap=cm.coolwarm)
cset = ax.contour(mu, lamda, z, zdir='x', offset=mu_min, cmap=cm.coolwarm)
cset = ax.contour(mu, lamda, z, zdir='y', offset=l_max, cmap=cm.coolwarm)

ax.set_xlabel('$\mu$')
ax.set_xlim(mu_min, mu_max)
ax.set_ylabel('$\lambda$')
ax.set_ylim(l_min, l_max)
ax.set_zlabel('$f$')

plt.show()
```

I would like to visualize the surface of $KL(q(x|\mu,\sigma)||p(x))$ for different $\mu$ and $\sigma$. To do this, we must choose a $p(x)$ to hold fixed, and then compute the KL for a range of values of $\mu$ and $\sigma$. We can then display the results as a surface in 3D. Let us choose $p(x)$ to be a standard normal with mean 0 and standard deviation 1. I defined a function above which calculates the value of the KL divergence for a set $q(x)$ and $p(x)$ using a numerical integration technique. I redefine it here and add comments for ease of use

```python
def reverse_kl_int(q,p,beg,end,n_samples):
    """Calculate the Reverse Kullback-Leibler Divergence between two density functions q(x) and p(x).
        q - a density function defined using the gmm class above.
        p - a density function defined using the gmm class above.
        beg - the beginning of the interval which to integrate over.
        end - the end of the interval which to integrate over.
        n_samples - the number of sample points within the interval to use in the integral approximation.
    """
    a = beg
    b = end
    samples = np.linspace(a,b,n_samples)
    qs = q(samples)
    return (qs*np.log(qs/p(samples))).sum()*((b-a)/n_samples)
```

Below we define a standard normal distribution. We use the gaussian mixture model (gmm) class but only define a single gaussian at mean $\mu=0$ and standard deviation $\sigma=1$ with a weight of $\pi=1$. So it is a regular univariate guassian.

```python
p = gmm([1], np.array([[0,1]]))
```

Now we need to iteratively define many different $q(x)$ gaussians for means varying from $-3 \le \mu \le 3$ and standard deviations varying from $0.1 \le \sigma \le 2.1$ (double for loop) and for each one we need to calculate the KL divergence using the above function reverse_kl_int. For all of the $q(x)$ densities use beg=-6, end=6, and n_samples=1000. Save all of these results into a matrix and then plot them using 3D plotting routines such as the one demonstrated above in this notebook.

---
### MAP

We have observed that stochastic approximation in the current problem is difficult. It is sensitive to learning rate parameters. This suggests that there may be an approach to determining them that is intrinsic to the problem. While we are also exploring this route, for now we breifly identify another alternative that may aleviate some of the responsibility the parameters currently bear.

Rather than minimizing the divergence between $q(x|\theta)$ and $p(x)$, let us consider the partial KL between $q(x,\theta)$ and $p(x)$. Here we define the partial KL to be over only the $x$ random variable. In other words it is the gradient of $KL(q(x,\theta)||p(x))$ with respect to $\theta$ and the the output would be a function of $\theta$. A $\theta^*$ that minimizes this is a local extrema of the full $KL(q(x,\theta)||p(x))$.

\begin{align}
KL(q(x,\theta)||p(x)) &= \int \int q(x,\theta) \log \frac{q(x,\theta)}{p(x)} dx d\theta \\
\nabla_\theta KL(q(x,\theta)||p(x)) &= \int q(x,\theta) \log \frac{q(x,\theta)}{p(x)} dx \\
&= \int q(x|\theta)q(\theta) \log \frac{q(x|\theta)q(\theta)}{p(x)} dx \\
\end{align}

The goal is for $q(x)$ to be as close to $p(x)$ as possible. Given that, it is interesting to see the term in the log resembling the posterior $q(\theta|x)$. This approach seems to be a proxy for MAP.

\begin{align}
\nabla_\theta KL(q(x,\theta)||p(x)) &= \int q(x|\theta)q(\theta) \log \frac{q(x|\theta)q(\theta)}{p(x)} dx \\
& = q(\theta) \int q(x|\theta) \log \frac{q(x|\theta)}{p(x)} dx +q(\theta) \log q(\theta) \\
& = q(\theta) KL(q(x|\theta)||p(x)) +q(\theta) \log q(\theta) \\
\end{align}

We use the upper bound derived previously

\begin{align}
\nabla_\theta KL(q(x,\theta)||p(x)) &= q(\theta) KL(q(x|\theta)||p(x)) +q(\theta) \log q(\theta) \\
&\le q(\theta) \bigg(\frac{\log \lambda}{1-\lambda} \mathbb{E}_p \big[q(x|\theta)\big] -  \frac{\log \lambda}{1-\lambda} - H[q] \bigg) +q(\theta) \log q(\theta) \\
\end{align}

We once again wish to minimize the upper bound with respect to $\theta$. This reduces to

\begin{align}
min_\theta J(\theta) &\le min_\theta \bigg[ q(\theta) \bigg(\frac{\log \lambda}{1-\lambda} \mathbb{E}_p \big[q(x|\theta)\big] -  \frac{\log \lambda}{1-\lambda} - H[q] \bigg) +q(\theta) \log q(\theta) \bigg]\\
&= max_\theta \big[ q(\theta) \big(\mathbb{E}_p \big[q(x|\theta)\big] + H[q] \big) + q(\theta) \log q(\theta) \big]\\
\end{align}

for $\lambda \ll 1$ and $q(\theta) \lt 1$. If we choose our prior to be improper such that $p(\theta) = 1$ This approach reduces to the maximum likelihood approach we derived earlier. The hope is that if we choose priors that constrain the gaussian parameters to reasonable values then the optimization problem may be less sensitive to  the learning rates.

### Information Gain

It is edifying to consider a third approach. We would like to choose the next set of parameters $\theta_{i+1}$ such that we always gain information. We define $G_P(q_i, q_{i+1})$ to be the information gained when moving from $q_i$ to $q_{i+1}$ relative to a distribution $P$.

\begin{align}
G_P(q_i, q_{i+1}) = KL(q(x|\theta_i)||p(x))-KL(q(x|\theta_{i+1})||p(x))
\end{align}

We would like to maximizing the information gain $G$ with respect to the next set of parameters $\theta_{i+1}$. We were already at $\theta_i$ so we treat them as known and the maximization reduces to

\begin{align}
\max_{\theta_{i+1}} G_P(q_i, q_{i+1}) &= \max_{\theta_{i+1}} \bigg[KL(q(x|\theta_i)||p(x)) -KL(q(x|\theta_{i+1})||p(x)) \bigg] \\
&= \min_{\theta_{i+1}} \bigg[KL(q(x|\theta_{i+1})||p(x)) \bigg] \\
\end{align}

And we see we're left with the same minimization problem that we initially posed and can use the same upper bound previously derived.

Firing threholds change in neurons...

@article{azouz2000dynamic,
  title={Dynamic spike threshold reveals a mechanism for synaptic coincidence detection in cortical neurons in vivo},
  author={Azouz, Rony and Gray, Charles M},
  journal={Proceedings of the National Academy of Sciences},
  volume={97},
  number={14},
  pages={8110--8115},
  year={2000},
  publisher={National Acad Sciences}
}

dissertation/KL Upper Bound and Applications.ipynb


# Mecánica con SymPy

_Si SymPy te ha parecido hasta ahora un CAS decente e incluso interesante (nada como tener los resultados en $\LaTeX$ incrustados en el notebook y la sintaxis de Python para hacer cálculo simbólico) entonces espera a ver el paquete `mechanics`. Con él, podremos manipular velocidades y aceleraciones de sólidos expresadas en distintos sistemas de referencia con una facilidad impresionante._

_Tienes disponible la documentación de `mechanics` en http://docs.sympy.org/0.7.5/modules/physics/mechanics/index.html._

## Sistemas de referencia

El objeto primordial que vamos a manejar van a ser los sistemas de referencia. Podremos definir relaciones geométricas entre ellos y de esta forma las transformaciones de vectores entre un sistema y otro serán triviales.

La manera usual de empezar a trabajar con SymPy es importar la función `init_session`:
```
from sympy import init_session
init_session(use_latex=True)```

 Esta función ya se encarga de importar todas las funciones básicas y preparar las salidas gráficas. Sin embargo, en este momento, esta función se encuentra en mantenimiento para su uso dentro de los notebooks por lo que activaremos la salida gráfica e importaremos las funciones de la manera usual. Puedes consultar el estado de la corrección en: https://github.com/sympy/sympy/pull/13300 y https://github.com/sympy/sympy/issues/13319 .

```python

```

```python

```

Todo lo que necesitamos está en `sympy.physics.mechanics`, incluyendo la clase `ReferenceFrame`. Nada más crear un sistema de referencia podemos acceder a sus versores unitarios: `x`, `y` y `z`.

http://docs.sympy.org/0.7.5/modules/physics/vector/vectors.html

```python

```

```python

```

Y para definir vectores solo tenemos que **multiplicar cada componente por su versor**:

```python

```

De ahora en adelante, para trabajar como si nos enfrentáramos a un problema de la escuela, vamos a hacer dos cosas:

* Definir un sistema inercial $1$ del que partir, para así poder referir todos los demás sistemas a él.
* Que los versores de ese sistema sean $i, j, k$.

```python

```

Y para no tener que hacerlo siempre, un pequeño truco de magia:

```python
# Definimos nuestra propia clase para que los versores sean IJK
# aeropython: preserve
class IJKReferenceFrame(ReferenceFrame):
    def __init__(self, name):
        super().__init__(name, latexs=['\mathbf{%s}_{%s}' % (idx, name) for idx in ("i", "j", "k")])
        self.i = self.x
        self.j = self.y
        self.k = self.z
```

```python

```

### Álgebra vectorial

Nuestros vectores funcionan también con símbolos, y podemos realizar las operaciones de producto escalar y producto vectorial con ellos.

```python

```

```python

```

```python

```

```python

```

```python

```

```python

```

Podemos hallar también la norma de los vectores con su método `magnitude` e incluso normalizarlos con `normalize`:

```python

```

```python

```

##### Ejercicio

Usando directamente la fórmula para la derivada en ejes móviles:

$$\left(\frac{\operatorname{d}\!\mathbf{a}}{\operatorname{d}\!t}\right)_1 = \left(\frac{\operatorname{d}\!\mathbf{a}}{\operatorname{d}\!t}\right)_0 + \mathbf{\omega}_{01}\! \times \mathbf{a}$$

Calcula la derivada del vector de posición $R \mathbf{i}_0$, siendo $A_0$ un sistema de referencia que gira respecto al inercial con velocidad angular $\mathbf{\omega}_{01}=\Omega \mathbf{k}_0$. **¿Cuál es el módulo de la derivada?**

```python

```

```python

```

```python

```

```python

```

```python

```

Si no especificaste `positive=True` vas a ver algo como $\sqrt{\Omega^2 R^2}$. Debería haber una forma de simplificar esta expresión _a posteriori_, pero de momento no funciona del todo bien. Preparando este notebook nos hemos dado cuenta y ya les hemos avisado :) https://github.com/sympy/sympy/issues/8326

### Movimiento relativo

¿A quién no le gusta multiplicar matrices de rotación? Para esa minoría que lo detesta, existe SymPy. Para ello debemos especificar la orientación de nuestros sistemas de referencia usando el método `orient`, y recuperaremos la matriz de cosenos directores usando el método `dcm`.

```python

```

```python

```

Usando el argumento `Axis` hemos especificado que rotamos el sistema un ángulo especificado alrededor de un eje. Otros métodos son:

* `Body`: se especifican los tres ángulos de Euler.
* `Space`: igual que `Body`, pero las rotaciones se aplican en orden inverso.
* `Quaternion`: utilizando cuaternios, rotación alrededor de un vector unitario $\lambda$ una cantidad $\theta$.

¿Qué es lo bueno de usar uno de estos métodos? ¡Que **siempre** tenemos la transformación bien definida! Es imposible meter "a capón" una matriz de rotación que sea incorrecta o absurda.

#### Diferente sistema de referencia

Para expresar un vector en otro sistema de referencia, no hay más que usar los métodos `express` o `to_matrix`:

```python

```

```python

```

#### Símbolos dinámicos

Si queremos especificar que un símbolo puede variar con el tiempo, hay que usar la función `dynamicsymbols`:

```python

```

```python

```

Y pedir su derivada con el método `diff`:

```python

```

##### Ejercicio

(Sacado de Cuerva et al. "Teoría de los Helicópteros")

**Obtener la matriz de rotación de la pala $B$ respecto a los ejes $A1$.**

```python

```

```python

```

```python

```

```python

```

```python

```

```python

```

```python

```

#### Velocidad angular

También podemos hallar la velocidad angular de un sistema respecto a otro usando el método `ang_vel_in`:

```python

```

```python

```

```python

```

En ocasiones, la representación gráfica puede fallar, pero se puede volver a desactivar y activar llamando a la función`init_printing(pretty_print=True)` con diferentes valores (True/False) para `pretty_print`

### Derivada en ejes móviles

Hacer una derivada con la fórmula lo hace cualquiera, pero SymPy puede encargarse automáticamente.

```python

```

```python
#v1.diff(dynamicsymbols._t, A2)
```

```python

```

```python

```

### Puntos, velocidades y la rueda que no desliza

El último paso que nos queda para completar la cinemática es la posibilidad de definir puntos en sólidos y aplicar su campo de velocidades. SymPy también permite esto, y para ello no tenemos más que importar la clase `Point`.

```python

```

```python

```

Para trabajar como lo haríamos en la escuela, vamos a especificar que $O$ es el origen de $A$, y para eso vamos a imponer que su velocidad es cero con el método `set_vel`:

```python

```

Para definir nuevos puntos, podemos utilizar el método `locate_new`:

```python

```

Y para obtener vectores de un punto a otro, el método `pos_from`:

```python

```

La notación de este paquete está influenciada por el libro Kane, T. R. & Levinson, D. A. "Dynamics, Theory and Applications". Es ligeramente distinto a como estudiamos nosotros en la escuela, pero ¡están abiertos a que les hagamos cualquier tipo de sugerencia! https://github.com/sympy/sympy/issues/2584#issuecomment-31552654

Por último, el **campo de velocidades de un sólido rígido** se formula usando el método `v2pt_theory`.

$$v^P_A = v^O_A + \omega_{A_1 A} \times \mathbf{OP}$$

Este método pertenece *al punto del cual queremos conocer la velocidad* y recibe tres parámetros:

* `O`, punto de velocidad conocida respecto a A
* `A`, sistema de referencia donde queremos calcular la velocidad
* `A1`, sistema de referencia donde están fijos ambos puntos (_sistema de arrastre_)

Por tanto, para hallar la velocidad del punto que acabamos de crear:

```python

```

##### Ejercicio

(Apuntes de Óscar López Rebollal)

**¡Halla la velocidad y la aceleración de $P$!**

```python
# Creamos nuestros sistemas de referencia
```

```python
# Creamos los símbolos dinámicos necesarios
```

```python
# Orientamos los sistemas de referencia
```

```python

```

```python
# Creamos el punto C, centro del disco, y especificamos su velocidad
# respecto a A1
```

```python
# Localizamos el punto P, punto fijo del disco, respecto a C, en
# el sistema A2 (que gira solidariamente con el disco)
```

```python
# Hallamos la velocidad de P en A1, expresada en A0
# ¡Con esta llamada ya estamos diciendo que C y P son fijos en A2!
```

**Misión cumplida :)**

---

_Hemos hecho un repaso bastante profundo de las posibilidades del paquete `mechanics` de SymPy. Nos hemos dejado algunas cosas en el tintero pero no demasiadas: esta funcionalidad aún se está expandiendo y necesita pulir algunos detalles._

**Referencias**

* Capítulo de **aeromecánica** del libro de Cuerva y otros http://nbviewer.ipython.org/gist/Juanlu001/7711865
* Estabilidad longitudinal de un Boeing 747 http://nbviewer.ipython.org/github/AlexS12/Mecanica_Vuelo/blob/master/MVII_MatrizSistema.ipynb

_¿Serás tú el siguiente que publique un notebook usando SymPy? ;)_

Si te ha gustado esta clase:

Tweet

---

####
¡Síguenos en Twitter!

###### Follow @Pybonacci   Follow @Alex__S12   Follow @newlawrence

#####
Curso AeroPython por Juan Luis Cano Rodriguez y Alejandro Sáez Mollejo se distribuye bajo una Licencia Creative Commons Atribución 4.0 Internacional.

#####

---
_Las siguientes celdas contienen configuración del Notebook_

_Para visualizar y utlizar los enlaces a Twitter el notebook debe ejecutarse como [seguro](http://ipython.org/ipython-doc/dev/notebook/security.html)_

    File > Trusted Notebook

```python

```

```python
# Esta celda da el estilo al notebook
from IPython.core.display import HTML
css_file = '../styles/aeropython.css'
HTML(open(css_file, "r").read())
```

/* This template is inspired in the one used by Lorena Barba
in the numerical-mooc repository: https://github.com/numerical-mooc/numerical-mooc
We thank her work and hope you also enjoy the look of the notobooks with this style */

<link href='http://fonts.googleapis.com/css?family=Source+Sans+Pro|Josefin+Sans:400,700,400italic|Ubuntu+Condensed' rel='stylesheet' type='text/css'>

El estilo se ha aplicado =)


# Control Architecture

The cascading control architecture will be developed for the 2D drone. As you can remember the drone consists of two propellers located at a distance $l$ from the center of mass. It can generate a thrust by them to overcome the earth gravity and can rotate along the center of mass by creating thrust difference between these two propellers, allowing it to move laterally. In this exercise, we will use the highly simplified model of the drone in order to concentrate our efforts on the controller architecture and not on the drone itself.

Similarly the state can be described by next vector: $X = [z , y, \phi, \dot{z}, \dot{y},\dot{\phi}]$. Thus we will truck drones position in 2 dimension and its rotation along the $x$ axis.

```python
%matplotlib inline
%config InlineBackend.figure_format = 'retina'

import numpy as np
import math
from math import sin, cos
import matplotlib.pyplot as plt
import matplotlib.pylab as pylab
import jdc
import random

pylab.rcParams['figure.figsize'] = 10, 10
```

### Linearized drone.
As you remember the equations of motion for a 2D drone have the next form for a given $\phi$ angle:
$$
\begin{align}
\ddot{z} &= g - \frac{k_f}{m}(\omega_1^2+\omega_2^2) \cos(\phi) \\
\ddot{y} &= \frac{k_f}{m}(\omega_1^2+\omega_2^2) \sin(\phi)\\
\ddot{\phi} &= \frac{l k_f}{I}(\omega_1^2 - \omega_2^2)
\end{align}
$$

For a small rolling angle $\phi \ll 1$, $\sin{\phi} = \phi$ and $\cos{\phi} =1 $. Additionally we will simplify the drone dynamics and will replace the total force  and torque by $u_1$ and $u_2$ inputs from the controller.

$$
\begin{align}
F &= k_f(\omega_1^2 + \omega_2^2) = u_1 \\
\tau &= k_f(\omega_1^2 - \omega_2^2) = u_2
\end{align}
$$
The given modification keep the drone dynamics intact while helping us to avoid all the complex calculations that we had to perform to control it using the older model. The price that we have to pay for this simplification is that now we need to transfer drone parameters like mass and moment of inertia to the control object in order to get the correct control values.

Thus the simplified linear drone model will have the next equations of motion:
$$
\begin{align}
\ddot{z} &= g - \frac{u_1}{m} \\
\ddot{y} &= g\phi \\
\ddot{\phi} &= \frac{u_2}{I_{xx}}
\end{align}
$$

The `LinearDroneInTwoD` object will have the similar properties and function as the one that we covered in the last lesson. The `advance_state` function will take controls parameters $u_1$ and $u_2$ with $dt$ time and will return the new drone's state vector.

```python
class LinearDroneInTwoD:

    def __init__(self,
                 I_x = 0.1, # moment of inertia around the x-axis
                 m = 0.2,   # mass of the vehicle
                ):

        self.I_x = I_x
        self.m = m

        self.u1 = 0.0
        self.u2 = 0.0
        self.g = 9.81

        self.X = np.array([0.0,0.0,0.0,0.0,0.0,0.0])

    @property
    def y_dot_dot(self):
        phi = self.X[2]
        return self.g * phi

    @property
    def z_dot_dot(self):
        return self.g - self.u1/self.m

    @property
    def phi_dot_dot(self):
        return self.u2 / self.I_x

    def advance_state(self, u1, u2,  dt):
        self.u1 = u1
        self.u2 = u2

        X_dot = np.array([self.X[3],
                        self.X[4],
                        self.X[5],
                        self.z_dot_dot,
                        self.y_dot_dot,
                        self.phi_dot_dot])

        # Change in state will be
        self.X = self.X + X_dot * dt
        return self.X
```

### Controller
In this section, we will create the controller object described in class. The schematic description of the cascading controller looks like next:

The controller architecture uses three PD controllers to control the drone. One PD controller is used to generate the thrust and control the altitude while two PD controllers are used to control the lateral motion. The first one receives the desired lateral position information containing $y$ , $\dot{y}$ and $\ddot{y}$ based on which is outputs the commanded $\phi$ roll angle. The second PD controller is used to sets a torque value to achieve the desired angle. In most cases, the inner controller responsible for roll angle operates at the higher frequency than the ones that are used to control the position.

```python
class LinearCascadingController:

    def __init__(self,
                 m,
                 I_x,
                 z_k_p=1.0,
                 z_k_d=1.0,
                 y_k_p=1.0,
                 y_k_d=1.0,
                 phi_k_p=1.0,
                 phi_k_d=1.0):

        self.z_k_p = z_k_p
        self.z_k_d = z_k_d
        self.y_k_p = y_k_p
        self.y_k_d = y_k_d
        self.phi_k_p = phi_k_p
        self.phi_k_d = phi_k_d

        self.g = 9.81
        self.I_x = I_x
        self.m = m
```

The altitude PD controller will take input parameters of $(z_{target},z_{actual},\dot{z}_{target}, \dot{z}_{actual}, \ddot{z}_{target})$ and outputs the $u_1$ instead of $\bar{u}_1$. The reson behind transfering the mass and the moment of inertia of the drone to controller is to convert the $\bar{u}_1$ into $u_1$. As a reminder $\bar{u_1} = g - \frac{u_1}{m}$.

```python
%%add_to LinearCascadingController

def altitude_controller(self,
                z_target,
                z_actual,
                z_dot_target,
                z_dot_actual,
                z_dot_dot_target
                ):

    z_err = z_target - z_actual
    z_err_dot = z_dot_target - z_dot_actual

    p_term = self.z_k_p * z_err
    d_term = self.z_k_d * z_err_dot

    u_1_bar = p_term + d_term + z_dot_dot_target

    u_1 = self.m* (self.g - u_1_bar)

    return u_1
```

The latteral motion is executed by rolling the drone along the $x$ axis with small angle which will creat the force along the desired motion dirction. The controller takes input parameters of $(y_{target},y_{actual},\dot{y}_{target}, \dot{y}_{actual}, \ddot{y}_{target})$ and outputs the $\phi_{commanded}$

```python
%%add_to LinearCascadingController

def lateral_controller(self,
                        y_target,
                        y_actual,
                        y_dot_target,
                        y_dot_actual,
                        y_dot_dot):

    y_err = y_target - y_actual
    y_err_dot = y_dot_target - y_dot_actual

    p_term = self.y_k_p * y_err
    d_term = self.y_k_d * y_err_dot

    phi_commanded = (p_term + d_term + y_dot_dot) / self.g

    return phi_commanded

```

Attitude controller uses the same PD controller which will take input from the lateral direction controller $\phi_{commanded}$ and will output the torque value. Keep in mind that we are using PD controller and giving $\phi_{commanded}$ and $\dot{\phi}_{commanded} = 0 $.

```python
%%add_to LinearCascadingController

def attitude_controller(self,
                        phi_target,
                        phi_actual,
                        phi_dot_target,
                        phi_dot_actual):

    phi_err = phi_target - phi_actual
    phi_err_dot = phi_dot_target - phi_dot_actual

    p_term = self.phi_k_p * phi_err
    d_term = self.phi_k_d * phi_err_dot

    u_2 = (p_term + d_term ) * self.I_x

    return u_2
```

Before using the controller developed above, we need to create the flight path. Which we have chosen to be figure 8 executed for 200 seconds with position update rate of 10 Hz $(dt=0.1)$. The executed path is described as:
$$
\begin{align}
z & = a_z \sin{\omega_z t} \\
y & = a_y \cos{\omega_y t}
\end{align}
$$

where $\omega_y = \omega_z / 2$.

```python
# Flight path is
total_time = 200.0
dt = 0.1
omega_z = 0.2
omega_y = 0.1
a_z = 1.0
a_y = 1.0

t=np.linspace(0.0,total_time,int(total_time/dt))

# desired path over time
z_path = a_z * np.sin(omega_z * t)
# desired velocity over time in order to execute the given flight path
z_dot_path = a_z * omega_z * np.cos(omega_z * t)
# desired acceleration over time in order to execute the given flight path
z_dot_dot_path= -a_z * omega_z**2 * np.sin(omega_z * t)

# desired path over time
y_path = a_y * np.cos(omega_y * t)
# desired velocity over time in order to execute the given flight path
y_dot_path = -a_y * omega_y * np.sin(omega_y * t)
# desired acceleration over time in order to execute the given flight path
y_dot_dot_path= -a_y * omega_y**2 * np.cos(omega_y * t)

plt.plot(y_path,z_path)
plt.axis('equal')
plt.title('Flight trajectory').set_fontsize(20)
plt.xlabel('$y$ [$m$]').set_fontsize(20)
plt.ylabel('$z$ [$m$]').set_fontsize(20)
plt.xticks(fontsize = 14)
plt.yticks(fontsize = 14)
plt.show()
```

### Executing the flight

As mentioned in class the inner loop controller responsible for torque runs at the higher frequency than the ones that are responsible for the total thrust and the desired roll angle. First, we will declare how fast the inner controller runs compared to others.

The executing flight starts with pre-flight preparation of first declaring the drone object and controller object and then provide the initial state of the drone next will initialize the array that records the drone state over time.

Compared to the previous version the flight is executed by marching through time and transferring the desired trajectory to the drone which will generate the output of commanded thrust $u_1$ and roll angle $\phi_{commanded}$. Next, the inner controller is executed for as much as it is faster compared to the external PD controllers. During each time the proper torque value is generated $u_2$. The given parameters are transferred to the drone to update the state based on the $u_1$, $u_2$ with time advance of $dt'$. Where $dt'$ is inner controller time step.

_Direction:_ input controller coefficients manually. As each controller is responsible for different parameters, it is possible that the coefficients might be several orders of magnitude different from each other.

```python
# how has is inner loop compared to external one
inner_loop_acc = 10

# creating the co-axial drone object
Controlled_Drone=LinearDroneInTwoD()

# creating the control system object
control_system = LinearCascadingController(Controlled_Drone.m,
                                           Controlled_Drone.I_x,
                                             z_k_p=0.1,
                                             z_k_d=10.0,
                                             y_k_p=0.1,
                                             y_k_d=10.0,
                                             phi_k_p=10.0,
                                             phi_k_d=50.0)

# declaring the initial state of the drone with zero hight and zero velocity
Controlled_Drone.X = np.array([z_path[0],
                               y_path[0],
                               math.atan2(y_dot_path[0],z_dot_path[0]),
                               z_dot_path[0],
                               y_dot_path[0],
    (math.atan2(y_dot_path[1],z_dot_path[1])-math.atan2(y_dot_path[0],z_dot_path[0]))/dt])

# array for recording the state history
linear_drone_state_history = Controlled_Drone.X

# executing the flight
for i in range(0,z_path.shape[0]-1):

    u_1 = control_system.altitude_controller(z_path[i],
                                           Controlled_Drone.X[0],
                                           z_dot_path[i],
                                           Controlled_Drone.X[3],
                                           z_dot_dot_path[i])

    phi_commanded = control_system.lateral_controller(y_path[i],
                                            Controlled_Drone.X[1],
                                            y_dot_path[i],
                                            Controlled_Drone.X[4],
                                            y_dot_dot_path[i])

    for _ in range(inner_loop_acc):

        u_2 = control_system.attitude_controller(phi_commanded,
                                                 Controlled_Drone.X[2],
                                                 0.0,
                                                 Controlled_Drone.X[5])

        # calculating the new state vector
        drone_state = Controlled_Drone.advance_state(u_1, u_2,  dt/inner_loop_acc)

    # generating a history of vertical positions for the drone
    linear_drone_state_history = np.vstack((linear_drone_state_history, drone_state))

plt.plot( y_path, z_path,linestyle='-',marker='.',color='red')
plt.plot(linear_drone_state_history[:,1],linear_drone_state_history[:,0],linestyle='-',color='blue')
plt.title('Flight trajectory').set_fontsize(20)
plt.xlabel('$y$ [$m$]').set_fontsize(20)
plt.ylabel('$z$ [$m$]').set_fontsize(20)
plt.xticks(fontsize = 14)
plt.yticks(fontsize = 14)
plt.legend(['Planned path','Executed path'],fontsize = 14)
plt.axis('equal')
plt.show()
```

# Non-linear Drone

The non_linear drone contrary to the linear drone will not try to expand the trigonometric functions and will not use their approximated form

$$
\begin{align}
\ddot{z} &= g - \frac{k_f}{m}(\omega_1^2+\omega_2^2) \cos(\phi) \\
\ddot{y} &= \frac{k_f}{m}(\omega_1^2+\omega_2^2) \sin(\phi)\\
\ddot{\phi} &= \frac{l k_f}{I}(\omega_1^2 - \omega_2^2)
\end{align}
$$
Using the total thrust and the torque values obtained from the controller the nonlinear drone dynamic has the next form.
$$
\begin{align}
\ddot{z} &= (g - \frac{u_1}{m})\cos{\phi} \\
\ddot{y} &= g\sin{\phi} \\
\ddot{\phi} &= \frac{u_2}{I_{xx}}
\end{align}
$$

The nonlinear drone object is similar to the linear drone object with the addition of the nonlinear terms to calculate the drones acceleration in vertical and horizontal directions.

```python
class NonLinearDroneInTwoD:

    def __init__(self,
                 I_x = 0.1, # moment of inertia around the x-axis
                 m = 0.2,   # mass of the vehicle
                ):

        self.I_x = I_x
        self.m = m

        self.u1 = 0.0
        self.u2 = 0.0
        self.g = 9.81

        self.X = np.array([0.0,0.0,0.0,0.0,0.0,0.0])

    @property
    def y_dot_dot(self):
        phi = self.X[2]
        return self.g * math.sin(phi)

    @property
    def z_dot_dot(self):
        phi = self.X[2]
        return self.g - self.u1*math.cos(phi)/self.m

    @property
    def phi_dot_dot(self):
        return self.u2 / self.I_x

    def advance_state(self, u1, u2,  dt):
        self.u1 = u1
        self.u2 = u2

        X_dot = np.array([self.X[3],
                        self.X[4],
                        self.X[5],
                        self.z_dot_dot,
                        self.y_dot_dot,
                        self.phi_dot_dot])

        # Change in state will be
        self.X = self.X + X_dot * dt
        return self.X
```

### Nonlinear Controller
In this section, we will create the controller object described in class. The schematic description of the cascading controller looks like next:

The controller architecture uses three PD controllers to control the drone. One PD controller is used to generate the thrust and control the altitude while two PD controllers are used to control the lateral motion. The first one receives the desired lateral position information containing $y$ , $\dot{y}$ and $\ddot{y}$ based on which is outputs the commanded $\phi$ roll angle. The second PD controller is used to sets a torque value to achieve the desired angle. In most cases, the inner controller responsible for roll angle operates at the higher frequency than the ones that are used to control the position.

```python
class NonLinearCascadingController:

    def __init__(self,
                 m,
                 I_x,
                 z_k_p=1.0,
                 z_k_d=1.0,
                 y_k_p=1.0,
                 y_k_d=1.0,
                 phi_k_p=1.0,
                 phi_k_d=1.0):

        self.z_k_p = z_k_p
        self.z_k_d = z_k_d
        self.y_k_p = y_k_p
        self.y_k_d = y_k_d
        self.phi_k_p = phi_k_p
        self.phi_k_d = phi_k_d

        self.g = 9.81
        self.I_x = I_x
        self.m = m
```

The altitude PD controller will take input parameters of $(z_{target},z_{actual},\dot{z}_{target}, \dot{z}_{actual}, \ddot{z}_{target})$ and outputs the $u_1$ instead of $\bar{u}_1$. The reason behind transfering the mass and the moment of inertia of the drone to controller is to convert the $\bar{u}_1$ into $u_1$. As a reminder for nonlinear model $\bar{u_1} = (g - \frac{u_1}{m})\cos{\phi}$.

```python
%%add_to NonLinearCascadingController

def altitude_controller(self,
                z_target,
                z_actual,
                z_dot_target,
                z_dot_actual,
                z_dot_dot_target,
                phi_actual):

    z_err = z_target - z_actual
    z_err_dot = z_dot_target - z_dot_actual

    p_term = self.z_k_p * z_err
    d_term = self.z_k_d * z_err_dot

    u_1_bar = p_term + d_term + z_dot_dot_target

    u_1 = self.m* (self.g - u_1_bar)/ math.cos(phi_actual)

    return u_1

```

The lateral motion is executed by rolling the drone along the $x$ axis with a small angle which will create the force along the desired motion direction. The controller takes input parameters of $(y_{target},y_{actual},\dot{y}_{target}, \dot{y}_{actual}, \ddot{y}_{target})$ and outputs the $\phi_{commanded}$ As we are using the nonlinear model we roll angle is taken as $\sin^{-1}$. Luckily the $\sin^{-1}$ function is symmetric relative to the 0 and we do not need an additional condition to track the proper angle from rolling clockwise or counterclockwise. But keep in mind that if you are using the functions that are not one to one and onto you might need to implement an additional technique to track the true proper value.

```python
%%add_to NonLinearCascadingController

def lateral_controller(self,
                        y_target,
                        y_actual,
                        y_dot_target,
                        y_dot_actual,
                        y_dot_dot
                        ):
    y_err = y_target - y_actual
    y_err_dot = y_dot_target - y_dot_actual

    p_term = self.y_k_p * y_err
    d_term = self.y_k_d * y_err_dot

    phi_commanded = math.asin( (p_term + d_term + y_dot_dot) / self.g )

    return phi_commanded

```

Attitude controller uses the same PD controller which will take input from the lateral direction controller $\phi_{commanded}$ and will output the torque value. Keep in mind that we are using PD controller and giving $\phi_{commanded}$ and $\dot{\phi}_{commanded} = 0 $.

```python
%%add_to NonLinearCascadingController

def attitude_controller(self,
                        phi_target,
                        phi_actual,
                        phi_dot_target,
                        phi_dot_actual
                        ):
    phi_err = phi_target - phi_actual
    phi_err_dot = phi_dot_target - phi_dot_actual

    p_term = self.phi_k_p * phi_err
    d_term = self.phi_k_d * phi_err_dot

    u_2 = (p_term + d_term ) * self.I_x

    return u_2
```

Before using the controller developed above, we need to create the flight path. Which we have chosen to be figure 8 executed for 200 seconds with position update rate of 10 Hz $(dt=0.1)$. The executed path is described as:
$$
\begin{align}
z & = a_z \sin{\omega_z t} \\
y & = a_y \cos{\omega_y t}
\end{align}
$$

where $\omega_y = \omega_z / 2$.

```python
# Flight path is
total_time = 200.0
dt = 0.1
omega_z = 0.2
omega_y = 0.1
a_z = 1.0#0.25
a_y = 1.0

t=np.linspace(0.0,total_time,int(total_time/dt))

# desired path over time
z_path = a_z * np.sin(omega_z * t)
# desired velocity over time in order to execute the given flight path
z_dot_path = a_z * omega_z * np.cos(omega_z * t)
# desired acceleration over time in order to execute the given flight path
z_dot_dot_path= -a_z * omega_z**2 * np.sin(omega_z * t)

# desired path over time
y_path = a_y * np.cos(omega_y * t)
# desired velocity over time in order to execute the given flight path
y_dot_path = -a_y * omega_y * np.sin(omega_y * t)
# desired acceleration over time in order to execute the given flight path
y_dot_dot_path= -a_y * omega_y**2 * np.cos(omega_y * t)

plt.plot(y_path,z_path)
plt.axis('equal')
plt.xlabel('$y$ [$m$]').set_fontsize(20)
plt.ylabel('$z$ [$m$]').set_fontsize(20)
plt.xticks(fontsize = 14)
plt.yticks(fontsize = 14)
plt.show()
```

```python
# how fast is inner loop compared to external one
inner_loop_acc = 10

t1=np.linspace(0.0,total_time,int(total_time*inner_loop_acc/dt))

# creating the co-axial drone object
Controlled_Drone=NonLinearDroneInTwoD()

# creating the control system object
control_system = NonLinearCascadingController(Controlled_Drone.m,
                                             Controlled_Drone.I_x,
                                             z_k_p=0.1,
                                             z_k_d=10.0,
                                             y_k_p=0.1,
                                             y_k_d=10.0,
                                             phi_k_p=10.0,
                                             phi_k_d=50.0
                                          )

# declaring the initial state of the drone with zero hight and zero velocity
Controlled_Drone.X = np.array([z_path[0],
                               y_path[0],
                               math.atan2(y_dot_path[0],z_dot_path[0]),
                               z_dot_path[0],
                               y_dot_path[0],
    (math.atan2(y_dot_path[1],z_dot_path[1])-math.atan2(y_dot_path[0],z_dot_path[0]))/dt])

# array for recording the state history
non_linear_drone_state_history = Controlled_Drone.X

# executing the flight
for i in range(0,z_path.shape[0]-1):

    u_1 = control_system.altitude_controller(z_path[i],
                                           Controlled_Drone.X[0],
                                           z_dot_path[i],
                                           Controlled_Drone.X[3],
                                           z_dot_dot_path[i],
                                           Controlled_Drone.X[2])

    phi_commanded = control_system.lateral_controller(y_path[i],
                                            Controlled_Drone.X[1],
                                            y_dot_path[i],
                                            Controlled_Drone.X[4],
                                            y_dot_dot_path[i])

    for _ in range(inner_loop_acc):

        u_2 = control_system.attitude_controller(phi_commanded,
                                                 Controlled_Drone.X[2],
                                                 0.0,
                                                 Controlled_Drone.X[5])

        # calculating the new state vector
        drone_state = Controlled_Drone.advance_state(u_1, u_2,  dt/inner_loop_acc)

    # generating a history of vertical positions for the drone
    non_linear_drone_state_history = np.vstack((non_linear_drone_state_history, drone_state))

plt.plot( y_path, z_path,linestyle='-',marker='.',color='red')
plt.plot(non_linear_drone_state_history[:,1],non_linear_drone_state_history[:,0],linestyle='-',color='blue')
plt.title('Flight trajectory').set_fontsize(20)
plt.xlabel('$y$ [$m$]').set_fontsize(20)
plt.ylabel('$z$ [$m$]').set_fontsize(20)
plt.xticks(fontsize = 14)
plt.yticks(fontsize = 14)
plt.legend(['Planned path','Executed path'],fontsize = 14)
plt.axis('equal')
plt.show()

```

# Comparison of linear and non-linear controllers

In this section, we will compare the linear and nonlinear controllers by analyzing the path that drones took relative to the desired path.

For comparizon use the next set of controller inputs `z_k_p=0.1, z_k_d=10.0, y_k_p=0.1, y_k_d=10.0, phi_k_p=10.0, phi_k_d=50.0`.
Execute flight using both linear and nonlinear controllers. The vehicle state over time will be recorded in `non_linear_drone_state_history` for flight executed by the nonlinear controller and in `linear_drone_state_history` for flight executed by the linear controller.

```python
# Error calculation

non_linear_Err= np.sqrt((non_linear_drone_state_history[:,1] - y_path[:])**2 \
                        +(non_linear_drone_state_history[:,0] - z_path[:])**2)
linear_Err= np.sqrt((linear_drone_state_history[:,1] - y_path[:])**2 \
                    + (linear_drone_state_history[:,0] - z_path[:])**2)

t1=np.linspace(0.0,total_time,int(total_time*inner_loop_acc/dt))

plt.plot(t,non_linear_Err,color='red',marker='.')
plt.plot(t,linear_Err,color='blue')
plt.xlabel('$t$ [$s$]').set_fontsize(20)
plt.ylabel('$\epsilon$ [$m$]').set_fontsize(20)
plt.xticks(fontsize = 14)
plt.yticks(fontsize = 14)
plt.legend(['non_linear','linear'],fontsize = 14)
plt.show()
```

If you were able to obtain the flight paths which are virtually similar for the linear and nonlinear controllers it is because the angle of roll is changing little from the 0. Next plot will show the roll angle over time. As mentioned above for the small roll angle $\sin{\phi} = \phi$ and $\cos{\phi} = 1$. Unfortunately, this assumption is not true for the large angles what is why for the second set of parameters the flight path executed by non-linear controller looks different from the flight path executed linear controller.

```python
plt.plot(t,non_linear_drone_state_history[:,2]*180/math.pi,color='red', marker= '.')
plt.plot(t,linear_drone_state_history[:,2]*180/math.pi,color='blue')
plt.xlabel('$t$ [$s$]').set_fontsize(20)
plt.ylabel('roll angle [$^{\circ}$ degrees]').set_fontsize(20)
plt.xticks(fontsize = 14)
plt.yticks(fontsize = 14)
plt.legend(['non_linear','linear'],fontsize = 14)
plt.show()
```

```python

```

jupyter_notebooks/3_Controls/3_ControlArchitecture/Lesson 3 - Control Architecture.ipynb


# Expectation Maximisation

$
\newcommand{\x}{\mathbf{x}}
\newcommand{\y}{\mathbf{y}}
\newcommand{\z}{\mathbf{z}}
\newcommand{\class}{\mathcal{C}}
\newcommand{\mean}{\boldsymbol{\mu}}
\newcommand{\cov}{\boldsymbol{\Sigma}}
\newcommand{\icov}{\boldsymbol{\Sigma}^{-1}}
\newcommand{\normdist}{\mathcal{N}}
\newcommand{\tp}{\top}
\newcommand{\params}{\mathbf{\theta}}
$

| | |
|--|--|
| **Names** | *Anna Berger, Ashwin Sadananda Bhat* |
| **Group** | *AML 25* |

In this lab we will build a probabilistic classifier and estimate its parameters. In order to illustrate how the complexity of learning changes when we do not observe all the variables, we will start with the model depicted in figure 1. We will then switch to the graphical model depicted in figure 2 and compare the two
models’ performance.

Read through the complete exercise handout before starting your implementation: the hints at the end, and the instructions for the subsequent exercises will help you for the current one

## Gaussian Distribution

**Figure 1**: Bayesian classifier with Normally distributed observations. The class labels C are discrete variables in one-of-K format. The observations x are multivariate and continuous, and their distribution is described with a Gaussian probability density function.

In this model, we want to find the posterior probability
$p(\class_k|\x_n)$ for all data points $\x_n$. We get this from Bayes' rule

$$
  p(\class_k|\x_n) = \frac{ p(\x_n|\class_k) p(\class_k) }{
    \sum_j p(\x_n|\class_j) p(\class_j) },
$$

where we can see from the graph that the data elements are
conditionally independent given the class and we model the
class-conditional probability with a normal distribution

$$
  p(\x_n|\class_k) = \normdist(\x_n;\mean_k,\cov_k)\\
  = \frac{1}{(2\pi)^{d/2}|\cov|^{1/2}} \exp -\frac{1}{2}
    (\x_n-\mean_k)^\tp \icov_k (\x_n-\mean_k)
$$

Load the data in `target.mat`, which consists
of two classes `A` and `B`. Split the data in train and test
sets and plot the training data, in order to get a feel for it. Just by looking at the data, do you think the model will perform well?

**Question 1 [10 marks]**
Do the training, which consists of finding the prior probabilities for each class, $p(\class_k)$, and the parameters of the class-conditional distributions, the means $\mean_k$ and covariance matrices $\cov_k$. Now use Bayes' rule to compute the most likely class label for each data point in the test set.  Include the resulting confusion matrix and error rate in
you report.

```python
%matplotlib inline
import numpy as np
import scipy.stats as st
import scipy.ndimage as ndim
import matplotlib.pyplot as plt
import operator
import pylab
import time
from IPython import display

from matplotlib.patches import Ellipse

pylab.rcParams['figure.figsize'] = (10.0, 8.0)

def normPDF(data,mu,sigma):
    """My implementation of the Gaussian distribution. Returns the density, given mean mu and covariance sigma,
    for every data point (row) of data"""
    N,nDim = data.shape
    res = np.zeros(N)

    icov = np.linalg.inv(sigma)
    covdet = np.linalg.det(sigma)
    denom = np.sqrt((2*np.pi)**nDim * covdet)

    for i in range(N):
        diff = data[i,:]-mu
        res[i] = np.exp(-.5 * np.dot(diff, np.dot(icov,diff.T))) / denom

    return res

# Load in the data
D = np.load("target.npz")
data = D['data']
labels = D['labels']
A=data[labels==0,:]
B=data[labels==1,:]

nA = A.shape[0]
nB = B.shape[0]
N = nA + nB

data = np.vstack((A,B))
plt.scatter(A[:,0],A[:,1])
plt.scatter(B[:,0],B[:,1],20,'red')

labels = np.concatenate((np.zeros(nA),np.ones(nB)))

perm = np.random.permutation(N)
datap = data[perm,:]
labelp = labels[perm]

trainData = datap[:4*N/5,:]
trainLab  = labelp[:4*N/5]
testData  = datap[4*N/5:,:]
testLab   = labelp[4*N/5:]
```

```python
# [ANSQ1] Do not modify this line
# Answer Q1 is below
```

SHORT DISCUSSION FOR Q1

From the picture we can see assume that this model will not perform well as data points have the shape of concetric circles and the data points of one class aren't placed together in the space but separated by the data points of another class and it's hard to imagine such Gaussian distributions which can handle this problem.

```python
from sklearn.metrics import accuracy_score, confusion_matrix
```

```python
def norm_param(arr):
    return np.mean(arr, axis = 0), np.cov(np.transpose(arr))

def posterior(data, mu, sigma):
    p_x_0 = normPDF(data, mu[0], sigma[0])
    p_x_1 = normPDF(data, mu[1], sigma[1])
    return p_x_0 * priors[0] / (p_x_0 * priors[0] + p_x_1 * priors[1])

def classify(data, mu, sigma):
    posterior_c_0 = posterior(data, mu, sigma)
    predictions = [0 if prob > 0.5 else 1 for prob in posterior_c_0]
    return predictions
```

```python
classes = 2
priors = np.zeros(classes)
mu = np.zeros((classes, classes))
sigma = np.zeros((2, classes, classes))

for i in range(classes):
    class_data = trainData[trainLab == i]
    priors[i] = float(len(class_data)) / len(trainLab)
    mu[i], sigma[i] = norm_param(class_data)

y_pred = classify(testData, mu, sigma)
```

```python
# visualisation of trained models

fig = plt.gcf()
plt.clf()
ax = fig.add_subplot(111, aspect='equal')
plt.scatter(data[:,0], data[:,1])
for m in range(classes):
    [val,vect] = np.linalg.eig(sigma[m])
    e = Ellipse(xy=mu[m], width=2*np.sqrt(val[0]), height=2*np.sqrt(val[1]), angle=180*np.arctan2(vect[0,0],vect[1,0])/np.pi)
    e.set_facecolor([1,0,0])
    ax.add_artist(e)
plt.show()
```

```python
acc = accuracy_score(testLab, y_pred)
conf_matrix = confusion_matrix(testLab, y_pred)
print "error_rate = ", 1 - acc
print "conf_matrix = \n", conf_matrix
```

    error_rate =  0.44625
    conf_matrix =
    [[244 157]
     [200 199]]

SHORT DISCUSSION

As we assumed, the accuracy of the proposed model is low due to the fact that one Gaussian distribution per class can't represent the actual distribution of the datapoints of the class well.

## Mixture of Gaussians

**Figure 2**: Graphical model of a Bayesian classifier with observations described by a Mixture of Gaussians
(MOG). Each class has a number of mixture elements, which are each modelling part of the data. Which
datapoints belong to which mixture element is controlled by the latent variable $\z$.

In this model, the class-conditional distributions $p(\x|\class_k)$ are
modelled by a **mixture** of Gaussians rather than a single Gaussian, so that the total data-likelihood
\begin{align}
  p(\x_n,\class_k) &= p(\class_k) \sum_j p(\x_n|z_j,\class_k)p(z_j|\class_k),
\end{align}
where
\begin{align}
  p(\x_n|z_j,\class_k) = \normdist(\x_n;\mean_{jk},\cov_{jk}).
\end{align}

Since the latent variables controlling the mixture components are not observed, we need to optimise the log-likelihood with the EM
algorithm. You are provided with some code to help you pull this off:

```python
# [ANSQ2] [ANSQ3] Do not modify this line

def invalid(sigma):
    """Is this a valid covariance matrix?"""
    if np.isfinite(sigma).all():
        return np.linalg.cond(sigma)>1e3
    return True

class MoG:
    def __init__(self,data,nMix=2):
        self.pi = np.ones(nMix) / nMix
        self.nPoints, self.nDim = data.shape
        self.nMix = nMix
        self.data = data
        mu = np.mean(data)
        self.mu = []
        self.sigma = []
        self.resp = np.zeros((self.pi.shape[0], self.data.shape[0]))
        for i in range(nMix):
            self.mu.append(mu + .1 * i * np.random.randn(self.nDim))
            self.sigma.append(np.eye(self.nDim))

    def em(self,maxIter=100,minInc=1e-5):
        llhist = np.zeros(maxIter)
        for iteration in range(maxIter):
            resp, ll = self.expectation()
            llhist[iteration]=ll
            if iteration>0:
                delta = llhist[iteration] - llhist[iteration-1]
                print "Iteration",iteration,":",delta
#                 print llhist[:iteration+1],llhist[iteration], llhist[iteration-1],delta
                if delta < minInc:
                    if delta < 0.:
                        print "WARNING: log-likelihood decreased in iteration",iteration
                    llhist = llhist[:iteration]
                    break
            self.maximisation(resp)
            self.plot()
        return llhist

    def expectation(self):
        """Compute p(z|x,\theta)"""
        # Answer to Q2
        for i in range(self.nMix):
            self.resp[i] = self.pi[i] * normPDF(self.data, self.mu[i], self.sigma[i])

        self.resp = self.resp / sum(self.resp)
        return self.resp, sum(np.log(self.likelihood(self.data)))

    def maximisation(self,resp):
        # Answer to Q3
        count = np.sum(resp, axis = 1)
        self.pi = count / self.nPoints

        for i in range(self.nMix):
            self.mu[i] = np.dot(self.data.T, resp[i]) / count[i]

        for i in range(self.nMix):
            sigma_new = np.dot((self.data - self.mu[i]).T * resp[i], self.data - self.mu[i]) / count[i]

        # if values are too big then reinitialising
            if invalid(sigma_new):
                print "INVALID"
                mean = np.mean(self.data) + .1*np.random.randn(self.nDim)
                self.sigma[i] = np.eye(self.nDim)
            else:
                self.sigma[i] = sigma_new

    def plot(self):
        fig = plt.gcf()
        display.display(fig)
        display.clear_output(wait=True)
        plt.clf()
        ax = fig.add_subplot(111, aspect='equal')
        plt.scatter(self.data[:,0],self.data[:,1])
        for m in range(self.nMix):
            [val,vect] = np.linalg.eig(self.sigma[m])
            e = Ellipse(xy=self.mu[m], width=2*np.sqrt(val[0]), height=2*np.sqrt(val[1]), angle=180*np.arctan2(vect[0,0],vect[1,0])/np.pi)

            e.set_facecolor([1,0,0])
            ax.add_artist(e)
        plt.show()
        time.sleep(.1)

    def likelihood(self,data):
        """Compute the marginal likelihood of the data given the mixture model, p(x|\theta), for
        all elements in `data`"""
        p = np.zeros(data.shape[0])
        for m in range(self.nMix):
            p += self.pi[m] * normPDF(data,self.mu[m],self.sigma[m])
        return p
```

SHORT DISCUSSION

We noticed that the final position of the ellipses really depends on the initialisation.  Multiple runs may result in different position: rotation approximately 90 degrees. It can be explained by the symmetry of training data points or by the fact that this data can't be approximated well by this number of components in Gaussian mixture.

However, the relative position of three (there actually three of them, one is just covered by another) ellipses stays approximately the same from run to run.

```python
mog = MoG(A,3)
llhist = mog.em()
plt.figure()
plt.plot(llhist)
```

The code above requires a number of methods in order to work,
implementing the intialisation, E-step and M-step of the EM
algorithm. Those methods are:

* `(resp, ll) = expectation()` which implements the E-step
  of the algorithm. It uses the data, and the current parameter values. The function does not change
  the parameters but computes `resp`, a matrix that contains the
  probability of each mixture component given the data
  $p(\z|\x,\params)$, and `ll`, the log-likelihood of the data set
  under the mixture model. This is **Question 2 [10 marks]**
* `maximisation(resp)` wich uses the data and takes as inputs the corresponding responsibilities. The method updates the class by storing the updated parameter values. This is **Question 3 [10 marks]**

Include comments in your answer, explaining
how you compute the responsibilities $p(\z|\x,\params)$ and what the
updates are that maximise the complete expected log-likelihood.

**Question 4 [10 marks]**
Use the same data in `target.npz`, and optimise the model parameters for the classifier with
mixture-of-Gaussians distributions. Use the same train and test sets
as for exercise1 and compare the performance of both
classifiers. Discuss the differences you found.

*Optional Extra:* The contour plots of the distributions do not
reflect the priors ($p(\z|\class_l)$) and therefore do not give you an
exact idea of the probability density under the model. In order to
visualise that, you could plot the probability density under both
models in three dimensions.

### Hints
1. Include a check in your M-step to avoid singularities. You can
  check this by computing `c = np.linalg.cond(SIGMA)`: if c becomes larger
  than a given threshold (say, $10^{10}$), do not perform the
  parameter update and reset the mean and covariance. The provided function,
  `invalid(SIGMA)`, does that check for you.
1. The responsibilities $p(\z|\x,\params)$ are stored in the matrix
  `Q` in the E-step of the algorithm. You know the size of this
  matrix in advance: if you create a matrix of zeros of that size and
  fill in the values, your code will be much more efficient than if
  your grow the matrix as you compute the values.

```python
# [ANSQ4] Do not modify this line
# Answer to Q4

def training_mixture(data, k):
    mog = MoG(data, k)
    llhist = mog.em()
    return mog

k0 = 9
mog_0 = training_mixture(trainData[trainLab == 0], k0)
```

```python
k1 = 9
mog_1 = training_mixture(trainData[trainLab == 1], k1)
```

```python
def posterior(data):
    p_x_0 = mog_0.likelihood(data)
    p_x_1 = mog_1.likelihood(data)
    return p_x_0 * priors[0] / (p_x_0 * priors[0] + p_x_1 * priors[1])

def classify(data):
    posterior_c_0 = posterior(data)
    predictions = [0 if prob > 0.5 else 1 for prob in posterior_c_0]
    return predictions
```

```python
y_pred = classify(testData)
acc = accuracy_score(testLab, y_pred)
conf_matrix = confusion_matrix(testLab, y_pred)
print "error_rate on the test set = ", 1 - acc
print "conf_matrix = \n", conf_matrix
```

    error_rate on the test set =  0.02875
    conf_matrix =
    [[393   8]
     [ 15 384]]

The mixture model allows us to represent the distribution of our data points much more accurate than a single Gaussian distribution per class. However, the training step now takes much longer to compute.

## Discussion of the Implementation

**Question 5 [8 marks]**

Discuss the following with your teammate and include your thoughts in
your report:
1. What is a sensible way to initialise the parameters of your
  model?
1. How many components should you try? Having plotted the data, can
  you guess what number of components should be optimal? How does your
  classification accuracy evolve as the number of components change?
1. Do the results depend on the initialisation?
1. Why is hint 1 important?

[ANSQ5] Do not modify this line

Answer to Q5

1) As the EM algorithm usually takes more iterations to converge than the K-means algorithm and each cycle requires significantly more computation, it is common to find initialization for a Gaussian mixture model throught running K-means algorithm. The covariance matrices can be initialized to the sample covariances of the clusters, the same goes for the means - the means of the clusters. The mixing coefficients can be set to the fractions of data points assigned to respective clusters.

2)
* The optimal choice of k should strike a balance between maximum compression of the data using a single cluster, and maximum accuracy by assigning each data point to its own cluster.
* Yes, the plot may give us some hints on the number components: we can see how many relatively close groups of points there are and therefore, choose that number of groups as number of components. But we need to limit ourselves: as we grow the number of components, we need more data to estimate the parameters well and avoid overfitting.
* When we increase the number of components, the classification accuracy increases as the number of components grows up. But at some point, increasing the number of components doesn't result in any sufficient benefit in accuracy on the test set and we start overfitting on the train set: the accuracy on train set continues growing (we can come to the cricial case of number of components = number of points) but accuracy on test set starts decreasing.

3) Yes, the results depend on the intitialisation: in general, there multiple local maxima of log likelihood function and the EM algorithm is not guaranteed to find the largest of them.

4) It is important because we want our algorithm to converge as fast as possible, and EM algorithm can take multiple iterations to converge, that's why we try to choose sensible parameters not very far from actual values.

## Log-Probabilities

As we have seen in the class, one problem that occurs often when doing probabilistic modelling, is that since probabilities lay in the range $[0,\dots,1]$, the joint probability of multiple variables becomes very small very fast. This is a problem, because computers can only represent numbers with a finite precision and too small a probability will result in an
underflow, hence introducing numerical errors in an otherwise mathematically correct algorithm.

The solution that is typically used is then to use the logarithm of
probabilities, rather than the probabilities themselves. The products
of probabilities then become sums of log-probabilities:

$$
  \ln[p(a)p(b)] = \ln p(a) + \ln p(b)
$$

However a solution must be found when sums are involved. How do we
compute

$$
  \ln[p(a) + p(b)]
$$

when $\ln p(a)$ and $\ln p(b)$ are known? The solution is not to
do the following:

$$
  \ln[p(a) + p(b)] = \ln[ \exp (\ln p(a)) + \exp(\ln p(b)) ].
$$

As an example to illustrate this, try to compute $\ln[p(a)+p(b)]$ when $\ln
p(a) = -1000$ and $\ln p(b) = -1001$. (Really, do try the above
approach before reading on. The correct solution is $-999.69$.) We've seen that we can solve this problem as follows:

$$
  \ln[p(a)+p(b)] = \ln[ \exp( \ln p(a)) + \exp( \ln p(b))] \\
  = \ln\left[ \exp(\ln p(a)) \left( 1 + \frac{\exp( \ln p(b))}{\exp( \ln
      p(a))}\right)\right] \\
  = \ln p(a) + \ln[1+\exp( \ln p(b) - \ln p(a) ) ],
$$

which is now easy to compute, because either the difference between
$\ln p(a)$ and $\ln p(b)$ is small (and then the exponent can be
computed without numerical problems) or it is large (and then the
exponent of the difference goes to zero, so that the $\ln(1+0)
\rightarrow \ln(1) = 0$. You do still need to pay attention that you
take the largest value out of the $\ln$, so that the difference goes
to large negative values rather than large positive values.

The above discussion is pretty important: as we start using larger
data sets, the numerical issues become dominant.
In effect, you
should never use probabilities in your code and always work with
log-probabilities. The products then become sums, and the sums become
the function explained above. This has the added advantage that $\ln$
and $\exp$ are pretty expensive operations. If you use distributions
from the exponential family, such as the Gaussian, computing the
log-probability is computationally slightly cheaper than computing the
probability. A final benefit is that sometimes the code is easier to
debug, because it is clear what errors are due to implementation bugs
and what errors are due to the limited precision of the machine. Have a look at the lecture notes for more information on this topic.

** Question 6 [10 marks] **

You may have noticed such numerical problems in the previous exercise,
especially when using larger numbers of mixture components. In this
exercise, we will change the E-step of the previous exercise and
compute everything with log-probabilities.

1. First implement a function called  `logsumexp` that computes
  the sum of two log-probabilities as described above. Make sure that
  this function takes the largest value out of the logarithm. Write the
  function so that both scalars and matrices can be used as arguments.
1. Discuss how we can use this function to compute sums of more
  than two probabilities using log-probabilities.
1. Check your implementation by computing $\ln[p(a)+p(b)]$ when
  $\ln p(a)=-1000$ and $\ln p(b)=-1001$.
1. Now convert the E-step of the previous exercise. Implement the
  function `logNormPDF` which is functionally equivalent
  to `normPDF` but never computes the logarithm or exponent and is therefore numerically stable:
$$
    \normdist(\x_n;\mean,\cov)
    = \frac{1}{(2\pi)^{d/2}|\cov|^{1/2}} \exp -\frac{1}{2}
    (\x-\mean)^\tp \icov (\x-\mean)\\
    \ln \normdist(\x_n;\mean,\cov)
    = -\frac{1}{2} \left( d \ln 2\pi + \ln |\cov| + (\x-\mean)^\tp
    \icov (\x-\mean) \right)
$$

Notice, however, that only the function `expectation` is changed,
and that it must still return $p(\z|\x,\params)$, not $\ln
p(\z|\x,\params)$. In the adapted function, you compute $\ln
p(\z|\x,\params)$, but you need to exponentiate it before returning
the value. However now the computation does not suffer from the
underflows and is therefore more precise.

```python
# [ANSQ6] Do not modify this line
# Your answer to Question 6

def logsumexp(pa_log, pb_log):
    """Implementation of the logarithm of sum of two log-probabilities.
    Returns the sum of two log-probabilities"""
    if pb_log > pa_log:
        temp = pb_log
        pb_log = pa_log
        pa_log = temp
    return pa_log + np.log(1 + np.exp(pb_log - pa_log))

vect_logsumexp = np.vectorize(logsumexp)
```

If one wants to apply logsumexp function to a non-scalar, then it's possible to use vectorized version of this function which is available under the name "vect_logsumexp".

```python
vect_logsumexp([-1000, -2], [-1001, -3])
```

    array([-999.68673831,   -1.68673831])

2) To compute the sum of more than two probabilites using log-probabilities and the function defined above, we can apply the function logsumexp recursively: first compute the sum of the first two log-probabilities, and after that, add all the rest elements, one by one, to the sum.

Given the list $p = [p_0, p_1, p_2, ..., p_{n-1}]$ of $n$ log-probabiltites:
1. s = p_0
2. for i in range(1, n):
    * s = logsumexp(s, p_i)

3) We check the correctness of implementation of `logsumexp` function:

```python
logsumexp(-1000, -1001)
```

    -999.68673831248179

4) To implement the more efficient and stable version of the E-step of the EM algorithm, we make use of the following formula:

$$ \gamma(z_{nk}) = \frac{\pi_k \normdist(\x_n;\mean,\cov)}{\sum_{j=1}^K \pi_j \normdist(\x_j;\mean,\cov)} $$
$$ \ln \gamma(z_{nk}) = \ln\pi_k + \ln\normdist(\x_n;\mean,\cov) - \ln\sum_{j=1}^K \pi_j \normdist(\x_j;\mean,\cov) $$

First, we implement lognormPDF:

$$
    \ln \normdist(\x_n;\mean,\cov)
    = -\frac{1}{2} \left( d \ln 2\pi + \ln |\cov| + (\x-\mean)^\tp
    \icov (\x-\mean) \right)
$$

After that, we inherit the class MoG_log from class MoG and change the implementation of functions `expectation` and `likelihood` using new function `lognormPDF`.

```python
def lognormPDF(data,mu,sigma):
    """Implementation of the logarithm of Gaussian distribution. Returns the logarithm of density,
    given mean mu and covariance sigma, for every data point (row) of data"""
    N,nDim = data.shape
    res = np.zeros(N)

    icov = np.linalg.inv(sigma)
    covdet = np.linalg.det(sigma)
    denom = np.sqrt((2*np.pi)**nDim * covdet)

    for i in range(N):
        diff = data[i,:]-mu
        res[i] = -.5 * (nDim * np.log(2 * np.pi) + np.log(covdet)+ np.dot(diff, np.dot(icov,diff.T)))

    return res
```

#### Test correctness of the implementation

```python
np.exp(lognormPDF(data, mu[0], sigma[0]))
```

    array([ 0.00028789,  0.00122773,  0.00083388, ...,  0.00380406,
            0.00250394,  0.00331732])

```python
normPDF(data, mu[0], sigma[0])
```

    array([ 0.00028789,  0.00122773,  0.00083388, ...,  0.00380406,
            0.00250394,  0.00331732])

```python
class MoG_log(MoG):
    def expectation(self):
        """Compute p(z|x,\theta)"""
        # Answer to Q6.4
        for i in range(self.nMix):
            self.resp[i] = np.log(self.pi[i]) + lognormPDF(self.data, self.mu[i], self.sigma[i])

        sumexp = self.resp[0]
        for i in range(1, self.nMix):
            sumexp = vect_logsumexp(sumexp, self.resp[i])

        self.resp -= sumexp
        return np.exp(self.resp), sum(self.likelihood(self.data))

    def likelihood(self,data):
        """Compute the marginal likelihood of the data given the mixture model, p(x|\theta), for
        all elements in `data`"""
        p = np.log(self.pi[0]) + lognormPDF(self.data, self.mu[0], self.sigma[0])
        for m in range(1, self.nMix):
            p = vect_logsumexp(p, np.log(self.pi[m]) + lognormPDF(self.data, self.mu[m], self.sigma[m]))
        return p
```

```python
mog = MoG_log(A, 3)
llhist = mog.em()
```