# Rendezvous

Rendezvous problems involve the relative position, velocity, and acceleration of two objects in orbit around another (large) body—for example, two spacecraft in orbit around Earth.

```python
import numpy as np
%matplotlib inline
from matplotlib import pyplot as plt
```

## Relative coordinate system

Given two spacecraft A and B, where we know the position and velocity vectors at an instant in time (potentially via their orbital elements), we can determine the relative position, vector, and acceleration of B from the local coordinate system of A.

The problem: given $\vec{R}_A$, $\vec{R}_B$, $\vec{V}_A$, and $\vec{V}_B$, find $\vec{r}_{\text{rel}}$, $\vec{v}_{\text{rel}}$, and $\vec{a}_{\text{rel}}$ of B with respect to A.

First, determine the relative vectors of B with respect to A in the geocentric equatorial frame:

$$
\begin{align}
\vec{R}_{B/A} = \vec{R}_{\text{rel}} &= \vec{R}_B - \vec{R}_A \\
\vec{V}_{\text{rel}} &= \vec{V}_B - \vec{V}_A - \vec{\Omega} \times \vec{R}_{\text{rel}} \\
\vec{A}_{\text{rel}} &= \vec{A}_B - \vec{A}_A - \vec{\dot{\Omega}} \times \vec{R}_{\text{rel}} - \vec{\Omega} \times \vec{\Omega} \times \vec{R}_{\text{rel}} - 2 \vec{\Omega} \times \vec{V}_{\text{rel}} \;,
\end{align}
$$

where

$$
\begin{align}
\vec{A}_A &= -\frac{\mu}{R_A^3} \vec{R}_A \\
\vec{A}_B &= -\frac{\mu}{R_B^3} \vec{R}_B \;.
\end{align}
$$

We need the angular velocity $\Omega$ and angular acceleration $\dot{\Omega}$ associated with the moving coordinate system of spacecraft A, where $\vec{V}_A = \vec{\Omega} \times \vec{R}_A$, and we can determine the angular momentum

$$
\vec{h}_A = \vec{R}_A \times \left( \vec{\Omega} \times \vec{R}_A \right) \;.
$$

Then,

$$
\begin{align}
\vec{\Omega} &= \frac{\vec{h}_A}{R_A^2} = \frac{\vec{R}_A \times \vec{V}_A}{R_A^2} \\
\vec{\dot{\Omega}} &= \frac{d}{dt} \vec{\Omega} = \frac{-2 \vec{\Omega}}{R_A^2} \vec{V}_A \cdot \vec{R}_A
\end{align}
$$

These vectors give us the relative position, velocity, and acceleration in the geocentric equatorial frame, but we would like them in the local coordinate frame of spacecraft A.
In other words, where is spacecraft B and how fast is it moving, from the perspective of spacecraft A?

First, define the local coordinate system of spacecraft A:

$$
\begin{align}
\hat{i} &= \frac{\vec{R}_A}{R_A} \\
\hat{k} &= \frac{\vec{h}_A}{h_A} \\
\hat{j} &= \hat{k} \times \hat{i}
\end{align}
$$

We can write these unit direction vectors with respect to the geocentric equatorial frame:

$$
\begin{align}
\hat{i} &= L_x \hat{I} + M_x \hat{J} + N_x \hat{K} \\
\hat{j} &= L_y \hat{I} + M_y \hat{J} + N_y \hat{K} \\
\hat{k} &= L_z \hat{I} + M_z \hat{J} + N_z \hat{K} \;,
\end{align}
$$

where $L_x$ is the first element of $\hat{i}$, and so on.

The rotation matrix for converting from the local coordinate system to the geocentric equatorial frame is

$$
[Q_{xX}] = \begin{bmatrix} L_x & M_x & N_x \\
L_y & M_y & N_y \\
L_z & M_z & N_z \end{bmatrix}
$$

Then, we can determine the relative vectors from the local coordinate system of A:

$$
\begin{align}
\vec{r}_{\text{rel}} &= [Q_{xX}]^{\intercal} \vec{R}_{\text{rel}} \\
\vec{v}_{\text{rel}} &= [Q_{xX}]^{\intercal} \vec{V}_{\text{rel}} \\
\vec{a}_{\text{rel}} &= [Q_{xX}]^{\intercal} \vec{A}_{\text{rel}}
\end{align}
$$

## Linear orbit theory

We may also want to know how close or far apart two objects are as a function of time.
For example, if the Space Shuttle releases a satellite with some initial relative velocity,
how far away is it after some time?

We can answer this using linear orbit theory.
First, define the position of the object B relative to that of object A: $\vec{r} = \vec{R} + \vec{\delta}_r$, where $\vec{\delta}_r$ is relatively small (compared to orbital distances),
and insert this into the equation of motion:

$$
\vec{\ddot{R}} + \vec{\ddot{\delta}_r} = \frac{\mu}{r^3} \left( \vec{R} + \vec{\delta}_r \right) \;,
$$

where we can express the position of the second object in the denominator using a binomial expansion:

$$
r^{-3} = \left(r^2 \right)^{-3/2} \approx R^{-3} \left[ \frac{-2 R \delta_r}{R^2} \right]^{-3/2} \;.
$$

