```python
import sys
sys.path.append('python/')

import time
startTime = time.time()      # Calculate time for running this notebook

import numpy as np
import matplotlib.pyplot as plt
import load_galaxies as lg   # Load load_galaxies.py library
```

_Python help: Running the notebook the first time, make sure to run all cells to be able to make changes in the notebook. Hit Shift+Enter to run the cell or click on the top menu: Kernel > Restart & Run All > Restart and Run All Cells to rerun the whole notebook. If you make any changes in a cell, rerun that cell._

# Measured data plotting

In this notebook you can load radial velocity measurements of multiple galaxies from a prepared Python library. Plot them all in a single graph so you can compare them with each other.

Plotting these measurements is the first step of producing a rotation curve for a galaxy and an indication of how much mass the galaxy contains. Setting Newton's law of gravitation and the circular motion equation equal to each other, you can derive the equation for circular velocity in terms of enclosed mass and radius:

\begin{equation}
v(r) = \sqrt{\frac{G M_{enc}(r)}{r}}
\end{equation}

>where:

        $G$ = gravitational constant

        $M_{enc}(r)$ = enclosed mass as a function of radius

        $r$ = radius or distance from the center of the galaxy

Knowing the radial velocity of stars at different radii, you can estimate the mass that is enclosed in that radius. By measuring the brightnesses (photometric profile) of stars and the amount of gas, you can approximate the mass of "visible" matter. Compare it with the actual mass calculated from the radial velocities to get an idea of how much mass is "missing". The result is a ratio (mass-to-light ratio or M/L) that has been useful to describe the amount of dark matter in galaxies.

### Vocabulary
__Rotation curve__: velocity of stars and gas at distances from the center of the galaxy and plotted as a curve

__Radial velocity__: how fast stars and gas are moving at different distances from the center of the galaxy

__NGC__: New General Catalogue of galaxies

__UGC__: Uppsala General Catalogue of galaxies

__kpc__: kiloparsec: 1 kpc = 3262 light years = 3.086e+19 meters = 1.917e+16 miles

## Load data of multiple galaxies

Load the radii, velocities, and errors in velocities of multiple galaxies from a Python library.

```python
# NGC 5533
r_NGC5533, v_NGC5533, v_err_NGC5533 = lg.NGC5533['m_radii'],lg.NGC5533['m_velocities'],lg.NGC5533['m_v_errors']

# NGC 891
r_NGC0891, v_NGC0891, v_err_NGC0891 = lg.NGC0891['m_radii'],lg.NGC0891['m_velocities'],lg.NGC0891['m_v_errors']

# NGC 7814
r_NGC7814, v_NGC7814, v_err_NGC7814 = lg.NGC7814['m_radii'],lg.NGC7814['m_velocities'],lg.NGC7814['m_v_errors']

# NGC 5005
r_NGC5005, v_NGC5005, v_err_NGC5005 = lg.NGC5005['m_radii'],lg.NGC5005['m_velocities'],lg.NGC5005['m_v_errors']

# NGC 3198
r_NGC3198, v_NGC3198, v_err_NGC3198 = lg.NGC3198['m_radii'],lg.NGC3198['m_velocities'],lg.NGC3198['m_v_errors']

# UGC 89
#r_UGC89, v_UGC89, v_err_UGC89 = lg.UGC89['m_radii'],lg.UGC89['m_velocities'],lg.UGC89['m_v_errors']

# UGC 477
r_UGC477, v_UGC477, v_err_UGC477 = lg.UGC477['m_radii'],lg.UGC477['m_velocities'],lg.UGC477['m_v_errors']

# UGC 1281
r_UGC1281, v_UGC1281, v_err_UGC1281 = lg.UGC1281['m_radii'],lg.UGC1281['m_velocities'],lg.UGC1281['m_v_errors']

# UGC 1437
r_UGC1437, v_UGC1437, v_err_UGC1437 = lg.UGC1437['m_radii'],lg.UGC1437['m_velocities'],lg.UGC1437['m_v_errors']

# UGC 2953
r_UGC2953, v_UGC2953, v_err_UGC2953 = lg.UGC2953['m_radii'],lg.UGC2953['m_velocities'],lg.UGC2953['m_v_errors']

# UGC 4325
r_UGC4325, v_UGC4325, v_err_UGC4325 = lg.UGC4325['m_radii'],lg.UGC4325['m_velocities'],lg.UGC4325['m_v_errors']

# UGC 5253
r_UGC5253, v_UGC5253, v_err_UGC5253 = lg.UGC5253['m_radii'],lg.UGC5253['m_velocities'],lg.UGC5253['m_v_errors']

# UGC 6787
r_UGC6787, v_UGC6787, v_err_UGC6787 = lg.UGC6787['m_radii'],lg.UGC6787['m_velocities'],lg.UGC6787['m_v_errors']

# UGC 10075
r_UGC10075, v_UGC10075, v_err_UGC10075 = lg.UGC10075['m_radii'],lg.UGC10075['m_velocities'],lg.UGC10075['m_v_errors']
```

## Plot measured data with errorbars

Measured data points of 13 galaxies are plotted below.

1. __Change the limits of the x-axis to zoom in and out of the graph.__

_Python help: change the limits of the x-axis by modifying the two numbers (left and right) of the line: plt.xlim then rerun the notebook or the cell._

2. __Finding supermassive black holes:__ A high velocity at a radius close to zero (close to the center of the galaxy) indicates that there is a supermassive black hole is present at the center of that galaxy, changing the velocities of the close-by stars only. The reason the black hole does not have that much effect on the motion of stars at larger distances is because it acts as a point mass with negligible radius and the velocity drops off as $1 / \sqrt r$.

__Can you find the galaxies with a possible central supermassive black hole and hide the curves of the rest of the galaxies?__

_Python help: Turn off the display of all lines and go through them one by one. You can "turn off" the display of each galaxy by typing a # sign in front of the line "plt.errorbar"._

_Sneak peak: In the Interactive_\__Rotation_\__Curve_\__Plotting notebook you will be able to find out which stars are affected by the central supermassive black hole, or in other words, what we mean by "close-by" stars._

3. __What do you notice about the size of the error bars at radii close to the center and far from the center? What could be the reason?__

```python
# Define radius for plotting
r = np.linspace(0,100,100)

# Plot
plt.figure(figsize=(10.0,7.0))                                     # size of the plot
plt.title('Measured data of multiple galaxies', fontsize=14)      # giving the plot a title
plt.xlabel('Radius (kpc)', fontsize=12)                           # labeling the x-axis
plt.ylabel('Velocity (km/s)', fontsize=12)                        # labeling the y-axis
plt.xlim(0,20)                                                    # limits of the x-axis (default from 0 to 20 kpc)
plt.ylim(0,420)                                                   # limits of the y-axis (default from 0 to 420 km/s)

# Plotting the measured data
plt.errorbar(r_NGC5533,v_NGC5533,yerr=v_err_NGC5533,    label='NGC 5533',  marker='o', markersize=6, linestyle='none', color='royalblue')
plt.errorbar(r_NGC0891,v_NGC0891,yerr=v_err_NGC0891,    label='NGC 891',   marker='o', markersize=6, linestyle='none', color='seagreen')
plt.errorbar(r_NGC7814,v_NGC7814,yerr=v_err_NGC7814,    label='NGC 7814',  marker='o', markersize=6, linestyle='none', color='m')
plt.errorbar(r_NGC5005,v_NGC5005,yerr=v_err_NGC5005,    label='NGC 5005',  marker='o', markersize=6, linestyle='none', color='red')
plt.errorbar(r_NGC3198,v_NGC3198,yerr=v_err_NGC3198,    label='NGC 3198',  marker='o', markersize=6, linestyle='none', color='gold')
plt.errorbar(r_UGC477,v_UGC477,yerr=v_err_UGC477,       label='UGC 477',   marker='o', markersize=6, linestyle='none', color='lightpink')
plt.errorbar(r_UGC1281,v_UGC1281,yerr=v_err_UGC1281,    label='UGC 1281',  marker='o', markersize=6, linestyle='none', color='aquamarine')
plt.errorbar(r_UGC1437,v_UGC1437,yerr=v_err_UGC1437,    label='UGC 1437',  marker='o', markersize=6, linestyle='none', color='peru')
plt.errorbar(r_UGC2953,v_UGC2953,yerr=v_err_UGC2953,    label='UGC 2953',  marker='o', markersize=6, linestyle='none', color='lightslategrey')
plt.errorbar(r_UGC4325,v_UGC4325,yerr=v_err_UGC4325,    label='UGC 4325',  marker='o', markersize=6, linestyle='none', color='darkorange')
plt.errorbar(r_UGC5253,v_UGC5253,yerr=v_err_UGC5253,    label='UGC 5253',  marker='o', markersize=6, linestyle='none', color='maroon')
plt.errorbar(r_UGC6787,v_UGC6787,yerr=v_err_UGC6787,    label='UGC 6787',  marker='o', markersize=6, linestyle='none', color='midnightblue')
plt.errorbar(r_UGC10075,v_UGC10075,yerr=v_err_UGC10075, label='UGC 10075', marker='o', markersize=6, linestyle='none', color='y')

plt.legend(loc='upper right')
plt.show()
```

```python
# Time
executionTime = (time.time() - startTime)
ttt=executionTime/60
print(f'Execution time: {ttt:.2f} minutes')
```

    Execution time: 8.77 minutes

# References
>De Naray, Rachel Kuzio, Stacy S. McGaugh, W. J. G. De Blok, and A. Bosma. __"High-resolution optical velocity fields of 11 low surface brightness galaxies."__ The Astrophysical Journal Supplement Series 165, no. 2 (2006): 461. https://doi.org/10.1086/505345.

>De Naray, Rachel Kuzio, Stacy S. McGaugh, and W. J. G. De Blok. __"Mass models for low surface brightness galaxies with high-resolution optical velocity fields."__ The Astrophysical Journal 676, no. 2 (2008): 920. https://doi.org/10.1086/527543.

>Epinat, B., P. Amram, M. Marcelin, C. Balkowski, O. Daigle, O. Hernandez, L. Chemin, C. Carignan, J.-L. Gach, and P. Balard. __“GHASP: An Hα Kinematic Survey of Spiral and Irregular GALAXIES – Vi. New HΑ Data Cubes for 108 Galaxies.”__ Monthly Notices of the Royal Astronomical Society 388, no. 2 (July 19, 2008): 500–550. https://doi.org/10.1111/j.1365-2966.2008.13422.x.

>Fraternali, F., R. Sancisi, and P. Kamphuis. __“A Tale of Two Galaxies: Light and Mass in NGC 891 and NGC 7814.”__ Astronomy & Astrophysics 531 (June 13, 2011). https://doi.org/10.1051/0004-6361/201116634.

>Karukes, E. V., P. Salucci, and Gianfranco Gentile. __"The dark matter distribution in the spiral NGC 3198 out to 0.22 $R_{vir}$."__ _Astronomy & Astrophysics_ 578 (2015): A13. https://doi.org/10.1051/0004-6361/201425339.

>Lelli, Federico, ed. __“Spitzer Photometry and Accurate Rotation Curves.”__ SPARC, May 29, 2020. http://astroweb.cwru.edu/SPARC/.

>Noordermeer, Edo. __"The rotation curves of flattened Sérsic bulges."__ _Monthly Notices of the Royal Astronomical Society_ 385, no. 3 (2007): 1359-1364. https://doi.org/10.1111/j.1365-2966.2008.12837.x

>Richards, Emily E., L. van Zee, K. L. Barnes, S. Staudaher, D. A. Dale, T. T. Braun, D. C. Wavle, et al. __“Baryonic Distributions in the Dark Matter Halo of NGC 5005.”__ Monthly Notices of the Royal Astronomical Society 449, no. 4 (June 1, 2015): 3981–96. https://doi.org/10.1093/mnras/stv568.
***

```python

```


```python
import numpy as np
import matplotlib.pyplot as plt
```

# Compress Oil Flow Data using Probabilistic PCA
## 3 Phase Data
### Description
This is synthetic data modelling non-intrusive measurements on a pipe-line transporting a mixture of oil, water and gas. The flow in the pipe takes one out of three possible configurations: horizontally stratified, nested annular or homogeneous mixture flow. The data lives in a 12-dimensional measurement space, but for each configuration, there is only two degrees of freedom: the fraction of water and the fraction of oil. (The fraction of gas is redundant, since the three fractions must sum to one.) Hence, the data lives on a number of ‘sheets’ which locally are approximately 2-dimensional.

### Details
The files/variables contain:
1. Oilflow.txt contains 1000 data points,each data point is represented by a 12 dimensional vector.
2. Oilflow_label.txt contains the configuration of each data point represented as one-hot vector: (1, 0, 0) horizontally stratified, (0, 1, 0) nested annular and (0, 0, 1) homogeneous mixture.

source: adapted from https://github.com/anirudhseth/GPLVM-and-PCA/blob/master/Datasets

Let's first import the data and configurations into x and x_label respectively. Also create lists of colors for further visualization (different colors indicates different configurations)

```python
x = np.loadtxt('./data/Oilflow.txt')[..., np.newaxis]
x_label = np.loadtxt('./data/Oilflow_label.txt')
x_label = np.argmax(x_label, axis=1)

color = []
color_edge = []
shape = []
for i in range(x.shape[0]):
    if x_label[i] == 0:
        color += ['pink']
        color_edge += ['orange']
    elif x_label[i] == 1:
        color += ['green']
        color_edge += ['black']
    elif x_label[i] == 2:
        color += ['blue']
        color_edge += ['purple']
```

Denote the dimension of each data point as d_dim (12) and define the dimension of the latent space as 2. Denote the n_dim as the number of data points.

```python
d_dim = x.shape[-2]
z_dim = 2
n_dim = x.shape[0]
```

Assume $p(\mathbf{z}_i) = \mathcal{N}(\mathbf{0}, \mathbf{I})$ and $ \mathbf{x}_i = \mathbf{W}\mathbf{z}_i + \boldsymbol{\mu} + \boldsymbol{\epsilon}_i$ where $\boldsymbol{\epsilon}_i \sim \mathcal{N}(\mathbf{0}, \sigma^2\mathbf{I})$.

We are going to use EM algorithm to optimize parameters $\mathbf{W}, \boldsymbol{\mu}$ and $\sigma^2$.

Denote $\mathbf{W}$ as W, $\boldsymbol{\mu}$ as mu and $\sigma^2$ as sigma_2.

We iterate EM via following update equations

### E-step
We assign $q(\mathbf{z}_n) = p(\mathbf{z}_n|\mathbf{x},\mathbf{W}_{old},\boldsymbol{\mu}_{old},\sigma^2_{old})$

We evaluate
\begin{align}
		    \mathbb{E}_{q(\mathbf{z}_n)}[\mathbf{z}_n] = \mathbf{M}^{-1}\mathbf{W}^T_{old}(\mathbf{x}_n-\boldsymbol{\mu}_M)
\end{align} where $\boldsymbol{\mu}_M = \frac{1}{N}\sum_{n=1}^N \mathbf{x}_n = \overline{\mathbf{x}}$

and
\begin{align}
			\mathbb{E}_{q(\mathbf{z}_n)}[\mathbf{z}_n\mathbf{z}_n^T] &= Cov(\mathbf{z}_n,\mathbf{z}_n) + \mathbb{E}_{q(\mathbf{z}_n)}[\mathbf{z}_n]\mathbb{E}_{q(\mathbf{z}_n)}[\mathbf{z}_n]^T\\
			&=\sigma^2\mathbf{M}^{-1}+\mathbb{E}_{q(\mathbf{z}_n)}[\mathbf{z}_n]\mathbb{E}_{q(\mathbf{z}_n)}[\mathbf{z}_n]^T
\end{align}

where $\mathbf{M} =\mathbf{W}_{old}^T\mathbf{W}_{old}+\sigma_{old}^2\mathbf{I}$

