## Gaussian Processes (GP) with GPy

We will use GPy library for GP modeling [SheffieldML github page](https://github.com/SheffieldML/GPy).

Why **GPy**?

* Specialized library of GP models (regression, classification, GPLVM)
* Variety of covariance functions is implemented
* There are GP models for large-scale problems
* Easy to use

Uncomment and run the following line to install GPy library

```python
!pip install GPy
```

```python
from __future__ import print_function

import numpy as np
import matplotlib.pyplot as plt
import GPy

%matplotlib inline
```

Current documentation of GPy library can be found [here](http://gpy.readthedocs.org/en/latest/).

## Gaussian Process Regression

A data set $\left (X, \mathbf{y} \right ) = \left \{ (x_i, y_i), x_i \in \mathbb{R}^d, y_i \in \mathbb{R} \right \}_{i = 1}^N$ is given.

Assumption:
$$
y = f(x) + \varepsilon,
$$
where $f(x)$ is a Gaussian Processes and $\varepsilon \sim \mathcal{N}(0, \sigma_n^2)$ is a Gaussian noise .

Posterior distribution of function value $y^*$ at point $x^*$
$$
y_* | X, \mathbf{y}, x_* \sim \mathcal{N}(m(x_*), \sigma(x_*)),
$$
with predictive mean and variance given by
$$
m(x_*) = \mathbf{k}^T \mathbf{K}_y^{-1} \mathbf{y} = \sum_{i = 1}^N \alpha_i k(x_*, x_i),
$$
$$
\sigma^2(x_*) = k(x_*, x_*) - \mathbf{k}^T\mathbf{K}_y^{-1}\mathbf{k},
$$
where
$$
\mathbf{k} = \left ( k(x_*, x_1), \ldots, k(x_*, x_N) \right )^T
$$
$$
\mathbf{K}_y = \|k(x_i, x_j)\|_{i, j = 1}^N + \sigma_n^2 \mathbf{I}
$$

### Exercises

1. What the posterior variance at the points from the training set is equal to? What if the noise variance is equal to 0?

2. Suppose that we want to minimize some unknown function $f(\mathbf{x})$.
   We are given a set of observations $y_i = f(\mathbf{x}_i) + \varepsilon_i$, where $\varepsilon_i \sim \mathcal{N}(0, \sigma^2)$.
   Using the observations we built a GP model $\hat{f}(\mathbf{x})$.
   Now, let us consider the value called *improvement*:
   $$
   I(\mathbf{x}) = \max(0, y^* - f(\mathbf{x})), \quad \mbox{where } y^* \mbox{ is currently found minimum value of } f(\mathbf{x})
   $$
   To choose the next candidate for the minimum we would like to maximize the *Expected Improvement*
   $$
       EI(x) = \mathbb{E}_f I(\mathbf{x})
   $$

   1. Express the $EI(\mathbf{x})$ in terms $\Phi(\cdot)$ and $\phi(\cdot)$ - the pdf and cdf of the standard normal distribution $\mathcal{N}(0, 1)$.
   2. Assuming $\sigma = 0$ what is the value of $EI(\mathbf{x})$ for any value $y_i$ from the dataset?

### Solution

1. Let's apply SVD decomposition to the matrix $\mathbf{K}$: $\mathbf{K} = U D U^\top$. Then for the $i$-th point from the dataset we have
    $$
    \begin{align}
    \sigma(x_i) &= k(x_i, x_i) - \mathbf{k}^\top (\mathbf{K} + \lambda I)^{-1} \mathbf{k} \\
    &= U_i^\top D U_i - U_i^\top D U^\top (UDU^\top + \lambda I)^{-1} U^\top U D U_i \\
    &= U_i^\top ( D - D (D + \lambda I)^{-1})D) U_i < k(x_i, x_i)
    \end{align}
    $$
    If $\sigma_n = 0$, then $\sigma(x_i) = 0$
2. Let $\mu$ and $\sigma$ are mean and the variance of the GP $f(\mathbf{x})$ at point $\mathbf{x}$. Then $z = \frac{f - \mu}{\sigma} \sim \mathcal{N}(0, 1)$,
   $$
   \begin{align}
       EI(\mathbf{x}) &= \int_{-\infty}^{y^*} (y^* - f)p(f)df = \int_{-\infty}^{(y^* - \mu) / \sigma}(y^* - \sigma z - \mu)\phi(z)dz \\
       &= (y^* - \mu) \Phi\left(\frac{y^* - \mu}{\sigma}\right) - \int_{-\infty}^{(y^* - \mu) \sigma)} \sigma z \phi(z)dz \\
       &= (y^* - \mu) \Phi\left(\frac{y^* - \mu}{\sigma}\right) + \int_{-\infty}^{(y^* - \mu) \sigma)} \sigma d(-\phi(z)) \\
       &= (y^* - \mu) \Phi\left(\frac{y^* - \mu}{\sigma}\right) + \sigma \phi\left(\frac{y^* - \mu}{\sigma}\right)
   \end{align}
   $$
   If $\sigma_n = 0$, then $EI(\mathbf{x}_i) = \max(y^* - \mu(\mathbf{x}_i), 0)$

## Building GPR model

Lets fit GPR model for function $f(x) = − \cos(\pi x) + \sin(4\pi x)$ in $[0, 1]$,
with noise $y(x) = f(x) + \epsilon$, $\epsilon \sim \mathcal{N}(0, 0.1)$.

```python
N = 10
X = np.linspace(0.05, 0.95, N).reshape(-1, 1)
Y = -np.cos(np.pi * X) + np.sin(4 * np.pi * X) + \
    np.random.normal(loc=0.0, scale=0.1, size=(N, 1))
plt.figure(figsize=(5, 3))
plt.plot(X, Y, '.')
```

#### 1. Define covariance function

The most popular kernel - RBF kernel - has 2 parameters: `variance` and `lengthscale`, $k(x, y) = \sigma^2 \exp\left ( -\dfrac{\|x - y\|^2}{2l^2}\right )$,
where `variance` is $\sigma^2$, and `lengthscale` - $l$.

```python
input_dim = 1
variance = 1
lengthscale = 0.2
kernel = GPy.kern.RBF(input_dim, variance=variance,
                      lengthscale=lengthscale)
```

#### 2. Create GPR model

```python
model = GPy.models.GPRegression(X, Y, kernel)
print(model)
model.plot(figsize=(5, 3))
```

### Parameters of the covariance function

Values of parameters of covariance function can be set like:  `k.lengthscale = 0.1`.

Let's change the value of `lengthscale` parameter and see how it changes the covariance function.

```python
k = GPy.kern.RBF(1)
theta = np.asarray([0.2, 0.5, 1, 2, 4, 10])
figure, axes = plt.subplots(2, 3, figsize=(8, 4))
for t, ax in zip(theta, axes.ravel()):
    k.lengthscale = t
    k.plot(ax=ax)
    ax.set_ylim([0, 1])
    ax.set_xlim([-4, 4])
    ax.legend([t])
```

### Task
Try to change parameters to obtain more accurate model (we fix noise variance to some more correct value).

```python
######## Your code here ########
kernel = GPy.kern.RBF(1, lengthscale=0.1)
model = GPy.models.GPRegression(X, Y, kernel)
```

```python
model.Gaussian_noise.variance.fix(0.01)
print(model)
model.plot()
```

### Tuning parameters of the covariance function

The parameters are tuned by maximizing likelihood. To do it just use `optimize()` method of the model.

```python
model = GPy.models.GPRegression(X, Y, kernel)
model.optimize()
print(model)
model.plot(figsize=(5, 3))
```

### Noise variance

Noise variance acts like a regularization in GP models. Larger values of noise variance lead to more smooth model.
Let's check it: try to change noise variance to some large value, to some small value and see the results.

Noise variance accessed like this: `model.Gaussian_noise.variance = 1`

```python
######## Your code here ########
model.Gaussian_noise.variance = 0.01
model.plot(figsize=(5, 3))
```

Now, let's generate more noisy data and try to fit model.

```python
N = 40
X = np.linspace(0.05, 0.95, N).reshape(-1, 1)
Y = -np.cos(np.pi * X) + np.sin(4 * np.pi * X) + \
    np.random.normal(loc=0.0, scale=0.5, size=(N, 1))

kernel = GPy.kern.RBF(1)
model = GPy.models.GPRegression(X, Y, kernel)
model.optimize()
print(model)
model.plot(figsize=(5, 3))
```

Now, let's fix noise variance to some small value and fit the model

```python
kernel = GPy.kern.RBF(1)
model = GPy.models.GPRegression(X, Y, kernel)
model.Gaussian_noise.variance.fix(0.01)
model.optimize()
model.plot(figsize=(5, 3))
```

## Approximate multi-dimensional function

```python
def rosenbrock(x):
    x = 0.5 * (4 * x - 2)
    y = np.sum((1 - x[:, :-1])**2 +
                100 * (x[:, 1:] - x[:, :-1]**2)**2, axis=1)
    return y
```

```python
from mpl_toolkits.mplot3d import Axes3D
from matplotlib import cm
from sklearn.metrics import mean_squared_error

def plot_2d_func(func, n_rows=1, n_cols=1, title=None):
    grid_size = 100
    x_grid = np.meshgrid(np.linspace(0, 1, grid_size), np.linspace(0, 1, grid_size))
    x_grid = np.hstack((x_grid[0].reshape(-1, 1), x_grid[1].reshape(-1, 1)))
    y = func(x_grid)
    fig = plt.figure(figsize=(n_cols * 6, n_rows * 6))
    ax = fig.add_subplot(n_rows, n_cols, 1, projection='3d')
    ax.plot_surface(x_grid[:, 0].reshape(grid_size, grid_size), x_grid[:, 1].reshape(grid_size, grid_size),
                    y.reshape(grid_size, grid_size),
                    cmap=cm.jet, rstride=1, cstride=1)
    if title is not None:
        ax.set_title(title)
    return fig
```

#### Here how the function looks like in 2D

```python
fig = plot_2d_func(rosenbrock)
```

### Training set

```python
dim = 3
x_train = np.random.rand(300, dim)
y_train = rosenbrock(x_train)
```

### Task

Try to approximate Rosenbrock function using RBF kernel.
**Hint**: if results are not good maybe it is due to bad local minimum. You can do one of the following things:
1. Try to use multi-start by calling `model.optimize_restarts(n_restarts)` method of the model.
2. Constrain model parameters to some reasonable bounds. You can do it for example as follows:
`model.Gaussian_noise.variance.constrain_bounded(0, 1)`

```python
######## Your code here ########
kernel = GPy.kern.RBF(dim)
model = GPy.models.GPRegression(x_train, y_train.reshape(-1, 1), kernel)
model.optimize()
```

    <paramz.optimization.optimization.opt_lbfgsb at 0x1a23042630>

```python
x_test = np.random.rand(3000, dim)
y_test = rosenbrock(x_test)
y_pr = model.predict(x_test)[0]

mse = mean_squared_error(y_test.ravel(), y_pr.ravel())
print('\nMSE: {}'.format(mse))
```

    MSE: 4.4115261419461254e-05

### Covariance functions

Short info about covariance function can be printed using `print(k)`.

```python
k = GPy.kern.RBF(1)
print(k)
```

      rbf.         |  value  |  constraints  |  priors
      variance     |    1.0  |      +ve      |
      lengthscale  |    1.0  |      +ve      |

You can plot the covariance function using `plot()` method.

```python
k.plot(figsize=(5, 3))
```

## More "complex" functions
The most popular covariance function is RBF. However, not all the functions can be modelled using RBF covariance function. For example, approximations of discontinuous functions will suffer from oscillations, approximation of curvy function may suffer from oversmoothing.

```python
def heaviside(x):
    return np.asfarray(x > 0)

def rastrigin(x):
    """
    Parameters
    ==========
        x : ndarray - 2D array in [0, 1]

    Returns
    =======
        1D array of values of Rastrigin function
    """
    scale = 8  # 10.24
    x = scale * x - scale / 2
    y = 10 * x.shape[1] + (x**2).sum(axis=1) - 10 * np.cos(2 * np.pi * x).sum(axis=1)
    return y
```

```python
fig = plot_2d_func(rastrigin, 1, 2, title='Rastrigin function')

x = np.linspace(-1, 1, 100)
y = heaviside(x)

ax = fig.add_subplot(1, 2, 2)
ax.plot(x, y)
ax.set_title('Heaviside function')
plt.show()
```

#### Example of oscillations
As you can see there are oscillations in viscinity of discontinuity because we are trying to approximate
discontinuous function using infinitily smooth function.

```python
np.random.seed(42)
X = np.random.rand(30, 1) * 2 - 1
y = heaviside(X)

k = GPy.kern.RBF(input_dim=1, variance=1., lengthscale=1.)

m = GPy.models.GPRegression(X, y, k)
m.optimize()
m.plot(figsize=(5, 3))
plt.ylim([-0.2, 1.2])
```

#### Example of oversmoothing
Actually, the GP model only approximates trend of the function.
All the curves are treated as noise.
The knowledge about this (in fact there is some repeated structure) should be incorporated into the model via kernel function.

```python
np.random.seed(42)

X = np.random.rand(300, 2)
y = rastrigin(X)

k = GPy.kern.RBF(input_dim=2)

m = GPy.models.GPRegression(X, y.reshape(-1, 1), k)
m.optimize()
fig = plot_2d_func(lambda x: m.predict(x)[0])
```

### Covariance functions in GPy

Popular covariance functions: `Exponential`, `Matern32`, `Matern52`, `RatQuad`, `Linear`, `StdPeriodic`.