Then, based on the assumptions that $\vec{R} = R \hat{i}$ and that the orbit of object A
is circular, we can solve for the relative position vector of object B as a function of time
using the **Chlohessy-Wiltshire equations** (i.e., modified Hill's equations):

$$
\begin{align}
\ddot{\delta_x} - 3 n^2 \delta_x - 2 n \dot{\delta_y} &= 0 \\
\ddot{\delta_y} + 2 n \dot{\delta_x} &= 0 \\
\ddot{\delta_z} + n^2 \delta_z &= 0 \;,
\end{align}
$$

where $n = \sqrt{\mu / R^3}$ is the mean motion.
To solve for the relative position vector as a function of time, we can
decompose this into a system of six first-order ODEs, and integrate in time.
The initial conditions would be that at $t = 0$,
the initial position components are zero ($\delta_x, \delta_y, \delta_z = 0$),
and the initial velocity components $\dot{\delta}_x$, $\dot{\delta}_y$, and $\dot{\delta}_z$ are given.

After some time, the distance of object B from A would then be $\delta_r = \sqrt{\delta_x^2 + \delta_y^2 + \delta_z^2}$.

## Cowell's method

We may also want to keep track of the original orbit along with the new orbit,
both in the case of a rendezvous problem but also when analyzing orbital perturbations.
**Cowell's method** allows us to track/integrate the original Kepler orbit,
along with the *perturbed* orbit (i.e., the oscultating orbit) of either the
original vehicle or an ejected object.

First, define the position of the object in the perturbed orbit:
$\vec{r} = \vec{\rho} + \vec{\delta}_r$, where $\vec{\rho}$ is the position in the
original Kepler orbit.
Then, we can write equations of motion for both orbits:

$$
\begin{align}
\vec{\ddot{\rho}} + \frac{\mu}{\rho^3} \vec{\rho} &= 0 \\
\vec{\ddot{\delta}}_r + \frac{\mu}{\delta_r^3} \vec{\delta}_r &= \vec{a}_p \;,
\end{align}
$$

where $\vec{a}_p$ is the acceleration of a perturbing force.
In the case of thrust, this is

$$
\vec{a}_p = \frac{T \dot{\delta}_r}{ \delta_r m} \;,
$$

but this term can also include other perturbations such as atmospheric drag.

## Encke formulation

$$
\begin{align}
\vec{\ddot{\delta}}_r &= \frac{\mu}{\rho^3} \left( F \vec{r} - \vec{\delta}_r \right) + \frac{T \vec{v}}{v m} \\
\vec{\ddot{\rho}} &= -\frac{\mu}{\rho^3} \vec{\rho} \\
\dot{m} &= -\frac{T}{I_{\text{sp}} g_0} \;,
\end{align}
$$

where

$$
\begin{align}
\phi &= \frac{-(\delta_x \rho_x + \delta_y \rho_y + \delta_z \rho_z )}{\rho^2} \\
F &= 1 - (1 - 2 \phi)^{-3/2} \\
\vec{v} &= \vec{\dot{\delta}}_r + \vec{\dot{\rho}} \\
\vec{r} &= \vec{\delta}_r + \vec{\rho} \;.
\end{align}
$$

In three dimensions, this problem requires the integration of 13 first-order ODEs in time for 13 variables, all of which need initial conditions.

For example, if we are looking for the new perturbed orbit after some engine burn for
some time, then the initial relative position and velocity components would be zero:
$\delta_x, \delta_y, \delta_z, \dot{\delta}_x, \dot{\delta}_y, \dot{\delta}_z = 0$,
the initial position and velocity vectors of the Kepler orbit would be based on the
state vector where the burn starts ($\rho_x, \rho_y, \rho_z, \dot{\rho}_x, \dot{\rho}_y, \dot{\rho}_z$), and the initial mass is the starting mass of the vehicle ($m = m_0$).

After some integrating for some time, we can determine the new position and velocity:

$$
\begin{align}
\vec{R} &= \left[ \rho_x + \delta_x , \rho_y + \delta_y, \rho_z + \delta_z \right] \\
\vec{V} &= \left[ \dot{\rho}_x + \dot{\delta}_x , \dot{\rho}_y + \dot{\delta}_y, \dot{\rho}_z + \dot{\delta}_z \right] \;.
\end{align}
$$

```python

```


# Exercise 3: formation control

```python
# header to start
%matplotlib notebook
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import matplotlib as mp
import math
import pickle
import IPython
import scipy.linalg
```

```python
def getLaplacian(E, n_vertices, directed=False):
    L = np.zeros((n_vertices, n_vertices))
    for e in E:
        if directed:
            L[e[1]][e[1]] += 1
            L[e[1]][e[0]] = -1
        else:
            L[e[1]][e[1]] += 1
            L[e[0]][e[0]] += 1
            L[e[1]][e[0]] = -1
            L[e[0]][e[1]] = -1
    return L

def getIncidence(E, n_vertices):
    D = np.zeros((n_vertices, len(E)))
    for i, e in enumerate(E):
        D[e[0]][i] = -1
        D[e[1]][i] = 1
    return D

def getMSD(x, y, E):
    MSD = np.zeros((len(E),))
    for i in range(len(E)):
        e = E[i]
        x1 = x[e[0]]
        x2 = x[e[1]]
        y1 = y[e[0]]
        y2 = y[e[1]]

        d = np.power(x1-x2, 2) + np.power(y1-y2, 2)
        d = np.sqrt(d)
        MSD[i] = d

    return MSD
```

## Helper function to display results

This function can be used to display the behavior of the robots in 2D

```python
def make_animation(plotx,E,xl=(-2,2),yl=(-2,2),inter=25, display=False):
    '''
    takes a graph and motion of vertexes in 2D and returns an animation
    E: list of edges (each edge is a pair of vertexes)
    plotx: a matrix of states ordered as (x1, y1, x2, y2, ..., xn, yn) in the rows and time in columns
    xl and yl define the display boundaries of the graph
    inter is the interval between each point in ms
    '''
    fig = mp.figure.Figure()
    mp.backends.backend_agg.FigureCanvasAgg(fig)
    ax = fig.add_subplot(111, autoscale_on=False, xlim=xl, ylim=yl)
    ax.grid()

    list_of_lines = []
    for i in E: #add as many lines as there are edges
        line, = ax.plot([], [], 'o-', lw=2)
        list_of_lines.append(line)

    def animate(i):
        for e in range(len(E)):
            vx1 = plotx[2*E[e][0],i]
            vy1 = plotx[2*E[e][0]+1,i]
            vx2 = plotx[2*E[e][1],i]
            vy2 = plotx[2*E[e][1]+1,i]
            list_of_lines[e].set_data([vx1,vx2],[vy1,vy2])
        return list_of_lines

    def init():
        return animate(0)

    ani = animation.FuncAnimation(fig, animate, np.arange(0, len(plotx[0,:])),
        interval=inter, blit=True, init_func=init)
    plt.close(fig)
    plt.close(ani._fig)
    if(display==True):
        IPython.display.display_html(IPython.core.display.HTML(ani.to_html5_video()))
    return ani
```

We wish the control the formation of 4 robots randomly distributed in the environment to keep the formation shown in the figure of Exercise 2.

## Question 1
Assume each agent has state space dynamics $\dot{\mathbf{p}}_i =\mathbf{u}_i$, with $\mathbf{u}_i$ in $\mathbb{R}^2$ and $\mathbf{u} = [\mathbf{u}_1, \mathbf{u}_2, \mathbf{u}_3, \mathbf{u}_4]$.

Implement the second order linear control law seen in the class
$$\mathbf{u} = -k \mathbf{L} \mathbf{x} + k \mathbf{D z}_{ref}$$

where $k>0$ is a positive gain and $\mathbf{D}$ is the incidence matrix of the graph.

Simulate the control law for several random initial conditions of the agents (in 2D). What do you observe? How does it compare to the same control law but for a framework with a complete graph?

```python
with open('example_animation.pickle', 'rb') as f:
    data = pickle.load(f)
E = data['E']
n_vertices = 4
L = getLaplacian(E, n_vertices, directed=False)
D = getIncidence(E, n_vertices)

Px_des = np.array([0, 1, 1, (2-math.sqrt(14))/4]).T
Py_des = np.array([0, 0, -1, -(math.sqrt(14)+2)/4]).T

x = np.array([0, 0.5, -0.3, -0.1]).T
y = np.array([0.1, 0.2, 0.3, 0.1]).T

K = 2 * np.eye(4)

xs = x
ys = y
ts = [0]
t = 0
dt = 0.001

while t < 10:
    zx_ref = np.matmul(D.T, Px_des)
    dx = -np.matmul(K, np.matmul(L, x)) + np.matmul(K, np.matmul(L, Px_des))
    zy_ref = np.matmul(D.T, Py_des)
    dy = -np.matmul(K, np.matmul(L, y)) + np.matmul(K, np.matmul(L, Py_des))
    x += dt*dx
    y += dt*dy
    xs = np.vstack((xs, x))
    ys = np.vstack((ys, y))
    t += dt
    ts.append(t)

data = np.zeros((0, xs.shape[0]))
for i in range(xs.shape[1]):
    data = np.vstack((data, xs[:, i].T))
    data = np.vstack((data, ys[:, i].T))

plotx = data[:,::50]
make_animation(plotx, E, inter=50, display=True)
```

    <matplotlib.animation.FuncAnimation at 0x7fb15a96acf8>

### Complete Graph

```python
E = [[0, 1], [1, 2], [0, 2], [0, 3], [2, 3], [1, 3]]
n_vertices = 4
L = getLaplacian(E, n_vertices, directed=False)
D = getIncidence(E, n_vertices)

Px_des = np.array([0, 1, 1, (2-math.sqrt(14))/4]).T
Py_des = np.array([0, 0, -1, -(math.sqrt(14)+2)/4]).T

x = np.array([0, 0.5, -0.3, -0.1]).T
y = np.array([0.1, 0.2, 0.3, 0.1]).T

K = 2 * np.eye(4)

xs = x
ys = y
ts = [0]
t = 0
dt = 0.001

while t < 10:
    zx_ref = np.matmul(D.T, Px_des)
    dx = -np.matmul(K, np.matmul(L, x)) + np.matmul(K, np.matmul(L, Px_des))
    zy_ref = np.matmul(D.T, Py_des)
    dy = -np.matmul(K, np.matmul(L, y)) + np.matmul(K, np.matmul(L, Py_des))
    x += dt*dx
    y += dt*dy
    xs = np.vstack((xs, x))
    ys = np.vstack((ys, y))
    t += dt
    ts.append(t)

data = np.zeros((0, xs.shape[0]))
for i in range(xs.shape[1]):
    data = np.vstack((data, xs[:, i].T))
    data = np.vstack((data, ys[:, i].T))

plotx = data[:,::50]
make_animation(plotx, E, inter=50, display=True)
```

    <matplotlib.animation.FuncAnimation at 0x7fb17e3d5860>

The control law for both the incomplete and complete graph renders the same result.

## Question 2
Assume each agent has state space dynamics $\dot{\mathbf{p}}_i =\mathbf{u}_i$, with $\mathbf{u}_i$ in $\mathbb{R}^2$ and $\mathbf{u} = [\mathbf{u}_1, \mathbf{u}_2, \mathbf{u}_3, \mathbf{u}_4]$.

We now consider the following control law
$$\begin{equation}
\mathbf{u} = \mathbf{R}_\mathcal{G}^T(\mathbf{p}) (\mathbf{g}_d - \mathbf{g}_\mathcal{G}(\mathbf{p}))
\end{equation}$$
where $\mathbf{R}_\mathcal{G}$ is the rigidity matrix associated to the graph of the framework, $\mathbf{g}_d$ is the vector of desired square distance between agents and $\mathbf{g}_\mathcal{G}$ is the measured square distance between each agent.

Simulate the control law for several random initial conditions of the agents (in 2D). What do you observe? How does it compare to the same control law but for a framework with a complete graph?

```python
gd = np.array([1, 1, math.sqrt(2), 1.5, 1.5])
R = np.array([[-2, 0, 2, 0, 0, 0, 0, 0],
                  [0, 0, 0, 2, 0, -2, 0, 0],
                  [-2, 2, 0, 0, 2, -2, 0, 0],
                  [(math.sqrt(14)-2)/2, (math.sqrt(14)+2)/2, 0, 0,
                   0, 0, (2-math.sqrt(14))/2, -(math.sqrt(14)+2)/2],
                  [0, 0, 0, 0, (2+math.sqrt(14))/2, (math.sqrt(14)-2)/2,
                   -(2+math.sqrt(14))/2, (2-math.sqrt(14))/2]])

p = np.array([0, 0.1, 0.5, 0.2, -0.3, 0.3, -0.1, 0.1]).T
ps = p
ts = [0]
t = 0
dt = 0.001
E = [[0, 1], [1, 2], [0, 2], [0, 3], [2, 3]]

while t < 10:
    x = [p[0], p[2], p[4], p[6]]
    y = [p[1], p[3], p[5], p[7]]
    u = np.matmul(R.T, gd-getMSD(x, y, E))
    p += dt*u
    ps = np.vstack((ps, p))
    t += dt
    ts.append(t)

data = ps.T
plotx = data[:,::50]
make_animation(plotx, E, inter=50, display=True)
```

    <matplotlib.animation.FuncAnimation at 0x7fb140f85ef0>

```python
### Complete Graph
```

```python
gd = np.array([1, 1, math.sqrt(2), 1.5, 1.5, math.sqrt(2)*(math.sqrt(14)+2)/4])
R = np.array([[-2, 0, 2, 0, 0, 0, 0, 0],
              [0, 0, 0, 2, 0, -2, 0, 0],
              [-2, 2, 0, 0, 2, -2, 0, 0],
              [(math.sqrt(14)-2)/2, (math.sqrt(14)+2)/2, 0, 0,
               0, 0, (2-math.sqrt(14))/2, -(math.sqrt(14)+2)/2],
              [0, 0, 0, 0, (2+math.sqrt(14))/2, (math.sqrt(14)-2)/2,
               -(2+math.sqrt(14))/2, (2-math.sqrt(14))/2],
              [0, 0, (math.sqrt(14)+2)/4, (math.sqrt(14)+2)/4, 0, 0, -(math.sqrt(14)+2)/4, -(math.sqrt(14)+2)/4]])
p = np.array([0, 0.1, 0.5, 0.2, -0.3, 0.3, -0.1, 0.1]).T
ps = p
ts = [0]
t = 0
dt = 0.001
E = [[0, 1], [1, 2], [0, 2], [0, 3], [2, 3], [1, 3]]

while t < 10:
    x = [p[0], p[2], p[4], p[6]]
    y = [p[1], p[3], p[5], p[7]]
    u = np.matmul(R.T, gd-getMSD(x, y, E))
    p += dt*u
    ps = np.vstack((ps, p))
    t += dt
    ts.append(t)

data = ps.T
plotx = data[:,::50]
make_animation(plotx, E, inter=50, display=True)
```

    <matplotlib.animation.FuncAnimation at 0x7fb140ebe358>

## Question 3

How would you compare both control laws? What are the pros and cons of each of them?

The first controller is only rotation invariant, while the second one is neither. This can be seen from the controller definition, we see that for the first controller it can also be written as,
$$\dot{P}_x = -KLP_x - KLP_x^{\mathrm{des}}$$
$$\dot{P}_y = -KLP_y - KLP_y^{\mathrm{des}}$$
we can see that as long as the relative $x$ and $y$ distance are the same as desired the controller will output 0, which disregards the abosolute translation of the system.

For the second controller for the stable system all required is the distance between vertices are equal to the desired distance thus it is rotation and translation invariant.

```python
# for example assume that you have simulated a formation control in 2D and stored the data in a file
# we load the data needed for the display
with open('example_animation.pickle', 'rb') as f:
    data = pickle.load(f)

# this is the list of edges (as we usually define them for an undirected graph)
E = data['E']
print('the list of edges is:')
print(E)

# this is the time of simulation
t = data['t']

# this is an array containing the evolution of the states of the robot
# x[0,:] contains the time evolution of the x variable of robot 1
# x[1,:] contains the time evolution of the y variable of robot 1
# x[2,:] contains the time evolution of the x variable of robot 2
# etc
x = data['x']

# since we simulated with a small delta t = 0.001, we want to subsample for display
# we just take data every 50ms
plotx = x[:,::50]

make_animation(plotx, E, inter=50, display=True)

# a video showing the behavior of the robots and the connection between the robots should be displayed below
```

    the list of edges is:
    [[0, 1], [0, 2], [0, 3], [1, 2], [2, 3]]

    <matplotlib.animation.FuncAnimation at 0x7fb140ec5b70>


# Motor selection
*Written by Marc Budinger (INSA Toulouse) and Scott Delbecq (ISAE-SUPAERO), Toulouse, France.*

**Sympy** package permits us to work with symbolic calculation.

```python
from math import pi

from sympy import Symbol
from sympy import *
```

## Design graph

The following diagram represents the design graph of the motor’s selection. The mean speed/thrust (Ωmoy & Tmoy), the max speed/thrust (Ωmax & Tmax) and the battery voltage are assumed to be known here.

> **Questions:**
* Give the 2 main sizing problems you are able to detect here.
* Propose one or multiple solutions (which can request equation manipulation, addition of design variables, addition of constraints)
* Orientate the arrows and write equations order, inputs/outputs at each step of this part of sizing procedure, additional constraints

### Sizing code and optimization

> Exercice: propose a sizing code for the selection of a motor.

```python
# Specifications

# Reference parameters for scaling laws
# Motor reference
# Ref : AXI 5325/16 GOLD LINE
T_nom_mot_ref = 2.32  # [N.m] rated torque
T_max_mot_ref = 85./70.*T_nom_mot_ref # [N.m] max torque
R_mot_ref = 0.03  # [Ohm] resistance
M_mot_ref = 0.575 # [kg] mass
K_mot_ref = 0.03 # [N.m/A] torque coefficient
T_mot_fr_ref = 0.03 # [N.m] friction torque (zero load, nominal speed)

# Assumption
T_pro_to=1.0#[N.m] Propeller Torque during takeoff
Omega_pro_to=1.0#[rad/s] Propeller speed during takeoff
T_pro_hov=1.0#[N.m] Propeller Torque during hover
Omega_pro_hov=1.0#[rad/s] Propeller speed during hover
U_bat_est= 4.0#[V] Battery voltage value (estimation)

```

Define the design variables as a symbol under `variableExample= Symbol('variableExample')`

```python
#Design variables
k_mot=Symbol('k_mot')#[-] over sizing coefficient on the motor torque (1,400)
k_speed_mot=Symbol('k_speed_mot')#[-] over sizing coefficient on the motor speed (1,10)
```

```python
#Equations:
#-----
T_nom_mot = k_mot * T_pro_hov   # [N.m] Motor nominal torque per propeller

M_mot = M_mot_ref * (T_nom_mot/T_nom_mot_ref)**(3./3.5) # [kg] Motor mass

# Selection with take-off speed
K_mot = U_bat_est / (k_speed_mot*Omega_pro_to) # [N.m/A] or [V/(rad/s)] Kt motor

R_mot = R_mot_ref * (T_nom_mot/T_nom_mot_ref)**(-5./3.5)*(K_mot/K_mot_ref)**2.  # [Ohm] motor resistance
T_mot_fr = T_mot_fr_ref * (T_nom_mot/T_nom_mot_ref)**(3./3.5) # [N.m] Friction torque
T_max_mot = T_max_mot_ref * (T_nom_mot/T_nom_mot_ref)

# Hover current and voltage
I_mot_hov = (T_pro_hov+T_mot_fr) / K_mot # [I] Current of the motor per propeller
U_mot_hov = R_mot*I_mot_hov + Omega_pro_hov*K_mot # [V] Voltage of the motor per propeller
P_el_mot_hov = U_mot_hov*I_mot_hov # [W] Hover : electrical power

# Takeoff current and voltage
I_mot_to = (T_pro_to+T_mot_fr) / K_mot # [I] Current of the motor per propeller
U_mot_to = R_mot*I_mot_to + Omega_pro_to*K_mot # [V] Voltage of the motor per propeller
P_el_mot_to = U_mot_to*I_mot_to # [W] Takeoff : electrical power
```


# General instructions

## High Level Description

In this assignment you are asked to implement K-means clustering to identify main clusters in
the data, use the discovered centroid of cluster for classification. Specifically, you will

 - Implement K-means clustering algorithm to identify clusters in a two-dimensional toy-dataset.
 - Implement image compression using K-means clustering algorithm.
 - Implement classification using the centroids identified by clustering on digits dataset.
 - Implement K-means++ clustering algorithm to identify clusters in a two-dimensional toy-dataset i.e. implement the kmeans++ function to compute the centers.

NOTE: You only need to make changes in Kmeans.py and use KmeansTest.py for testing purposes and to see your results. You can find all TODO's sequentially in the Kmeans.py file.

Depending on your environment you may need to install the python library named, ”pillow”, which is used by matplotlib to process some of the images needed for this assignment.

You can install it by running ’pip3 install pillow’ in your command line.

```python
!pip3 install pillow
```

### Grading Guidelines (50 points):

You are only required to submit Kmeans.py as that is the only file where you will be making any changes.
 - get_k_means_plus_plus_center_indices - 5 points (5 *1)
 - transform_image - 10 points (5 * 2 test cases) We are checking the MSE and the number of iterations for this
 - Kmeans( ) class on Toy dataset - 15 points (3 * 5 test cases) We are checking the centroid and membership for Kmeans and Kmeans++
 - KmeansClassifier( ) class - 20 points (5 * 4 test cases) We are checking the accuracy and the centroids of the assignments.

### Office Hours

Ashir Alam (ashirala@usc.edu)

April 5th  12:00pm- 1:00pm Leavey 301F

April 9th 10am - 12pm Leavey 301F

April 16th 10am - 12pm Leavey 301F

### Dataset for K-Means Clustering

We will use 2 datasets - 2-D Toy Dataset and Digits datasets for K means part.

Toy Dataset is a two-dimensional dataset generated from 4 Gaussian distributions. We will use this
dataset to visualize the results of our algorithm in two dimensions. You can find it in data_loader.py

We will use digits dataset from sklearn to test K-means based classifier and generate digits using
Gaussian Mixture model. Each data point is a 8 × 8 image of a digit. This is similar to MNIST but less
complex. There are 10 classes in digits dataset.

Link for Digits dataset: sklearn.datasets.digits http://scikit-learn.org/stable/modules/generated/sklearn.datasets.load_digits.html#sklearn.datasets.load_digits

## 1. K Means Clustering

Recall that for a dataset $ x_1, . . . , x_N ∈ R^D $, the K-means distortion objective is:
$$ F(\{\mu_k\}, \{r_{nk}\}) = \sum_{i=1}^N \sum_{k=1}^K r_{nk} \|\mu_k- x_n\|_2^2     \qquad  (1)   $$

where $ µ_1, . . . , µ_K $ are centroids of the K clusters and $ r_{ik} ∈ {0, 1} $ represents whether example i belongs to cluster k.

Clearly, fixing the centroids and minimizing J over the assignment give

$$ \begin{equation}
    \hat r_{ik} = \begin{cases}
        1 & k = argmin_{k'} \|\mu_{k'}-x_n\|_2^2 \\
        0 & \text{Otherwise.}
    \end{cases}
    \label{eq:opt_membership}
\end{equation} \qquad  (2)$$

On the other hand, fixing the assignment and minimizing $J$ over the centroids give
$$ \begin{equation}
    \hat \mu_k =\frac{ \sum_{i=1}^N r_{nk} x_n}{\sum_{i=1}^N r_{nk}}
    \label{eq:opt_mean}
\end{equation} \qquad  (3) $$

What the K-means algorithm does is simply to alternate between these two steps.

### 1.1 Implementing k-means++ algorithm

Recall from lecture Kmeans++. Please refer to the algorithm below. In simple terms, cluster centers are initially chosen at random from the set of input observation vectors, where the probability of choosing vector x is high if x is not near any previously chosen centers.

Here is a one-dimensional example. Our observations are $ [0, 1, 2, 3, 4] $. Let the first center, $ c1 $, be 0. The probability that the next cluster center, $ c2 $, is x is proportional to $ ||c1-x||^2 $. So, $ P(c2 = 1) = 1a, P(c2 = 2) = 4a, P(c2 = 3) = 9a, P(c2 = 4) = 16a $, where $ a = 1/(1+4+9+16) $.

Suppose $ c2 = 4 $. Then, $ P(c3 = 1) = 1a, P(c3 = 2) = 4a, P(c3 = 3) = 1a $, where $ a = 1/(1+4+1) $.

For more insights, follow this: http://ilpubs.stanford.edu:8090/778/1/2006-13.pdf

Implement Algorithm by filling out the TODO parts in function **get_k_means_plus_plus_center_indices** of file **kmeans.py**. You can test this function on Vocareum separately.

```

def get_k_means_plus_plus_center_indices(n, n_cluster, x, generator=np.random):
    :param n: number of samples in the data
    :param n_cluster: the number of cluster centers required
    :param x: data - numpy array of points
    :param generator: random number generator from 0 to n for choosing the first cluster at random
            The default is np.random here but in grading, to calculate deterministic results,
            We will be using our own random number generator.

    :return: the center points array of length n_clusters with each entry being index to a sample
             which is chosen as centroid.

```
If the generator is still not clear, its basically a np.random but helps us control the result during testing. SO wherever you would use np.random, use generator instead.

### 1.2 Implementing K-means clustering algorithm

Implement Algorithm 1 by filling out the TODO parts (**fit** function) in class **KMeans** of file **kmeans.py**. Note the following:

 - Initialize means by picking self.n_cluster from N data points
 - Update means and membership until convergence or until you have made self.max_iter updates.
 - return (means, membership, number_of_updates)
 - If at some iteration, there exists a cluster k with no points assigned to it, then do not update the centroid of this cluster for this round.
 - While assigning a sample to a cluster, if there’s a tie (i.e. the sample is equidistant from two centroids), you should choose the one with smaller index (like what numpy.argmin does).
 - For each k, we are trying to compare based on the Euclidean distance.

 ``` <br>
 Class KMeans:
        Attr:
            n_cluster - Number of cluster for kmeans clustering (Int)
            max_iter - maximum updates for kmeans clustering (Int)
            e - error tolerance (Float)
            generator - random number generator from 0 to n for choosing the first cluster at random
                The default is np.random here but in grading, to calculate deterministic results,
                We will be using our own random number generator.

            def __init__(self, n_cluster, max_iter=100, e=0.0001, generator=np.random):
                self.n_cluster = n_cluster
                self.max_iter = max_iter
                self.e = e
                self.generator = generator

            def fit(self, x, centroid_func=get_lloyd_k_means):
                Finds n_cluster in the data x
                params:
                x - N X D numpy array
                centroid_func - To specify which algorithm we are using to compute the centers(Lloyd(regular) or Kmeans++) The default is Lloyd's Kmeans.

                returns: A tuple (centroids a n_cluster X D numpy array, y a length (N,) numpy array where cell i is the ith sample's assigned cluster, number_of_updates a Int)
            Note: Number of iterations is the number of time you update the assignment

 ```

After you complete the implementation, run KmeansTest.py to see the results of this on toy
dataset. You should be able to see three images generated in plots folder. In particular, you can see
toy dataset predicted labels.png and toy dataset real labels.png and compare the clusters identified by the algorithm against the real clusters. Your implementation should be able to recover the correct clusters sufficiently well. Representative images are shown in fig. 2. Red dots are cluster centroids.
Note that color coding of recovered clusters may not match that of correct clusters. This is due to mis-match
in ordering of retrieved clusters and correct clusters (which is fine).

### 1.3 Classification with k-means

Another application of clustering is to obtain a faster version of the nearest neighbor algorithm. Recall that nearest neighbor evaluates the distance of a test sample from every training point to predict its class, which can be very slow. Instead, we can compress the entire training dataset to just the K centroids, where each centroid is now labeled as the majority class of the corresponding cluster. After this compression the prediction time of nearest neighbor is reduced from O(N) to just O(K) (see Algorithm 2 for the pseudocode).

Complete the **fit** and **predict** function in **KMeansClassifier** in file **kmeans.py** . Once completed,
run **KmeansTest.py** to evaluate the classifier on a test set (digits). For comparison, the script will also print accuracy of a logistic classifier and a nearest neighbor classifier. (Note: a naive K-means classifier may not do well but it can be an effective unsupervised method in a classification pipeline .)

Note: 1) break ties in the same way as in previous problems; 2) if some centroid doesn’t contain any
point, set the label of this centroid as 0.