```python
def E_step(W, sigma_2, mu):
    # a @ b = np.matmul(a,b); a * b is element-wise multiplication
    W_T = W.transpose((0, -1, -2))
    M = W_T @ W + sigma_2 * np.eye(z_dim)
    M_inv = np.linalg.inv(M)

    # compute E[z_n] as post_mean and E[z_n]^T @ E[z_n] as post_mean_dot
    post_mean = (M_inv @ W_T) @ (x-mu)
    post_mean_T = post_mean.transpose((0, -1, -2))
    post_cov = sigma_2 * M_inv

    post_mean_dot = post_cov + post_mean @ post_mean_T
    return post_mean, post_mean_dot
```

### M-step
Given the $\mathbb{E}_{q(\mathbf{z}_n)}[\mathbf{z}_n]$ and $\mathbb{E}_{q(\mathbf{z}_n)}[\mathbf{z}_n\mathbf{z}_n^T]$ computed in the E-step, we update the model parameters $\mathbf{W}$ and $\sigma^2$ (The $\boldsymbol{\mu}_M$ is computed as mu$=\frac{1}{N}\sum_{n=1}^N \mathbf{x}_n$).
\begin{align}
			\boldsymbol{\mu}_M &= \overline{\mathbf{x}} =\frac{1}{N}\sum_{n=1}^N \mathbf{x}_n \\
			\mathbf{W}_{new}&=\Big[\sum_{n=1}^{N}(\mathbf{x}_n-\boldsymbol{\mu}_M)\mathbb{E}_{q(\mathbf{z}_n)}[\mathbf{z}_n]^T \Big]\Big[\sum_{n=1}^{N}\mathbb{E}_{q(\mathbf{z}_n)}[\mathbf{z}_n\mathbf{z}_n^T]  \Big]^{-1}\\
			\sigma^2_{new} &= \frac{1}{ND}\sum_{n=1}^{N} \Big[||\mathbf{x}_n-\boldsymbol{\mu}_M||^2 + trace\big[\mathbb{E}_{q(\mathbf{z}_n)}[\mathbf{z}_n\mathbf{z}_n^T]\mathbf{W}^T\mathbf{W}\big] - 2\mathbb{E}_{q(\mathbf{z}_n)} [\mathbf{z}_n]^T\mathbf{W}^T(\mathbf{x}_n-\boldsymbol{\mu}_M)\Big]
		\end{align}

```python
def M_step(x, W, sigma_2, mu, post_mean, post_mean_dot):
    post_mean_T = post_mean.transpose((0, -1, -2))
    W = ((x-mu) @ post_mean_T).sum(axis=0, keepdims=True) @ np.linalg.inv(post_mean_dot.sum(axis=0, keepdims=True))
    W_T = W.transpose((0, -1, -2))
    sigma_2 = (((x - mu) ** 2).sum(axis=-2)
               + np.trace(post_mean_dot @ W_T @ W, axis1=-2, axis2=-1)[..., np.newaxis]
               - (2 * post_mean_T @ W_T @ (x - mu)).squeeze(-1)
               ).sum(axis=0) / (d_dim * n_dim)
    return W, sigma_2
```

Put everythin together, we can learn the model parameters and visualize the learnt 2-dimensional latent space.

```python
# initialize parameters
# x = wz + mu
W = np.random.rand(1, d_dim, z_dim)
W_T = W.transpose((0, -1, -2))
mu = np.mean(x, axis=0)

# sigma square
sigma_2 = np.exp2(np.random.rand(1))

# stop precision
epsilon = 1.0
# maximum iterations
max_itr = 100
# iteration count
itr = 0
# plotting frequency
plot_freq = 1

while epsilon > 0.001 and itr < max_itr:
    # E-step
    post_mean, post_mean_dot = E_step(W, sigma_2, mu)

    # M-step
    # save old W and sigma_2 for computing stop criterion
    _W = W.copy()
    _sigma_2 = sigma_2.copy()

    W, sigma_2 = M_step(x, W, sigma_2, mu, post_mean, post_mean_dot)

    epsilon = np.sqrt(np.mean((W-_W)**2) + np.mean((sigma_2-_sigma_2)**2))
    itr += 1

    # evaluate log-likelihood of original data log p(x) as log_px
    C = W @ W_T + sigma_2 * np.eye(d_dim)
    log_px = -0.5 * n_dim * np.log(np.linalg.det(C)) - 0.5 * ((x - mu).transpose((0, -1, -2)) @ np.linalg.inv(C) @ (x-mu)).sum(axis=0).squeeze(-1)

    print(f'itration:{itr}, log-likelihood:{log_px.item()}, epsilon:{epsilon}')

    # plot the z space
    if itr % plot_freq == 0:
        plt.scatter(post_mean[:, 0, 0], post_mean[:, 1, 0], c=color,
                    linewidths=2,
                    marker="s",
                    edgecolor=color_edge,
                    s=30)

        plt.xlabel("1-st dimension of the latent space")
        plt.ylabel("2-nd dimension of the latent space")
        plt.pause(3)
```

```python

```

```python

```


# Probabilistic Programming 4: Gaussian dynamical systems
## Variational inference

### Preliminaries

- Goal
  - Learn to apply ForneyLab to a dynamical system.