* Exponential:
$$
k(x, x') = \sigma^2 \exp \left (-\frac{r}{l} \right), \quad r = \|x - x'\|
$$

* Matern32
$$
k(x, x') = \sigma^2 \left (1 + \sqrt{3}\frac{r}{l} \right )\exp \left (-\sqrt{3}\frac{r}{l} \right )
$$

* Matern52
$$
k(x, x') = \sigma^2 \left (1 + \sqrt{5}\frac{r}{l} + \frac{5}{3}\frac{r^2}{l^2} \right ) \exp \left (-\sqrt{5}\frac{r}{l} \right )
$$

* RatQuad
$$
k(x, x') = \left ( 1 + \frac{r^2}{2\alpha l^2}\right )^{-\alpha}
$$

* Linear
$$
k(x, x') = \sum_i \sigma_i^2 x_i x_i'
$$

* Poly
$$
k(x, x') = \sigma^2 (x^T x' + c)^d
$$

* StdPeriodic
$$
k(x, x') = \sigma^2 \exp\left ( -2 \frac{\sin^2(\pi r)}{l^2}\right )
$$

## Combination of covariance functions

* Sum of covariance function is a valid covariance function:

$$
k(x, x') = k_1(x, x') + k_2(x, x')
$$

* Product of covariance functions is a valid covariance funciton:
$$
k(x, x') = k_1(x, x') k_2(x, x')
$$

### Combinations of covariance functions in GPy

In GPy to combine covariance functions you can just use operators `+` and `*`.

Let's plot some of the combinations

```python
covariance_functions = [GPy.kern.Linear(input_dim=1), GPy.kern.StdPeriodic(input_dim=1), GPy.kern.RBF(input_dim=1, lengthscale=1)]
operations = {'+': lambda x, y: x + y, '*': lambda x, y: x * y}

figure, axes = plt.subplots(len(operations), len(covariance_functions), figsize=(9, 6))

import itertools
axes = axes.ravel()
count = 0
for j, base_kernels in enumerate(itertools.combinations(covariance_functions, 2)):
    for k, (op_name, op) in enumerate(operations.items()):
        kernel = op(base_kernels[0], base_kernels[1])
        kernel.plot(ax=axes[count])
        axes[count].set_title('{} {} {}'.format(base_kernels[0].name, op_name, base_kernels[1].name),
                              fontsize=14)
        count += 1
figure.tight_layout()
```

### Additive kernels

One of the popular approach to model the function of interest is
$$
f(x) = \sum_{i=1}^d f_i(x_i) + \sum_{i < j} f_{ij}(x_i, x_j) + \ldots
$$

**Example**: $\quad f(x_1, x_2) = f_1(x_1) + f_2(x_2)$
To model it using GP use additive kernel $\quad k(x, y) = k_1(x_1, y_1) + k_2(x_2, y_2)$.

More general - add kernels each depending on subset of inputs
$$
k(x, y) = k_1(x, y) + \ldots + k_D(x, y),
$$
where, for example, $k_1(x, x') = k_1(x_1, x_1'), \; k_2(x, x') = k_2((x_1, x_3), (x_1', x_3'))$, etc.

Here is an example of ${\rm RBF}(x_1) + {\rm RBF}(x_2)$

```python
k1 = GPy.kern.RBF(1, active_dims=[0])
k2 = GPy.kern.RBF(1, active_dims=[1])

kernel = k1 + k2

x = np.meshgrid(np.linspace(-3, 3, 50), np.linspace(-3, 3, 50))
x = np.hstack((x[0].reshape(-1, 1), x[1].reshape(-1, 1)))
z = kernel.K(x, np.array([[0, 0]]))

from mpl_toolkits.mplot3d import Axes3D
from matplotlib import cm

figure = plt.figure()
ax = figure.add_subplot(111, projection='3d')
ax.plot_surface(x[:, 0].reshape(50, 50), x[:, 1].reshape(50, 50), z.reshape(50, 50), cmap=cm.jet)
plt.show()
```

### Kernels on arbitrary types of objects

Kernels can be defined over all types of data structures: text, images, matrices, graphs, etc. You just need to define similarity between objects.

#### Kernels on categorical data

* Represent your categorical variable as a by a one-of-k encoding: $\quad x = (x_1, \ldots, x_k)$.
* Use RBF kernel with `ARD=True`: $\quad k(x , x') = \sigma^2 \prod_{i = 1}^k\exp{\left ( -\dfrac{(x_i - x_i')^2}{\sigma_i^2} \right )}$. The lengthscale will now encode whether the rest of the function changes.
* Short lengthscales for categorical variables means your model is not sharing any information between data of different categories.

## 2 Sampling from GP

So, you have defined some complex kernel.
You can plot it to see how it looks and guess what kind of functions it can approximate.
Another way to do it is to actually generate random functions using this kernel.

GP defines distribution over functions, which is defined by its *mean function* $m(x)$ and *covariance function* $k(x, y)$: for any set $\mathbf{x}_1, \ldots, \mathbf{x}_N \in \mathbb{R}^d \rightarrow$ $\left (f(\mathbf{x}_1), \ldots, f(\mathbf{x}_N) \right ) \sim \mathcal{N}(\mathbf{m}, \mathbf{K})$,
where $\mathcal{m} = (m(\mathbf{x}_1, \ldots, \mathbf{x}_N)$, $\mathbf{K} = \|k(\mathbf{x}_i, \mathbf{x}_j)\|_{i,j=1}^N$.

Sampling procedure:

1. Generate set of points $\mathbf{x}_1, \ldots, \mathbf{x}_N$.
2. Calculate mean and covariance matrix $\mathcal{m} = (m(\mathbf{x}_1, \ldots, \mathbf{x}_N)$, $\mathbf{K} = \|k(\mathbf{x}_i, \mathbf{x}_j)\|_{i,j=1}^N$.
3. Generate vector from multivariate normal distribution $\mathcal{N}(\mathbf{m}, \mathbf{K})$.

Below try to change RBF kernel to some other kernel and see the results.

```python
k = GPy.kern.Matern52(input_dim=1, lengthscale=0.3)

X = np.linspace(0, 5, 500).reshape(-1, 1)

mu = np.zeros(500)
C = k.K(X, X)

Z = np.random.multivariate_normal(mu, C, 3)

plt.figure()
for i in range(3):
    plt.plot(X, Z[i, :])
```

### Task

Build a GP model that predicts airline passenger counts on international flights.

```python
!wget https://github.com/adasegroup/ML2020_seminars/raw/master/seminar11/data/airline.npz
```

    /bin/sh: wget: command not found

```python
from scipy.io import loadmat

data = np.load('airline.npz')

X = data['X']
y = data['y']

train_indices = list(range(70)) + list(range(90, 129))
test_indices = range(70, 90)
X_train = X[train_indices]
y_train = y[train_indices]

X_test = X[test_indices]
y_test = y[test_indices]

plt.figure(figsize=(5, 3))
plt.plot(X_train, y_train, '.')
plt.show()
```

You need to obtain something like this

```python
def plot_model(X, y, model):
    x = np.linspace(1948, 1964, 400).reshape(-1, 1)
    prediction_mean, prediction_var = model.predict(x)
    prediction_std = np.sqrt(prediction_var).ravel()
    prediction_mean = prediction_mean.ravel()

    plt.figure(figsize=(5, 3))
    plt.plot(X, y, '.', label='Train data')
    plt.plot(x, prediction_mean, label='Prediction')
    plt.fill_between(x.ravel(), prediction_mean - prediction_std, prediction_mean + prediction_std, alpha=0.3)
```

#### Let's try RBF kernel

```python
######## Your code here ########
k_rbf = GPy.kern.RBF(1)
```

As you can see below it doesn't work ;(

```python
model = GPy.models.GPRegression(X, y, k_rbf)
model.optimize()
print(model)
plot_model(X_train, y_train, model)
```

We will try to model this data set using 3 additive components: trend, seasonality and noise.
So, the kernel should be a sum of 3 kernels:
`kernel = kernel_trend + kernel_seasonality + kernel_noise`

#### Let's first try to model trend

Trend is almost linear with some small nonlinearity, so you can use sum of linear kernel with some other which gives this small nonlinearity.

```python
######## Your code here ########
k_trend = GPy.kern.Poly(1, order=1) + GPy.kern.RBF(1)
```

```python
model = GPy.models.GPRegression(X, y, k_trend)
model.optimize()
print(model)
plot_model(X_train, y_train, model)
```

#### Let's model periodicity
Just periodic kernel will not work (why?).
Try to use product of periodic kernel with some other kernel (or maybe 2 other kernels).
Note that the amplitude increases with x.

```python
######## Your code here ########
k_trend = GPy.kern.Poly(1, order=1) + GPy.kern.RBF(1)
k_seasonal = GPy.kern.StdPeriodic(1) * GPy.kern.RBF(1) *  GPy.kern.Linear(1)
```

```python
kernel = k_trend + k_seasonal
model = GPy.models.GPRegression(X, y, kernel)
model.optimize()
print(model)
plot_model(X_train, y_train, model)
```

#### Let's add noise model
The dataset is heteroscedastic, i.e. noise variance depends on x: it increases linearly with x.
Noise can be modeled using `GPy.kern.White(1)`, but it assumes that noise variance is the same at every x.
By what kernel it should be multiplied?

```python
######## Your code here ########
k_trend = GPy.kern.Poly(1, order=1) + GPy.kern.RBF(1)
k_periodicity = GPy.kern.StdPeriodic(1) * GPy.kern.RBF(1) * GPy.kern.Linear(1)
k_noise = GPy.kern.White(1) * GPy.kern.Linear(1)
```

```python
kernel = k_trend + k_periodicity + k_noise
model = GPy.models.GPRegression(X, y, kernel)
model.optimize()
print(model)
plot_model(X_train, y_train, model)
```

# Automatic covariance structure search

```python
plt.figure(figsize=(7, 5));

X = data['X']
y = data['y']

idx_test = np.where((X[:,0] > 1957))[0]
idx_train = np.where((X[:,0] <= 1957))[0]
X_train = X[idx_train]
y_train = y[idx_train]

X_test = X[idx_test]
y_test = y[idx_test]

plt.plot(X_train, y_train, '.', color='red');
plt.plot(X_test, y_test, '.', color='green');
```

```python
def plot_model_learned(X, y, train_idx, test_idx, model):
    prediction_mean, prediction_var = model.predict(X)
    prediction_std = np.sqrt(prediction_var).ravel()
    prediction_mean = prediction_mean.ravel()

    plt.figure(figsize=(7, 5))
    plt.plot(X, y, '.')
    plt.plot(X[train_idx], y[train_idx], '.', color='green')
    plt.plot(X, prediction_mean, color='red')
    plt.fill_between(X.ravel(), prediction_mean - prediction_std, prediction_mean + prediction_std, alpha=0.3)
```

## Expressing Sturcture Through Kernels

For example:

$$
\underbrace{\text{RBF}\times\text{Lin}}_\text{increasing trend} + \underbrace{\text{RBF}\times\text{Per}}_\text{varying-amplitude periodic} + \underbrace{\text{RBF}}_\text{residual}
$$

## Greedy Searching for the Optimum Kernel Combination

One can wonder: how to automatically search the kernel structure? We can optimize some criteria, which balance between a loss function value and the complexity of the model.
Reasinobale candidate for this is BIC-criteria:

$$
BIC = - 2. \text{Log-Liklihood} + m \cdot\log{n}
$$

where $n$ sample size and $m$ number of the parameters.

However, the procedure of fitting Gaussian Process is quite expensive $O(n^3)$. Hence,  instead of the combinatorial search through all possible combinations, we grow the kernel structure greedy.

You can find more details at the https://github.com/jamesrobertlloyd/gp-structure-search. For now, we present toy-example algorithm.

Consider the set of operations:

$$
\text{Algebra: } +,\times
$$

and the set of basic kernels:

$$
\text{Kernels: } \text{Poly}, \text{RBF}, \text{Periodic}
$$

For each level we select extenstion of our current kernel with the lowest BIC. This is an example of the possible kernel grow process (mark notes the lowest BIC at the level):

```python
def train_model_get_bic(X_train, y_train, kernel, num_restarts=3):
    '''
    Input:
        X_train: numpy array of train features, n*d (d>=1)
        y_train: numpy array n*1
        kernel: GPy object kern
        num_restars: number of the restarts of the optimization routine
    Output:
        bic value
    '''
    kernel = kernel.copy()
    model = GPy.models.GPRegression(X_train, y_train, kernel, None)
    model.optimize_restarts(num_restarts, verbose=False)

    log_ll = model.log_likelihood()
    N = X_train.shape[0]
    m = model.param_array.shape[0]
    bic = -2.*log_ll + m * np.log(N)

    return bic
```

```python
def _make_all_products(op_list, kernel_list):
    '''
    Find product pairs
    '''
    product_index = np.where(np.array(op_list) == '*')[0]
    if len(product_index) == 0:
        return kernel_list

    product_index = product_index[0]
    product_kernel = kernel_list[product_index] * kernel_list[product_index + 1]

    if len(op_list) == product_index + 1:
        kernel_list_copy = kernel_list[:product_index] + [product_kernel]
        op_list_copy = op_list[:product_index]
    else:
        kernel_list_copy = kernel_list[:product_index] + [product_kernel] + kernel_list[product_index + 2:]
        op_list_copy = op_list[:product_index] + op_list[product_index + 1:]

    return _make_all_products(op_list_copy, kernel_list_copy)

class GreedyKernel:
    '''
    Class for greedy growing kernel structure
    '''
    def __init__(self, algebra, base_kernels):
        self.algebra = algebra
        self.base_kernels = base_kernels
        self.kernel = None
        self.kernel_list = []
        self.op_list = []
        self.str_kernel = None

    def _make_kernel(self, op_list, kernel_list):
        '''
        Sumation in kernel experssion
        '''
        kernels_to_sum = _make_all_products(op_list, kernel_list)
        new_kernel = kernels_to_sum[0]
        for k in kernels_to_sum[1:]:
            new_kernel = new_kernel + k
        return new_kernel

    def init_kernel(self, X_train, y_train):
        '''
        Initialization of first kernel
        '''
        bic = np.zeros(len(self.base_kernels))
        for k, kernel in enumerate(self.base_kernels):
            bic[k] = train_model_get_bic(X_train, y_train, kernel)
        self.kernel = self.base_kernels[np.argmin(bic)]
        self.kernel_list.append(self.kernel.copy())
        self.str_kernel = str(self.kernel.name)

    def grow_level(self, X_train, y_train):
        '''
        Select optimal extension of current kernel
        '''
        best_bic = np.inf
        best_op, best_kernel = None, None

        for k, kernel in enumerate(self.base_kernels):
            for a, (op_name, op) in enumerate(self.algebra.items()):
                new_kernel = self._make_kernel(self.op_list + [op_name], self.kernel_list + [kernel])
                bic = train_model_get_bic(X_train, y_train, new_kernel)
                if bic < best_bic:
                    best_op = op_name
                    best_kernel = kernel
                    best_bic = bic

        self.kernel_list.append(best_kernel.copy())
        self.op_list.append(best_op)

        new_kernel = self._make_kernel(self.op_list, self.kernel_list)
        str_new_kernel = '{} {} {}'.format(self.str_kernel, best_op, best_kernel.name)

        return new_kernel, str_new_kernel

    def grow_tree(self, X_train, y_train, max_depth):
        '''
        Greedy kernel growing
        '''
        if self.kernel == None:
            self.init_kernel(X_train, y_train)

        for i in range(max_depth):
            self.kernel, self.str_kernel = self.grow_level(X_train, y_train)
            print(self.str_kernel)

    def fit_model(self, X_train, y_train, kernel, num_restarts=1):
        model = GPy.models.GPRegression(X_train, y_train, kernel)
        model.optimize_restarts(num_restarts, verbose=False)
        return model
```

```python
# operations under kernels:

algebra = {'+': lambda x, y: x + y,
           '*': lambda x, y: x * y
          }

# basic kernels list:

poly_kern = GPy.kern.Poly(input_dim=1, order=1)

periodic_kern = GPy.kern.StdPeriodic(input_dim=1)
periodic_kern.period.constrain_bounded(1e-2, 1e1)
periodic_kern.lengthscale.constrain_bounded(1e-2, 1e1)

rbf_kern = GPy.kern.RBF(input_dim=1)
rbf_kern.lengthscale.constrain_bounded(1e-2, 1e1)

bias_kern = GPy.kern.Bias(1)

kernels_list = [poly_kern, periodic_kern, rbf_kern]
```

    reconstraining parameters std_periodic.period
    reconstraining parameters std_periodic.lengthscale
    reconstraining parameters rbf.lengthscale

The algorithm is not perfect, but still it returns something reasonable.
In order to improve it you can think of more advanced greed strategy.

```python
GK = GreedyKernel(algebra, kernels_list)
GK.grow_tree(X_train, y_train, 4)
model = GK.fit_model(X_train, y_train, GK.kernel)
plot_model_learned(X, y, idx_train, idx_test, model)
```

# Appendix: Gaussian Process Classification

### Classification

A data set $\left (X, \mathbf{y} \right ) = \left \{ (x_i, y_i), x_i \in \mathbb{R}^d, y_i \in \{+1, -1\} \right \}_{i = 1}^N$ is given.

Assumption:
$$
p(y = +1 \; | \; x) = \sigma(f(x)) = \pi(x),
$$
where latent function $f(x)$ is a Gaussian Processes.

We need to produce a probabilistic prediction
$$
\pi_* = p(y_* \; | \; X, \mathbf{y}, x_*) = \int \sigma(f_*) p(f_* \; | \; X, \mathbf{y}, x_*) df_*,
$$
$$
p(f_* \; | \; X, \mathbf{y}, x_*) = \int p(f_* \; | \; X, x_*, \mathbf{f}) p(\mathbf{f} \; | \; X, \mathbf{y}) d\mathbf{f},
$$
where $p(\mathbf{f} \; |\; X, \mathbf{y}) = \dfrac{p(\mathbf{y} | X, \mathbf{f}) p(\mathbf{f} | X)}{p(\mathbf{y} | X)}$ is the posterior over the latent variables.

Both integrals are intractable.

Use approximation technique like Laplace approximation or Expectation Propagation.

```python
from matplotlib import cm

def cylinder(x):
    y = (1 / 7.0 - (x[:, 0] - 0.5)**2 - (x[:, 1] - 0.5)**2) > 0
    return y

np.random.seed(42)
X = np.random.rand(40, 2)
y = cylinder(X)

x_grid = np.meshgrid(np.linspace(0, 1, 100), np.linspace(0, 1, 100))
y_grid = cylinder(np.hstack((x_grid[0].reshape(-1, 1), x_grid[1].reshape(-1, 1)))).reshape(x_grid[0].shape)

positive_idx = y == 1
plt.figure(figsize=(5, 3))
plt.plot(X[positive_idx, 0], X[positive_idx, 1], '.', markersize=10, label='Positive')
plt.plot(X[~positive_idx, 0], X[~positive_idx, 1], '.', markersize=10, label='Negative')
im = plt.contour(x_grid[0], x_grid[1], y_grid, 10, cmap=cm.hot)
plt.colorbar(im)
plt.legend()
plt.show()
```

```python
kernel = GPy.kern.RBF(2, variance=1., lengthscale=0.2, ARD=True)

model = GPy.models.GPClassification(X, y.reshape(-1, 1), kernel=kernel)
model.optimize()
print(model)

def plot_model_2d(model):

    model.plot(levels=40, resolution=80, plot_data=False, figsize=(5, 3))
    plt.plot(X[positive_idx, 0], X[positive_idx, 1], '.', markersize=10, label='Positive')
    plt.plot(X[~positive_idx, 0], X[~positive_idx, 1], '.', markersize=10, label='Negative')
    plt.legend()
    plt.show()

plot_model_2d(model)
```

Let's change lengthscale to some small value

```python
model.rbf.lengthscale = [0.05, 0.05]
plot_model_2d(model)
```

## Bonus Task
Try to approximate rastrigin function

```python
fig = plot_2d_func(rastrigin)
```

### Training set

```python
np.random.rand(42)
x_train = np.random.rand(200, 2)
y_train = rastrigin(x_train)
```

#### Hint: you can constrain parameters of the covariance functions, for example
`model.std_periodic.period.constrain_bounded(0, 0.2)`.

```python
######## Your code here ########
kernel = GPy.kern.RBF(2) * GPy.kern.StdPeriodic(2)
model = GPy.models.GPRegression(x_train, y_train.reshape(-1, 1), kernel)
# model.Gaussian_noise.variance.constrain_bounded(0, 0.01)
model.mul.std_periodic.period.constrain_bounded(0, 0.2)
model.optimize_restarts(5)
```

```python
print(model)

x_test = np.random.rand(1000, 2)
y_test = rastrigin(x_test)
y_pr = model.predict(x_test)[0]

mse = mean_squared_error(y_test.ravel(), y_pr.ravel())
print('MSE: {}'.format(mse))

fig = plot_2d_func(lambda x: model.predict(x)[0])
```


## References:

1. Higher-order multivariate Markov chains and their applications; Wai-Ki Ching , Michael K.Ng , Eric S.Fungb, 2008

=>Implementationa and usage example shown here

2. A multivariate Markov chain model for categorical data sequences and its applications in demand predictions
   Wai‐Ki Ching  Eric S. Fung  Michael K. Ng

3. Higher-Order Markov Chain Models for Categorical Data Sequences* Wai Ki Ching, Eric S. Fung, Michael K. Ng [Implemented here as WeightedMarkov]

0. <a src="https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=1&cad=rja&uact=8&ved=0ahUKEwjM66WC3JbXAhVil1QKHTj1CakQFggtMAA&url=http%3A%2F%2Fwww.ccsenet.org%2Fjournal%2Findex.php%2Fmas%2Farticle%2FviewFile%2F6040%2F4874_1_1_1&usg=AOvVaw0fZh5XmtYpF14lVZRfr6aZ">  Application of Markov Chains to Analyze and Predict the Time Series </a>

```python
import numpy as np;
import pandas as pd;
import importlib
import numpy as np
from collections import defaultdict
import sys

#Call delModules before reimporting to fresh imported classes
def delModules(mod):
    mods = []
    for c,md in enumerate(sys.modules):
        if (md == mod or md.startswith(mod+".") ):
            #print("Delete: " , md)
            mods += [md]
    for m in sorted(mods):
        del sys.modules[m]

delModules("WeightedMarkov")

from WeightedMarkov.NaiveMarkov import *
from WeightedMarkov.WeightedHOMVMarkov import *

```

```python
s1 = [2,1,3,3,4,3,2,1,3,3,2,1]
s1 = [1,0,2,2,3,2,1,0,2,2,1,0]
s2 = [2,4,4,4,4,2,3,3,1,4,3,3]
s2 = [1,3,3,3,3,1,2,2,0,3,2,2]
X=[s1,s2]
order = 2
n=2       # => order
s=len(X)  # => Number of series
hm = WeightedHOMVMarkov(X,4, order)
f, p, x = hm.fit();
#dumps=hm.Dump()

c,A,b = hm.PrepareMatrices()
hm.DisplayCAb(0)
# => One is good - uncomment to see the other CAB too
#hm.DisplayCAb(1)
#print ("c=", c, "\nA=\n", A, "\nb=", b)
sol = hm.Solve(False)
hm.DumpSolution()
for i,s in enumerate(hm.sol):
    print("X_({}) => ".format(i), end='')
    for f in np.array([_ for _ in sol[i]['x'].T][:-1]):
        print("{0:.4f}".format(f), end=' ' )
    print()

```

$$c =\begin{bmatrix}
0 & 0 & 0 & 0 & 1
\end{bmatrix}
A =\begin{bmatrix}
1 & 1 & 1 & 1 & 0\\\\-1 & -1 & -1 & -1 & 0\\\\-1 & 0 & 0 & 0 & 0\\\\0 & -1 & 0 & 0 & 0\\\\0 & 0 & -1 & 0 & 0\\\\0 & 0 & 0 & -1 & 0\\\\-1/4 & -1/6 & -11/36 & -1/6 & -1\\\\-1/2 & -7/12 & -4/9 & -7/12 & -1\\\\1/4 & 1/6 & 11/36 & 1/6 & -1\\\\1/2 & 7/12 & 4/9 & 7/12 & -1\\\\0 & 0 & 0 & 0 & -1
\end{bmatrix}
b =\begin{bmatrix}
1\\\\-1\\\\0\\\\0\\\\0\\\\0\\\\-1/4\\\\-5/12\\\\1/4\\\\5/12\\\\0
\end{bmatrix}
description =\begin{bmatrix}
\lambda_0+\lambda_1+\lambda_2+\lambda_3 & <= & +1\\\\-\lambda_0-\lambda_1-\lambda_2-\lambda_3 & <= & -1\\\\\lambda_0 & >= & 0\\\\\lambda_1 & >= & 0\\\\\lambda_2 & >= & 0\\\\\lambda_3 & >= & 0\\\\->\\\\->\\\\->\\\\->\\\\w & >= & 0
\end{bmatrix}
$$

$$
        x^{(j)}_{r+1} = \sum_{k=1}^{s}\sum_{h=1}^{n} \lambda^{(h)}_{jk} P^{(jk)}_h x^{(k)}_{r-h+1},
        j = 1,2,...s, n = order
        $$

$$
        \begin{equation}
        \begin{aligned}
        x^{(1)}_{r+1} &= 0.0948 P_1^{(1, 1)} X_{r}^{(1)} +0.031 P_2^{(1, 1)} X_{r-1}^{(1)} +0.8431 P_1^{(1, 2)} X_{r}^{(2)} +0.031 P_2^{(1, 2)} X_{r-1}^{(2)} \\x^{(2)}_{r+1} &= 0.2 P_1^{(2, 1)} X_{r}^{(1)} +0.2 P_2^{(2, 1)} X_{r-1}^{(1)} +0.6 P_1^{(2, 2)} X_{r}^{(2)} \\
        \end{aligned}
        \end{equation}
        $$

    X_(0) => 0.0948 0.0310 0.8431 0.0310
    X_(1) => 0.2000 0.2000 0.6000 0.0000

```python
hm.SelfEval();

```

    Printing the predictions once:
    #: 0, P. Matrix: [ 0.0155  0.1686  0.6472  0.1686], Pred: 2, Actual:2
    #: 1, P. Matrix: [ 0.2   0.12  0.12  0.56], Pred: 3, Actual:3
    =======================Metrics :   Series 1
    orig=>[2, 2, 3, 2, 1, 0, 2, 2, 1, 0]
    pred=>[2 2 2 2 1 0 0 2 2 0]
    Total 10, correct 7, acc: 0.70
    class:0 total:2, correct:2, accuracy:1.0
    class:1 total:2, correct:1, accuracy:0.5
    class:2 total:5, correct:4, accuracy:0.8
    class:3 total:1, correct:0, accuracy:0.0
    =======================Metrics :   Series 2
    orig=>[3, 3, 3, 1, 2, 2, 0, 3, 2, 2]
    pred=>[3 3 3 3 2 2 2 3 3 2]
    Total 10, correct 7, acc: 0.70
    class:0 total:1, correct:0, accuracy:0.0
    class:1 total:1, correct:0, accuracy:0.0
    class:2 total:4, correct:3, accuracy:0.75
    class:3 total:4, correct:4, accuracy:1.0

## Example  from Reference [1]

```python
ProductA=" 6 6 6 6 2 6 2 6 2 2 6 2 6 6 2 6 2 4 4 4 5 6 6 1 2 2 6 6 6 2 6 2 6 6 2 6 2 2 6 2 1 2 2 6 6 6 2 1262662262226262222262266661226222262222332326666262662 6266266223433131216166166262622266162612162622226616622 62223444646166166661622266662662262622262226666322622222 26262226226626662223334166166161666616662122222236666626"
ProductB=" 1 6 6 1 6 1 1 1 1 1 1 6 6 6 1 2 1 6 6 1 1 1 6 6 2 1 6 6 1 1 1 6 1 2 1 6 2 2 2 2 2 6 1 6 6 1 2 1666111661111611216161162626663661662223226661162662626 6136611122322622216161162111221611112611116161216166161 22223322266662116111616161611662116611262666126161111616 11661661616611662222222226666166616616611613335166666666"
ProductC=" 6 6 6 6 6 6 6 2 6 6 6 6 6 6 6 2 6 6 6 6 2 6 6 6 2 2 6 6 6 6 6 6 6 1 6 2 6 6 6 6 6 6 6 6 2 6 6 1261661626666666266626616666666336321221661616666661666 1666666666662666666662266261266626626626162621266226262 26266622266266226121266226612216262211563616612261626616 26266616166222123616161616661166666166616116666666616616"
ProductD=" 6 2 2 2 2 3 3 4 4 4 5 4 3 3 6 2 6 6 6 3 4 4 3 3 3 3 3 2 6 6 3 4 4 4 4 3 4 2 6 2 2 6 2 2 6 6 3 4544636662626622644543434462662262662662662626355544436 2662626226266264444446366262626266222222222333554533362 66226222262322363223444455446626262222222554455262662626 22334454443436262222222222234444544432226222626262222232"
ProductE=" 6 2 2 2 2 3 3 4 4 4 5 4 3 3 6 2 6 6 2 3 4 4 3 4 4 3 3 2 2 6 3 4 4 4 4 3 4 2 3 2 2 6 3 3 6 6 3 4545332662626622644444454462662262662662662626344444446 2662626666262264444446336222626222222222222364555524662 66226222262322363223444455433626222632222554444362662626 22334454444436262226222222234444544432226662626262222222"

pA=[int(c) - 1 for c in ProductA if c != ' ']
pB=[int(c) - 1 for c in ProductB if c != ' ']
pC=[int(c) - 1 for c in ProductC if c != ' ']
pD=[int(c) - 1 for c in ProductD if c != ' ']
pE=[int(c) - 1 for c in ProductE if c != ' ']
X=[pA,pB,pC,pD,pE]

order = 8
n=6       # => number of States
s=len(X)  # => Number of series

hm = WeightedHOMVMarkov(X, n, order)
f, p, x = hm.fit();
#h.Dump()
c,A,b = hm.PrepareMatrices()
#hm.DisplayCAb(0)
#hm.DisplayCAb(1)
#print ("c=", c, "\nA=\n", A, "\nb=", b)
sol = hm.Solve(False)
hm.DumpSolution();

#for i,s in enumerate(hm.sol):
#    print("X_({}) => ".format(i), end='')
#    for f in np.array([_ for _ in sol[i]['x'].T][:-1]):
#        print("{0:.4f}".format(f), end=' ' )
#    print()

```

$$
        x^{(j)}_{r+1} = \sum_{k=1}^{s}\sum_{h=1}^{n} \lambda^{(h)}_{jk} P^{(jk)}_h x^{(k)}_{r-h+1},
        j = 1,2,...s, n = order
        $$

$$
        \begin{equation}
        \begin{aligned}
        x^{(1)}_{r+1} &=  P_1^{(1, 2)} X_{r}^{(2)} \\x^{(2)}_{r+1} &= 0.6364 P_1^{(2, 2)} X_{r}^{(2)} +0.3636 P_3^{(2, 2)} X_{r-2}^{(2)} \\x^{(3)}_{r+1} &=  P_1^{(3, 5)} X_{r}^{(5)} \\x^{(4)}_{r+1} &= 0.2994 P_8^{(4, 2)} X_{r-7}^{(2)} +0.4324 P_1^{(4, 5)} X_{r}^{(5)} +0.2681 P_2^{(4, 5)} X_{r-1}^{(5)} \\x^{(5)}_{r+1} &= 0.2718 P_8^{(5, 2)} X_{r-7}^{(2)} +0.6738 P_1^{(5, 4)} X_{r}^{(4)} +0.0544 P_2^{(5, 5)} X_{r-1}^{(5)} \\
        \end{aligned}
        \end{equation}
        $$

```python
ac=hm.SelfEval();
```

    Printing the predictions once:
    #: 0, P. Matrix: [ 0.0606  0.4444  0.0101  0.0101  0.0101  0.4646], Pred: 5, Actual:1
    #: 1, P. Matrix: [ 0.404   0.1368  0.0239  0.      0.      0.4353], Pred: 5, Actual:0
    #: 2, P. Matrix: [ 0.1639  0.2295  0.      0.      0.      0.6066], Pred: 5, Actual:5
    #: 3, P. Matrix: [ 0.      0.2145  0.1397  0.3594  0.1184  0.1679], Pred: 3, Actual:3
    #: 4, P. Matrix: [ 0.      0.1324  0.1348  0.5013  0.1321  0.0995], Pred: 3, Actual:3
    =======================Metrics :   Series 1
    orig=>[1, 1, 5, 1, 5, 5, 1, 5, 1, 3, 3, 3, 4, 5, 5, 0, 1, 1, 5, 5, 5, 1, 5, 1, 5, 5, 1, 5, 1, 1, 5, 1, 0, 1, 1, 5, 5, 5, 1, 0, 1, 5, 1, 5, 5, 1, 1, 5, 1, 1, 1, 5, 1, 5, 1, 1, 1, 1, 1, 5, 1, 1, 5, 5, 5, 5, 0, 1, 1, 5, 1, 1, 1, 1, 5, 1, 1, 1, 1, 2]
    pred=>[5 5 5 5 5 5 5 5 1 5 5 5 5 5 5 5 5 1 5 5 5 5 5 5 5 5 1 5 5 1 1 1 1 1 5 5 5 5 5 1 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 1 5 5 5 5 5 5 5 1 5 1 5 5 5 1 5 5 5 5 5 1 1 1 1]
    Total 261, correct 129, acc: 49.43
    class:0 total:22, correct:0, accuracy:0.0
    class:1 total:107, correct:30, accuracy:0.2803738317757009
    class:2 total:13, correct:0, accuracy:0.0
    class:3 total:9, correct:0, accuracy:0.0
    class:4 total:1, correct:0, accuracy:0.0
    class:5 total:109, correct:99, accuracy:0.908256880733945
    =======================Metrics :   Series 2
    orig=>[0, 0, 0, 5, 5, 5, 0, 1, 0, 5, 5, 0, 0, 0, 5, 5, 1, 0, 5, 5, 0, 0, 0, 5, 0, 1, 0, 5, 1, 1, 1, 1, 1, 5, 0, 5, 5, 0, 1, 0, 5, 5, 5, 0, 0, 0, 5, 5, 0, 0, 0, 0, 5, 0, 0, 1, 0, 5, 0, 5, 0, 0, 5, 1, 5, 1, 5, 5, 5, 2, 5, 5, 0, 5, 5, 1, 1, 1, 2, 1]
    pred=>[5 5 5 5 0 0 0 5 1 5 5 0 5 5 5 0 0 1 5 5 0 5 5 5 0 5 1 5 5 1 1 1 1 1 5 5 0 0 5 1 5 5 0 0 5 5 5 0 0 5 5 5 5 0 5 5 1 5 5 5 0 5 5 0 1 0 1 0 5 0 5 0 0 5 0 0 1 1 1 1]
    Total 261, correct 125, acc: 47.89
    class:0 total:94, correct:39, accuracy:0.4148936170212766
    class:1 total:53, correct:28, accuracy:0.5283018867924528
    class:2 total:9, correct:0, accuracy:0.0
    class:4 total:1, correct:0, accuracy:0.0
    class:5 total:104, correct:58, accuracy:0.5576923076923077
    =======================Metrics :   Series 3
    orig=>[5, 5, 5, 5, 5, 5, 5, 1, 5, 5, 5, 5, 1, 5, 5, 5, 1, 1, 5, 5, 5, 5, 5, 5, 5, 0, 5, 1, 5, 5, 5, 5, 5, 5, 5, 5, 1, 5, 5, 0, 1, 5, 0, 5, 5, 0, 5, 1, 5, 5, 5, 5, 5, 5, 5, 1, 5, 5, 5, 1, 5, 5, 0, 5, 5, 5, 5, 5, 5, 5, 2, 2, 5, 2, 1, 0, 1, 1, 0, 5]
    pred=>[5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5]
    Total 261, correct 162, acc: 62.07
    class:0 total:39, correct:0, accuracy:0.0
    class:1 total:54, correct:0, accuracy:0.0
    class:2 total:5, correct:0, accuracy:0.0
    class:4 total:1, correct:0, accuracy:0.0
    class:5 total:162, correct:162, accuracy:1.0
    =======================Metrics :   Series 4
    orig=>[3, 3, 4, 3, 2, 2, 5, 1, 5, 5, 5, 2, 3, 3, 2, 2, 2, 2, 2, 1, 5, 5, 2, 3, 3, 3, 3, 2, 3, 1, 5, 1, 1, 5, 1, 1, 5, 5, 2, 3, 4, 3, 3, 5, 2, 5, 5, 5, 1, 5, 1, 5, 5, 1, 1, 5, 3, 3, 4, 3, 2, 3, 2, 3, 3, 5, 1, 5, 5, 1, 1, 5, 1, 5, 5, 1, 5, 5, 1, 5]
    pred=>[3 3 3 3 3 3 1 1 1 1 1 1 1 3 3 3 3 3 3 1 1 1 1 1 3 3 3 3 3 3 1 1 1 1 1 1 5 1 1 5 3 3 3 3 3 1 1 1 1 1 1 1 1 1 1 1 1 3 3 3 3 3 3 3 3 3 1 1 1 1 1 1 1 5 1 1 1 1 1 1]
    Total 261, correct 142, acc: 54.41
    class:1 total:92, correct:89, accuracy:0.967391304347826
    class:2 total:34, correct:1, accuracy:0.029411764705882353
    class:3 total:50, correct:42, accuracy:0.84
    class:4 total:17, correct:0, accuracy:0.0
    class:5 total:68, correct:10, accuracy:0.14705882352941177
    =======================Metrics :   Series 5
    orig=>[3, 3, 4, 3, 2, 2, 5, 1, 5, 5, 1, 2, 3, 3, 2, 3, 3, 2, 2, 1, 1, 5, 2, 3, 3, 3, 3, 2, 3, 1, 2, 1, 1, 5, 2, 2, 5, 5, 2, 3, 4, 3, 4, 2, 2, 1, 5, 5, 1, 5, 1, 5, 5, 1, 1, 5, 3, 3, 3, 3, 3, 3, 4, 3, 3, 5, 1, 5, 5, 1, 1, 5, 1, 5, 5, 1, 5, 5, 1, 5]
    pred=>[3 3 3 3 3 3 3 1 1 1 1 1 3 3 3 3 3 3 3 3 1 1 1 3 3 3 3 3 3 3 1 1 1 1 1 1 1 1 1 3 3 3 3 3 1 3 1 1 1 1 1 1 1 1 1 1 1 3 3 3 3 3 3 3 3 3 1 1 1 1 1 1 1 1 1 1 1 1 1 1]
    Total 261, correct 142, acc: 54.41
    class:1 total:92, correct:84, accuracy:0.9130434782608695
    class:2 total:31, correct:0, accuracy:0.0
    class:3 total:60, correct:58, accuracy:0.9666666666666667
    class:4 total:14, correct:0, accuracy:0.0
    class:5 total:64, correct:0, accuracy:0.0

```python
# Compute the stationary distribution of one the matrices to show usage
#k=np.matrix([[0.5,.5,0], [0.25,0.5,0.25],[0,0.5,0.5]]).T
k=hm.pS[(1,1,1)]
d,v=hm.StationaryDist(k);
```

    Eigen Values/Vectors of
    [[ 0.      0.0459  0.1538  0.1111  0.      0.1228]
     [ 0.3182  0.4679  0.2308  0.      0.      0.4211]
     [ 0.0455  0.055   0.3077  0.1111  0.      0.0088]
     [ 0.      0.0092  0.2308  0.4444  0.      0.0088]
     [ 0.      0.      0.      0.1111  0.      0.    ]
     [ 0.6364  0.422   0.0769  0.2222  1.      0.4386]]
    [[ 1.  1.  1.  1.  1.  1.]]
    =EV:[ 1.      0.4845  0.2819 -0.1542 -0.0016  0.0481]
    [[ 0.1375 -0.066  -0.0709  0.5888 -0.6736  0.4502]
     [ 0.6811  0.7686  0.7679  0.2464  0.6598 -0.7877]
     [ 0.0812 -0.1382  0.2506 -0.0822 -0.0109  0.0943]
     [ 0.0562 -0.586  -0.3763  0.0391 -0.0036 -0.0454]
     [ 0.0062 -0.1344 -0.1483 -0.0282  0.249  -0.1049]
     [ 0.7123  0.1559 -0.423  -0.7639 -0.2208  0.3934]]

    index=0 Stat Disy:
    [[ 0.1375  0.6811  0.0812  0.0562  0.0062  0.7123]]
    StatPI:
    [[ 0.0821  0.4067  0.0485  0.0336  0.0037  0.4254]]
    [[ 0.0821]
     [ 0.4067]
     [ 0.0485]
     [ 0.0336]
     [ 0.0037]
     [ 0.4254]])

```python

```

```python

```


## EEML2019: ConvNets and Computer Vision Tutorial (PART II)

### Knowledge distillation: Distilling a pre-trained teacher model into a smaller student model

* Define student model (custom Resnet-21)
* Load pre-trained teacher model (Resnet-50)
* Add KL distillation loss between teacher and student
* Observe the impact of temperature in softmax for teacher predictions
* Train student with the joint loss

###  The total loss for the student is:
\begin{equation}
\mathcal{L} = \mathcal{L}_{\text{classif}} + \lambda \mathcal{L}_{\text{distill}}
\end{equation}

For classification loss we use the regular cross-entropy and for the distillation loss, we use Kullback-Leibler (KL) divergence. $\lambda$ is a normalisation factor explained below.

**Reminder**:

Given two distributions $t$ and $s$, we define their cross-entropy over a given set as:

$$H(t,s) = H(t) + \text{KL}(t,s),$$

where $H(t)$ is the entropy of $t$, i.e. $H(t) = \sum_{i=1}^{N}t(x_i) \cdot \log t(x_i)$

and $\text{KL}(t,s)$ is the KL divergence between $t$ and $s$, i.e. $\text{KL}(t,s) = \sum_{i=1}^{N}t(x_i) \cdot \log \frac{t(x_i)}{s(x_i)} . $

However, in most cases of interest to us, $t$ is a constant (either ground truth labels or teacher predictions also considered as constant), so the entropy term can be ignored since its gradient is 0.

Hence we can use cross-entropy $H(t,s)$ for both losses:
- the mismatch between ground truth and student predictions.
- the mismatch between teacher and student distributions.

In the context of distillation, it is useful to also remember that the outputs of the network are logits, which we interpret as probabilities when passed through softmax:

$$p_i^{(T)} =\frac{\exp{(\text{logits}_i / T) }}{\sum_j \exp{(\text{logits}_j / T) }}. $$

$T$ is the softmax temperature usually set to 1. Setting it to a higher value smooths the output probability distribution, an effect desired in distillation. More precisely, we will use

\begin{equation}
\mathcal{L}_{\text{distill}} = H(\text{p}_{\text{teacher}}^{(T)}, \text{p}_{\text{student}}^{(T)}),
\end{equation}

**The normalisation factor**

$\lambda$ is a normalisation factor that ensures the gradients of the two loss terms are comparable in scale. Note that the gradients of the distill loss term scale as $\frac{1}{T^2}$ due to the logits being divided by $T$.  Hence we use $$\lambda = T^2$$ to bring distillation term gradients to the same scale as the classification term gradients.

```
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function

import math
import time

import tensorflow as tf

# Don't forget to select GPU runtime environment in Runtime -> Change runtime type
device_name = tf.test.gpu_device_name()
if device_name != '/device:GPU:0':
  raise SystemError('GPU device not found')
print('Found GPU at: {}'.format(device_name))

import numpy as np

# Plotting library.
from matplotlib import pyplot as plt
import pylab as pl
from IPython import display

import collections
import enum
```

    Found GPU at: /device:GPU:0

```
# Reset graph
tf.reset_default_graph()
```

### Copy the pretrained weights of teacher model on the virtual machine
- we won't do this today as the checkpoint has about 250M

```
# from google.colab import files

# uploaded = files.upload()

# for fn in uploaded.keys():
#   print('User uploaded file "{name}" with length {length} bytes'.format(
#       name=fn, length=len(uploaded[fn])))
```

## Download dataset to be used for training and testing
* Cifar-10 equivalent of MNIST for natural RGB images

* 60000 32x32 colour images in 10 classes: airplane, automobile, bird, cat, deer, dog, frog, horse, ship, truck

* train: 50000; test: 10000

```
cifar10 = tf.keras.datasets.cifar10
# (down)load dataset
(train_images, train_labels), (test_images, test_labels) = cifar10.load_data()

# Check sizes of tensors
print ('Size of training images')
print (train_images.shape)
print ('Size of training labels')
print (train_labels.shape)
print ('Size of test images')
print (test_images.shape)
print ('Size of test labels')
print (test_labels.shape)

assert train_images.shape[0] == train_labels.shape[0]
```

## Display the images
The gallery function below shows sample images from the data, together with their labels.

```
MAX_IMAGES = 10
def gallery(images, label, title='Input images'):
  class_dict = [u'airplane', u'automobile', u'bird', u'cat', u'deer', u'dog', u'frog', u'horse', u'ship', u'truck']
  num_frames, h, w, num_channels = images.shape
  num_frames = min(num_frames, MAX_IMAGES)
  ff, axes = plt.subplots(1, num_frames,
                          figsize=(num_frames, 1),
                          subplot_kw={'xticks': [], 'yticks': []})
  for i in range(0, num_frames):
    if num_channels == 3:
      axes[i].imshow(np.squeeze(images[i]))
    else:
      axes[i].imshow(np.squeeze(images[i]), cmap='gray')
    axes[i].set_title(class_dict[label[i][0]])
    plt.setp(axes[i].get_xticklabels(), visible=False)
    plt.setp(axes[i].get_yticklabels(), visible=False)
  ff.subplots_adjust(wspace=0.1)
  plt.show()
```

```
gallery(train_images, train_labels)
```

## Prepare the data for training and testing
* for training, we use stochastic optimizers (e.g. SGD, Adam), so we need to sample at random mini-batches from the training dataset
* for testing, we iterate sequentially through the test set

```
# define dimension of the batches to sample from the datasets
BATCH_SIZE_TRAIN = 100 #@param
BATCH_SIZE_TEST = 100 #@param

# create Dataset objects using the data previously downloaded
dataset_train = tf.data.Dataset.from_tensor_slices((train_images, train_labels))
# we shuffle the data and sample repeatedly batches for training
batched_dataset_train = dataset_train.shuffle(100000).repeat().batch(BATCH_SIZE_TRAIN)
# create iterator to retrieve batches
iterator_train = batched_dataset_train.make_one_shot_iterator()
# get a training batch of images and labels
(batch_train_images, batch_train_labels) = iterator_train.get_next()

# check that the shape of the training batches is the expected one
print ('Shape of training images')
print (batch_train_images)
print ('Shape of training labels')
print (batch_train_labels)
```

```
# we do the same for test dataset
dataset_test = tf.data.Dataset.from_tensor_slices((test_images, test_labels))
batched_dataset_test = dataset_test.repeat().batch(BATCH_SIZE_TEST)
iterator_test = batched_dataset_test.make_one_shot_iterator()
(batch_test_images, batch_test_labels) = iterator_test.get_next()
print ('Shape of test images')
print (batch_test_images)
print ('Shape of test labels')
print (batch_test_labels)
```

```
# Squeeze labels and convert from uint8 to int32 - required below by the loss op
batch_test_labels = tf.cast(tf.squeeze(batch_test_labels), tf.int32)
batch_train_labels = tf.cast(tf.squeeze(batch_train_labels), tf.int32)
```

## Preprocess input for training and testing

```
# Data augmentation
# - scale image to [-1 , 1]
# - during training: apply horizontal flip randomly
# - random crop after padding

def train_image_preprocess(h, w, num_transf=None):
  def fn(image):
    image = tf.image.convert_image_dtype(image, dtype=tf.float32)
    image = image * 2 - 1
    # image = tf.reshape(image, (32, 32, 3))
    image = tf.image.random_flip_left_right(image)
    # Data augmentation: pad images and randomly sample a (h, w) patch.
    image = tf.pad(image, [[0, 0], [4, 4], [4, 4], [0, 0]], mode='CONSTANT')
    image = tf.random_crop(image, size=(BATCH_SIZE_TRAIN, h, w, 3))
    return image
  return fn

def test_image_preprocess():
  def fn(image):
    image = tf.image.convert_image_dtype(image, dtype=tf.float32)
    image = image * 2 - 1
    # image = tf.reshape(image, (32, 32, 3))
    return image
  return fn

```

## Define the models

```
# define parameters of resnet blocks for two resnet models
ResNetBlockParams = collections.namedtuple(
    "ResNetBlockParams", ["output_channels", "bottleneck_channels", "stride"])

# teacher
BLOCKS_50 = (
    (ResNetBlockParams(256, 64, 1),) * 2 + (ResNetBlockParams(256, 64, 2),),
    (ResNetBlockParams(512, 128, 1),) * 3 + (ResNetBlockParams(512, 128, 2),),
    (ResNetBlockParams(1024, 256, 1),) * 5 + (ResNetBlockParams(1024, 256, 2),),
    (ResNetBlockParams(2048, 512, 1),) * 3)

# student
BLOCKS_21 = (
    (ResNetBlockParams(256, 64, 1),) + (ResNetBlockParams(256, 64, 2),),
    (ResNetBlockParams(512, 128, 1),) + (ResNetBlockParams(512, 128, 2),),
    (ResNetBlockParams(1024, 256, 1),) + (ResNetBlockParams(1024, 256, 2),),
    (ResNetBlockParams(2048, 512, 1),))
```

```
#@title Utils
# initializer
he_initializer = tf.contrib.layers.variance_scaling_initializer()
# helper functions
def _fixed_padding(inputs, kernel_size):
  """Pads the input along the spatial dimensions."""
  pad_total = kernel_size - 1
  pad_begin = pad_total // 2
  pad_end = pad_total - pad_begin
  padded_inputs = tf.pad(inputs, [[0, 0], [pad_begin, pad_end],
                                  [pad_begin, pad_end], [0, 0]])
  return padded_inputs

def _max_pool2d_same(inputs, kernel_size, stride, padding):
  """Strided 2-D max-pooling with fixed padding.
  When padding='SAME' and stride > 1, we do fixed zero-padding followed by
  max_pool2d with 'VALID' padding."""

  if padding == "SAME" and stride > 1:
    padding = "VALID"
    inputs = _fixed_padding(inputs, kernel_size)
  return tf.layers.MaxPooling2D(kernel_size, strides=stride, padding=padding)(inputs)

def _conv2d_same(inputs, num_outputs, kernel_size, stride, use_bias=False,
                 name="conv_2d_same"):
  """Strided 2-D convolution with 'SAME' padding. If stride > 1, we do fixed
  zero-padding, followed by conv2d with 'VALID' padding."""
  if stride == 1:
    padding = "SAME"
  else:
    padding = "VALID"
    inputs = _fixed_padding(inputs, kernel_size)

  return tf.layers.Conv2D(num_outputs, kernel_size, strides=stride,
                          padding=padding, use_bias=use_bias, name=name,
                          kernel_initializer=he_initializer)(inputs)
```

```
# define resnet block v2
def resnet_block(inputs, output_channels, bottleneck_channels, stride,
                 training=None, name="resnet_block"):
  """Create a resnet block."""
  num_input_channels = inputs.get_shape()[-1]
  batch_norm_args = {
      "training": training
      }
  # ResNet V2 uses pre-activation, where the batch norm and relu are before
  # convolutions, rather than after as in ResNet V1.
  preact = tf.layers.BatchNormalization(name=name+"/bn_preact")(inputs,
                                                                **batch_norm_args)
  preact = tf.nn.relu(preact)

  if output_channels == num_input_channels:
    # Use subsampling to match output size.
    # Note we always use `inputs` in this case, not `preact`.
    if stride == 1:
      shortcut = inputs
    else:
      shortcut = _max_pool2d_same(inputs, 1, stride=stride, padding="SAME")
  else:
    # Use 1x1 convolution shortcut to increase channels to `output_channels`.
    shortcut = tf.layers.Conv2D(output_channels, 1, stride,
                                use_bias=False,
                                name=name+"/conv_shortcut")(preact)
  ###########################
  # YOUR CODE HERE copy the code you implemented in Part 1

  output = shortcut + residual

  return output
```

```
# stack resnet blocks
def _build_resnet_blocks(inputs, blocks, batch_norm_args):
  """Connects the resnet block into the graph."""
  outputs = []

  for num, subblocks in enumerate(blocks):
    with tf.variable_scope("block_{}".format(num)):
      for i, block in enumerate(subblocks):
        args = {
            "name": "resnet_block_{}".format(i)
        }
        args.update(block._asdict())
        args.update(batch_norm_args)
        inputs = resnet_block(inputs, **args)
        outputs += [inputs]

  return outputs
```

```
# define full architecture: input convs, resnet blocks, output classifier
def resnet_v2(inputs, blocks, is_training=True,
              num_classes=1000,
              use_global_pool=True, name="resnet_v2"):
  """ResNet V2."""
  blocks = tuple(blocks)

  batch_norm_args = {
      "training": is_training
  }

  outputs = []
  with tf.variable_scope(name, reuse=tf.AUTO_REUSE):
    # Add initial non-resnet conv layer and max_pool
    inputs = _conv2d_same(inputs, 64, 7, stride=2, name="root")
    inputs = _max_pool2d_same(inputs, 3, stride=2, padding="SAME")
    outputs += [inputs]

    # Stack resnet blocks
    resnet_outputs = _build_resnet_blocks(inputs, blocks, batch_norm_args)

    outputs += resnet_outputs

    # Take the activations of the last resnet block.
    inputs = resnet_outputs[-1]

    inputs = tf.layers.BatchNormalization(name="bn_postnorm")(inputs,
                                                              **batch_norm_args)
    inputs = tf.nn.relu(inputs)
    outputs += [inputs]

    if use_global_pool:
      inputs = tf.reduce_mean(inputs, [1, 2], name="use_global_pool",
                              keepdims=True)
      outputs += [inputs]

    # Add output classifier
    inputs = tf.layers.Conv2D(num_classes, 1, name="logits")(inputs)
    inputs = tf.squeeze(inputs, axis=[1, 2])
    outputs += [inputs]

  return outputs[-1]
```

## Set up training pipeline

```
# First define the preprocessing ops for the train/test data
crop_height = 32 #@param
crop_width = 32 #@param
preprocess_fn_train = train_image_preprocess(crop_height, crop_width)
preprocess_fn_test = test_image_preprocess()
NUM_CLASSES = 10 #@param
```

### Instantiate teacher

```
teacher_blocks = BLOCKS_50
# teacher runs in inference mode
with tf.variable_scope("teacher"):
  teacher_predictions = resnet_v2(preprocess_fn_train(batch_train_images),
                                  teacher_blocks,
                                  num_classes=NUM_CLASSES, is_training=False)

```

### We do not want to alter the teacher weights, so apply `tf.stop_gradients` to `teacher_predictions`

```
################
# YOUR CODE HERE teacher_predictions = tf.stop_gradient...
```

### Load pre-trained weights
- we won't do this today since the checkpoint was not uploaded

```
# # Create saver to restore the pre-trained model
# saver = tf.train.Saver(var_map, reshape=True)
```

### Instantiate student and get predictions

```
student_blocks = BLOCKS_21
with tf.variable_scope("student"):
  student_train_predictions = resnet_v2(preprocess_fn_train(batch_train_images),
                                        student_blocks,
                                        num_classes=NUM_CLASSES,
                                        is_training=True)
  print (student_train_predictions)

  student_test_predictions = resnet_v2(preprocess_fn_test(batch_test_images),
                                       student_blocks,
                                       num_classes=NUM_CLASSES,
                                       is_training=False)
  print (student_test_predictions)
```

```
# Get number of parameters in a scope by iterating through the trainable variables
def get_num_params(scope):
  total_parameters = 0
  for variable in tf.trainable_variables(scope):
    # shape is an array of tf.Dimension
    shape = variable.get_shape()
    variable_parameters = 1
    for dim in shape:
      variable_parameters *= dim.value
    total_parameters += variable_parameters
  return total_parameters
```

```
# Get number of parameters in the models.
print ("Total number of parameters of teacher model")
print (get_num_params("teacher"))
print ("Total number of parameters of student model")
print (get_num_params("student"))
```

    Total number of parameters of teacher model
    23520842
    Total number of parameters of student model
    9496394

### Set up the training for student, adding the distillation loss weighted by the square of temperature as explained above.

Normally we use T = 1, but for distillation we use T>1, e.g. T=5. We will visualise later the impact of T on logits.

```
T_distill = 5.0
T_normal = 1.0
```

#### First define the regular cross-entropy classification loss

```
def classification_loss(logits=None, labels=None):
  # We reduce over batch dimension, to ensure the loss is a scalar.
  return tf.reduce_mean(
      tf.nn.sparse_softmax_cross_entropy_with_logits(
          labels=labels, logits=logits))
```

#### Define the distillation loss

You can do this either with

* `tf.distributions.kl_divergence` between the teacher and student distributions, respectively; or
* `softmax_cross_entropy_with_logits`. Remember that in this case the labels are expected to sum to 1, while the output of the teacher network is logits. So we need to apply `softmax` on the `teacher_predictions`.

```
# Using tf.distributions.kl_divergence
# pp = tf.distributions.Categorical(logits=teacher_predictions)
# qq = tf.distributions.Categorical(logits=student_train_predictions)

# distill_kl_loss = tf.reduce_mean(tf.distributions.kl_divergence(pp, qq))

```

```
# OR simpler, using cross entropy
################
# YOUR CODE HERE distill_kl_loss = tf.reduce_mean(tf.nn.softmax_cross_entropy_with_logits_v2(...
```

#### Define the joint training loss

```
################
# YOUR CODE HERE lambda_ = ...
# YOUR CODE HERE train_loss = classification_loss...
# YOUR CODE HERE add the weighted distillation term
```

```
# For evaluation, we look at top_k_accuracy since it's easier to interpret; normally k=1 or k=5
def top_k_accuracy(k, labels, logits):
  in_top_k = tf.nn.in_top_k(predictions=tf.squeeze(logits), targets=labels, k=k)
  return tf.reduce_mean(tf.cast(in_top_k, tf.float32))
```

```
#@title Set up the training; better to start with lower lr and longer training schedule
def get_optimizer(step):
  """Get the optimizer used for training."""
  lr_schedule = (80e3, 100e3, 110e3)
  lr_schedule = tf.to_int64(lr_schedule)
  lr_factor = 0.1

  lr_init = 0.01
  num_epochs = tf.reduce_sum(tf.to_float(step >= lr_schedule))
  lr = lr_init * lr_factor**num_epochs

  return tf.train.MomentumOptimizer(learning_rate=lr, momentum=0.9)

# Create a global step that is incremented during training; useful for e.g. learning rate annealing
global_step = tf.train.get_or_create_global_step()

# instantiate the optimizer
optimizer = get_optimizer(global_step)

# Get training ops, including BatchNorm update ops
training_op = optimizer.minimize(train_loss, global_step)
update_ops = tf.group(*tf.get_collection(tf.GraphKeys.UPDATE_OPS))
training_op = tf.group(training_op, update_ops)

# Display loss function
def plot_losses(loss_list, steps):
  display.clear_output(wait=True)
  display.display(pl.gcf())
  pl.plot(steps, loss_list, c='b')
  time.sleep(1.0)
```

### Teacher and student accuracy

```
test_acc = top_k_accuracy(1, batch_test_labels, student_test_predictions)

# We compute the accuracy of the teacher on the train set to make sure that
# the loading of the pre-trained weights was successful; this should be above 90%;
# today it is close to random since the teacher doesn't use pretrained weights
acc_teacher = top_k_accuracy(1, batch_train_labels, teacher_predictions)
```

### Define ops to visualise the impact of softmax temperature on output distributions

```
probs_high_temp = tf.nn.softmax(tf.div(teacher_predictions, T_distill))
probs_low_temp = tf.nn.softmax(tf.div(teacher_predictions, T_normal))
```

### Define training parameters

```
# Define number of training iterations and reporting intervals
TRAIN_ITERS = 90e3 #@param
REPORT_TRAIN_EVERY = 100 #@param
PLOT_EVERY = 500 #@param
REPORT_TEST_EVERY = 1000 #@param
TEST_ITERS = 100 #@param
```

### Create the session and initialise variables

```
sess = tf.Session()
sess.run(tf.global_variables_initializer())
```

### Load pre-trained weights for teacher, and check accuracy to make sure the import was successful

```
# saver.restore(sess, "resnet50.ckpt")

num_batches = 100  # 100 batches * 64 samples per batch = 6400 out of 50000
avg_accuracy = 0.0
for _ in range(num_batches):
  accuracy = sess.run(acc_teacher)
  avg_accuracy += accuracy
avg_accuracy /= num_batches

# expected_accuracy > 90% if we had loaded a pretrained checkpoint
print ("Teacher accuracy on a subset of the train set {:.3f}%".format(avg_accuracy))
```

### Visualize the impact of temperature on the logits

```
probs_ht, probs_lt, gt = sess.run([probs_high_temp, probs_low_temp, tf.one_hot(batch_train_labels, NUM_CLASSES)])
# pick one sample and plot
idx = 10
plt.plot(probs_ht[idx], c='r', label='High Temp')
plt.plot(probs_lt[idx], c='g', label='Low Temp')
plt.plot(gt[idx], 'b--', label='GT')
plt.xlim([0,9])
plt.legend()
plt.show()
```

### Train the model.

If running out of memory, reduce the BATCH_SIZE_TRAIN, e.g. 32 or 16.

Note that the execution is slower and more memory is needed now, since for each training iteration of the student we need to run the forward pass for the teacher as well.

### Training the model

```
# Get test ops
test_acc_op = top_k_accuracy(1, batch_test_labels, student_test_predictions)
train_acc_op = top_k_accuracy(1, batch_train_labels, student_train_predictions)
```

```
train_iter = 0
losses = []
steps = []
for train_iter in range(int(TRAIN_ITERS)):
  _, train_loss_np = sess.run([training_op, train_loss])

  if (train_iter % REPORT_TRAIN_EVERY) == 0:
    losses.append(train_loss_np)
    steps.append(train_iter)
  if (train_iter % PLOT_EVERY) == 0:
    pass
    # plot_losses(losses, steps)

  if (train_iter % REPORT_TEST_EVERY) == 0:
    avg_acc = 0.0
    train_avg_acc = 0.0
    for test_iter in range(TEST_ITERS):
      acc, acc_train = sess.run([test_acc_op, train_acc_op])
      avg_acc += acc
      train_avg_acc += acc_train

    avg_acc /= (TEST_ITERS)
    train_avg_acc /= (TEST_ITERS)
    print ('Test acc at iter {0:5d} out of {1:5d} is {2:.2f}%'.format(int(train_iter), int(TRAIN_ITERS), avg_acc*100.0))
    print ('Train acc at iter {0:5d} out of {1:5d} is {2:.2f}%'.format(int(train_iter), int(TRAIN_ITERS), train_avg_acc*100.0))
```


# TPK4170 Robotics - Trajectory Generation
## 2016-09-29
### Lars Tingelstad

# Introduction

## Definitions of Path and Trajectory

### [Corke - Robotics, Vision and Control:](http://link.springer.com/book/10.1007%2F978-3-642-20144-8)

A **path is a spatial construct** – a locus in space that leads from an initial pose to a final pose. A **trajectory is a path with specified timing**. For example there is a path from A to B, but there is a trajectory from A to B in $10\ \rm s$ or at $2\ \rm m/ \rm s$.

### [Siciliano et. al. - Robotics: ](http://link.springer.com/book/10.1007/978-1-84628-642-1)

A **path denotes the locus of points** in the **joint space**, or in the **operational space**, which the manipulator has to follow in the execution of the assigned motion; **a path is then a pure geometric description of motion**. On the other hand, **a trajectory is a path on which a timing law is specified**, for instance in terms of velocities and/or accelerations at each point.

#### Locus

In geometry, a locus (plural: loci) (Latin word for "place", "location") is a set of points (commonly, a line, a line segment, a curve or a surface), whose location satisfies or is determined by one or more specified conditions.

#### Joint Space

The joint space (configuration space) denotes the space in which the $(n \times 1)$ vector
of joint variables
$$\boldsymbol{\mathrm{q}} = \begin{pmatrix} q_1\\ \vdots \\ q_n \end{pmatrix}$$ is defined; it is $q_i = \theta_i$ for a revolute joint and $q_i = d_i$ for a prismatic joint.

#### Operational Space

Operational space is the space in which the manipulator task is specified, typically $\mathbb{R}^3$. A position and orientation can then be described by, e.g., a $(6 \times 1)$ vector $\boldsymbol{\mathrm{x}}_e$ of positions $\boldsymbol{\mathrm{p}}_e$ and Euler angles $\boldsymbol{\mathrm{\phi}}_e$ where
$$\boldsymbol{\mathrm{x}}_e = \begin{pmatrix} \boldsymbol{\mathrm{p}}_e \\\boldsymbol{\mathrm{\phi}}_e \end{pmatrix}.$$

## Examples
### Standard Trajectory Generation

```octave
%%html

```

### Online Trajectory Generation

#### Tracking a Swinging Target with a Robot Manipulator using Visual Sensing

**Description from YouTube:**

```octave
%%html

```

#### Collision Avoidance Using the [Reflexxes Motion Libraries](http://www.reflexxes.ws/)

**Interactive notebook:**

[](http://mybinder.org:/repo/tingelst/rmltypeii)

```octave
%%html

```

#### The Duel: Timo Boll vs. KUKA Robot

```octave
%%html

```

# Joint Space Trajectories

A manipulator motion is typically assigned in the operational space in terms of trajectory parameters such as
- the initial and final end-effector pose
- possible intermediate poses
- travelling time along particular geometric paths.

If it is desired to plan a trajectory in the joint space, the values of the joint variables have to be determined first from the end-effector position and orientation specified by the user. It is then necessary to resort to an inverse kinematics algorithm

The planning algorithm generates a function $\boldsymbol{\mathrm{q}}(t)$ interpolating the given vectors of joint variables at each point, in respect of the imposed constraints.

In general, a joint space trajectory planning algorithm is required to have
the following features:
 - the generated trajectories should not very demanding from a computational viewpoint,
 - the joint positions and velocities should be continuous functions of time (continuity of accelerations may be imposed, too),
 - undesirable effects should be minimized, e.g., nonsmooth trajectories interpolating a sequence of points on a path

## Point-to-Point Motion

In *point-to-point motion*, the manipulator has to move from an initial joint configuration $\boldsymbol{\mathrm{q}}_i$ to a final joint configuration $\boldsymbol{\mathrm{q}}_f$ in a given time $t_f$, that is,
$$\boldsymbol{\mathrm{q}}(0) = \boldsymbol{\mathrm{q}}_i$$
and
$$\boldsymbol{\mathrm{q}}(t_f) = \boldsymbol{\mathrm{q}}_f.$$

In this case, the actual end-effector path is of no concern.

In the following we consider point-to-point motion of a single joint with position $q(t)$, velocity $\dot q(t)$ and acceleration $\ddot q(t)$.

### Cubic Polynomials

A **joint-space trajectory** can be generated using a **cubic (third-order) polynomial**.

The position $q(t)$ is given by the polynomial
$$ q(t) = a_3 t^3 + a_2 t^2 + a_1 t + a_0$$
which results in a parabolic velocity profile
$$\dot{q}(t) = 3 a_3 t^2 + 2 a_2 t + a_1$$
and a linear acceleration profile
$$\ddot{q}(t) = 6 a_3 t + 2 a_2.$$

The polynomial $q(t)$ has four coeffiecients, $a_3, a_2, a_1, a_0$. It is then possible to impose constraints on the initial position $q(0) = q_i$, the final position $q(t_f) = q_f$, the initial velocity  $\dot q(0) = \dot q_i$ and the final velocity $\dot q(tf) = \dot q_f$. The initial and final joint velocity values $q_i$ and $q_f$ are usually set to zero.

```octave
qi = 0 % Initital position
qf = pi % Final position
dqi = 0 % Initial velocity
dqf = 0 % Final velocity
t = 2 % Duration of the motion
```

    qi = 0
    qf =  3.1416
    dqi = 0
    dqf = 0
    t =  2

Determination of a specific trajectory is given by the solution to the following system of equations:
\begin{align}
a_0 &= q_i \\
a_3 t_f^3 + a_2 t_f^2 + a_1 t_f + a_0 &= q_f \\
a_1 &= \dot{q}_i \\
3 a_3 t_f^2 + 2 a_2 t_f + a_1 &= \dot{q}_f,
\end{align}
which can be written in matrix form as:
\begin{align}
\begin{pmatrix} q_i \\ q_f \\ \dot q_i \\ \dot q_f \end{pmatrix} =
\begin{pmatrix}
  0 & 0 & 0 & 1 \\
  t^3 & t^2 & t & 1 \\
  0 & 0 & 1 & 0 \\
  3t^2 & 2t & 1 & 0
  \end{pmatrix}
  \begin{pmatrix} a_3 \\ a_2 \\ a_1 \\ a_0 \end{pmatrix}.
\end{align}
Since the matrix is square we can solve for the coefficient vector $(a_3, a_2, a_1, a_0)^\top$ using standard linear algebra methods.

We form the matrix
$$ \boldsymbol{\mathrm{A}} =
\begin{pmatrix}
  0 & 0 & 0 & 1 \\
  t^3 & t^2 & t & 1 \\
  0 & 0 & 1 & 0 \\
  3t^2 & 2t & 1 & 0
  \end{pmatrix}.
$$

```octave
A = [0, 0, 0, 1;
     t^3, t^2, t, 1;
     0, 0, 1, 0;
     3 * t^2, 2 * t, 1.0, 0.0]
```

    A =

        0    0    0    1
        8    4    2    1
        0    0    1    0
       12    4    1    0

We then form the vector $$\boldsymbol{\mathrm{b}} = \begin{pmatrix} q_i \\ q_f \\ \dot q_i \\ \dot q_f \end{pmatrix}.$$

```octave
b = [qi, qf, dqi, dqf]'
```

    b =

       0.00000
       3.14159
       0.00000
       0.00000

We then solve for the polynomial coeffiecient vector
$$\boldsymbol{\mathrm{p}} = \begin{pmatrix} a_3 \\ a_2 \\ a_1 \\ a_0 \end{pmatrix}.$$

```octave
p = A \ b
```

    p =

      -0.78540
       2.35619
       0.00000
       0.00000

#### Position
The positions $q(t)$ can then be found by evaluating the polynomial
$$ q(t) = a_3 t^3 + a_2 t^2 + a_1 t + a_0.$$

```octave
ts = [0:0.01:t];
qs = polyval(p, ts);
```

The positions $q(t)$ can be plotted:

```octave
%plot inline -f 'svg'
plot(ts, qs, 'linewidth',2)
```

The velocities $\dot q(t)$ can be found by evaluating
$$\dot{q}(t) = 3 a_3 t^2 + 2 a_2 t + a_1.$$

```octave
dqs = polyval([3 * p(1), 2 * p(2), p(3)],ts);
```

The velocities $\dot q(t)$ can be plotted:

```octave
plot(ts, dqs, 'linewidth',2, 'color',[0,0.5,0])
```

The accelerations $\ddot q(t)$ can found by evaluating
$$\ddot{q}(t) = 6 a_3 t + 2 a_2.$$

```octave
ddqs = polyval([6 * p(1), 2 * p(2)],ts);
```

The accelerations $\ddot q(t)$ can be plotted:

```octave
plot(ts, ddqs, 'linewidth', 2, 'color', 'r')
title ("Cubic Trajectory");
xlabel ("time");
ylabel ("acceleration");
```

We can also create a function that returns the trajectory

```octave
function [ts, qs, dqs, ddqs] = cubic_trajectory(qi, qf, dqi, dqf, t)
    b = [qi, qf, dqi, dqf]';
    A = [0, 0, 0, 1;
         t^3, t^2, t, 1;
         0, 0, 1, 0;
         3 * t^2, 2 * t, 1.0, 0.0];
    p = A \ b;
    ts = [0:0.01:t];
    qs = polyval(p, ts);
    dqs = polyval([3 * p(1), 2 * p(2), p(3)],ts);
    ddqs = polyval([6 * p(1), 2 * p(2)],ts);
end
```

```octave
[ts, qs, dqs, ddqs] = cubic_trajectory(0, pi, 0, 0, 10);
plot(ts,qs, 'linewidth', 2, ts,dqs, 'linewidth', 2, ts, ddqs, 'linewidth', 2)
```

### Quintic Polynomials
If it is desired to assign also the initial and final values of acceleration, six constraints have to be satisfied and then a polynomial of at least fifth-order is needed. The motion timing law for the generic joint is then given by
\begin{align}
q(t) &= a_5 t^5 + a_4 t^4 + a_3 t^3 + a_2 t^2 + a_1 t + a_0 \\
\dot{q}(t) &= 5 a_5 t^4 + 4 a_4 t^3 + 3  a_3 t^2 + 2 a_2 t + a_1 \\
\ddot{q}(t) &= 20 a_5 t^3 + 12 a_4 t^2 + 6 a_3 t + 2 a_2.
\end{align}

This can be written in matrix form as
\begin{align}
\begin{pmatrix} q_i \\ q_f \\ \dot q_i \\ \dot q_f \\ \ddot q_i \\ \ddot q_f \end{pmatrix} =
\begin{pmatrix}
  0 & 0 & 0 & 0 & 0 & 1 \\
  t^5 & t^4 & t^3 & t^2 & t & 1 \\
  0 & 0 & 0 & 0 & 1 & 0 \\
  5t^4 & 4t^3 & 3t^2 & 2t & 1 & 0 \\
  0 & 0 & 0 & 2 & 0 & 0 \\
  20t^3 & 12t^2 & 6t & 2 & 0 & 0
  \end{pmatrix}
  \begin{pmatrix}a_5 \\ a_4 \\ a_3 \\ a_2 \\ a_1 \\ a_0 \end{pmatrix}.
\end{align}

```octave
qi = 0;
qf = 1;
dqi = 0.0;
dqf = 0;
ddqi = 0;
ddqf = 0;
t = 50;
ts = [0:0.01:t];
```

We form the matrix
$$ \boldsymbol{\mathrm A} =
\begin{pmatrix}
  0 & 0 & 0 & 0 & 0 & 1 \\
  t^5 & t^4 & t^3 & t^2 & t & 1 \\
  0 & 0 & 0 & 0 & 1 & 0 \\
  5t^4 & 4t^3 & 3t^2 & 2t & 1 & 0 \\
  0 & 0 & 0 & 2 & 0 & 0 \\
  20t^3 & 12t^2 & 6t & 2 & 0 & 0
  \end{pmatrix} $$

```octave
A = [0, 0, 0, 0, 0, 1;
     t^5, t^4, t^3, t^2, t, 1;
     0, 0, 0, 0, 1, 0;
     5*t^4, 4*t^3, 3*t^2, 2*t, 1, 0;
     0, 0, 0, 2, 0, 0;
     20*t^3, 12*t^2, 6*t, 2, 0, 0]
```

    A =

               0           0           0           0           0           1
       312500000     6250000      125000        2500          50           1
               0           0           0           0           1           0
        31250000      500000        7500         100           1           0
               0           0           0           2           0           0
         2500000       30000         300           2           0           0

and the the vector $$\boldsymbol{\mathrm{b}} = \begin{pmatrix} q_i \\ q_f \\ \dot q_i \\ \dot q_f \\ \ddot q_i \\ \ddot q_f \end{pmatrix}.$$

```octave
b = [qi, qf, dqi, dqf, ddqi, ddqf]'
```

    b =

       0
       1
       0
       0
       0
       0

We then solve for the polynomial coeffiecient vector
$$\boldsymbol{\mathrm{p}} = \begin{pmatrix} a_5 \\ a_4 \\ a_3 \\ a_2 \\ a_1 \\ a_0 \end{pmatrix}.$$

```octave
p = A \ b
```

    p =

       1.9200e-08
      -2.4000e-06
       8.0000e-05
       0.0000e+00
       0.0000e+00
       0.0000e+00

#### Position

```octave
plot(ts, polyval(p, ts), 'linewidth',2);
title ("Quintic Trajectory");
xlabel ("Time");
ylabel ("position");
```

#### Velocity

```octave
plot(ts, polyval([5 * p(1), 4 * p(2), 3 * p(3), 2 * p(4), p(5)], ts),
     'linewidth', 2, 'color', [0, 0.5, 0])
title ("Quintic Trajectory");
xlabel ("Time");
ylabel ("velocity");
```

#### Acceleration

```octave
plot(ts, polyval([20 * p(1), 12 * p(2), 6 * p(3), 2 * p(4)], ts),
'linewidth',2, 'color', [1,0,0])
title ("Quintic Trajectory");
xlabel ("Time");
ylabel ("Acceleration");
```

We can also create a function that returns the trajectory

```octave
function [ts, qs, dqs, ddqs] = quintic_trajectory(qi, qf, dqi, dqf, ddqi, ddqf, t)
    b = [qi, qf, dqi, dqf, ddqi, ddqf]';
    A = [0, 0, 0, 0, 0, 1;
     t^5, t^4, t^3, t^2, t, 1;
     0, 0, 0, 0, 1, 0;
     5*t^4, 4*t^3, 3*t^2, 2*t, 1, 0;
     0, 0, 0, 2, 0, 0;
     20*t^3, 12*t^2, 6*t, 2, 0, 0];
    p = A \ b;
    ts = [0:0.01:t];
    qs = polyval(p, ts);
    dqs = polyval([5 * p(1), 4 * p(2), 3 * p(3), 2 * p(4), p(5)], ts);
    ddqs = polyval([20 * p(1), 12 * p(2), 6 * p(3), 2 * p(4)], ts);
end
```

```octave
[ts, qs, dqs, ddqs] = quintic_trajectory(0, 1, 0.5, 0, 0, 0, 10);
plot(ts,qs, 'linewidth', 2, ts,dqs, 'linewidth', 2, ts, ddqs, 'linewidth', 2)
```

### Linear Segments Parabolic Blends (LSPB)

This type of trajectory is appropriate when a constant velocity is desired along a portion of the path.

The LSPB trajectory is such that the velocity is initially “ramped up” to its desired value and then “ramped down” when it approaches the goal position, resulting in a *trapezoidal* velocity profile.

To achieve this we specify the desired trajectory in three parts:
1. The first part of the trajectory, from time $t_0$ to time $t_c$, is a quadratic polynomial. This results in a linear “ramp-up” velocity.
2. At time $t_c$, called the blend time, the trajectory switches to a linear function. This corresponds to a constant velocity.
3. At time $t_f − t_c$ the trajectory switches once again to a quadratic polynomial so that the velocity is linear. This is a linear "ramp-down" velocity.

We are given the intitial position $q(0) = q_i$, the final position $q(t_f) = q_f$ and the duration of the motion.

The trajectory has to satisfy some constraints to ensure the transition from $q_i$ to $q_f$ in a time $t_f$. The velocity at the end of the parabolic segment must be equal to the (constant) velocity of the linear segment, that is,
$$\begin{align}
\ddot q_c t_c &= \frac{q_m - q_c}{t_m - t_c} \\
q_m &= \frac{q_f + q_i}{2} \\
t_m &= t_f / 2 .
\end{align}
$$

The joint position $q_c$ after the ramp-up with constant acceleration is found
$$
q_c = q_i + \frac{1}{2} \ddot q_c t_c^2.
$$

The blend time $t_c$ can then be found by solving
$$\ddot q_c t_c^2 - \ddot q_c t_c t_f + q_f - q_i = 0$$ for $t_c$.
This gives
$$t_c = \frac{t_f}{2} - \frac{1}{2}\sqrt{\frac{t_f^2 \ddot q_c - 4(q_f - q_i)}{\ddot q_c}}.$$

Acceleration is then subject to the constraint
$$|\ddot q_c| \geq \frac{4|q_f - q_i|}{t_f^2}$$

The polynomials for the whole trajectory can then be found:
$$q(t) =
\begin{cases}
q_i + \frac{1}{2}\ddot q_c t^2 & 0 \leq t \leq t_c \\
q_i + \ddot q_c t_c (t - t_c / 2) & t_c \leq t \leq t_f - t_c \\
q_f - \frac{1}{2}\ddot q_c(t_f - t)^2 & t_f - t_c \leq t \leq t_f \\
 \end{cases}
$$

#### Example trajectory

```octave
qi = 0
qf = pi
tf = 1
ddq_abs = 6 * pi
```

    qi = 0
    qf =  3.1416
    tf =  1
    ddq_abs =  18.850

Check the acceleration constraint
$$|\ddot q_c| \geq \frac{4|q_f - q_i|}{t_f^2}.$$

```octave
ddq_min = 4 * abs(qf - qi) / tf^2
```

    ddq_min =  12.566

```octave
ddq_abs > ddq_min
```

    ans =  1

Compute the blend time using
$$t_c = \frac{t_f}{2} - \frac{1}{2}\sqrt{\frac{t_f^2 \ddot q_c - 4(q_f - q_i)}{\ddot q_c}}.$$

```octave
tc = tf/2 - 0.5 * sqrt((tf^2*ddq_abs - 4*(qf-qi)) / ddq_abs)
```

    tc =  0.21132

Check the maximum blend time

```octave
tc_max = tf/2 - 0.5 * sqrt((tf^2*ddq_min - 4*(qf-qi)) / ddq_min)
```

    tc_max =  0.50000

As seen the maximum blend time is half the duration of the motion

##### Compute the whole trajectory
$$q(t) =
\begin{cases}
q_i + \frac{1}{2}\ddot q_c t^2 & 0 \leq t \leq t_c \\
q_i + \ddot q_c t_c (t - t_c / 2) & t_c \leq t \leq t_f - t_c \\
q_f - \frac{1}{2}\ddot q_c(t_f - t)^2 & t_f - t_c \leq t \leq t_f\\
 \end{cases}
$$

First segment (ramp-up):

```octave
ts_ramp = [0:0.01:tc];
qs_rampup = qi + 0.5 * ddq_abs * ts_ramp.^2;
dqs_rampup = ddq_abs * ts_ramp;
ddqs_rampup = repmat(ddq_abs,1,size(qs_rampup)(2));
```

Middle segment (constant velocity):

```octave
ts_mid = [tc:0.01:tf-tc];
qs_mid = qi + ddq_abs * tc * (ts_mid - 0.5 * tc);
len = size(qs_mid)(2);
dqs_mid = repmat(ddq_abs*tc,1,len);
ddqs_mid = repmat(0,1,len);
```

Final segment (ramp down):

```octave
ts_rampdown = [tf-tc:0.01:tf];
qs_rampdown = qf - 0.5 * ddq_abs * (tf - ts_rampdown).^2;
dqs_rampdown = ddq_abs * tc - ddq_abs * ts_ramp;
ddqs_rampdown = repmat(-ddq_abs,1,size(qs_rampup)(2));
```

Assemble the whole trajectory

```octave
ts = [ts_ramp, ts_mid, ts_rampdown];
qs = [qs_rampup, qs_mid, qs_rampdown];
dqs = [dqs_rampup, dqs_mid, dqs_rampdown];
ddqs = [ddqs_rampup, ddqs_mid, ddqs_rampdown];
plot(ts, qs, 'linewidth', 2,
     ts, dqs, 'linewidth', 2,
     ts, ddqs, 'linewidth', 2);
```

```octave
function [ts, qs, dqs, ddqs] = lspb_trajectory(qi, qf, tf, acc_scale)
    ddq_min = 4 * abs(qf - qi) / tf^2;
    ddq_abs = ddq_min * acc_scale;
    tc = tf/2 - 0.5 * sqrt((tf^2*ddq_abs - 4*(qf-qi)) / ddq_abs);

    ts_ramp = [0:0.01:tc];
    qs_rampup = qi + 0.5 * ddq_abs * ts_ramp.^2;
    dqs_rampup = ddq_abs * ts_ramp;
    ddqs_rampup = repmat(ddq_abs,1,size(qs_rampup)(2));

    ts_mid = [tc:0.01:tf-tc];
    qs_mid = qi + ddq_abs * tc * (ts_mid - 0.5 * tc);
    len = size(qs_mid)(2);
    dqs_mid = repmat(ddq_abs*tc,1,len);
    ddqs_mid = repmat(0,1,len);

    ts_rampdown = [tf-tc:0.01:tf];
    qs_rampdown = qf - 0.5 * ddq_abs * (tf - ts_rampdown).^2;
    dqs_rampdown = ddq_abs * tc - ddq_abs * ts_ramp;
    ddqs_rampdown = repmat(-ddq_abs,1,size(qs_rampup)(2));

    ts = [ts_ramp, ts_mid, ts_rampdown];
    qs = [qs_rampup, qs_mid, qs_rampdown];
    dqs = [dqs_rampup, dqs_mid, dqs_rampdown];
    ddqs = [ddqs_rampup, ddqs_mid, ddqs_rampdown];
end
```

```octave
[ts, qs, dqs, ddqs] = lspb_trajectory(0,1,1,1.2);
plot(ts, qs, 'linewidth', 2,
     ts, dqs, 'linewidth', 2,
     ts, ddqs, 'linewidth', 2);
```

#### Alternate LSPB formulation

We choose the blend time $t_b$ so that the position curve is symmetric. Suppose that $t_0 = 0$ and $\dot q(t_f) = 0 = \dot{q}(0)$.

Then between times $0$ and $t_b$ we have
$$
q(t) = a_0 + a_1 t + a_2 t^2
$$
with velocity
$$\dot q(t) = a_1 + 2 a_2 t.$$

Since $q_0 = 0$ and $\dot q(0)$:
\begin{align}
a_0 = q_0
a_1 = 0
\end{align}

At time $t_b$ the velocity should be equal to a given constant $V$, thus,
$$a_2 = \frac{V}{2t_b}.$$

##### Initial segment
The first part of the trajectory is then given by
\begin{align}
q(t) &= q_0 + \frac{V}{2t_b} t^2 \\
&= q_0 + \frac{\alpha}{2} t^2 \\
\dot q(t) &= \frac{V}{t_b} t = \alpha t \\
\ddot q(t) &= \frac{V}{t_b} = \alpha.
\end{align}
Here, $\alpha$ denotes the acceleration.

##### Mid segment

$$q(t) = a_0 + a_1 t = a_0 + V t$$

From symmetry:
$$q\left( \frac{t_f}{2} \right) = \frac{q_0 + q_f}{2}$$
Thus,
$$\frac{q_0 + q_f}{2} = a_0 + V\frac{t_f}{2}$$

$$a_0 = \frac{q_0 + q_f - V t_f}{2}$$

The two segments blend at time $t_b$:
$$q_0 + \frac{V}{2} t_b = \frac{q_0 + q_f - V t_f}{2} + V t_b$$

The blend time is then:

$$t_c = \frac{q_i - q_f + V t_f}{\dot q}$$

The velocity must satisfy
$$\frac{q_f - q_i}{t_f} < V \leq \frac{2(q_f - q_i)}{t_f}  $$

The whole trajectory is then

$$q(t) =
\begin{cases}
q_i + \frac{1}{2}\ddot q_c t^2 & 0 \leq t \leq t_c \\
(q_f + q_i - \dot q t_f) / 2 + \dot q t  & t_c \leq t \leq t_f - t_c \\
q_f - \frac{1}{2}\ddot q_c t_f^2 + \dot q t_f t  - \frac{1}{2} \ddot q t^2 & t_f - t_c  \leq t \leq t_f\\
 \end{cases}
$$

Here, $\dot q = V$ and $\ddot q = \alpha$.

##### Implementation based on the lspb function in Corke

```octave
function [q, dq, ddq] = lspb_trajectory(q0, q1, t)
    t0 = t;
    t = (0:t-1)';
    tf = max(t(:));

    V = (q1 - q0)/tf * 1.5;
    tb = (q0 - q1 + V*tf)/V;
    a = V/tb;

    q = zeros(length(t),1);
    dq = q;
    ddq = q;

    for i = 1:length(t)
        tt = t(i);
        if tt <= tb
            q(i) = q0 + a/2*tt^2;
            dq(i) = a*tt;
            ddq(i) = a;
        elseif tt <= (tf-tb)
            q(i) = (q1+q0-V*tf)/2 + V*tt;
            dq(i) = V;
            ddq(i) = 0;
        else
            q(i) = q1 -a/2*tf^2 + a*tf*tt - a/2*tt^2;
            dq(i) = a*tf - a*tt;
            ddq(i) = -a;
        end
    end
end
```

```octave
[q, dq, ddq] = lspb_trajectory(0,100,100);
plot(q, 'linewidth', 2)
title ("LSPB Trajectory");
xlabel ("Time");
ylabel ("Position");
```

```octave
plot(dq, 'linewidth', 2, 'color', [0,0.5,0])
title ("LSPB Trajectory");
xlabel ("Time");
ylabel ("Velocity");
```

```octave
plot(ddq, 'linewidth', 2, 'color', 'r')
title ("LSPB Trajectory");
xlabel ("Time");
ylabel ("Acceleration");
```

## Motion Through a Sequence of Points

In several applications, the path is described in terms of a number of points greater than two.

For instance, even for the simple point-to-point motion of a **pick-and-place** task, it may be worth assigning two intermediate points between the initial point and the final point; suitable positions can be set for lifting off and setting down the object, so that reduced velocities are obtained with respect to direct transfer of the object.

### Time instants

```octave
t = [0;2;4];
```

### Via points vector

```octave
q_v = [0 2 3];
```

```octave
scatter(t, q_v, 'red', 'filled');
```

### Vector of durations of parabolic blends

```octave
D_t = 0.3*ones(size(t));
```

### Initial and final velocities

```octave
dq_i = 0;
dq_f = 0;
```

### Sample time

```octave
Ts = 0.01;
```

### Trajectory generation using

```octave
[time,q,dq,ddq] = lin_par(t,q_v,D_t,dq_i,dq_f,Ts);
```

```octave
scatter(t, q_v, 'red', 'filled');
hold on;
plot(time, q, 'linewidth', 2)
title ("Motion Through a Sequence of Points");
xlabel ("Time");
ylabel ("Position");
```

```octave
plot(time, dq, 'linewidth', 2, 'color', [0,0.5,0])
title ("Motion Through a Sequence of Points");
xlabel ("Time");
ylabel ("Velocity");
```

```octave
plot(time, ddq, 'linewidth', 2, 'color', 'red')
title ("Motion Through a Sequence of Points");
xlabel ("Time");
ylabel ("Acceleration");
```

# Operational Space

A joint space trajectory planning algorithm generates a time sequence of values for the joint variables $q(t)$ so that the manipulator is taken from the initial to the final configuration.

The resulting end-effector motion is not easily predictable.

Whenever it is desired that the end-effector motion follows a geometrically specified path in the **operational space**, it is necessary to plan trajectory execution directly in the same space.

## Position

Let $\boldsymbol{\mathrm p}_i \in \mathbb R^3$ be the start position and let $\boldsymbol{\mathrm p}_f \in \mathbb R^3$ be the final position. A position vector $\boldsymbol{\mathrm p}$ can then be written
$$\boldsymbol{\mathrm p} =
\begin{pmatrix}
x \\ y \\ z
\end{pmatrix},
$$
where $x,y,z \in \mathbb R$.

Consider the vector function
$$
\boldsymbol{\mathrm p} = \boldsymbol{\mathrm f}(\sigma),
$$
with $\sigma \in [\sigma_i, \sigma_f]$. The sequence of values of $\boldsymbol{\mathrm p}$ is called a path in space.

We employ the arc length $s$ as the parameter. The start point is then
$$\boldsymbol{\mathrm p}_i = \boldsymbol{\mathrm f}(0)$$
and the end point is
$$\boldsymbol{\mathrm p}_f = \boldsymbol{\mathrm f}(s).$$

### Rectilinear path

Again, Let $\boldsymbol{\mathrm p}_i \in \mathbb R^3$ be the start position and let $\boldsymbol{\mathrm p}_f \in \mathbb R^3$ be the final position. Consider the linear segment connecting $\boldsymbol{\mathrm p}_i$ and $\boldsymbol{\mathrm p}_f$.

The parametric representation of this path is
$$
\boldsymbol{\mathrm p}(s) = \boldsymbol{\mathrm p}_i + \frac{\boldsymbol{\mathrm p}_f - \boldsymbol{\mathrm p}_i}{\|\boldsymbol{\mathrm p}_f - \boldsymbol{\mathrm p}_i\|} s.$$

Here
$$\boldsymbol{\mathrm p}(0) = \boldsymbol{\mathrm p}_i$$
and
$$\boldsymbol{\mathrm p}(\|\boldsymbol{\mathrm p}_f - \boldsymbol{\mathrm p}_i\|) = \boldsymbol{\mathrm p}_f$$

Differentiating $\boldsymbol{\mathrm p}(s)$ with respect to $s$ gives:
\begin{align}
\frac{d \boldsymbol{\mathrm p}}{ds} &=
\frac{\boldsymbol{\mathrm p}_f - \boldsymbol{\mathrm p}_i}{\|\boldsymbol{\mathrm p}_f - \boldsymbol{\mathrm p}_i\|} \\
\frac{d^2 \boldsymbol{\mathrm p}}{ds^2} &= 0
\end{align}

#### Trajectory
To create a trajectory from the path $\boldsymbol{\mathrm p}(s)$ we can use any of the methods from joint space trajectory generation. The variable to interpolate is then the arc distance.

```octave
pi = [0,0,0]';
pf = [1,2,3]';
```

```octave
length = norm(pf-pi)
```

    length =  3.7417

```octave
direction = (pf - pi) / length
norm(direction)
```

    direction =

       0.26726
       0.53452
       0.80178

    ans =  1

```octave
[xs, dxs, ddxs] = lspb_trajectory(pi(1), pf(1), 50);
[ys, dys, ddys] = lspb_trajectory(pi(2), pf(2), 50);
[zs, dzs, ddzs] = lspb_trajectory(pi(3), pf(3), 50);
```

```octave
plot(xs, 'linewidth',2); hold on;
plot(ys, 'linewidth',2, 'color', [0, 0.5, 0])
plot(zs, 'linewidth',2, 'color', 'red')
```

## Orientation

Consider the end-effector orientation. The orientation can be represented by a rotation matrix $\boldsymbol{\mathrm R}$ of the end-effector frame with respect to the base-frame.

### Euler angles

We first consider orientation represented by Euler angles, e.g., XYZ Euler angles.

The rotation matrix around the z-axis is:
$$\boldsymbol{\mathrm R}_z =
\begin{pmatrix}
\cos \theta & -\sin \theta & 0 \\
\sin \theta & \cos \theta & 0 \\
0 & 0 & 1
\end{pmatrix}
$$

```octave
function R = rotz(theta)
    ct = cos(theta);
    st = sin(theta);
    R = [ct, -st, 0;
         st, ct, 0;
         0, 0, 1];
end
```

```octave
R = rotz(pi/6)
```

    R =

       0.86603  -0.50000   0.00000
       0.50000   0.86603   0.00000
       0.00000   0.00000   1.00000

The rotation matrix around the y-axis is:
$$\boldsymbol{\mathrm R}_z =
\begin{pmatrix}
\cos \theta & 0 &  \sin \theta \\
0 & 1 & 0 \\
-\sin \theta & 0 & \cos \theta
\end{pmatrix}
$$

```octave
function R = roty(th)
    ct = cos(th);
    st = sin(th);
    R = [ct, 0, st;
         0, 1, 0;
         -st, 0, ct];
end
```

```octave
Ry = roty(pi/6)
```

    Ry =

       0.86603   0.00000   0.50000
       0.00000   1.00000   0.00000
      -0.50000   0.00000   0.86603

The rotation matrix around the y-axis is:
$$\boldsymbol{\mathrm R}_z =
\begin{pmatrix}
1 & 0 & 0 \\
0 & \cos \theta &  \sin \theta \\
0 & -\sin \theta &  \cos \theta
\end{pmatrix}
$$

```octave
function R = rotx(th)
    ct = cos(th);
    st = sin(th);
    R = [1, 0, 0;
         0, ct, -st;
         0, st, ct];
end
```

```octave
rpy = [1,2,3];
```

```octave
R = rotx(rpy(1)) * roty(rpy(2)) * rotz(rpy(3))
```

    R =

       0.411982   0.058727   0.909297
      -0.681243  -0.642873   0.350175
       0.605127  -0.763718  -0.224845

```octave
rotx(1)
```

    error: 'rotx' undefined near line 1 column 1

```octave

```