The prediction accuracy baseline is 0.77 for KMeans Lloyd(regular) algorithm and 0.72 for KMeans++ algorithm. Note: these differ on different datasets and in more cases Kmeans++ works better.

```

    Class KMeansClassifier:

        Attr:
            n_cluster - Number of cluster for kmeans clustering (Int)
            max_iter - maximum updates for kmeans clustering (Int)
            e - error tolerance (Float)
            generator - random number generator from 0 to n for choosing the first cluster at random
            The default is np.random here but in grading, to calculate deterministic results,
            We will be using our own random number generator.

        def __init__(self, n_cluster, max_iter=100, e=1e-6, generator=np.random):
            self.n_cluster = n_cluster
            self.max_iter = max_iter
            self.e = e
            self.generator = generator

        def fit(self, x, y, centroid_func=get_lloyd_k_means):

            Train the classifier
            params:
                x - N X D size  numpy array
                y - (N,) size numpy array of labels
                centroid_func - To specify which algorithm we are using to compute the centers(Lloyd(regular) or Kmeans++) The default is Lloyd's Kmeans.
            returns:
                None
            Stores following attributes:
                self.centroids : centroids obtained by kmeans clustering (n_cluster X D numpy array)
                self.centroid_labels : labels of each centroid obtained by
                    majority voting (N,) numpy array)

         def predict(self, x):

            Predict function
            params:
                x - N X D size  numpy array
            returns:
                predicted labels - numpy array of size (N,)

```