- Materials
  - Mandatory
    - These lecture notes.
  - Optional
      - [Probabilistic Programming notebook](https://github.com/bertdv/BMLIP/tree/master/lessons/notebooks/probprog/Probabilistic-Programming.ipynb)
      - David Blei (https://doi.org/10.1146/annurev-statistics-022513-115657)

```julia
# Package managing
using Pkg
Pkg.activate("workspace")
Pkg.instantiate()

include("../scripts/pp-4.jl")

using Logging; disable_logging(LogLevel(0))
using LinearAlgebra
using LaTeXStrings
using ForneyLab
using Plots
pyplot();
```

## Data generation

In this notebook, we will be considering continuous state-space models. We will focus on the simplest form: the Gaussian dynamical system:

$$\begin{align}
x_k =&\ A x_{k-1} + \epsilon_{k} \\
y_k =&\ B x_k + \eta_k \, ,
\end{align}$$

where $x_k \in \mathbb{R}^{N}$ is the state, $y_k \in \mathbb{R}^{M}$ is the measurement. Process noise and measurement noise are assumed to be zero mean Gaussian with covariance matrices $Q$ and $R$ respectively.

We'll generate the data in the same manner as the HMM setting. We start with an initial state $x_0$, apply the state transition $Ax_0$, and add Gaussian noise $\epsilon_1$. We then take the current state $x_1$, apply the emission transformation $B x_1$ and add Gaussian noise again $\eta_1$. This is repeated until we've reached $T$ observations.

```julia
# Dimensionalities
N = 2
M = 2

# Length of time-series
T = 50

# Lenght of time step
Δt = 1.0

# Transition matrix of latent variables
transition = [1.0 Δt;
              0.0 1.0]

# Emission matrix for observed variables
emission = [1.0 0.;0. 1.0]

# Process noise (latent variables)
process_noise = [0.2*Δt 0.;0. 0.1*Δt]

# Measurement noise (observations)
measurement_noise = [0.1 0.;0. 0.1]

# Preallocate data arrays
states = zeros(N, T)
observations = zeros(M, T)

# Initial state
state_0 = [0., 0.]

# Keep previous state in memory
state_tmin = state_0

# Generate data for entire time-series
for t = 1:T

    # Transition from previous state
    states[:,t] = sample(ProbabilityDistribution(Multivariate, GaussianMeanVariance, m=transition * state_tmin, v=process_noise))

    # Emission of current state
    observations[:,t] = sample(ProbabilityDistribution(Multivariate, GaussianMeanVariance, m=emission * states[:,t], v=measurement_noise))

    # Update previous state
    state_tmin = states[:,t]

end

# Visualization of states
plot(states[1,:], states[2,:], color="red", label="states", grid=false, xlabel="position x-coordinate", ylabel="position y-coordinate")
scatter!(observations[1,:], observations[2,:], color="blue", label="observations")
```

Since we don't have a time-axis anymore, it is hard to see how the process evolves. Animating the path is helpful.

```julia
"Animation of cart's path"

# Plot initial state
plot([states[1,1]], [states[2,1]],
           color="red",
           label="states",
           grid=false,
           xlabel="position x-coordinate",
           ylabel="position y-coordinate",
           xlims=[-60,60],
           ylims=[-10,10])

# Plot initial observation
scatter!([observations[1,1]], [observations[2,1]], color="blue", label="observations")

anim = @animate for t = 2:T

    title!("t = "*string(t))

    # Plot new state
    plot!([states[1,t-1:t]], [states[2,t-1:t]], color="red", label="")

    # Plot new observation
    scatter!([observations[1,t]], [observations[2,t]], color="blue", label="")

end

gif(anim, "example-GDS.gif", fps = 3)
```

## Model specification

The Bayesian formulation of the LGDS state-space model takes the following form:

$$\begin{align}
p(x_k \mid x_{k-1}) =&\ \mathcal{N}(x_k \mid A x_{k-1}, Q)\\
p(y_k \mid x_k) =&\ \mathcal{N}(y_k \mid B x_k, R) \, .
\end{align}$$

We can easily specify the states and observations as multivariate Gaussian distributions. If we do not know the process and measurement noise matrices, we can add Wishart priors to them. Similarly, if we don't know the transition and emission matrices, we can priors for them as well. Since these can take on negative values, we would specify them as multivariate Gaussians;

$$\begin{align}
p(Q) \sim&\ \mathcal{W}(V_Q, n_Q) \\
p(R) \sim&\ \mathcal{W}(V_R, n_R) \\
p(A) \sim&\ \mathcal{N}(m_A, S_A) \\
p(B) \sim&\ \mathcal{N}(m_B, S_B) \, .
\end{align}$$

ForneyLab is designed for dynamical systems and excels when using Gaussians. It can do inference in a recursive, time-slice-based manner. That is, it performs a series of variational message passing iterations in each time-slice, moving forward through time.

### Model 1: state estimation

We will stick to state estimation for now. We will dynamically "fix" the previous states and observations, allowing us to focus the variational message passing procedure to the "current" state and observation in the time-slice.

```julia
# Initialize a graph
graph1 = FactorGraph()

# Preallocate variable arrays
x = Vector{Variable}(undef, T)
y = Vector{Variable}(undef, T)

# Prior for initial state
@RV x_0 ~ GaussianMeanPrecision([0., 0.], [1. 0.;0. 1.])

# Keep previous state in memory
x_tmin = x_0

# Iterate over time
for t = 1:T

    # State transition
    @RV ϵ_t ~ GaussianMeanVariance([0., 0.], process_noise)
    @RV x[t] = transition * x_tmin + ϵ_t
    x[t].id = :x_*t

    # State transition
    @RV η_t ~ GaussianMeanVariance([0., 0.], measurement_noise)
    @RV y[t] = emission * x[t] + η_t
    y[t].id = :x_*t

    # Tell FL that y is observed
    placeholder(y[t], :y, index=t, dims=(2,))

    # Update previous state
    x_tmin = x[t]
end
```

We'll now construct the algorithm and infer results.

```julia
# Number of optimization iterations
num_iterations = 10

# Define recognition distribution
q = RecognitionFactorization(x, ids=[:X])

# Generate variational algorithm
algo = variationalAlgorithm(q)

# Load algorithm
eval(Meta.parse(algo))

# Initialize recognition distribution marginals
marginals = Dict{Symbol, ProbabilityDistribution}()
for t = 1:T
    marginals[:x_*t] = vague(ForneyLab.GaussianMeanVariance, 2)
end

# Initialize data
data = Dict(:y => [observations[:,t] for t = 1:T])

# Run algorithm
for i = 1:num_iterations

    # Update states
    stepX!(data, marginals)

end;
```

Let's check whether everything went ok. We'll visualize the state estimations.

```julia
# Preallocation
states_estimated = zeros(2,T)
states_sem = zeros(2,T)

for t = 1:T

    # State estimates
    states_estimated[:,t] = mean(marginals[:x_*t])

    # Variance of the state estimates
    states_sem[:,t] = sqrt.(var(marginals[:x_*t]))
end

# Visualization
plot(states[1,:], states[2,:], color="red", label="states", grid=false)
plot!(states_estimated[1,:], states_estimated[2,:], ribbon=[states_sem, states_sem], fillalpha=0.2, color="green", label="inferred")
scatter!(observations[1,:], observations[2,:], color="blue", label="observations")
```

We're going to inspect some messages. Let's open up the algorithm. Let's look up the marginal for $x_1$. It will be the multiplication of two messages, one consisting of the state transition prediction and the other consisting of the measurement likelihood.

```julia
# Find index of marginal computation in algorithm string
ix = findfirst("marginals[:x_1]", algo)

# Print line at index
println(algo[ix[1]:ix[end]+42])
```

    marginals[:x_1] = messages[351].dist * messages[347].dist

We can re-run the inference algorithm such that we get an array of all messages in the graph.

```julia
# Re-initialize recognition distribution marginals
marginals = Dict{Symbol, ProbabilityDistribution}()
for t = 1:T
    marginals[:x_*t] = vague(ForneyLab.GaussianMeanVariance, 2)
end

# Initialize messages
messages = Vector{Message}(undef, 698)

# Re-run inference algorithm now including the message array
for i = 1:num_iterations

    # Update states
    stepX!(data, marginals, messages)

end;
```

Let's visualize the two messages and the resulting state marginal.

```julia
# Collect state transition prediction
m_statepred = convert(ProbabilityDistribution{Multivariate, GaussianMeanVariance}, messages[351].dist)

# Collect measurement likelihood
m_likelihood = convert(ProbabilityDistribution{Multivariate, GaussianMeanVariance}, messages[347].dist)

# Collect corrected prediction
state_marginal = convert(ProbabilityDistribution{Multivariate, GaussianMeanVariance}, marginals[:x_1])

# # Extract x-coordinates
m_statepred_x = ProbabilityDistribution(Univariate, GaussianMeanVariance, m=m_statepred.params[:m][1], v=m_statepred.params[:v][1,1])
m_likelihood_x = ProbabilityDistribution(Univariate, GaussianMeanVariance, m=m_likelihood.params[:m][1], v=m_likelihood.params[:v][1,1])
state_marginal_x = ProbabilityDistribution(Univariate, GaussianMeanVariance, m=state_marginal.params[:m][1], v=state_marginal.params[:v][1,1])

# Plot of the prediction, noisy measurement, and corrected prediction for x-coordinate
plot_messages(m_statepred_x, m_likelihood_x, state_marginal_x)
title!(L"Factors contributing to marginal of state $x_1$")
```

The state marginal is dominated by the observation, because the state prediction is based purely on a prior distribution. If we now look at the situation for $x_2$, things should be different.

```julia
# Collect state transition prediction
m_statepred = convert(ProbabilityDistribution{Multivariate, GaussianMeanVariance}, messages[357].dist)

# Collect measurement likelihood
m_likelihood = convert(ProbabilityDistribution{Multivariate, GaussianMeanVariance}, messages[343].dist)

# Collect corrected prediction
state_marginal = convert(ProbabilityDistribution{Multivariate, GaussianMeanVariance}, marginals[:x_2])

# # Extract x-coordinates
m_statepred_x = ProbabilityDistribution(Univariate, GaussianMeanVariance, m=m_statepred.params[:m][1], v=m_statepred.params[:v][1,1])
m_likelihood_x = ProbabilityDistribution(Univariate, GaussianMeanVariance, m=m_likelihood.params[:m][1], v=m_likelihood.params[:v][1,1])
state_marginal_x = ProbabilityDistribution(Univariate, GaussianMeanVariance, m=state_marginal.params[:m][1], v=state_marginal.params[:v][1,1])

# Plot of the prediction, noisy measurement, and corrected prediction for x-coordinate
plot_messages(m_statepred_x, m_likelihood_x, state_marginal_x)
title!(L"Factors contributing to marginal of state $x_2$")
```

Now, the state prediction has more influence on the actual state marginal.

### Model 2: estimate states and process noise

Weĺl now expand on the model. What if we don't know process noise? Can we estimate it simultaneously?

```julia
# Initialize a graph
graph2 = FactorGraph()

# Preallocate variable arrays
x = Vector{Variable}(undef, T)
y = Vector{Variable}(undef, T)

# Prior for process noise
@RV Q ~ Wishart([1. 0.;0. 1.], 2, id=:Q)

# Prior for initial state
@RV x_0 ~ GaussianMeanPrecision([0., 0.], [1. 0.;0. 1.])

# Keep previous state in memory
x_tmin = x_0

# Iterate over time
for t = 1:T

    # State transition
    @RV ϵ_t ~ GaussianMeanPrecision([0., 0.], Q, id=:ϵ_*t)
    @RV x[t] = transition * x_tmin + ϵ_t

    # State transition
    @RV η_t ~ GaussianMeanVariance([0., 0.], measurement_noise, id=:η_*t)
    @RV y[t] = emission * x[t] + η_t

    # Tell FL that y is observed
    placeholder(y[t], :y, index=t, dims=(2,))

    # Update previous state
    x_tmin = x[t]
end
```

We'll need to add a recognition factor for the process noise covariance matrix $Q$.

```julia
# Define recognition distribution
q = RecognitionFactorization(x, Q, ids=[:X, :Q])

# Generate variational algorithm
algo = variationalAlgorithm(q)

# Load algorithm
eval(Meta.parse(algo))

# Initialize recognition distribution marginals
marginals = Dict{Symbol, ProbabilityDistribution}()
marginals[:Q] = vague(ForneyLab.Wishart, 2)
for t = 1:T
    marginals[:x_*t] = vague(ForneyLab.GaussianMeanVariance, 2)
end

# Initialize data
data = Dict(:y => [observations[:,t] for t = 1:T])

# Run algorithm
for i = 1:num_iterations

    # Update states
    stepX!(data, marginals)

    # Update process noise estimate
    stepQ!(data, marginals)

end;
```

```julia
# Preallocation
states_estimated = zeros(2,T)
states_sem = zeros(2,T)

for t = 1:T

    # State estimates
    states_estimated[:,t] = mean(marginals[:x_*t])

    # Variance of the state estimates
    states_sem[:,t] = sqrt.(var(marginals[:x_*t]))
end

# Visualization
plot(states[1,:], states[2,:], color="red", label="states", grid=false)
plot!(states_estimated[1,:], states_estimated[2,:], ribbon=[states_sem, states_sem], fillalpha=0.2, color="green", label="inferred")
scatter!(observations[1,:], observations[2,:], color="blue", label="observations")
```

```julia
# Estimated process noise
pnoise_estimate = inv(mean(marginals[:Q]))

# Visualization
l = @layout [a b]
h1 = Plots.bar(0:4, reshape(process_noise, (4,)), label="true", ylims=[0., 1.])
h2 = Plots.bar(reshape(pnoise_estimate, (4,)), label="estimated", ylims=[0., 1.])
plot(h1, h2, layout=l, size=(1000,500))
```

### Model 3: estimate states, transition and process noise

We won't be estimating the transition matrix simultaneously. Multiplications of variables require the construction of a "composite node", which is a larger factor with internal variables integrated out and custom messages to external variables. It relates to the concept of 'drawing a box' around variables, as Bert discussed in his lecture on Factor Graphs. We won't be constructing composite nodes in this mini-course, but if you're interested you can ask us about it.

One of the PhD students in the lab has built an \emph{AutoRegressive} node, which is a composite node that buffers previous states and multiplies with a variable transition matrix. This node allows ForneyLab to run adaptive Kalman filters and adaptive auto-regressive models. We're using it for audio processing applications. This is the kind of research we do in BIASlab. If you're excited and curious, ask us about possible master research projects.

### Assignment: simultaneous process and measurement noise estimation

For theoretical reasons, it is difficult to estimate process and measurement noise simultaneously. To see this, in this assignment you're going to extend Model 2 to include measurement noise. What happens to the estimates when you change priors?

```julia
### Your code here
```


# Beam finite element matrices
Based on: \
[1] Neto, M. A., Amaro, A., Roseiro, L., Cirne, J., & Leal, R. (2015). Finite Element Method for Beams. In Engineering Computation of Structures: The Finite Element Method (pp. 115–156). Springer International Publishing. http://doi.org/10.1007/978-3-319-17710-6_4

The beam finite element, its nodes and degrees-of-freedom (DOFs) can be seen below:

```python
from sympy import *
init_printing()

def symb(x, y):
    return symbols('{0}_{1}'.format(x, y), type = float)

E, A, L, G, I_1, I_2, I_3, rho = symbols('E A L G I_1 I_2 I_3 rho', type = float)
```

The finite element matrices should have order 12, accounting for each of the element's DOFs, shown below:

```python
u = Matrix(12, 1, [symb('u', v + 1) for v in range(12)])
transpose(u)
```

$\displaystyle \left[\begin{array}{cccccccccccc}u_{1} & u_{2} & u_{3} & u_{4} & u_{5} & u_{6} & u_{7} & u_{8} & u_{9} & u_{10} & u_{11} & u_{12}\end{array}\right]$

## Axial deformation along $x_1$
In terms of generic coordinates $v_i$:

```python
v_a = Matrix(2, 1, [symb('v', v + 1) for v in range(2)])
transpose(v_a)
```

$\displaystyle \left[\begin{matrix}v_{1} & v_{2}\end{matrix}\right]$

which are equivalent to the $u_i$ coordinates in the following way:
$$
    \mathbf{v} = \mathbf{R} \mathbf{u},
$$
where:
$$
    v_1 = u_1, \\
    v_2 = u_7,
$$
with the following coordinate transformation matrix:

```python
R = zeros(12)
R[1 - 1, 1 - 1] = 1
R[2 - 1, 7 - 1] = 1
R[:2, :]
```

$\displaystyle \left[\begin{array}{cccccccccccc}1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\end{array}\right]$

### Stiffness matrix
Eq. (3.15) of [1]:

```python
K_a = (E * A / L) * Matrix([[1, -1],
                            [-1, 1]])
K_a
```

$\displaystyle \left[\begin{matrix}\frac{A E}{L} & - \frac{A E}{L}\\- \frac{A E}{L} & \frac{A E}{L}\end{matrix}\right]$

### Inertia matrix
Eq. (3.16) of [1]:

```python
M_a = (rho * A * L / 6) * Matrix([[2, 1],
                                  [1, 2]])
M_a
```

$\displaystyle \left[\begin{matrix}\frac{A L \rho}{3} & \frac{A L \rho}{6}\\\frac{A L \rho}{6} & \frac{A L \rho}{3}\end{matrix}\right]$

## Torsional deformation around $x_1$
According to [1], one can obtain the matrices for the torsional case from the axial case by replacing the elasticity modulus $E$ and the cross-sectional area $A$ by the shear modulus $G$ and the polar area moment of inertia $I_1$.

In terms of generic coordinates $v_i$:

```python
v_t = Matrix(2, 1, [symb('v', v + 3) for v in range(2)])
transpose(v_t)
```

$\displaystyle \left[\begin{matrix}v_{3} & v_{4}\end{matrix}\right]$

which are equivalent to the $u_i$ coordinates in the following way:
$$
    v_3 = u_4, \\
    v_4 = u_{10},
$$
with the following coordinate transformation matrix:

```python
R[3 - 1,  4 - 1] = 1
R[4 - 1, 10 - 1] = 1
R[0:4, :]
```

$\displaystyle \left[\begin{array}{cccccccccccc}1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\end{array}\right]$

### Stiffness matrix

```python
K_t = K_a.subs([(E, G), (A, I_1)])
K_t
```

$\displaystyle \left[\begin{matrix}\frac{G I_{1}}{L} & - \frac{G I_{1}}{L}\\- \frac{G I_{1}}{L} & \frac{G I_{1}}{L}\end{matrix}\right]$

### Inertia matrix

```python
M_t = M_a.subs([(E, G), (A, I_1)])
M_t
```

$\displaystyle \left[\begin{matrix}\frac{I_{1} L \rho}{3} & \frac{I_{1} L \rho}{6}\\\frac{I_{1} L \rho}{6} & \frac{I_{1} L \rho}{3}\end{matrix}\right]$

## Bending on the plane $x_1-x_3$
In this case the bending torsion occurs around the $x_2$ axis. In terms of generic coordinates $v_i$:

```python
v_b13 = Matrix(4, 1, [symb('v', v + 9) for v in range(4)])
transpose(v_b13)
```

$\displaystyle \left[\begin{matrix}v_{9} & v_{10} & v_{11} & v_{12}\end{matrix}\right]$

which are equivalent to the $u_i$ coordinates in the following way:
$$
    v_9    = u_3, \\
    v_{10} = u_5, \\
    v_{11} = u_9, \\
    v_{12} = u_{11},
$$
with the following coordinate transformation matrix:

```python
R[9  - 1,  3 - 1] = 1
R[10 - 1,  5 - 1] = 1
R[11 - 1,  9 - 1] = 1
R[12 - 1, 11 - 1] = 1
R
```

$\displaystyle \left[\begin{array}{cccccccccccc}1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\end{array}\right]$

### Stiffness matrix

```python
K_b13 = (E * I_2 / L**3) * Matrix([[ 12    ,  6 * L   , -12    ,  6 * L   ],
                                   [  6 * L,  4 * L**2, - 6 * L,  2 * L**2],
                                   [-12    , -6 * L   ,  12    , -6 * L   ],
                                   [  6 * L,  2 * L**2, - 6 * L,  4 * L**2]])

if (not K_b13.is_symmetric()):
    print('Error in K_b13.')
K_b13
```

$\displaystyle \left[\begin{matrix}\frac{12 E I_{2}}{L^{3}} & \frac{6 E I_{2}}{L^{2}} & - \frac{12 E I_{2}}{L^{3}} & \frac{6 E I_{2}}{L^{2}}\\\frac{6 E I_{2}}{L^{2}} & \frac{4 E I_{2}}{L} & - \frac{6 E I_{2}}{L^{2}} & \frac{2 E I_{2}}{L}\\- \frac{12 E I_{2}}{L^{3}} & - \frac{6 E I_{2}}{L^{2}} & \frac{12 E I_{2}}{L^{3}} & - \frac{6 E I_{2}}{L^{2}}\\\frac{6 E I_{2}}{L^{2}} & \frac{2 E I_{2}}{L} & - \frac{6 E I_{2}}{L^{2}} & \frac{4 E I_{2}}{L}\end{matrix}\right]$

### Inertia matrix

```python
M_b13 = (rho * L * A / 420) * Matrix([[ 156    ,  22 * L   ,   54    , -13 * L   ],
                                      [  22 * L,   4 * L**2,   13 * L, - 3 * L**2],
                                      [  54    ,  13 * L   ,  156    , -22 * L   ],
                                      [- 13 * L, - 3 * L**2, - 22 * L,   4 * L**2]])

if (not M_b13.is_symmetric()):
    print('Error in M_b13.')
M_b13
```

$\displaystyle \left[\begin{matrix}\frac{13 A L \rho}{35} & \frac{11 A L^{2} \rho}{210} & \frac{9 A L \rho}{70} & - \frac{13 A L^{2} \rho}{420}\\\frac{11 A L^{2} \rho}{210} & \frac{A L^{3} \rho}{105} & \frac{13 A L^{2} \rho}{420} & - \frac{A L^{3} \rho}{140}\\\frac{9 A L \rho}{70} & \frac{13 A L^{2} \rho}{420} & \frac{13 A L \rho}{35} & - \frac{11 A L^{2} \rho}{210}\\- \frac{13 A L^{2} \rho}{420} & - \frac{A L^{3} \rho}{140} & - \frac{11 A L^{2} \rho}{210} & \frac{A L^{3} \rho}{105}\end{matrix}\right]$

## Bending on the plane $x_1-x_2$
In this case the bending torsion occurs around the $x_3$ axis, but in the opposite direction. The matrices are similar to the case of bending on the $x_1-x_3$ plane, needing proper coordinate transformation and replacing the index of the area moment of inertia from 2 to 3.

Written in terms of generic coordinates $v_i$:

```python
v_b12 = Matrix(4, 1, [symb('v', v + 5) for v in range(4)])
transpose(v_b12)
```

$\displaystyle \left[\begin{matrix}v_{5} & v_{6} & v_{7} & v_{8}\end{matrix}\right]$

which are equivalent to the $u_i$ coordinates in the following way:
$$
    v_5 =  u_2, \\
    v_6 = -u_6, \\
    v_7 =  u_8, \\
    v_8 = -u_{12},
$$
with the following coordinate transformation matrix:

```python
R[5 - 1,  2 - 1] =  1
R[6 - 1,  6 - 1] = -1
R[7 - 1,  8 - 1] =  1
R[8 - 1, 12 - 1] = -1
R
```

### Stiffness matrix

```python
K_b12 = K_b13.subs(I_2, I_3)
K_b12
```

$\displaystyle \left[\begin{matrix}\frac{12 E I_{3}}{L^{3}} & \frac{6 E I_{3}}{L^{2}} & - \frac{12 E I_{3}}{L^{3}} & \frac{6 E I_{3}}{L^{2}}\\\frac{6 E I_{3}}{L^{2}} & \frac{4 E I_{3}}{L} & - \frac{6 E I_{3}}{L^{2}} & \frac{2 E I_{3}}{L}\\- \frac{12 E I_{3}}{L^{3}} & - \frac{6 E I_{3}}{L^{2}} & \frac{12 E I_{3}}{L^{3}} & - \frac{6 E I_{3}}{L^{2}}\\\frac{6 E I_{3}}{L^{2}} & \frac{2 E I_{3}}{L} & - \frac{6 E I_{3}}{L^{2}} & \frac{4 E I_{3}}{L}\end{matrix}\right]$

### Inertia matrix

```python
M_b12 = M_b13
if (not M_b12.is_symmetric()):
    print('Error in M_b12.')
M_b12
```

$\displaystyle \left[\begin{matrix}\frac{13 A L \rho}{35} & \frac{11 A L^{2} \rho}{210} & \frac{9 A L \rho}{70} & - \frac{13 A L^{2} \rho}{420}\\\frac{11 A L^{2} \rho}{210} & \frac{A L^{3} \rho}{105} & \frac{13 A L^{2} \rho}{420} & - \frac{A L^{3} \rho}{140}\\\frac{9 A L \rho}{70} & \frac{13 A L^{2} \rho}{420} & \frac{13 A L \rho}{35} & - \frac{11 A L^{2} \rho}{210}\\- \frac{13 A L^{2} \rho}{420} & - \frac{A L^{3} \rho}{140} & - \frac{11 A L^{2} \rho}{210} & \frac{A L^{3} \rho}{105}\end{matrix}\right]$

## Assembly of the full matrices
Accounting for axial loads, torques and bending in both planes.

```python
RAR = lambda A: transpose(R)*A*R
transpose(R**-1*u)
```

$\displaystyle \left[\begin{array}{cccccccccccc}u_{1} & u_{5} & u_{9} & u_{3} & u_{10} & - u_{6} & u_{2} & u_{7} & u_{11} & u_{4} & u_{12} & - u_{8}\end{array}\right]$

```python
K_f = diag(K_a, K_t, K_b12, K_b13)
K = RAR(K_f)
if (not K.is_symmetric()):
    print('Error in K.')
K
```

$\displaystyle \left[\begin{array}{cccccccccccc}\frac{A E}{L} & 0 & 0 & 0 & 0 & 0 & - \frac{A E}{L} & 0 & 0 & 0 & 0 & 0\\0 & \frac{12 E I_{3}}{L^{3}} & 0 & 0 & 0 & - \frac{6 E I_{3}}{L^{2}} & 0 & - \frac{12 E I_{3}}{L^{3}} & 0 & 0 & 0 & - \frac{6 E I_{3}}{L^{2}}\\0 & 0 & \frac{12 E I_{2}}{L^{3}} & 0 & \frac{6 E I_{2}}{L^{2}} & 0 & 0 & 0 & - \frac{12 E I_{2}}{L^{3}} & 0 & \frac{6 E I_{2}}{L^{2}} & 0\\0 & 0 & 0 & \frac{G I_{1}}{L} & 0 & 0 & 0 & 0 & 0 & - \frac{G I_{1}}{L} & 0 & 0\\0 & 0 & \frac{6 E I_{2}}{L^{2}} & 0 & \frac{4 E I_{2}}{L} & 0 & 0 & 0 & - \frac{6 E I_{2}}{L^{2}} & 0 & \frac{2 E I_{2}}{L} & 0\\0 & - \frac{6 E I_{3}}{L^{2}} & 0 & 0 & 0 & \frac{4 E I_{3}}{L} & 0 & \frac{6 E I_{3}}{L^{2}} & 0 & 0 & 0 & \frac{2 E I_{3}}{L}\\- \frac{A E}{L} & 0 & 0 & 0 & 0 & 0 & \frac{A E}{L} & 0 & 0 & 0 & 0 & 0\\0 & - \frac{12 E I_{3}}{L^{3}} & 0 & 0 & 0 & \frac{6 E I_{3}}{L^{2}} & 0 & \frac{12 E I_{3}}{L^{3}} & 0 & 0 & 0 & \frac{6 E I_{3}}{L^{2}}\\0 & 0 & - \frac{12 E I_{2}}{L^{3}} & 0 & - \frac{6 E I_{2}}{L^{2}} & 0 & 0 & 0 & \frac{12 E I_{2}}{L^{3}} & 0 & - \frac{6 E I_{2}}{L^{2}} & 0\\0 & 0 & 0 & - \frac{G I_{1}}{L} & 0 & 0 & 0 & 0 & 0 & \frac{G I_{1}}{L} & 0 & 0\\0 & 0 & \frac{6 E I_{2}}{L^{2}} & 0 & \frac{2 E I_{2}}{L} & 0 & 0 & 0 & - \frac{6 E I_{2}}{L^{2}} & 0 & \frac{4 E I_{2}}{L} & 0\\0 & - \frac{6 E I_{3}}{L^{2}} & 0 & 0 & 0 & \frac{2 E I_{3}}{L} & 0 & \frac{6 E I_{3}}{L^{2}} & 0 & 0 & 0 & \frac{4 E I_{3}}{L}\end{array}\right]$

```python
M_f = diag(M_a, M_t, M_b12, M_b13)
M = RAR(M_f)
if (not M.is_symmetric()):
    print('Error in M.')
M
```

$\displaystyle \left[\begin{array}{cccccccccccc}\frac{A L \rho}{3} & 0 & 0 & 0 & 0 & 0 & \frac{A L \rho}{6} & 0 & 0 & 0 & 0 & 0\\0 & \frac{13 A L \rho}{35} & 0 & 0 & 0 & - \frac{11 A L^{2} \rho}{210} & 0 & \frac{9 A L \rho}{70} & 0 & 0 & 0 & \frac{13 A L^{2} \rho}{420}\\0 & 0 & \frac{13 A L \rho}{35} & 0 & \frac{11 A L^{2} \rho}{210} & 0 & 0 & 0 & \frac{9 A L \rho}{70} & 0 & - \frac{13 A L^{2} \rho}{420} & 0\\0 & 0 & 0 & \frac{I_{1} L \rho}{3} & 0 & 0 & 0 & 0 & 0 & \frac{I_{1} L \rho}{6} & 0 & 0\\0 & 0 & \frac{11 A L^{2} \rho}{210} & 0 & \frac{A L^{3} \rho}{105} & 0 & 0 & 0 & \frac{13 A L^{2} \rho}{420} & 0 & - \frac{A L^{3} \rho}{140} & 0\\0 & - \frac{11 A L^{2} \rho}{210} & 0 & 0 & 0 & \frac{A L^{3} \rho}{105} & 0 & - \frac{13 A L^{2} \rho}{420} & 0 & 0 & 0 & - \frac{A L^{3} \rho}{140}\\\frac{A L \rho}{6} & 0 & 0 & 0 & 0 & 0 & \frac{A L \rho}{3} & 0 & 0 & 0 & 0 & 0\\0 & \frac{9 A L \rho}{70} & 0 & 0 & 0 & - \frac{13 A L^{2} \rho}{420} & 0 & \frac{13 A L \rho}{35} & 0 & 0 & 0 & \frac{11 A L^{2} \rho}{210}\\0 & 0 & \frac{9 A L \rho}{70} & 0 & \frac{13 A L^{2} \rho}{420} & 0 & 0 & 0 & \frac{13 A L \rho}{35} & 0 & - \frac{11 A L^{2} \rho}{210} & 0\\0 & 0 & 0 & \frac{I_{1} L \rho}{6} & 0 & 0 & 0 & 0 & 0 & \frac{I_{1} L \rho}{3} & 0 & 0\\0 & 0 & - \frac{13 A L^{2} \rho}{420} & 0 & - \frac{A L^{3} \rho}{140} & 0 & 0 & 0 & - \frac{11 A L^{2} \rho}{210} & 0 & \frac{A L^{3} \rho}{105} & 0\\0 & \frac{13 A L^{2} \rho}{420} & 0 & 0 & 0 & - \frac{A L^{3} \rho}{140} & 0 & \frac{11 A L^{2} \rho}{210} & 0 & 0 & 0 & \frac{A L^{3} \rho}{105}\end{array}\right]$


## Problem 1

Given: $\Lambda$ is a poisson process on $\mathbb{R}$. Each point $x_i$ in $\Lambda$ underoges some
markging process' resulting in $N=\{(x_1,U_1), (x_2,U_2) \dots (x_n, U_n)\}$ where $U_i$ are iid generated from some process witg density $\mu$

This essentially is a generalisation of the Colouring theorem.
Let the joint 'marked' distribution $\Lambda'$ be denoted by: $f(x,y)$ where $x\ \in \ \Lambda$ and $y\ \in U$
and $(x,y) \in \Lambda'$ which is in $\mathbb{R}^2$
Consider $F = \sum_{x_i\ \in\ \Lambda}f(x_i,y_i)$

Fact 1: $\Lambda' = \{(X,U)|X\in \Lambda\}$ is an independent process. (This is only true conditional on $X$)

Thus, $E[e^{-sF}|X \in \Lambda] = \Pi_{X_i \in \Lambda} f(X_i, U_i) = \Pi_{X_i \in \Lambda}\int_{U}e^{-sf(X_i,U_i)}p(X_i,U_i)dU_i$

Fact 2: In any region $\Lambda_i$, the number of points $N(\Lambda_i) \sim Poisson(\mu_i)$ where $\mu_i = \int_{\Lambda_i} \mu(x)dx$ and hence, consider:

$F = \sum_{i=1}^k f(X_i) = \sum_{i=1}f_iN_i^k$ being $k$ disjoint sets where $f$ is any measurable function. then

$$
\begin{align}
E[e^{sF}] &= \Pi_{i=1}^k E[e^{sf_iN_i}]\\
&= \Pi(e^{\lambda_i(e^{fs}-1)})\ \text{where} \lambda_i = \int_{A_i}\lambda(x)dx\\
&= e^{\sum_{i=1}^k \int(e^{fs}-1)\lambda(x)dx}\\
&= e^{\sum_{i=1}^k \int(e^{fs}-1)\lambda(x)dx}\\
&= e^{ \int_{\Lambda}(e^{fs}-1)\lambda(x)dx}\\
\end{align}
$$

or $E[e^{-sF}] = exp(-\int_{\Lambda}(1-e^{fs})\lambda(x)dx)$

Now, consider $E[e^{-sF'}]$ where $F'$ is the sum of independent random variable $f(X_i, U_i)$
Conditional on $\Lambda$:

$E[e^{-sF'}|\Lambda] = \Pi_{X_i\in \Lambda}E[e^{-sf(X_i,U_i)}]$

Consider the measurable function $f'(x) = -log(\int_U e^{-f(X,U)}p(X,U)du)$, then:

[Couldn't take it further from here, but the idea should be to define $F^* = \sum f(X,U)$. I was not able to come up with a generating function for this. The idea was to show that it has generating function that for a poisson]

--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
**Aliter**

*[Source Grimmett and Stirzker, 3rd Edition 6.13.5]*

Consider $f: R \longrightarrow R^2$ denoting the distribution of points in $\Lambda'$
then for any set $B\subset R^2$, the number of points of f(\Lambda') in B is given by $N_B = |\Lambda'  \cup f^{-1}B| \sim Poisson(B)$
Given project disjoint sets $B_i$ in $R^2$, there pre-images in $R$ are also disjoint and

$\Lambda(A) \sim Poisson(\int_A \lambda(x)dx)$

$\Lambda'(B) \sim \Lambda(f^{-1}B) = Poisson(\int_{f^{-1}B}\lambda(x)dx) = Poisson(\int_B \lambda(x)dx\mu(y)dy$

## Problem 2

Given Raindrops fall as a $PPP(\lambda drops/cm^2)$ on $R^2 \times [0, \infty)$ and each drop scatters as in a radius of circle $r \sim exp(1/cm)$.
To find: Probability density of first drop touching the origin.

$\lambda=1$

Define $U_i$ to be iid $Bernoulli(p(x_k,y_k)$ given by:
$$
U_k = \begin{cases}
1 & if \sqrt{x_k^2+y_k^2} \leq r_k\\
0 & otherwise
\end{cases}
$$

By coloring theorem $\implies$ $\Lambda'=\{(r_k,x_k,y_k\}\sim PPP(\lambda p(x_k,y_k))$
For the drop to splash the origin with radius '$r$':

Consider
$\Lambda^1 = \{(t_i,x_i,y_i,r_i) \in \Lambda: U_k=1\}$
$\tau = min\{t_k: (t_k,x_k,y_k,r_k) \in \Lambda'\}$

Consider $P(t < \tau, R<r)$ = P[no points in $[0,r] \times [0, t] \times R^2$] = $P[\Lambda^1([0,r] \times [0, t] \times R^2)=0]$

$P[\Lambda^1([0,r] \times [0, 1] \times R^2)=0]=exp(-\int_{0}^r \int_{0}^{1} \int_{R^2}\lambda p(x,y)dxdydtdr) =exp(-\lambda 2\pi tr)$

Now, $P(R<r)=\int P(t,R)dt = exp(-2\pi\lambda r)$

Thus, $R \sim exponential({2\pi\lambda})$

$ER=\int_0^{\infty} re^{-2\pi\lambda r}dr=\frac{1}{2\pi} = 0.15$

```python
### Simulation
%matplotlib inline
from __future__ import division
import numpy as np
import matplotlib.pyplot as plt
np.random.seed(1)
import math
N=1000
s=0
def R(x,y):
    return math.sqrt(x*x+y*y)
for i in range(N):
    r=-100
    y=0
    x=0
    while R(x,y)>r:
        S=np.random.uniform(size=2)
        x=S[0]
        y=S[1]
        r=np.random.exponential(1)
        s+=r
print 'Average radius: {}'.format(s/N)

```

    Average radius: 2.02946448281

The simulation results do not seem to be close to the expected results of 0.15

## Problem 3

## Part (a)

In order to simulate the continuous time MC, we make use of the instantaneous rate matrix $Q$ given by:
$Q=\lambda(P-I)$ where $\lambda=10^6$ The coninuous time MC is approximated to occur in discreted time steps of $10^{-6}$ seconds

$Q_{aa}$ = Total jump rate out of state a

$Q_{ab}$ = Jum rate from $a \longrightarrow b$

Due to the original transition matrix $P$ having extremely small entries, most of the time is spent in the same state. By approximating the holding time to be poisson, we arrive at the $(e^{tQ})_{ab}$ approximation for $P(Y_t=b|Y_0=a)$

## Part (b)

$\tau_\dagger = inf\{t \geq 0: Y_t=\dagger\}$ i.e $\tag_\dagger$ is the hitting time and $u(a) = E[\tau_\dagger|Y_0=a]$ defines the mean hitting time.

$u(\dagger)=0$

For $a \neq \dagger$

$u(a) = \text{Hold time in state a} + \sum_b \text{(fractional jump rate from $a$ to $b$)} \times u(b)$

Alternatively:

$u(a) = \frac{1}{-Q_{aa}} + \sum_{b \neq a}(\frac{Q_{ab}}{-Q_{aa}})u(b)$
 $\implies$
 $-Q_{aa}u(a) = 1 + \sum_{b \neq a} Q_{ab}u(b)$

 We thus solve for:

 $Q\vec{u}=-\vec{1}$

```python
k_a=2e-6
k_b=2e-6
k_p=5e-6
k_d=1e-5
ll = 1e6
P = np.matrix([[1-k_a-k_b, k_a ,k_b, 0, 0, 0],
               [k_a, 1-k_a-k_b, 0, k_b, 0, 0],
               [k_b, 0, 1-k_a-k_b, k_a, 0, 0],
               [0, k_b, k_a, 1-k_a-k_b-k_p, k_p, 0],
               [0, 0, 0, 0, 1-k_d, k_d],
               [0, 0, 0, k_d, 0, 1-k_d]], dtype=np.float64)
Q = ll*(P-np.eye(6))
print(Q)
```

    [[ -4.   2.   2.   0.   0.   0.]
     [  2.  -4.   0.   2.   0.   0.]
     [  2.   0.  -4.   2.   0.   0.]
     [  0.   2.   2.  -9.   5.   0.]
     [  0.   0.   0.   0. -10.  10.]
     [  0.   0.   0.  10.   0. -10.]]

```python
Qd= Q[:-1,:-1]
Qi = np.linalg.pinv(Qd)
u=(np.sum(Qi, axis=1)*-1)
u=u.tolist()
```

```python
def h(x):
    s=0
    ht=0
    cc=0
    for i in range(1,10000):
        new_state=x
        while new_state!=5:
            old_state=new_state
            probs = Q[old_state,:]/-Q[old_state,old_state]
            probs=probs.tolist()[0]
            probs[old_state]=0
            qaa = np.random.exponential(-1/Q[old_state,old_state])
            z=np.random.choice(6, 1, p=probs)
            new_state = z[0] #states[z[0]]
            s+=qaa
    return s/10000

```

## Starting state $\phi$

```python
print('From calculation: {}\t From Simulation: {}'.format(u[0][0],h(0)))
```

    From calculation: 1.90000000032	 From Simulation: 1.88676392061

## Starting state $\alpha$

```python
print('From calculation: {}\t From Simulation: {}'.format(u[1][0],h(1)))
```

    From calculation: 1.65000000026	 From Simulation: 1.64257724783

## Starting state $\beta$

```python
print('From calculation: {}\t From Simulation: {}'.format(u[2][0],h(2)))
```

    From calculation: 1.65000000026	 From Simulation: 1.65435248441

## Starting state $\alpha+\beta$

```python
print('From calculation: {}\t From Simulation: {}'.format(u[3][0],h(3)))
```

    From calculation: 0.900000000125	 From Simulation: 0.898660503545

## Starting state $pol$

```python
print('From calculation: {}\t From Simulation: {}'.format(u[4][0],h(4)))
```

    From calculation: 0.1	 From Simulation: 0.0986274216


---
title: Space Shuttle Reentry Trajectory
---

**Originally Contributed by**: Henrique Ferrolho

This tutorial demonstrates how to compute a reentry trajectory for the
[Space Shuttle](https://en.wikipedia.org/wiki/Space_Shuttle),
by formulating and solving a nonlinear programming problem.
The problem was drawn from Chapter 6 of
["Practical Methods for Optimal Control and Estimation Using Nonlinear Programming"](https://epubs.siam.org/doi/book/10.1137/1.9780898718577),
by John T. Betts.

The motion of the vehicle is defined by the following set of DAEs:

$$
\begin{align}
\dot{h} & = v \sin \gamma , \\
\dot{\phi} & = \frac{v}{r} \cos \gamma \sin \psi / \cos \theta , \\
\dot{\theta} & = \frac{v}{r} \cos \gamma \cos \psi , \\
\dot{v} & = -\frac{D}{m} - g \sin \gamma , \\
\dot{\gamma} & = \frac{L}{m v} \cos(\beta) + \cos \gamma \left ( \frac{v}{r} - \frac{g}{v} \right ) , \\
\dot{\psi} & = \frac{1}{m v \cos \gamma} L \sin(\beta) + \frac{v}{r \cos \theta} \cos \gamma \sin \psi \sin \theta , \\
q & \le q_U , \\
\end{align}
$$

where the aerodynamic heating on the vehicle wing leading edge is $q = q_a q_r$
and the dynamic variables are

$$
\begin{align}
     h & \quad \text{altitude (ft)},     \qquad & & \gamma \quad \text{flight path angle (rad)}, \\
  \phi & \quad \text{longitude (rad)},   \qquad & & \psi   \quad \text{azimuth (rad)},           \\
\theta & \quad \text{latitude (rad)},    \qquad & & \alpha \quad \text{angle of attack (rad)},   \\
     v & \quad \text{velocity (ft/sec)}, \qquad & & \beta  \quad \text{bank angle (rad)}.
\end{align}
$$

The aerodynamic and atmospheric forces on the vehicle are
specified by the following quantities (English units):

$$
\begin{align}
           D & = \frac{1}{2} c_D S \rho v^2,                  \qquad & a_0 & = -0.20704, \\
           L & = \frac{1}{2} c_L S \rho v^2,                  \qquad & a_1 & =  0.029244, \\
           g & = \mu / r^2,                                   \qquad & \mu & =  0.14076539 \times 10^{17}, \\
           r & = R_e + h,                                     \qquad & b_0 & =  0.07854, \\
        \rho & = \rho_0 \exp[-h/h_r],                         \qquad & b_1 & = -0.61592  \times 10^{-2}, \\
      \rho_0 & = 0.002378,                                    \qquad & b_2 & =  0.621408 \times 10^{-3}, \\
         h_r & = 23800,                                       \qquad & q_r & =  17700 \sqrt{\rho} (0.0001 v)^{3.07}, \\
         c_L & = a_0 + a_1 \hat{\alpha},                      \qquad & q_a & =  c_0 + c_1 \hat{\alpha} + c_2 \hat{\alpha}^2 + c_3 \hat{\alpha}^3, \\
         c_D & = b_0 + b_1 \hat{\alpha} + b_2 \hat{\alpha}^2, \qquad & c_0 & =  1.0672181, \\
\hat{\alpha} & = 180 \alpha / \pi,                            \qquad & c_1 & = -0.19213774 \times 10^{-1}, \\
         R_e & = 20902900,                                    \qquad & c_2 & =  0.21286289 \times 10^{-3}, \\
           S & = 2690,                                        \qquad & c_3 & = -0.10117249 \times 10^{-5}.
\end{align}
$$

The reentry trajectory begins at an altitude where the aerodynamic forces are quite small
with a weight of $w = 203000$ (lb) and mass $m = w / g_0$ (slug), where $g_0 = 32.174$ (ft/sec$^2$).
The initial conditions are as follows:

$$
\begin{align}
     h & = 260000 \text{ ft},  \qquad & v      & = 25600 \text{ ft/sec}, \\
  \phi & = 0      \text{ deg}, \qquad & \gamma & = -1    \text{ deg},    \\
\theta & = 0      \text{ deg}, \qquad & \psi   & = 90    \text{ deg}.
\end{align}
$$

The final point on the reentry trajectory occurs at the unknown (free) time $t_F$,
at the so-called terminal area energy management (TAEM) interface, which is defined by the conditions

$$
h = 80000 \text{ ft}, \qquad v = 2500 \text{ ft/sec}, \qquad \gamma = -5 \text{ deg}.
$$

As explained in the book, our goal is to maximize the final crossrange,
which is equivalent to maximizing the final latitude of the vehicle,
i.e., $J = \theta(t_F)$.

### Approach
We will use a discretized model of time, with a fixed number of discretized points, $n$.
The decision variables at each point are going to be the state of the vehicle and the controls commanded to it.
In addition, we will also make each time step size $\Delta t$ a decision variable;
that way, we can either fix the time step size easily,
or allow the solver to fine-tune the duration between each adjacent pair of points.
Finally, in order to approximate the derivatives of the problem dynamics, we will use either rectangular
or [trapezoidal](http://en.wikipedia.org/wiki/Trapezoidal_rule) integration.

### Disclaimer
Do not try to actually land a Space Shuttle using this notebook! 😛
There's no mesh refinement going on, which can lead to unrealistic trajectories having position
and velocity errors with orders of magnitude $10^4$ ft and $10^2$ ft/sec, respectively.

```julia
using Interpolations, Plots
using JuMP, Ipopt  # , KNITRO
```

```julia
# Global variables
const w  = 203000.0  # weight (lb)
const g₀ = 32.174    # acceleration (ft/sec^2)
const m  = w / g₀    # mass (slug)

# Aerodynamic and atmospheric forces on the vehicle
const ρ₀ =  0.002378
const hᵣ =  23800.0
const Rₑ =  20902900.0
const μ  =  0.14076539e17
const S  =  2690.0
const a₀ = -0.20704
const a₁ =  0.029244
const b₀ =  0.07854
const b₁ = -0.61592e-2
const b₂ =  0.621408e-3
const c₀ =  1.0672181
const c₁ = -0.19213774e-1
const c₂ =  0.21286289e-3
const c₃ = -0.10117249e-5

# Initial conditions
const hₛ = 2.6          # altitude (ft) / 1e5
const ϕₛ = deg2rad(0)   # longitude (rad)
const θₛ = deg2rad(0)   # latitude (rad)
const vₛ = 2.56         # velocity (ft/sec) / 1e4
const γₛ = deg2rad(-1)  # flight path angle (rad)
const ψₛ = deg2rad(90)  # azimuth (rad)
const αₛ = deg2rad(0)   # angle of attack (rad)
const βₛ = deg2rad(0)   # banck angle (rad)
const tₛ = 1.00         # time step (sec)

# Final conditions, the so-called Terminal Area Energy Management (TAEM)
const hₜ = 0.8          # altitude (ft) / 1e5
const vₜ = 0.25         # velocity (ft/sec) / 1e4
const γₜ = deg2rad(-5)  # flight path angle (rad)

# Number of mesh points (knots) to be used
const n = 2009

# Integration scheme to be used for the dynamics
const integration_rule = "rectangular"
```

    "rectangular"

```julia
user_options_ipopt = (
    "mu_strategy" => "monotone",
    "linear_solver" => "ma27",  # For the best results, it is advised to experiment different linear solvers.
                                # If Ipopt is not compiled with MA27/MA57, it may fallback to 'MUMPS'.
                                # In general, the linear solver MA27 is much faster than MUMPS.
)

# user_options_knitro = (
#     "algorithm" => KNITRO.KN_ALG_BAR_DIRECT,
#     "bar_murule" => KNITRO.KN_BAR_MURULE_QUALITY,
#     "linsolver" => KNITRO.KN_LINSOLVER_MA27,
# )

# Create JuMP model, using Ipopt as the solver
model = Model(optimizer_with_attributes(Ipopt.Optimizer, user_options_ipopt...))
# model = Model(optimizer_with_attributes(KNITRO.Optimizer, user_options_knitro...))

@variables(model, begin
               0 ≤ scaled_h[1:n]                # altitude (ft) / 1e5
                          ϕ[1:n]                # longitude (rad)
    deg2rad(-89) ≤        θ[1:n] ≤ deg2rad(89)  # latitude (rad)
            1e-4 ≤ scaled_v[1:n]                # velocity (ft/sec) / 1e4
    deg2rad(-89) ≤        γ[1:n] ≤ deg2rad(89)  # flight path angle (rad)
                          ψ[1:n]                # azimuth (rad)
    deg2rad(-90) ≤        α[1:n] ≤ deg2rad(90)  # angle of attack (rad)
    deg2rad(-89) ≤        β[1:n] ≤ deg2rad( 1)  # banck angle (rad)
    #        0.5 ≤       Δt[1:n] ≤ 1.5          # time step (sec)
                         Δt[1:n] == 1.0         # time step (sec)
end)

# Fix initial conditions
fix(scaled_h[1], hₛ; force=true)
fix(       ϕ[1], ϕₛ; force=true)
fix(       θ[1], θₛ; force=true)
fix(scaled_v[1], vₛ; force=true)
fix(       γ[1], γₛ; force=true)
fix(       ψ[1], ψₛ; force=true)

# Fix final conditions
fix(scaled_h[n], hₜ; force=true)
fix(scaled_v[n], vₜ; force=true)
fix(       γ[n], γₜ; force=true)

# Initial guess: linear interpolation between boundary conditions
xₛ = [hₛ, ϕₛ, θₛ, vₛ, γₛ, ψₛ, αₛ, βₛ, tₛ]
xₜ = [hₜ, ϕₛ, θₛ, vₜ, γₜ, ψₛ, αₛ, βₛ, tₛ]
interp_linear = LinearInterpolation([1, n], [xₛ, xₜ])
initial_guess = mapreduce(transpose, vcat, interp_linear.(1:n))
set_start_value.(all_variables(model), vec(initial_guess))

# Functions to restore `h` and `v` to their true scale
@NLexpression(model, h[j = 1:n], scaled_h[j] * 1e5)
@NLexpression(model, v[j = 1:n], scaled_v[j] * 1e4)

# Helper functions
# @NLexpression(model, c_L[j = 1:n], a₀ + a₁ * rad2deg(α[j]))
# @NLexpression(model, c_D[j = 1:n], b₀ + b₁ * rad2deg(α[j]) + b₂ * rad2deg(α[j])^2)
@NLexpression(model, c_L[j = 1:n], a₀ + a₁ * (180 * α[j] / π))
@NLexpression(model, c_D[j = 1:n], b₀ + b₁ * (180 * α[j] / π) + b₂ * (180 * α[j] / π)^2)
@NLexpression(model,   ρ[j = 1:n], ρ₀ * exp(-h[j] / hᵣ))
@NLexpression(model,   D[j = 1:n], 0.5 * c_D[j] * S * ρ[j] * v[j]^2)
@NLexpression(model,   L[j = 1:n], 0.5 * c_L[j] * S * ρ[j] * v[j]^2)
@NLexpression(model,   r[j = 1:n], Rₑ + h[j])
@NLexpression(model,   g[j = 1:n], μ / r[j]^2)

# Motion of the vehicle as a differential-algebraic system of equations (DAEs)
@NLexpression(model, δh[j = 1:n], v[j] * sin(γ[j]))
@NLexpression(model, δϕ[j = 1:n], (v[j] / r[j]) * cos(γ[j]) * sin(ψ[j]) / cos(θ[j]))
@NLexpression(model, δθ[j = 1:n], (v[j] / r[j]) * cos(γ[j]) * cos(ψ[j]))
@NLexpression(model, δv[j = 1:n], -(D[j] / m) - g[j] * sin(γ[j]))
@NLexpression(model, δγ[j = 1:n], (L[j] / (m * v[j])) * cos(β[j]) + cos(γ[j]) * ((v[j] / r[j]) - (g[j] / v[j])))
@NLexpression(model, δψ[j = 1:n], (1 / (m * v[j] * cos(γ[j]))) * L[j] * sin(β[j]) + (v[j] / (r[j] * cos(θ[j]))) * cos(γ[j]) * sin(ψ[j]) * sin(θ[j]))

# System dynamics
for j in 2:n
    i = j - 1  # index of previous knot

    if integration_rule == "rectangular"
        # Rectangular integration
        @NLconstraint(model, h[j] == h[i] + Δt[i] * δh[i])
        @NLconstraint(model, ϕ[j] == ϕ[i] + Δt[i] * δϕ[i])
        @NLconstraint(model, θ[j] == θ[i] + Δt[i] * δθ[i])
        @NLconstraint(model, v[j] == v[i] + Δt[i] * δv[i])
        @NLconstraint(model, γ[j] == γ[i] + Δt[i] * δγ[i])
        @NLconstraint(model, ψ[j] == ψ[i] + Δt[i] * δψ[i])
    elseif integration_rule == "trapezoidal"
        # Trapezoidal integration
        @NLconstraint(model, h[j] == h[i] + 0.5 * Δt[i] * (δh[j] + δh[i]))
        @NLconstraint(model, ϕ[j] == ϕ[i] + 0.5 * Δt[i] * (δϕ[j] + δϕ[i]))
        @NLconstraint(model, θ[j] == θ[i] + 0.5 * Δt[i] * (δθ[j] + δθ[i]))
        @NLconstraint(model, v[j] == v[i] + 0.5 * Δt[i] * (δv[j] + δv[i]))
        @NLconstraint(model, γ[j] == γ[i] + 0.5 * Δt[i] * (δγ[j] + δγ[i]))
        @NLconstraint(model, ψ[j] == ψ[i] + 0.5 * Δt[i] * (δψ[j] + δψ[i]))
    else
        @error "Unexpected integration rule '$(integration_rule)'"
    end
end

# Objective: Maximize crossrange
@objective(model, Max, θ[n])

# Solve for the control and state
status = optimize!(model)

# Show final crossrange of the solution
println("Final latitude θ = ", round(objective_value(model) |> rad2deg, digits = 2), "°")
```

    ******************************************************************************
    This program contains Ipopt, a library for large-scale nonlinear optimization.
     Ipopt is released as open source code under the Eclipse Public License (EPL).
             For more information visit http://projects.coin-or.org/Ipopt
    ******************************************************************************

    This is Ipopt version 3.13.2, running with linear solver ma27.

    Number of nonzeros in equality constraint Jacobian...:    72260
    Number of nonzeros in inequality constraint Jacobian.:        0
    Number of nonzeros in Lagrangian Hessian.............:   162574

    Total number of variables............................:    16063
                         variables with only lower bounds:     4014
                    variables with lower and upper bounds:     8033
                         variables with only upper bounds:        0
    Total number of equality constraints.................:    12048
    Total number of inequality constraints...............:        0
            inequality constraints with only lower bounds:        0
       inequality constraints with lower and upper bounds:        0
            inequality constraints with only upper bounds:        0

    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
       0  0.0000000e+00 6.54e+02 1.00e+00  -1.0 0.00e+00    -  0.00e+00 0.00e+00   0
       1 -6.3343098e-02 5.67e+03 1.69e+03  -1.0 1.83e+00    -  6.10e-01 1.00e+00f  1
       2 -6.9086603e-02 2.07e+03 3.21e+04  -1.0 7.82e-01   4.0 4.04e-01 1.00e+00h  1
       3 -4.7521502e-02 1.06e+03 3.25e+04  -1.0 6.32e-01   4.4 7.74e-01 6.88e-01h  1
       4 -4.7196157e-02 1.04e+03 3.23e+04  -1.0 4.67e-01   4.9 1.00e+00 1.40e-02h  1
       5 -4.6646809e-02 1.02e+03 3.26e+04  -1.0 4.99e-01   5.3 1.00e+00 2.28e-02h  1
       6 -4.3199203e-02 9.01e+02 2.88e+04  -1.0 4.65e-01   4.8 1.00e+00 1.21e-01h  1
       7 -4.1589219e-02 8.51e+02 2.86e+04  -1.0 4.56e-01   5.2 1.00e+00 5.69e-02h  1
       8 -3.9815973e-02 7.99e+02 5.04e+04  -1.0 4.61e-01   5.7 1.00e+00 6.26e-02h  1
       9 -3.5837693e-02 6.94e+02 3.29e+04  -1.0 5.45e-01   5.2 1.00e+00 1.37e-01h  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      10 -3.4315783e-02 6.54e+02 4.95e+04  -1.0 4.15e-01   5.6 1.00e+00 5.93e-02h  1
      11 -3.2793480e-02 6.14e+02 1.57e+05  -1.0 3.74e-01   6.0 1.00e+00 6.18e-02h  1
      12 -2.9817475e-02 5.43e+02 7.32e+04  -1.0 5.89e-01   5.6 1.00e+00 1.21e-01h  1
      13 -2.8732303e-02 5.17e+02 1.44e+05  -1.0 3.73e-01   6.0 1.00e+00 4.94e-02h  1
      14 -2.7624819e-02 4.91e+02 4.58e+05  -1.0 3.19e-01   6.4 1.00e+00 5.23e-02h  1
      15 -2.5514458e-02 4.44e+02 1.45e+05  -1.0 4.34e-01   5.9 1.00e+00 9.94e-02h  1
      16 -2.4689804e-02 4.25e+02 4.06e+05  -1.0 2.88e-01   6.4 1.00e+00 4.28e-02h  1
      17 -2.2692027e-02 3.84e+02 2.17e+05  -1.0 8.96e-01   5.9 1.00e+00 1.02e-01h  1
      18 -2.1971616e-02 3.68e+02 3.68e+05  -1.0 3.13e-01   6.3 1.00e+00 4.11e-02h  1
      19 -2.1205108e-02 3.52e+02 1.21e+06  -1.0 2.74e-01   6.7 1.00e+00 4.52e-02h  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      20 -1.9734474e-02 3.23e+02 3.13e+05  -1.0 3.22e-01   6.2 1.00e+00 8.63e-02h  1
      21 -1.9141687e-02 3.11e+02 1.06e+06  -1.0 2.62e-01   6.7 1.00e+00 3.79e-02h  1
      22 -1.7735591e-02 2.84e+02 3.63e+05  -1.0 3.32e-01   6.2 1.00e+00 8.89e-02h  1
      23 -1.7194352e-02 2.74e+02 9.42e+05  -1.0 2.72e-01   6.6 1.00e+00 3.76e-02h  1
      24 -1.5896638e-02 2.50e+02 4.02e+05  -1.0 3.81e-01   6.1 1.00e+00 8.89e-02h  1
      25 -1.5419422e-02 2.42e+02 8.63e+05  -1.0 2.93e-01   6.6 1.00e+00 3.60e-02h  1
      26 -1.4255760e-02 2.22e+02 4.51e+05  -1.0 6.09e-01   6.1 1.00e+00 8.63e-02h  1
      27 -1.3815887e-02 2.14e+02 8.35e+05  -1.0 3.13e-01   6.5 1.00e+00 3.58e-02h  1
      28 -1.3557670e-02 2.09e+02 6.95e+05  -1.0 2.69e+00   6.0 1.81e-01 2.08e-02h  1
      29 -1.3253507e-02 2.04e+02 8.32e+05  -1.0 3.66e-01   6.5 1.00e+00 2.50e-02h  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      30 -1.2564402e-02 1.93e+02 2.93e+06  -1.0 3.33e-01   6.9 1.00e+00 5.79e-02h  1
      31 -1.1900903e-02 1.82e+02 7.18e+05  -1.0 4.22e-01   6.4 1.00e+00 5.54e-02h  1
      32 -1.1555694e-02 1.77e+02 2.99e+06  -1.0 3.56e-01   6.8 1.00e+00 3.04e-02h  1
      33 -1.0947335e-02 1.68e+02 6.60e+05  -1.0 4.58e-01   6.4 1.00e+00 5.27e-02h  1
      34 -1.0544195e-02 1.62e+02 3.13e+06  -1.0 3.66e-01   6.8 1.00e+00 3.67e-02h  1
      35 -1.0095108e-02 1.56e+02 1.03e+06  -1.0 5.35e-01   6.3 1.00e+00 3.89e-02h  1
      36 -9.7865301e-03 1.51e+02 4.68e+06  -1.0 4.95e-01   6.7 1.00e+00 2.76e-02h  1
      37 -9.1324034e-03 1.43e+02 1.27e+06  -1.0 5.66e-01   6.3 1.00e+00 5.70e-02h  1
      38 -8.8501547e-03 1.39e+02 5.15e+06  -1.0 3.95e-01   6.7 1.00e+00 2.60e-02h  1
      39 -8.0375525e-03 1.30e+02 1.13e+06  -1.0 5.82e-01   6.2 1.00e+00 7.34e-02h  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      40 -7.6240699e-03 1.25e+02 4.36e+06  -1.0 3.52e-01   6.6 1.00e+00 4.05e-02h  1
      41 -7.1438175e-03 1.19e+02 1.36e+07  -1.0 3.42e-01   7.1 1.00e+00 4.83e-02h  1
      42 -6.1719132e-03 1.08e+02 3.11e+06  -1.0 3.54e-01   6.6 9.41e-01 9.84e-02h  1
      43 -5.3081700e-03 9.77e+01 8.49e+06  -1.0 3.12e-01   7.0 1.00e+00 9.73e-02h  1
      44 -3.6286106e-03 8.02e+01 6.45e+06  -1.0 2.96e-01   6.5 2.38e-01 2.03e-01h  1
      45 -2.9676043e-03 7.26e+01 6.18e+06  -1.0 2.28e-01   7.0 9.41e-04 9.97e-02h  1
      46 -2.7545976e-03 7.01e+01 6.28e+06  -1.0 3.54e-01   6.5 3.61e-04 3.49e-02h  1
      47 -2.7284479e-03 6.98e+01 1.07e+06  -1.0 2.37e-01   6.9 1.00e+00 4.50e-03h  1
      48 -6.3723607e-04 5.07e+01 1.08e+06  -1.0 4.06e-01   6.4 3.35e-02 3.55e-01h  1
      49 -5.3811689e-04 4.94e+01 9.13e+05  -1.0 3.65e-01   6.9 1.00e+00 2.65e-02h  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      50  7.5437165e-04 4.58e+01 7.31e+05  -1.0 4.78e-01   6.4 3.67e-01 3.47e-01h  1
      51  9.5641099e-04 4.23e+01 7.94e+05  -1.0 3.66e-01   6.8 1.00e+00 8.85e-02h  1
      52  1.8339791e-03 3.14e+01 6.61e+05  -1.0 3.83e-01   6.3 5.76e-01 4.23e-01h  1
      53  2.5314070e-03 1.18e+01 3.66e+05  -1.0 2.83e-01   5.9 7.30e-02 6.11e-01f  1
      54  2.5566378e-03 1.11e+01 3.46e+05  -1.0 6.55e-01   5.4 9.32e-02 5.92e-02f  1
      55  2.9678499e-03 2.17e+00 1.53e+05  -1.0 1.10e-01   5.8 2.53e-01 1.00e+00f  1
      56  2.9715632e-03 6.26e-03 4.62e+03  -1.0 4.87e-03   5.3 9.85e-01 1.00e+00f  1
      57  3.0331020e-03 4.58e-07 1.39e+02  -1.0 1.96e-03   4.9 1.00e+00 1.00e+00f  1
      58  3.5602871e-03 7.08e-05 1.17e+02  -1.0 5.56e-03   4.4 1.00e+00 8.75e-01f  1
      59  3.7601790e-03 8.87e-06 1.78e+01  -1.0 2.26e-03   3.9 1.00e+00 1.00e+00f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      60  4.6968224e-03 2.01e-04 1.89e+01  -1.0 7.19e-03   3.4 1.00e+00 1.00e+00f  1
      61  5.6733696e-03 3.15e-04 6.72e+00  -1.0 7.67e-03   2.9 1.00e+00 1.00e+00f  1
      62  8.1993558e-03 3.12e-03 5.67e+00  -1.0 1.94e-02   2.5 1.00e+00 1.00e+00f  1
      63  1.2104241e-02 3.84e-02 2.87e+00  -1.0 2.94e-02   2.0 1.00e+00 1.00e+00f  1
      64  2.0189014e-02 4.28e-01 1.92e+00  -1.0 5.92e-02   1.5 1.00e+00 1.00e+00f  1
      65  2.3315131e-02 5.18e-02 1.97e+00  -1.0 2.27e-02   1.9 1.00e+00 1.00e+00f  1
      66  3.0070243e-02 6.30e-01 1.39e+00  -1.0 4.82e-02   1.5 1.00e+00 1.00e+00f  1
      67  3.2625400e-02 7.66e-02 1.37e+00  -1.0 1.79e-02   1.9 1.00e+00 1.00e+00f  1
      68  3.8859978e-02 9.16e-01 1.27e+00  -1.0 4.93e-02   1.4 1.00e+00 1.00e+00f  1
      69  4.1165937e-02 1.08e-01 1.08e+00  -1.0 1.60e-02   1.8 1.00e+00 1.00e+00f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      70  4.7087350e-02 1.30e+00 1.28e+00  -1.0 5.59e-02   1.4 1.00e+00 1.00e+00f  1
      71  4.9277379e-02 1.47e-01 1.05e+00  -1.0 1.75e-02   1.8 1.00e+00 1.00e+00f  1
      72  5.5047421e-02 1.78e+00 1.26e+00  -1.0 6.16e-02   1.3 1.00e+00 1.00e+00f  1
      73  5.7189693e-02 1.93e-01 9.86e-01  -1.0 1.85e-02   1.7 1.00e+00 1.00e+00f  1
      74  5.8340406e-02 8.98e-02 2.03e-01  -1.7 1.29e-02   1.3 1.00e+00 1.00e+00f  1
      75  6.1889239e-02 8.08e-01 2.30e-01  -1.7 3.82e-02   0.8 1.00e+00 1.00e+00f  1
      76  7.0954393e-02 8.80e+00 2.31e-01  -1.7 1.20e-01   0.3 1.00e+00 1.00e+00f  1
      77  7.4368103e-02 5.72e-01 1.16e-01  -1.7 2.31e-02   0.7 1.00e+00 1.00e+00f  1
      78  7.5645870e-02 1.58e-01 2.07e-02  -2.5 1.36e-02   0.3 1.00e+00 1.00e+00h  1
      79  7.5958778e-02 5.91e-03 1.34e-03  -3.8 2.56e-03  -0.2 1.00e+00 1.00e+00h  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      80  7.6281162e-02 2.80e-03 3.51e-04  -5.7 1.81e-03  -0.7 1.00e+00 1.00e+00h  1
      81  7.7260527e-02 2.59e-02 3.62e-04  -5.7 5.50e-03  -1.2 1.00e+00 1.00e+00h  1
      82  8.0233183e-02 2.54e-01 3.74e-04  -5.7 1.70e-02  -1.7 1.00e+00 1.00e+00h  1
      83  8.9503188e-02 2.93e+00 4.11e-04  -5.7 5.60e-02  -2.1 1.00e+00 1.00e+00h  1
      84  9.3292307e-02 3.30e-01 3.11e-04  -5.7 1.61e-02  -1.7 1.00e+00 1.00e+00h  1
      85  1.0483151e-01 4.67e+00 4.23e-04  -5.7 6.34e-02  -2.2 1.00e+00 1.00e+00h  1
      86  1.0987479e-01 4.95e-01 4.81e-04  -5.7 2.78e-02  -1.8 1.00e+00 1.00e+00h  1
      87  1.3186068e-01 6.43e+02 2.05e-02  -5.7 9.80e-01  -2.2 4.00e-01 1.00e+00h  1
      88  1.4668209e-01 2.42e+02 1.15e-02  -5.7 1.72e-01  -1.8 1.00e+00 1.00e+00h  1
      89  1.5770031e-01 1.77e+02 8.44e-03  -5.7 2.84e-01  -2.3 1.00e+00 2.81e-01h  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      90  1.8834000e-01 4.18e+02 7.79e-03  -5.7 1.19e+01  -2.8 2.72e-02 6.33e-02h  1
      91  2.4430759e-01 1.55e+02 1.52e-03  -5.7 2.91e-01  -2.3 1.00e+00 1.00e+00h  1
      92  2.7261698e-01 3.20e+02 1.17e-03  -5.7 1.82e+00  -2.8 3.72e-01 2.59e-01h  1
      93  2.7590990e-01 2.88e+02 5.55e-03  -5.7 2.91e-01  -2.4 1.00e+00 9.83e-02h  1
      94  3.0194885e-01 2.27e+02 4.84e-03  -5.7 5.44e+00  -2.9 5.76e-02 2.21e-01h  1
      95  3.1894809e-01 1.26e+02 3.16e-03  -5.7 2.97e-01  -2.4 4.40e-01 4.52e-01h  1
      96  3.2036792e-01 1.07e+02 1.04e-02  -5.7 2.13e-01  -2.0 1.00e+00 1.54e-01h  1
      97  3.4549672e-01 3.72e+01 9.33e-04  -5.7 3.70e-01  -2.5 9.55e-01 6.83e-01h  1
      98  3.4958894e-01 2.18e+01 2.49e-03  -5.7 1.14e-01  -2.1 1.00e+00 4.11e-01h  1
      99  3.7186722e-01 1.35e+01 8.03e-04  -5.7 3.61e-01  -2.5 5.77e-01 6.79e-01h  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     100  3.8126592e-01 6.84e-01 9.59e-04  -5.7 1.28e-01  -2.1 1.00e+00 1.00e+00h  1
     101  3.9410111e-01 1.73e+00 9.95e-04  -5.7 4.90e-01  -2.6 4.45e-01 4.84e-01h  1
     102  3.9791537e-01 9.97e-01 2.88e-03  -5.7 1.34e-01  -2.2 1.00e+00 4.29e-01h  1
     103  4.2267620e-01 1.20e+01 1.06e-03  -5.7 6.21e-01  -2.6 3.63e-01 1.00e+00h  1
     104  4.2925878e-01 9.36e-01 3.63e-03  -5.7 1.52e-01  -2.2 6.64e-01 1.00e+00h  1
     105  4.3549818e-01 1.93e+00 2.10e-03  -5.7 8.64e-01  -2.7 1.52e-01 2.94e-01h  1
     106  4.4352082e-01 5.80e-01 8.49e-04  -5.7 1.68e-01  -2.3 1.00e+00 1.00e+00f  1
     107  4.5709315e-01 4.57e+01 4.96e-03  -5.7 2.60e+00  -2.7 1.05e-01 4.96e-01h  1
     108  4.6818779e-01 6.20e+00 6.21e-03  -5.7 1.06e+00  -2.3 1.00e+00 1.00e+00h  1
     109  4.9736625e-01 1.35e+01 6.43e-03  -5.7 6.85e-01  -2.8 9.16e-02 1.00e+00h  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     110  5.0752610e-01 2.96e+00 9.90e-04  -5.7 1.19e-01  -2.4 8.24e-01 1.00e+00h  1
     111  5.3306382e-01 9.96e+00 1.05e-03  -5.7 7.88e-01  -2.9 1.84e-01 1.00e+00h  1
     112  5.4009482e-01 2.59e+00 3.86e-04  -5.7 1.16e-01  -2.4 7.55e-01 1.00e+00h  1
     113  5.5492270e-01 1.95e+01 3.25e-04  -5.7 3.61e-01  -2.9 7.67e-01 1.00e+00h  1
     114  5.7867469e-01 1.21e+02 2.16e-03  -5.7 9.40e-01  -3.4 3.69e-01 1.00e+00h  1
     115  6.0105011e-01 2.46e+02 3.34e-02  -5.7 6.42e+00  -3.9 5.65e-02 1.82e-01h  1
     116  6.0876620e-01 6.35e+01 1.54e-02  -5.7 1.00e+00  -2.5 9.71e-01 9.62e-01h  1
     117  6.2419028e-01 4.88e+01 1.07e-02  -5.7 2.62e+00    -  6.29e-02 3.29e-01h  1
     118  6.1074851e-01 6.93e+01 5.39e-03  -5.7 7.24e-01  -3.0 7.27e-02 1.00e+00h  1
     119  5.9969204e-01 1.69e+02 2.96e-03  -5.7 7.49e-01  -3.5 4.65e-01 1.00e+00h  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     120  5.9578494e-01 8.96e+00 1.19e-03  -5.7 4.27e-01  -4.0 6.04e-01 1.00e+00H  1
     121  5.9650165e-01 9.43e+00 3.72e-04  -5.7 3.00e-01    -  7.87e-01 1.00e+00h  1
     122  5.9599152e-01 3.63e-02 1.20e-05  -5.7 1.18e-01    -  1.00e+00 1.00e+00h  1
     123  5.9598532e-01 1.52e-04 1.75e-06  -5.7 3.13e-02    -  1.00e+00 1.00e+00h  1
     124  5.9607738e-01 9.02e-03 1.49e-06  -8.6 6.56e-02    -  9.59e-01 1.00e+00h  1
     125  5.9604860e-01 5.48e-05 2.72e-07  -8.6 2.80e-02    -  1.00e+00 1.00e+00h  1
     126  5.9604675e-01 2.07e-06 7.25e-08  -8.6 1.48e-02    -  1.00e+00 1.00e+00h  1
     127  5.9604651e-01 2.55e-07 1.27e-08  -8.6 6.20e-03    -  1.00e+00 1.00e+00h  1
     128  5.9604649e-01 1.81e-08 9.01e-10  -8.6 1.65e-03    -  1.00e+00 1.00e+00h  1
     129  5.9604649e-01 2.14e-08 4.59e-10 -12.9 1.18e-03    -  9.99e-01 1.00e+00h  1

    Number of Iterations....: 129

                                       (scaled)                 (unscaled)
    Objective...............:  -5.9604648822552675e-01    5.9604648822552675e-01
    Dual infeasibility......:   4.5924084974112041e-10    4.5924084974112041e-10
    Constraint violation....:   9.2127296347221943e-09    2.1434971131384373e-08
    Complementarity.........:   1.6911024325149909e-10   -1.6911024325149909e-10
    Overall NLP error.......:   9.2127296347221943e-09    2.1434971131384373e-08

    Number of objective function evaluations             = 132
    Number of objective gradient evaluations             = 130
    Number of equality constraint evaluations            = 132
    Number of inequality constraint evaluations          = 0
    Number of equality constraint Jacobian evaluations   = 130
    Number of inequality constraint Jacobian evaluations = 0
    Number of Lagrangian Hessian evaluations             = 129
    Total CPU secs in IPOPT (w/o function evaluations)   =     12.277
    Total CPU secs in NLP function evaluations           =     30.827

    EXIT: Optimal Solution Found.
    Final latitude θ = 34.15°

```julia
ts = cumsum([0; value.(Δt)])[1:end-1]

plt_altitude = plot(ts, value.(scaled_h), legend = nothing, title = "Altitude (100,000 ft)")
plt_longitude = plot(ts, rad2deg.(value.(ϕ)), legend = nothing, title = "Longitude (deg)")
plt_latitude = plot(ts, rad2deg.(value.(θ)), legend = nothing, title = "Latitude (deg)")
plt_velocity = plot(ts, value.(scaled_v), legend = nothing, title = "Velocity (1000 ft/sec)")
plt_flight_path = plot(ts, rad2deg.(value.(γ)), legend = nothing, title = "Flight Path (deg)")
plt_azimuth = plot(ts, rad2deg.(value.(ψ)), legend = nothing, title = "Azimuth (deg)")

plt = plot(plt_altitude,  plt_velocity, plt_longitude, plt_flight_path, plt_latitude,
           plt_azimuth, layout=grid(3, 2), linewidth=2, size=(700, 700))
```

```julia
function q(h, v, a)
    ρ(h) = ρ₀ * exp(-h / hᵣ)
    qᵣ(h, v) = 17700 * √ρ(h) * (0.0001 * v)^3.07
    qₐ(a) = c₀ + c₁ * rad2deg(a) + c₂ * rad2deg(a)^2 + c₃ * rad2deg(a)^3
    # Aerodynamic heating on the vehicle wing leading edge
    qₐ(a) * qᵣ(h, v)
end

plt_attack_angle = plot(ts[1:end-1], rad2deg.(value.(α)[1:end-1]), legend=nothing, title="Angle of Attack (deg)")
plt_bank_angle = plot(ts[1:end-1], rad2deg.(value.(β)[1:end-1]), legend=nothing, title="Bank Angle (deg)")
plt_heating = plot(ts, q.(value.(scaled_h)*1e5, value.(scaled_v)*1e4, value.(α)), legend=nothing, title="Heating (BTU/ft/ft/sec)")

plt = plot(plt_attack_angle, plt_bank_angle, plt_heating, layout=grid(3, 1), linewidth=2, size=(700, 700))
```

```julia
plt = plot(rad2deg.(value.(ϕ)), rad2deg.(value.(θ)), value.(scaled_h),
           linewidth=2, legend=nothing, title="Space Shuttle Reentry Trajectory",
           xlabel="Longitude (deg)", ylabel="Latitude (deg)", zlabel="Altitude (100,000 ft)")
```


# Finite Difference Method
#### John S Butler john.s.butler@tudublin.ie   [Course Notes](https://johnsbutler.netlify.com/files/Teaching/Numerical_Analysis_for_Differential_Equations.pdf)    [Github](https://github.com/john-s-butler-dit/Numerical-Analysis-Python)

## Overview
This notebook illustrates the finite different method for a linear Boundary Value Problem.
### Example 2 Boundary Value Problem
To further illustrate the method we will apply the finite difference method to the this boundary value problem
\begin{equation}  \frac{d^2 y}{dx^2} + 2x\frac{dy}{dx}+y=3x^2,\end{equation}
with the boundary conditions
\begin{equation} y(0)=1,  y(1)=2. \end{equation}

```python
import numpy as np
import math
import matplotlib.pyplot as plt

```

## Discrete Axis
The stepsize is defined as
\begin{equation}h=\frac{b-a}{N}\end{equation}
here it is
\begin{equation}h=\frac{1-0}{10}\end{equation}
giving
\begin{equation}x_i=0+0.1 i\end{equation}
for $i=0,1,...10.$

```python
## BVP
N=10
h=1/N
x=np.linspace(0,1,N+1)
fig = plt.figure(figsize=(10,4))
plt.plot(x,0*x,'o:',color='red')
plt.xlim((0,1))
plt.xlabel('x',fontsize=16)
plt.title('Illustration of discrete time points for h=%s'%(h),fontsize=32)
plt.show()
```

## The Difference Equation
To convert the boundary problem into a difference equation we use 1st and 2nd order difference operators.

The first derivative can be approximated by the difference operators:
\begin{equation}D^{+}U_{i}=\frac{U_{i+1}-U_{i}}{h_{i+1}} \ \ \ \mbox{ Forward,} \end{equation}
\begin{equation}D^{-}U_{i}=\frac{U_{i}-U_{i-1}}{h_i} \ \ \ \mbox{ Backward,} \end{equation}
or
\begin{equation}D^{0}U_{i}=\frac{U_{i+1}-U_{i-1}}{x_{i+1}-x_{i-1}} \ \ \ \mbox{ Centered.} \end{equation}
The second derivative can be approxiamed
\begin{equation}\delta_x^{2}U_{i}=\frac{2}{x_{i+1}-x_{i-1}}\left(\frac{U_{i+1}-U_{i}}{x_{i+1}-x_{i}}-\frac{U_{i}-U_{i-1}}{x_{i}-x_{i-1}}\right) \ \ \ \mbox{ Centered in $x$ direction} \end{equation}

Given the differential equation
\begin{equation}  \frac{d^2 y}{dx^2} + 2x\frac{dy}{dx}+y=3x^2,\end{equation}
the difference equation is,
\begin{equation}\frac{1}{h^2}\left(y_{i-1}-2y_i+y_{i+1}\right)+2x_i\frac{y_{i+1}-y_{i-1}}{2h}+y_i=3x^2_i \ \ \ i=1,..,N-1. \end{equation}

Rearranging the equation we have the system of N-1 equations
\begin{equation}i=1: (\frac{1}{0.1^2}-\frac{2x_1}{0.2})\color{green}{y_{0}} -\left(\frac{2}{0.1^2}-1\right)y_1 +(\frac{1}{0.1^2}+\frac{2x_1}{0.2}) y_{2}=3x_1^2\end{equation}
\begin{equation}i=2: (\frac{1}{0.1^2}-\frac{2x_2}{0.2})y_{1} -\left(\frac{2}{0.1^2}-1\right)y_2 +(\frac{1}{0.1^2}+\frac{2x_2}{0.2}) y_{3}=3x_2^2\end{equation}
\begin{equation} ...\end{equation}
\begin{equation}i=8: (\frac{1}{0.1^2}-\frac{2x_8}{0.2})y_{7} -\left(\frac{2}{0.1^2}-1\right)y_8 +(\frac{1}{0.1^2}+\frac{2x_8}{0.2})y_{9}=3x_8^2\end{equation}
\begin{equation}i=9: (\frac{1}{0.1^2}-\frac{2x_9}{0.2})y_{8} -\left(\frac{2}{0.1^2}-1\right)y_9 +(\frac{1}{0.1^2}+\frac{2x_9}{0.2}) \color{green}{y_{10}}=3x_9^2\end{equation}
where the green terms are the known boundary conditions.

Rearranging the equation we have the system of 9 equations
\begin{equation}i=1:  -\left(\frac{2}{0.1^2}-1\right)y_1 +(\frac{1}{0.1^2}+\frac{2x_1}{0.2})y_{2}=-(\frac{1}{0.1^2}-\frac{2x_1}{0.2})\color{green}{y_{0}}+3x_1^2\end{equation}
\begin{equation}i=2: (\frac{1}{0.1^2}-\frac{2x_2}{0.2})y_{1} -\left(\frac{2}{0.1^2}-1\right)y_2 +(\frac{1}{0.1^2}+\frac{2x_2}{0.2}) y_{3}=3x_2^2\end{equation}
\begin{equation} ...\end{equation}
\begin{equation}i=8: (\frac{1}{0.1^2}-\frac{2x_8}{0.2})y_{7} -\left(\frac{2}{0.1^2}-1\right)y_8 +(\frac{1}{0.1^2}+\frac{2x_8}{0.2}) y_{9}=3x_8^2\end{equation}
\begin{equation}i=9: (\frac{1}{0.1^2}-\frac{2x_9}{0.2})y_{8} -\left(\frac{2}{0.1^2}-1\right)y_9 =-(\frac{1}{0.1^2}+\frac{2x_9}{0.2}) \color{green}{y_{10}}+3x_9^2\end{equation}
where the green terms are the known boundary conditions.
This is system can be put into matrix form
\begin{equation}  A\color{red}{\mathbf{y}}=\mathbf{b} \end{equation}
Where A is a $9\times 9 $ matrix of the form

which can be represented graphically as:

```python
A=np.zeros((N-1,N-1))
# Diagonal
for i in range (0,N-1):
    A[i,i]=-(2/(h*h)-1) # Diagonal

for i in range (0,N-2):
    A[i+1,i]=1/(h*h)-2*(i+1)*h/(2*h) ## Lower Diagonal
    A[i,i+1]=1/(h*h)+2*(i+1)*h/(2*h) ## Upper Diagonal

plt.imshow(A)
plt.xlabel('i',fontsize=16)
plt.ylabel('j',fontsize=16)
plt.yticks(np.arange(N-1), np.arange(1,N-0.9,1))
plt.xticks(np.arange(N-1), np.arange(1,N-0.9,1))
clb=plt.colorbar()
clb.set_label('Matrix value')
plt.title('Matrix A',fontsize=32)
plt.tight_layout()
plt.subplots_adjust()
plt.show()
```

$\mathbf{y}$ is the unknown vector which is contains the numerical approximations of the $y$.
\begin{equation}
\color{red}{\mathbf{y}}=\color{red}{
\left(\begin{array}{c} y_1\\
y_2\\
y_3\\
.\\
.\\
y_8\\
y_9
\end{array}\right).}
\end{equation}

```python
y=np.zeros((N+1))
# Boundary Condition
y[0]=1
y[N]=2
```

and the known right hand side is a known $9\times 1$ vector with the boundary conditions
\begin{equation}
\mathbf{b}=\left(\begin{array}{c}-99+3x_1^2\\
3x_2^2\\
3x_3^2\\
.\\
.\\
3x_8^2\\
-209+3x_9^2 \end{array}\right)
\end{equation}

```python
b=np.zeros(N-1)
for i in range (0,N-1):
    b[i]=3*h*(i+1)*h*(i+1)
# Boundary Condition
b[0]=-y[0]*(1/(h*h)-2*1*h/(2*h))+b[0]
b[N-2]=-y[N]*(1/(h*h)+2*9*h/(2*h))+b[N-2]
print('b=')
print(b)
```

    b=
    [-9.8970e+01  1.2000e-01  2.7000e-01  4.8000e-01  7.5000e-01  1.0800e+00
      1.4700e+00  1.9200e+00 -2.1557e+02]

## Solving the system
To solve invert the matrix $A$ such that
\begin{equation}A^{-1}Ay=A^{-1}b\end{equation}
\begin{equation}y=A^{-1}b\end{equation}
The plot below shows the graphical representation of $A^{-1}$.

```python
invA=np.linalg.inv(A)

plt.imshow(invA)
plt.xlabel('i',fontsize=16)
plt.ylabel('j',fontsize=16)
plt.yticks(np.arange(N-1), np.arange(1,N-0.9,1))
plt.xticks(np.arange(N-1), np.arange(1,N-0.9,1))
clb=plt.colorbar()
clb.set_label('Matrix value')
plt.title(r'Matrix $A^{-1}$',fontsize=32)
plt.tight_layout()
plt.subplots_adjust()
plt.show()

y[1:N]=np.dot(invA,b)
```

## Result
The plot below shows the approximate solution of the Boundary Value Problem (blue v).

```python
fig = plt.figure(figsize=(8,4))

plt.plot(x,y,'v',label='Finite Difference')
plt.xlabel('x')
plt.ylabel('y')
plt.legend(loc='best')
plt.show()
```

Chapter 06 - Boundary Value Problems/604_Boundary Value Problem Example 2.ipynb


```python
from sympy import *
i,C1,sigma_Flim = symbols('i C_1 sigma_FLim')
r1,r2,m,Z1,Z2,a,T,sigma,b = symbols('r_1 r_2 m Z_1 Z_2 a T sigma b')
#donnees = [i,C1,sigma_Flim]
inconnues = [r1,r2,m,Z1,Z2,a,T,sigma,b]
Eq = [r1+r2 - a,
      r1 - m*Z1/2,
      r2 - m*Z2/2,
      T - C1/r1,
      sigma - (5.5*T)/(b*m),
      r1 - i*r2]

Sol=solve(Eq,inconnues)

```

    [(i*r_2,
      r_2,
      m,
      2.0*i*r_2/m,
      2.0*r_2/m,
      r_2*(i + 1.0),
      C_1/(i*r_2),
      sigma,
      5.5*C_1/(i*m*r_2*sigma))]


# Tarea 1

Daniela Paz Díaz Mora

201710003-6

```python
import numpy as np

from sympy import Matrix
from sympy.abc import x, y
from numpy import linalg
```

## (a)

Se tiene que el polinomio caracteristico quedra en función de la matriz A dada por

```python
A=Matrix([[0.7,0.1,0,-0.2,0,0],[0,0.4,0.1,0,0.1,0.1],[0.9,0,0.8,0,0,0],[1,0,0,0,0,0],[0,1,0,0,0,0],[0,0,1,0,0,0]])
A.charpoly(x).as_expr()
```

$\displaystyle 1.0 x^{6} - 1.9 x^{5} + 1.26 x^{4} - 0.323 x^{3} - 0.021 x^{2} + 0.016 x + 2.49366499671666 \cdot 10^{-18}$

```python
coeff=[1,-1.9,1.26,-0.323,-0.021,0.016,0]
coeff.reverse()
np.roots(coeff)
```

    array([-5.58083612+0.j        ,  2.89366753+0.j        ,
            1.45032672+1.19082258j,  1.45032672-1.19082258j,
            1.09901515+0.j        ])

## (b)

```python
Aux_matrix=np.matrix(np.eye(6)-np.array(A), dtype='float')
```

```python
inv=np.linalg.inv(Aux_matrix)
psi=np.matrix([[2],[1],[0],[0],[0],[0]],dtype='float')
factor=np.dot(inv,psi)
J=np.matrix([[1,0,0,0,0,0],[0,1,0,0,0,0],[0,0,1,0,0,0]],dtype='float')
vector_medias=np.dot(J,factor)
```

```python
Matrix(vector_medias)
```

$\displaystyle \left[\begin{matrix}6.875\\14.375\\30.9375\end{matrix}\right]$

## (d)

```python
def an(array,n):
    (f,c)=array.shape
    if n==0:
        return np.eye(f)
    elif n==1:
        return array
    else:
        matrix=np.dot(array,an(array,n-1))
        return matrix
```

```python
A=np.array(A,dtype='float')
A.shape
```

    (6, 6)

```python
i=1
while i != 6:
    print('phi_',i, '= \n', np.dot(np.dot(J,an(A,i)),np.transpose(J)))
    i+=1
```

    phi_ 1 =
     [[0.7 0.1 0. ]
     [0.  0.4 0.1]
     [0.9 0.  0.8]]
    phi_ 2 =
     [[0.29 0.11 0.01]
     [0.09 0.26 0.22]
     [1.35 0.09 0.64]]
    phi_ 3 =
     [[0.072 0.083 0.029]
     [0.261 0.153 0.242]
     [1.341 0.171 0.521]]
    phi_ 4 =
     [[0.0185 0.0514 0.0425]
     [0.3825 0.1133 0.2349]
     [1.1376 0.2115 0.4429]]
    phi_ 5 =
     [[0.0368  0.03071 0.04744]
     [0.42696 0.09887 0.21455]
     [0.92673 0.21546 0.39257]]


# Kalman Filter Implementation in Python

Reference: **Probabilistic Robotics**

```python
#Import packages and library
import numpy as np
import math
import matplotlib.pyplot as plt
from scipy.stats import norm
%matplotlib inline
```

## State Vector

Constant Velocity Model for Ego Motion

$$x_k= \left[ \matrix{ x \\ y \\ \dot x \\ \dot y} \right] = \matrix{ \text{Position X} \\ \text{Position Y} \\ \text{Velocity in X} \\ \text{Velocity in Y}}$$

## Observation Model

$$y = \textbf{H} \cdot x$$

which is

$$y = \begin{bmatrix}0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{bmatrix} \cdot x$$ means: You observe the velocity directly in the correct unit

## Initial State $x_0$

$$x_{0} = \begin{bmatrix}0 \\ 0 \\ 0 \\ 0\end{bmatrix}$$

```python
x = np.matrix([[0,0,0,0]]).T
```

```python
x, x.shape
```

    (matrix([[0],
             [0],
             [0],
             [0]]), (4, 1))

## Initial Uncertainty $P_0$

$$P_{0} = \begin{bmatrix}\sigma^2_x & 0 & 0 & 0 \\ 0 & \sigma^2_y & 0 & 0 \\ 0 & 0 & \sigma^2_{\dot x} & 0 \\ 0 & 0 & 0 & \sigma^2_{\dot y} \end{bmatrix}$$

with $\sigma$ as the standard deviation

```python
σ = 1000.0
P = np.diag([σ, σ, σ, σ])
P, P.shape
```

    (array([[1000.,    0.,    0.,    0.],
            [   0., 1000.,    0.,    0.],
            [   0.,    0., 1000.,    0.],
            [   0.,    0.,    0., 1000.]]), (4, 4))

```python
fig = plt.figure(figsize=(6, 6))
im = plt.imshow(P, interpolation="none", cmap=plt.get_cmap('binary'))
plt.title('Initial Covariance Matrix $P$')
ylocs, ylabels = plt.yticks()
# set the locations of the yticks
plt.yticks(np.arange(7))
# set the locations and labels of the yticks
plt.yticks(np.arange(6),('$x$', '$y$', '$\dot x$', '$\dot y$'), fontsize=22)

xlocs, xlabels = plt.xticks()
# set the locations of the yticks
plt.xticks(np.arange(7))
# set the locations and labels of the yticks
plt.xticks(np.arange(6),('$x$', '$y$', '$\dot x$', '$\dot y$'), fontsize=22)

plt.xlim([-0.5,3.5])
plt.ylim([3.5, -0.5])

from mpl_toolkits.axes_grid1 import make_axes_locatable
divider = make_axes_locatable(plt.gca())
cax = divider.append_axes("right", "5%", pad="3%")
plt.colorbar(im, cax=cax);
```

## Dynamic Matrix $A$

It is calculated from the dynamics of the Egomotion.

$$x_{k+1} = x_{k} + \dot x_{k} \cdot \Delta t$$
$$y_{k+1} = y_{k} + \dot y_{k} \cdot \Delta t$$
$$\dot x_{k+1} = \dot x_{k}$$
$$\dot y_{k+1} = \dot y_{k}$$

```python
δt = 0.1 # Time Step between Filter Steps

A = np.matrix([[1.0, 0.0, δt, 0.0],
              [0.0, 1.0, 0.0, δt],
              [0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 1.0]])

A, A.shape
```

    (matrix([[1. , 0. , 0.1, 0. ],
             [0. , 1. , 0. , 0.1],
             [0. , 0. , 1. , 0. ],
             [0. , 0. , 0. , 1. ]]), (4, 4))

## Measurement Matrix $H$

We directly measure the Velocity $\dot x$ and $\dot y$

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

```python
H = np.matrix([[0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 1.0]])

H, H.shape
```

    (matrix([[0., 0., 1., 0.],
             [0., 0., 0., 1.]]), (2, 4))

## Measurement Noise Covariance $R$

Tells the Kalman Filter how 'bad' the sensor readings are.

$$R = \begin{bmatrix}\sigma^2_{\dot x} & 0 \\ 0 & \sigma^2_{\dot y} \end{bmatrix}$$

```python
rm = 100.0

R = np.matrix([[rm, 0],
               [0, rm]])

R, R.shape
```

    (matrix([[100.,   0.],
             [  0., 100.]]), (2, 2))

```python
# Plot between -10 and 10 with .001 steps.
xpdf = np.arange(-20, 20, 0.001)
plt.subplot(121)
plt.plot(xpdf, norm.pdf(xpdf,0,R[0,0]))
plt.title('$\dot x$')

plt.subplot(122)
plt.plot(xpdf, norm.pdf(xpdf,0,R[1,1]))
plt.title('$\dot y$')
plt.tight_layout()
```

### Process Noise Covariance $Q$

The Position of the car can be influenced by a force (e.g. wind), which leads to an acceleration disturbance (noise). This process noise has to be modeled with the process noise covariance matrix Q.

$$Q = \begin{bmatrix}\sigma_{x}^2 & \sigma_{xy} & \sigma_{x \dot x} & \sigma_{x \dot y} \\ \sigma_{yx} & \sigma_{y}^2 & \sigma_{y \dot x} & \sigma_{y \dot y} \\ \sigma_{\dot x x} & \sigma_{\dot x y} & \sigma_{\dot x}^2 & \sigma_{\dot x \dot y} \\ \sigma_{\dot y x} & \sigma_{\dot y y} & \sigma_{\dot y \dot x} & \sigma_{\dot y}^2 \end{bmatrix}$$

One can calculate Q as

$$Q = G\cdot G^T \cdot \sigma_v^2$$

with $G = \begin{bmatrix}0.5δt^2 & 0.5δt^2 & δt & δt\end{bmatrix}^T$ and $\sigma_v$ as the acceleration process noise, which can be assumed for a vehicle to be $8.8m/s^2$, according to: Schubert, R., Adam, C., Obst, M., Mattern, N., Leonhardt, V., & Wanielik, G. (2011). [Empirical evaluation of vehicular models for ego motion estimation](http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5940526). 2011 IEEE Intelligent Vehicles Symposium (IV), 534–539. doi:10.1109/IVS.2011.5940526

```python
sv = 8.8

G = np.matrix([[0.5*δt**2],
               [0.5*δt**2],
               [δt],
               [δt]])

Q = G*G.T*sv**2
Q
```

    matrix([[0.001936, 0.001936, 0.03872 , 0.03872 ],
            [0.001936, 0.001936, 0.03872 , 0.03872 ],
            [0.03872 , 0.03872 , 0.7744  , 0.7744  ],
            [0.03872 , 0.03872 , 0.7744  , 0.7744  ]])

```python
from sympy import Symbol, Matrix
from sympy.interactive import printing
printing.init_printing()
dt = Symbol('δt')
Qs = Matrix([[0.5*dt**2],[0.5*dt**2],[dt],[dt]])
Qs = Qs*Qs.T

Qs
```

$$\left[\begin{matrix}0.25 δt^{4} & 0.25 δt^{4} & 0.5 δt^{3} & 0.5 δt^{3}\\0.25 δt^{4} & 0.25 δt^{4} & 0.5 δt^{3} & 0.5 δt^{3}\\0.5 δt^{3} & 0.5 δt^{3} & δt^{2} & δt^{2}\\0.5 δt^{3} & 0.5 δt^{3} & δt^{2} & δt^{2}\end{matrix}\right]$$

```python
fig = plt.figure(figsize=(6, 6))
im = plt.imshow(Q, interpolation="none", cmap=plt.get_cmap('binary'))
plt.title('Process Noise Covariance Matrix $P$')
ylocs, ylabels = plt.yticks()
# set the locations of the yticks
plt.yticks(np.arange(7))
# set the locations and labels of the yticks
plt.yticks(np.arange(6),('$x$', '$y$', '$\dot x$', '$\dot y$'), fontsize=22)

xlocs, xlabels = plt.xticks()
# set the locations of the yticks
plt.xticks(np.arange(7))
# set the locations and labels of the yticks
plt.xticks(np.arange(6),('$x$', '$y$', '$\dot x$', '$\dot y$'), fontsize=22)

plt.xlim([-0.5,3.5])
plt.ylim([3.5, -0.5])

from mpl_toolkits.axes_grid1 import make_axes_locatable
divider = make_axes_locatable(plt.gca())
cax = divider.append_axes("right", "5%", pad="3%")
plt.colorbar(im, cax=cax);
```

## Identity Matrix $I$

```python
I = np.eye(4)

I, I.shape
```

    (array([[1., 0., 0., 0.],
            [0., 1., 0., 0.],
            [0., 0., 1., 0.],
            [0., 0., 0., 1.]]), (4, 4))

## Measurements

For example, we are using some random generated measurement values

```python
m = 100 # Measurements
vx= 20 # in X
vy= 10 # in Y

mx = np.array(vx+np.random.randn(m))
my = np.array(vy+np.random.randn(m))

measurements = np.vstack((mx,my))

print(measurements.shape)

print('Standard Deviation of Acceleration Measurements=%.2f' % np.std(mx))
print('You assumed %.2f in R.' % R[0,0])
```

    (2, 100)
    Standard Deviation of Acceleration Measurements=1.13
    You assumed 100.00 in R.

```python
fig = plt.figure(figsize=(16,5))

plt.step(range(m),mx, label='$\dot x$')
plt.step(range(m),my, label='$\dot y$')
plt.ylabel(r'Velocity $m/s$')
plt.title('Measurements')
plt.legend(loc='best',prop={'size':18})
```

```python
# Preallocation for Plotting
xt = []
yt = []
dxt= []
dyt= []
Zx = []
Zy = []
Px = []
Py = []
Pdx= []
Pdy= []
Rdx= []
Rdy= []
Kx = []
Ky = []
Kdx= []
Kdy= []

def savestates(x, Z, P, R, K):
    xt.append(float(x[0]))
    yt.append(float(x[1]))
    dxt.append(float(x[2]))
    dyt.append(float(x[3]))
    Zx.append(float(Z[0]))
    Zy.append(float(Z[1]))
    Px.append(float(P[0,0]))
    Py.append(float(P[1,1]))
    Pdx.append(float(P[2,2]))
    Pdy.append(float(P[3,3]))
    Rdx.append(float(R[0,0]))
    Rdy.append(float(R[1,1]))
    Kx.append(float(K[0,0]))
    Ky.append(float(K[1,0]))
    Kdx.append(float(K[2,0]))
    Kdy.append(float(K[3,0]))
```

```python
for n in range(len(measurements[0])):

    # Time Update (Prediction)
    # ========================
    # Project the state ahead
    x = A*x

    # Project the error covariance ahead
    P = A*P*A.T + Q

    # Measurement Update (Correction)
    # ===============================
    # Compute the Kalman Gain
    S = H*P*H.T + R
    K = (P*H.T) * np.linalg.pinv(S)

    # Update the estimate via z
    Z = measurements[:,n].reshape(2,1)
    y = Z - (H*x)                            # Innovation or Residual
    x = x + (K*y)

    # Update the error covariance
    P = (I - (K*H))*P

    # Save states (for Plotting)
    savestates(x, Z, P, R, K)
```

## Kalman Gains $K$

```python
def plot_K():
    fig = plt.figure(figsize=(16,9))
    plt.plot(range(len(measurements[0])),Kx, label='Kalman Gain for $x$')
    plt.plot(range(len(measurements[0])),Ky, label='Kalman Gain for $y$')
    plt.plot(range(len(measurements[0])),Kdx, label='Kalman Gain for $\dot x$')
    plt.plot(range(len(measurements[0])),Kdy, label='Kalman Gain for $\dot y$')

    plt.xlabel('Filter Step')
    plt.ylabel('')
    plt.title('Kalman Gain (the lower, the more the measurement fullfill the prediction)')
    plt.legend(loc='best',prop={'size':22})

plot_K()
```

## Uncertainty Matrix $P$

```python
def plot_P():
    fig = plt.figure(figsize=(16,9))
    plt.plot(range(len(measurements[0])),Px, label='$x$')
    plt.plot(range(len(measurements[0])),Py, label='$y$')
    plt.plot(range(len(measurements[0])),Pdx, label='$\dot x$')
    plt.plot(range(len(measurements[0])),Pdy, label='$\dot y$')

    plt.xlabel('Filter Step')
    plt.ylabel('')
    plt.title('Uncertainty (Elements from Matrix $P$)')
    plt.legend(loc='best',prop={'size':22})

plot_P()
```

## State Estimate $x$

```python
def plot_x():
    fig = plt.figure(figsize=(16,9))
    plt.step(range(len(measurements[0])),dxt, label='$\dot x$')
    plt.step(range(len(measurements[0])),dyt, label='$\dot y$')

    plt.axhline(vx, color='#999999', label='$\dot x_{real}$')
    plt.axhline(vy, color='#999999', label='$\dot y_{real}$')

    plt.xlabel('Filter Step')
    plt.title('Estimate (Elements from State Vector $x$)')
    plt.legend(loc='best',prop={'size':22})
    plt.ylim([0, 30])
    plt.ylabel('Velocity')

plot_x()
```

## Position x/y

```python
def plot_xy():
    fig = plt.figure(figsize=(16,16))
    plt.scatter(xt,yt, s=20, label='State', c='k')
    plt.scatter(xt[0],yt[0], s=100, label='Start', c='g')
    plt.scatter(xt[-1],yt[-1], s=100, label='Goal', c='r')

    plt.xlabel('X')
    plt.ylabel('Y')
    plt.title('Position')
    plt.legend(loc='best')
    plt.axis('equal')

plot_xy()
```

```python

```