### 1.4 Image compression with K-means
In this part, we will look at lossy image compression as an application of clustering. The idea is simply to treat each pixel of an image as a point $x_i$, then perform K-means algorithm to cluster these points, and finally replace each pixel with its centroid.

What you need to implement is to compress an image with K centroids given. Specifically, complete the
function **transform_image** in the file **kmeans.py**. You have to reduce the image pixels and size by replacing each RGB values with nearest code vectors based on Euclidean distance.

After your implementation, and after completing Kmeans class, when you run KmeansTest.py, you should be able to see an image compressed_baboon.png in the plots folder. You can see that this image is distorted as compared to the original baboon.tiff.

The ideal result should take about 35-40 iterations and the Mean Square Error should be less than 0.0098. It takes about 1-2 minutes to complete normally.

```
def transform_image(image, code_vectors):

        Quantize image using the code_vectors

        Return new image from the image by replacing each RGB value in image with nearest code vectors (nearest in euclidean distance sense)

        returns:
            numpy array of shape image.shape
```

```python
import numpy as np
```

```python
a = [40.25739993, -0.90848143]
b = [-40.11362305, 3.36270893]
```

```python
np.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2)
```

```python
!pwd
```

```python
import time
```

```python
!python kmeansTest.py
```

    [+] K-Means on Toy Dataset
    [+] K-Means Vanilla
    current obj is:21.005828715842064
    current obj is:11.775075840226158
    current obj is:7.798263032257955
    current obj is:5.196863304021689
    current obj is:1.7362525284421513
    current obj is:0.0
    finish at tieration:5
    time takes for init phase:4.410743713378906e-05
    time takes for assign phase:0.07600855827331543
    time takes for recenter phase:0.018338441848754883
    time takes for calculting phase:0.0002689361572265625
    [success] : kmeans clustering done on toy dataset
    Toy dataset K means clustering converged in 100 steps

    [+] K-Means Plus Plus
    [+] returning center for [450, 450] points: [105, 308, 433, 207, 21, 183, 353, 277, 92]
    current obj is:17.131773111149105
    current obj is:0.03837134352341161
    current obj is:0.0
    finish at tieration:2
    time takes for init phase:1.0967254638671875e-05
    time takes for assign phase:0.022047758102416992
    time takes for recenter phase:0.005051136016845703
    time takes for calculting phase:8.893013000488281e-05
    [success] : kmeans clustering done on toy dataset
    Toy dataset K means clustering converged in 100 steps

    [+] K-Means Classification
    [+] K-Means Vanilla
    current obj is:19.032172655325788
    current obj is:1.9431216658063568
    current obj is:0.6370326823068089
    current obj is:0.28907528333436705
    current obj is:0.07965732382552201
    current obj is:0.07649490073191392
    current obj is:0.0722215469749252
    current obj is:0.03875062012088978
    current obj is:0.06301439148451696
    finish at tieration:8
    time takes for init phase:0.00018095970153808594
    time takes for assign phase:0.4124763011932373
    time takes for recenter phase:0.07694768905639648
    time takes for calculting phase:0.0004820823669433594
    [*] Prediction accuracy of K-means classifier with 10 cluster is 0.7222222222222222

    [+] K-Means Plus Plus
    [+] returning center for [1347, 1347] points: [1126, 723, 204, 1139, 1112, 1111, 1110, 1109, 928, 941]
    current obj is:21.640707654161865
    current obj is:1.3674124715725702
    current obj is:0.7560187440116068
    current obj is:0.3333613690466702
    current obj is:0.20685348761320696
    current obj is:0.11875970020033064
    current obj is:0.12751659131413587
    finish at tieration:6
    time takes for init phase:0.015789031982421875
    time takes for assign phase:0.39307188987731934
    time takes for recenter phase:0.08215069770812988
    time takes for calculting phase:0.0005495548248291016
    [*] Prediction accuracy of K-means classifier with 10 cluster is 0.6711111111111111
    [*] Accuracy of logistic regression classifier is 0.9622222222222222
    [*] Accuracy of Nearest Neighbour classifier is 0.9933333333333333

```python
#a = [1,2,3]
b = [4,5,6]
np.true_divide(b, 4)
```

```python
a = []
a[1]+=1
a[1]
```

```python
a = [1,2,10,0]
print(3 in a)
```

```python
a = np.array([[1,2,3],[2,3,4]])
b = np.array([[4,5,6],[7,8,9]])
#a**2 + b**2 - 2*a*b
np.sum(a**2 + b**2 - 2*a*b)
```

    102

```python
np.argmin(np.sum(a**2 + b**2 - 2* a* b,axis=1))
```

```python
a = [1,1,1]
np.argmin(a)
```

```python
x = {1: 2, 3: 4, 4: 3, 2: 1, 0: 0}
sorted_x = sorted(x.items(), key=lambda kv: kv[1])
```

```python
sorted_x
```

```python
a = {1:2}
len(a)
```

```python
import time
```

```python
start = time.time()
a = np.array([ 0,1])
b = np.array([[ 3, 4],[5,6]])
# c = a
print(min(np.sum(a**2 + b**2 - 2*a*b,axis=1)))
#cur_distance = -1
end = time.time()
print(end-start)
```

```python
start = time.time()
# a = np.array([1,2,3])
# #b = np.array([3])
# # np.linalg.norm(a - b)
# #print(np.true_divide(a,b))
# c = np.array(a)
cur_distance = float('inf')
end = time.time()
print(end-start)
```

```python
import numpy as np
```

```python
a = [[[0.64313725, 0.58823529, 0.27843137],
  [0.24705882, 0.22352941, 0.12156863],
  [0.29411765, 0.16862745, 0.03921569],
  [0.45882353, 0.46666667, 0.26666667],
  [0.55294118, 0.66666667, 0.39607843],
  [0.70196078, 0.7372549,  0.4627451 ]],
[[0.03529412, 0.04313725, 0.04705882],
  [0.03921569, 0.04705882, 0.04313725],
  [0.04313725, 0.05882353, 0.04705882],
  [0.01960784, 0.03137255, 0.01960784],
  [0.00784314, 0.01960784, 0.        ],
  [0.01568627, 0.01960784, 0.00784314]]]
# print(np.array(a).shape)

M,N = np.array(a).shape

for i in range(2):
    for j in range(6):
        print(a[i][j])
```

    (2, 6, 3)
    [0.64313725, 0.58823529, 0.27843137]
    [0.24705882, 0.22352941, 0.12156863]
    [0.29411765, 0.16862745, 0.03921569]
    [0.45882353, 0.46666667, 0.26666667]
    [0.55294118, 0.66666667, 0.39607843]
    [0.70196078, 0.7372549, 0.4627451]
    [0.03529412, 0.04313725, 0.04705882]
    [0.03921569, 0.04705882, 0.04313725]
    [0.04313725, 0.05882353, 0.04705882]
    [0.01960784, 0.03137255, 0.01960784]
    [0.00784314, 0.01960784, 0.0]
    [0.01568627, 0.01960784, 0.00784314]

```python
[ 9.25739993 -0.90848143]
[[ 1.46803737  8.83069153]
 [-8.33991861 -2.98825783]
 [-4.49239055 -8.02316563]
 [ 6.81286498 -5.80567293]
 [-4.63815699  7.82263388]
 [ 6.9738859   5.73474059]
 [ 9.00454882  0.12461644]
 [ 1.66467893 -9.12247052]
 [-8.25184968  3.16991064]]
[155.52565903 313.99108957 239.6754694   29.95823571 269.31887645
  49.34683509   1.13122489 125.11902879 323.20710352]
```

```python
a = 9.25739993
b = 1.46803737

c = -0.90848143
d = 8.83069153
(a-b)**2 + (c-d) **2

```

    155.5256590359249

```python

```


# Non Linear Controller

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

%load_ext autoreload
%autoreload 2

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

from drone import Drone2D

import trajectories2
import simulate
import plotting2

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

#### TODO - Implement 2D controller with NON-LINEAR control equations.

The non-linear equations of motion are shown below.

$$\begin{align}
\ddot{z} &= g - \frac{u_1}{m}\cos\phi
\\
\\
\ddot{y} &= \frac{u_1}{m}\sin\phi
\\
\\
\ddot{\phi} &= \frac{u_2}{I_x}
\end{align}$$

These equations can be solved for $u_1$, $\phi_{\text{command}}$, and $u_2$.

$$\begin{align}
u_1 &= \frac{m(g - \bar{u_1})}{\cos\phi}
\\
\\
\phi_{\text{command}} &= \sin^{-1}\left(\frac{m\ddot{y}_{\text{target}}}{u_1}\right)
\\
\\
u_2 &= I_x \bar{u}_2
\end{align}$$

The first equation will be useful when implementing `altitude_controller`, the second when implementing `lateral_controller`, and the third when implementing `altitude_controller`.

Note that $\ddot{y}_{\text{target}}$ is like $\bar{u}_1$ or $\bar{u}_2$. It comes from PD control on the controller's inputs.

```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

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

        # TODO (recommended to do AFTER attitude)
        #   Implement feedforward PD control to calculate
        #   u_1_bar and then use the non-linear math from above
        #   to transform u_1_bar into u_1 and then return u_1
        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) / np.cos(phi_actual)

        return u_1

    def lateral_controller(self,
                        y_target,
                        y_actual,
                        y_dot_target,
                        y_dot_actual,
                        u_1,
                        y_dot_dot_ff=0.0,
                        ):

        # TODO (recommended to do AFTER attitude)
        #   Implement feedforward PD control to calculate
        #   y_dot_dot_target and then use the non-linear math from above
        #   to transform y_dot_dot_target into phi_commanded
        #   and then return phi_commanded
        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
        y_dot_dot_target = p_term + d_term + y_dot_dot_ff
        phi_commanded = np.arcsin(self.m * y_dot_dot_target / u_1)
        return phi_commanded

    def attitude_controller(self,
                        phi_target,
                        phi_actual,
                        phi_dot_actual,
                        phi_dot_target=0.0
                        ):
        # TODO (recommended to do FIRST)
        #   Implement PD control to calculate u_2_bar
        #   and then use the linear math from above to
        #   transform u_2_bar into u_2 and then return u_2
        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_bar = p_term + d_term
        u_2 = self.I_x * u_2_bar
        return u_2
```

The flight path we'll use to test our controller is a figure 8 described as follows:
$$
\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
# TESTING CELL
#
# Note - this cell will only give nice output when your code
#  is working AND you've tuned your parameters correctly.
#  you might find it helpful to come up with a strategy
#  for first testing the inner loop controller and THEN
#  testing the outer loop.
#
# Run this cell when you think your controller is ready!
#
# You'll have to tune the controller gains to get good results.

#### CONTROLLER GAINS (TUNE THESE) ######

z_k_p   = 0.1
z_k_d   = 1.0
y_k_p   = 1.0
y_k_d   = 1.0
phi_k_p = 100.0
phi_k_d = 10.0

#########################################

drone = Drone2D()

# INSTANTIATE CONTROLLER
non_linear_controller = NonLinearCascadingController(
    drone.m,
    drone.I_x,
    z_k_p=z_k_p,
    z_k_d=z_k_d,
    y_k_p=y_k_p,
    y_k_d=y_k_d,
    phi_k_p=phi_k_p,
    phi_k_d=phi_k_d
)

# TRAJECTORY PARAMETERS (you don't need to change these)
total_time = 30.0
omega_z = 1.0       # angular frequency of figure 8

# GENERATE FIGURE 8
z_traj, y_traj, t = trajectories2.figure_8(omega_z, total_time, dt=0.02)
z_path, z_dot_path, z_dot_dot_path = z_traj
y_path, y_dot_path, y_dot_dot_path = y_traj

# SIMULATE MOTION
linear_history     = simulate.zy_flight(z_traj,
                                        y_traj,
                                        t,
                                        non_linear_controller,
                                        inner_loop_speed_up=10)
# PLOT RESULTS
pylab.rcParams['figure.figsize'] = 10,10
plotting2.plot_zy_flight_path(z_path, y_path, linear_history)
```