# 1. Preliminaries.

Before talking about deep learning and deep learning framework. Let's establish a bit of notation. From here on, we will talk about optimizing models. Think of a model as a set of functions. For example, a model could be a family of functions described as
$$F(x; w) = x + w \text{ with } x,w \in \mathbb{R}$$
$$\text{or equivantly } F(x; w) = \{f:\mathbb{R}\rightarrow\mathbb{R}\texttt{ }\vert\texttt{ }f(x) = x + w, w \in \mathbb{R}\}$$

However, we will use the first notation. Now, let's make a very important distinction. We call the arguments before ";" **inputs** (such is $x$). Instead, we call the argument after ";" **parameters** (such is $w$).

In python a model could look something like this:

```python
def F(w):
    return lambda x: x + w

print(f"F returns functions. For example F(5) = {F(5)}")
print(f"if F returns functions we can call F(5). For example, F(5)(5) = {F(5)(5)}")
```

    F returns functions. For example F(5) = <function F.<locals>.<lambda> at 0x7fe3a81a3d00>
    if F returns functions we can call F(5). For example, F(5)(5) = 10

______

## 1.1 Training/Optimizing

What does it mean to train or optimize a model? It means searching among the family of functions for those who satisfy desired properties. For example, we could want to search for symmetric functions. Or we could search for functions that interpolate a set of points. Ultimately, this amount to find good parameters. You can search for good parameters any ways you desire. You can even randomly sample parameters until you find good ones.

______

## 1.2 Optimize a constant function.

Let's make a concrete, yet super simple example. This should help you to understand how all blocks fall together. So consider the following model:

$$F(;w_1) = w_1^2 + 1$$

Where $w_1 \in [-10,10]$. $F$ is a model which can be instantiated in infinitely many functions. Each one of these functions has no input and constant real output.  This function is not particularly useful to optimize but it can help you to understand the methodology.

Now suppose that we want to find in $F$ the function with the lowest output. For example, if we set $w_1 = 5$ we get $g(x) = 5^2+1 = 26$ which is not very good. Instead, if we set $w_1 = 2$ we get $s(x)=2^2+1=5$ which is already better. Let's formalize what we want:

$$\min_{w \in [-10,10]}\{F(;w_1)\} = \min_{w \in [-10, 10]}\{w_1^2 + 1\}$$

______

### 1.1.1 Zeroth Order Optimization (or derivative-free optimization)
Zeroth order optimization tries to optimize models without knowledge of gradients. 0th order optimization is useful when you cannot compute gradients of the model (it may require too much time, too much memory, or you do not know the model at all). These methods are often called black-box optimization, as they require only to be able to query the model. One common kind of zeroth-order algorithm is evolutionary ones.

Let's see a super simple example of Zeroth Order optimization that randomly search for the optimal value.

```python
import random

# let's define our model as before
def F1(w1):
    return lambda: w1**2 + 1

# let's search for good functions by randomly sampling w1.
best_score = float("+inf")
best_param = None
for i in range(10000):
    w1 = random.uniform(-10, 10)
    if F1(w1)() < best_score:
        best_score, best_param = F1(w1)(), w1

print(f"best parameter found w1={best_param} which yield f(w1)={F1(best_param)()}")
```

    best parameter found w1=-0.0003224070760445841 which yield f(w1)=1.0000001039463227

Which is pretty close to the optimal value ($1$). Unfortunately, randomly searching works only on super simple cases. If we want to tackle real world problems we need to searching more intelligently.

______

### 1.1.2 First Order Optimization

First-order optimization algorithms require the knowledge of the derivative of $F$. The most used first-order technique is gradient descent. With respect to zeroth-order algorithms, we have more knowledge so usually, we can obtain good results faster. However, computing gradients does require time and memory. Let's compute the derivative of $F$ wrt. $w_1$.

$$\frac{dF(;w_1)}{dw_1} = \frac{d w_1^2 + 1}{d w_1} = 2w_1$$

Now, no matter how we choose $w1$, we can compute the slope of the model. By knowing the slope, we can make small steps towards smaller and smaller functions. Let me visualize a little bit better what I mean.

```python
import matplotlib
%matplotlib widget
%matplotlib inline

import ipywidgets as widgets
import matplotlib.pyplot as plt
import numpy as np

@widgets.interact(w1=(-10, 10, 0.25))
def update(w1 = 1.0):
    w = np.linspace(-10, 10,1000)
    y = F1(w)()
    plt.plot(w, y)

    plt.scatter(w1, F1(w1)())

    x = np.linspace(-10,10)
    y = 2*x*w1 - F1(w1)() + 2
    plt.plot(x,y)

    plt.gca().set_xlim((-5,+5))
    plt.gca().set_ylim((-2,+10))

```

    interactive(children=(FloatSlider(value=1.0, description='w1', max=10.0, min=-10.0, step=0.25), Output()), _do…

The blue line is our model. Remember, each point of our model is actually a function, a constant function.For each of these points we can compute a derivative, the derivative tells us the slope of the model. By following the slope we can obtain a function with a higher value. By following the negative slope, we can obtain a function with a lower value. Step by step, we can obtain increasingly bigger functions of increasingly smaller functions.

Just for fun, let's write an algorithm that performs the gradient descent of our model.

```python
import random

def F     (w1): return lambda: w1**2 + 1
def dF_dw1(w1): return 2*w1

param_w1 = random.uniform(-10,10)
learning_rate = 0.001

for i in range(10000):
    param_w1 = param_w1 - learning_rate * dF_dw1(param_w1) # compute a small step in the direction of the slope

print(f"best parameter found w1={param_w1} which yield f(w1)={F(param_w1)()}")
```

    best parameter found w1=-1.281963925035065e-08 which yield f(w1)=1.0000000000000002

______

### 1.1.3 Second Order Optimization

We could go a step beyond and use even the second derivative. There are a lot of reasons to use and to not use the second derivative. However, we will not cover these optimization methods.
______

## 1.2 Optimize a simple function

Now that we have seen how to optimize a model that produced constant functions, we can now try to optimize a model that describes more interesting functions. Consider this model:

$$ F(x;w_1, w_2) = x*w_1 + w_2 $$

Firstly, let's see what kinds of functions does our $F$ describes.

```python
def F(w1, w2):
    return lambda x: x*w1+w2

@widgets.interact(w1=(-10, 10, 0.25), w2=(-10, 10, 0.25))
def plot(w1=7.5, w2=7.5):
    x = np.linspace(-10, 10)
    y = F(w1, w2)(x)
    plt.plot(x, y)
    plt.grid()
    plt.gca().set_xlim((-5,+5))
    plt.gca().set_ylim((-10,+10))

```

    interactive(children=(FloatSlider(value=7.5, description='w1', max=10.0, min=-10.0, step=0.25), FloatSlider(va…

These are all straight lines. No matter which $w_1$ and $w_2$ you will choose. $f$ will always describe a straight line. Let's suppose that we want to find the function with these input-output relations:

| x | y |
|---|---|
| 1 | 2 |
| 2 | 3 |
| 3 | 4 |
| 4 | 5 |

A function from our model is good when fed with $x$s it outputs something close to the described $y$s. Let's define a bit more formally what does this means. So, we derive yet another model from F, which we call L.

$$ L(x; w_1, w_2) = \sum_{(x,y) \in D} (F(x; w1, w2) - y)^2 $$

$L$ is close to $0$ when $F(x_i; w1, w2)$ is similar to $y_i$, for all $i$. In fact, $L$ is exactly $0$ when $F(x_i; w1, w2) = y_i$, for all $i$. So, optimizing this model yields exactly the property that we desire. Again, we need to compute the gradients.

Now we need the derivative wrt. $w_1$:
$$
\begin{align}
\frac{\partial L(x;w_1,w_2)}{\partial w_1} = & \frac{\partial \sum_{(x,y) \in D} (F(x; w1, w2) - y)^2}{d w_1}  \\
= & \sum_{(x,y) \in D} \frac{\partial(F(x; w1, w2) - y)^2}{\partial w_1} \\
= & \sum_{(x,y) \in D} \frac{\partial(x*w_1+w_2 - y)^2}{\partial w_1}  \\
= & \sum_{(x,y) \in D} 2(x*w_1+w_2 - y) \frac{\partial x*w_1+w_2 - y}{\partial w_1}  \\
= & \sum_{(x,y) \in D} 2(x*w_1+w_2 - y)x
\end{align}
$$

And also wrt. $w_2$:

$$
\begin{align}
\frac{\partial L(x;w_1,w_2)}{\partial w_2} = & \frac{d \sum_{(x,y) \in D} (F(x; w1, w2) - y)^2}{\partial w_2} \\
= & \sum_{(x,y) \in D} \frac{\partial(F(x; w1, w2) - y)^2}{\partial w_2} \\
= & \sum_{(x,y) \in D} \frac{\partial(x*w_1+w_2 - y)^2}{\partial w_2} \\
= & \sum_{(x,y) \in D} 2(x*w_1+w_2 - y) \frac{dx*w_1+w_2 - y}{\partial w_2} \\
= & \sum_{(x,y) \in D} 2(x*w_1+w_2 - y)
\end{align}
$$

Now, we have again all the pieces to perform our first order optimization, as we did before.

```python
import numpy as np
import random

x = np.array([1,2,3,4])
y = np.array([2,3,4,5])

def F(w1, w2):
    return lambda x: x*w1+w2

def dF_dw1(w1, w2):
    return (2*(x * w1 + w2 - y)*x).sum()

def dF_dw2(w1, w2):
    return (2*(x * w1 + w2 - y)).sum()

param_w1 = random.uniform(-10,10)
param_w2 = random.uniform(-10,10)
learning_rate = 0.001

for i in range(100000):
    param_w1, param_w2 = (param_w1 - learning_rate * dF_dw1(param_w1, param_w2),
                          param_w2 - learning_rate * dF_dw2(param_w1, param_w2))

print(f"best parameter found w1={param_w1, param_w2} which yield f(2;w1,w2)={F(param_w1, param_w2)(2)}")
```

    best parameter found w1=(1.0000000000000249, 0.999999999999931) which yield f(2;w1,w2)=2.999999999999981

_________________________________________
# 2. Automatic Differentiation.

As you have noticed, computing the gradients by hand is a real pain, and it becomes unmanageable real fast. Fortunately, this work can be done automatically. Again there are many ways to perform differentiation. Let's review a few of them. First let's define the gradient of a function $f(x_1, x_2, \dots, x_n)$

$$\nabla f(x_1, x_2, \dots, x_n) =
\begin{bmatrix}
\frac{\partial f}{\partial x_1}(x_1, x_2, \dots, x_n) \\
\frac{\partial f}{\partial x_2}(x_1, x_2, \dots, x_n) \\
\dots \\
\frac{\partial f}{\partial x_n}(x_1, x_2, \dots, x_n)
\end{bmatrix}$$

There are two main ways to compute $\nabla f(x_1, x_2, \dots, x_n)$.
* **Numerical differentiation**. We are only interested in computing the value of the gradient for a particular point in the domain of $f$.
* **Symbolic differentiation**. We are interested in the formula representing the gradients for $f$ for each point in the domain of $f$.

There are several techniques to compute both Numerical differentiation and Symbolic differentiation. Let's review very simple ones.
______

# 2.1. Numerical Differentiation.

To get an approximation of the derivative of a function $f$ in a point $(x_0, x_1, \dots, x_n)$, we can use the definition:

$$\nabla f(x_0, x_1, \dots, x_n) =
\begin{bmatrix}
\frac{\partial f}{\partial x_1}(x_1, x_2, \dots, x_n) \\
\frac{\partial f}{\partial x_2}(x_1, x_2, \dots, x_n) \\
\dots \\
\frac{\partial f}{\partial x_n}(x_1, x_2, \dots, x_n)
\end{bmatrix} =
\begin{bmatrix}
\lim_{\epsilon \rightarrow 0} \frac{f(x_0+\epsilon,x_1,\dots,x_n)-f(x_1, x_2, \dots, x_n)}{\epsilon} \\
\lim_{\epsilon \rightarrow 0} \frac{f(x_0,x_1+\epsilon,\dots,x_n)-f(x_1, x_2, \dots, x_n)}{\epsilon} \\
\dots \\
\lim_{\epsilon \rightarrow 0} \frac{f(x_0,x_1,\dots,x_n+\epsilon)-f(x_1, x_2, \dots, x_n)}{\epsilon}
\end{bmatrix}
$$

```python
def f1(x): return x**3
def dfdx(f,eps=0.001): return lambda x:(f(x+eps) - f(x))/(eps)

@widgets.interact(x0=(-2, 2, 0.001))
def plot(x0=1):
    x = np.linspace(-10, 10, 1000)
    y = f1(x)
    plt.plot(x, y)

    plt.scatter(x0, f1(x0))

    x = np.linspace(-10, 10, 1000)
    y = (x-x0)*dfdx(f1)(x0) + f1(x0)
    y1 = 3*x**2
    plt.plot(x, y)

    plt.gca().set_xlim((-5,+5))
    plt.gca().set_ylim((-10,+10))

```

    interactive(children=(FloatSlider(value=1.0, description='x0', max=2.0, min=-2.0, step=0.001), Output()), _dom…

This method is perfectly fine and usable. However, it requires computing $f$ as many times as the number of its arguments. Computing $f$ this way may require too much time.
______

## 2.2. Symbolic Differentiation

Instead of recomputing $f$ with small changes, we can compute the gradient symbolically ones. We can use the symbolic formula to obtain the numerical gradients at any point of the domain. Moreover, we can also apply optimizations to the symbolic representation of the gradients. But first, we need to talk about how we represent functions. Functions such as
$$f(x) = x^2 + 5x + 2$$
which may look complex, are actually a composition of simpler operations. In this case, there are only three operations: exponentiation ($pow(x,y)=x^y$), multiplication ($mul(x,y)=x*y$), and summation ($sum(x,y)=x+y$). Therefore:

$$f(x) = x^2 + 5x + 2 = sum(sum(pow(x,2),mul(5,x)),2)$$

_____
### 2.2.1 Chain Rule.

Before stepping into the heart of symbolic automatic differentiation, let's see how you can differentiate a composition of functions. Let $f(x) = g(h(x))$ then,

$$\frac{df(x)}{dx} = \frac{dg(h(x))}{dx} = \frac{dg}{dh}\frac{dh}{dx}$$

Or in the multivariate case. Let $f(x) = g(h_1(x),\dots,h_n(x))$

$$\frac{df(x)}{dx} =
\frac{g(h_1(x),\dots,h_n(x))}{dx} =
\frac{\partial g}{\partial h_1}\frac{dh_1}{dx} + \dots + \frac{\partial g}{\partial h_n}\frac{dh_n}{dx}$$
______

#### 2.2.1.1 Example 1 sum().

$$\frac{d sum(f(x),g(x))}{dx} =
\frac{\partial sum}{\partial f}\frac{df}{dx} + \frac{\partial sum}{\partial g}\frac{dg}{dx} =
\frac{df}{dx} + \frac{dg}{dx}$$
______

#### 2.2.1.2 Example 2 mul().

$$\frac{d mul(f(x),g(x))}{dx} =
\frac{\partial mul}{\partial f}\frac{df}{dx} + \frac{\partial mul}{\partial g}\frac{dg}{dx} =
g\frac{df}{dx} + f\frac{dg}{dx}$$
______

#### 2.2.1.3 Example 3 pow().

$$\frac{d pow(f(x),g(x))}{dx} =
\frac{\partial pow}{\partial f}\frac{df}{dx} + \frac{\partial pow}{\partial g}\frac{dg}{dx} =
gf^{g-1}\frac{df}{dx} + log(f)f^g\frac{dg}{dx}$$

Now, we have the tools to differentiate the function mentioned before (the one in the tree).

$$
\begin{aligned}
\frac{d sum(sum(pow(x,2),mul(5,x)),2)}{dx} =& \frac{d sum(pow(x,2),mul(5,x))}{dx} + \frac{d2}{dx} \\
= & \frac{d pow(x,2)}{dx} + \frac{d mul(5,x)}{dx} + 0 \\
= & 2x^{2-1}\frac{dx}{dx} + log(x)x^2\frac{d2}{dx} + x\frac{d5}{dx} + 5\frac{dx}{dx} \\
= & 2x + 0 + 0 + 5 \\
= & 2x + 5
\end{aligned}
$$

Ok, that was quite a bit of work. At least, it was fairly mechanical. Note that, we only used the knowledge of the derivative of $sum$, $sum$, and $pow$. Given these three pieces, we can derivate any composition of $sum$, $mul$, and $pow$. You can write a differentiation engine in very few lines of code.

```python
class sum:
    def __init__(self, f, g):
        self.f, self.g = f, g
    def diff(self, var):
        return f"({self.f.diff(var)} + {self.g.diff(var)})"
    def tostr(self):
        return f"({self.f.tostr()} + {self.g.tostr()})"

class mul:
    def __init__(self, f, g):
        self.f, self.g = f, g
    def diff(self, var):
        return f"({self.f.tostr()} * {self.g.diff(var)} + {self.g.tostr()} * {self.f.diff(var)})"
    def tostr(self):
        return f"({self.f.tostr()} * {self.g.tostr()})"

class pow:
    def __init__(self, f, g):
        self.f, self.g = f, g
    def diff(self, var):
        return f"({self.g.tostr()} * {self.f.tostr()} ^ ({self.g.tostr()}-1))({self.f.diff(var)}) + (log({self.f.tostr()}) * {self.f.tostr()} ^ {self.g.tostr()})({self.g.diff(var)})"
    def tostr(self):
        return f"({self.f.tostr()}) ^ ({self.g.tostr()})"

class var:
    def __init__(self, name):
        self.name = name
    def diff(self, var):
        if self.name == var.name: return "1"
        return "0"
    def tostr(self):
        return self.name

class const:
    def __init__(self, value):
        self.value = value
    def diff(self, var):
        return "0"
    def tostr(self):
        return self.value

```

```python
x    = var("x")
two  = const("2")
five = const("5")

f = sum(sum(pow(x, two), mul(five, x)), const(2))

print("     f(x) =", f.tostr())
print("d/dx f(x) =", f.diff(x))
```

         f(x) = (((x) ^ (2) + (5 * x)) + 2)
    d/dx f(x) = (((2 * x ^ (2-1))(1) + (log(x) * x ^ 2)(0) + (5 * 1 + x * 0)) + 0)

```python
x    = var("x")
y    = var("y")
two  = const("2")
five = const("5")

f = sum(sum(pow(x, two), mul(five, y)), const(2))
print("     f(x,y) =", f.tostr())
print("d/dx f(x,y) =", f.diff(x))
print("d/dy f(x,y) =", f.diff(y))

```

         f(x,y) = (((x) ^ (2) + (5 * y)) + 2)
    d/dx f(x,y) = (((2 * x ^ (2-1))(1) + (log(x) * x ^ 2)(0) + (5 * 0 + y * 0)) + 0)
    d/dy f(x,y) = (((2 * x ^ (2-1))(0) + (log(x) * x ^ 2)(0) + (5 * 1 + y * 0)) + 0)

We have seen a method for computing derivatives symbolically and a method for computing derivatives numerically. In practice, neither of these methods are used in modern automatic differentiation APIs. Usually, much more efficient algorithms are used to perform automatic differentiation. However, this introduction should give you a brief introduction to the main concepts used in automatic differentiation algorithms.

_____________________

# 3.Tensorflow

[Tensorflow] is one of the most famous automatic differentiation libraries. It provides API to access fast, parallel automatic differentiation libraries. [Tensorflow] does provide much more than automatic differentiation. It provides the whole infrastructure to define and train complex models with millions of parameters.
______

## 3.1 Tensorflow: Automatic Differentiation
Before defining a complex neural network, let's see how we can use [Tensorflow] to do what we have already done by hand.

[Tensorflow]: https://www.tensorflow.org/

```python
import tensorflow as tf

x = tf.Variable(10.0) # A variable is what we called a parameter.

for i in range(1000):

    with tf.GradientTape() as tape:
        # Here, operations perfomed are registered.
        # Later, these operations will be used to compute gradients.
        y = x**2 + 5*x + 2

    # Now, we can get the derivative of y with respect of x
    dy_dx = tape.gradient(y, x)

    # With the derivative, we can, for example, minimize f(x).
    x.assign(x - 0.001*dy_dx)

print(f"x**2 + 5*x + 2 is low for x={x.numpy()}. ({x.numpy()})**2 + 5*({x.numpy()}) + 2={x.numpy()**2 + 5*x.numpy() + 2}")
```

    x**2 + 5*x + 2 is low for x=-0.8116925954818726. (-0.8116925954818726)**2 + 5*(-0.8116925954818726) + 2=-1.399618107849264

In something like 5 lines of code, we obtained the same behavior that we obtained with our custom functions.

```python
import matplotlib
%matplotlib widget
%matplotlib inline

from IPython.display import clear_output

import ipywidgets        as widgets
import matplotlib.pyplot as plt
import numpy             as np
import tensorflow        as tf

slider = widgets.FloatSlider(description="lr", min=0, max=1,step=0.001)
button = widgets.Button(description="step")
output = widgets.Output()
display(button, slider, output)

x = tf.Variable(10.0)

def on_button_clicked(b):
    with output:
        with tf.GradientTape() as tape: y = x**2 + 5*x + 2
        x.assign(x - slider.value*tape.gradient(y, x))
        clear_output(wait=True)
        plt.plot(np.linspace(-10,10,100),
                 np.linspace(-10,10,100)**2 + 5*np.linspace(-10,10,100) + 2)
        plt.scatter([x],[x**2+5*x+2], c="red")
        plt.show()

button.on_click(on_button_clicked)

```

    Button(description='step', style=ButtonStyle())

    FloatSlider(value=0.0, description='lr', max=1.0, step=0.001)

    Output()

# Exercises
Try out the previous methods by minimizing these functions:
* $x^2 + x$ wrt. $x$.
* $x^4 -3x^2 +2$ wrt. $x$.
* $x^2 + y^2$ wrt. $x$ and $y$.

Try maximizing these functions:
* $-x^2 + 2 $
* $x^2 -x^4$


# Dump "Complementary filter"

Attitude estimation is determined from attitude propagation based on Gyro and measurements of tilt, roll and yaw
based on accelerometer and magnetometer, following [ahrs_python:doc](https://ahrs.readthedocs.io/en/latest/filters/complementary.html).

Basic idea is to combine attitude propagation (gyro, $\mathbf{q}_\mathrm{\omega}$) and attitude measurement
(magnetometer and accelormehter, $\mathbf{q}_\mathrm{am}$ with a complementary factor $\alpha$:

$
\mathbf{q} = (1 - \alpha) \mathbf{q}_\omega + \alpha\mathbf{q}_{am}, \quad \alpha \in (0, 1)
$

* yaw angle $\psi$: for North east down frame the angle between north and x axis in horizontal place
* pitch angle $\phi$: angle between horizontal plane and x axis
* roll angle $\theta$:

```python
from matplotlib import pyplot as plt
import numpy as np
import sympy
import quaternionic as quat
from scipy.integrate import cumtrapz
from sympy import sin, cos, symbols
from sympy.matrices import rot_axis1, rot_axis2, rot_axis3
from sympy.algebras.quaternion import Quaternion
```

## Configuration parameters

```python

```

```python
# num of samples
T = 15 #s
n = 50000

# Gyro
gyro_noise = 0.003
gyro_bias = [0.002, 0.02, 0.01]

# Accelerometer
acc_noise = 0.02
acc_bias = [0.1, 0.07, 0.1]

# Magnetometer
mag_noise = 0.01
mag_bias = np.zeros(3) #[0.03, 0.02, 0.04]
```

```python
# timesteps between samples
Δt = T / n

# rotation velocities in body frame
ωs = np.zeros((n, 3), dtype=float)
ωs[:,1] = np.sin(np.linspace(0, 2*np.pi, n))/25
```

```python
θs = cumtrapz(ωs, dx=Δt, axis=0, initial=0.0)

plt.plot(θs[:,0],'-')
plt.plot(θs[:,1],'--')
plt.plot(θs[:,2],'x', markevery=n//20)
```

```python
_tmp = np.array([[0.0, 1.0, 0.0]] * n) * θs[:,1][:,np.newaxis]
qs = quat.array.from_axis_angle(_tmp)
```

## Simulated Measurements

### Gyro

```python
ωs_ = ωs + np.random.normal(gyro_bias, scale=gyro_noise, size=(n,3))
```

```python
plt.plot(ωs_[:,0])
plt.plot(ωs_[:,1], '--')
plt.plot(ωs_[:,2], 'x', markevery=n//24)
```

### Accelerometer

```python
g = np.array([0,0,-1])

a = (qs.to_rotation_matrix @ g) \
    + np.random.normal(acc_bias, scale=acc_noise, size=(n, 3))
```

```python
plt.plot(a[:,0])
plt.plot(a[:,1])
plt.plot(a[:,2])
plt.grid()
```

### Magnetometer

```python
bs_0 = np.array([19355.97, 0, 4495.36]) # values from https://www.geomatrix.co.uk/tools/magnetic-field-calculator/
bs_0 = bs_0 / np.linalg.norm(bs_0)

ms = qs.to_rotation_matrix @ bs_0  \
    + np.random.normal(mag_bias, scale=mag_noise, size=(n,3))
```

```python
plt.plot(ms[:,0], label='0')
plt.plot(ms[:,1], label='1')
plt.plot(ms[:,2], label='2')

plt.legend();
```

## Attitude Propagation

```python
def propagate_attitude(q, ω, Δt):
    q0 = q.to_scalar_part
    q1, q2, q3 = q.to_vector_part
    G = np.array([
        [-q1, q0, q3,-q2],
        [-q2,-q3, q0, q1],
        [-q3, q2,-q1, q0]
    ])

    return q + quat.array(0.5 * G.T @ (ω * Δt))
```

```python
import sympy

qw, qx, qy, qz = sympy.symbols("q_w, q_x, q_y, q_z", real=True)
ωx, ωy, ωz, Δt = sympy.symbols(r"omega_x, omega_y, omega_z, dt", real=True)

G = sympy.Matrix([
        [-qx, qw, qz,-qy],
        [-qy,-qz, qw, qx],
        [-qz, qy,-qx, qw]
])

Q = sympy.Matrix([qw, qx, qy, qz])

sol = (Q + 0.5 * G.T * sympy.Matrix([ωx, ωy, ωz]) * Δt)

sol.simplify()

sol
```

$\displaystyle \left[\begin{matrix}- 0.5 dt \left(\omega_{x} q_{x} + \omega_{y} q_{y} + \omega_{z} q_{z}\right) + q_{w}\\0.5 dt \left(\omega_{x} q_{w} - \omega_{y} q_{z} + \omega_{z} q_{y}\right) + q_{x}\\0.5 dt \left(\omega_{x} q_{z} + \omega_{y} q_{w} - \omega_{z} q_{x}\right) + q_{y}\\0.5 dt \left(- \omega_{x} q_{y} + \omega_{y} q_{x} + \omega_{z} q_{w}\right) + q_{z}\end{matrix}\right]$

```python
sympy.lambdify((qw, qx, qy, qz, ωx, ωy, ωz, Δt), sol)(1, 0, 0, 0, 1.0, 1.0, 0.0, 0.1)
```

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

```python
sympy.ccode(sol)
```

    '// Not supported in C:\n// ImmutableDenseMatrix\nMatrix([\n[-0.5*dt*(omega_x*q_x + omega_y*q_y + omega_z*q_z) + q_w],\n[ 0.5*dt*(omega_x*q_w - omega_y*q_z + omega_z*q_y) + q_x],\n[ 0.5*dt*(omega_x*q_z + omega_y*q_w - omega_z*q_x) + q_y],\n[0.5*dt*(-omega_x*q_y + omega_y*q_x + omega_z*q_w) + q_z]])'

```python
import quaternionic as quat
import numpy as np
q_test = quat.array((1.0, 0.0, 0.0, 0.0))
a = propagate_attitude(q_test, np.array((0.1, 0.0, 0.0)), 0.1)
a / a.norm
```

    quaternionic.array([0.999975  , 0.00499988, 0.        , 0.        ])

```python
q_gyro = quat.array(np.zeros((n, 4)))
q_gyro[0, :] = (1.0, 0, 0, 0)

for i, ω in enumerate(ωs_[1:, :], start=1):
    q_gyro[i, :] = propagate_attitude(q_gyro[i-1,:], ω, Δt)
```

```python
plt.plot(q_gyro.to_axis_angle)
```

## Attitude Measurement (Acc, Mag)

pitch $\phi$ and roll $\theta$ from accelerometer

$
\begin{array}{rcl}
\theta &=& \mathrm{arctan2}(a_y, a_z) \\
\phi &=& \mathrm{arctan2}\big(-a_x, \sqrt{a_y^2+a_z^2}\big)
\end{array}
$

yaw $\psi$ from mag

$
\begin{array}{rcl}
\mathbf{b} &=&
\begin{bmatrix}
    \cos\theta & \sin\theta\sin\phi & \sin\theta\cos\phi \\
    0 & \cos\phi & -\sin\phi \\
    -\sin\theta & \cos\theta\sin\phi & \cos\theta\cos\phi
\end{bmatrix}
\begin{bmatrix}m_x \\ m_y \\ m_z\end{bmatrix} \\
\begin{bmatrix}b_x \\ b_y \\ b_z\end{bmatrix} &=&
\begin{bmatrix}
    m_x\cos\theta + m_y\sin\theta\sin\phi + m_z\sin\theta\cos\phi \\
    m_y\cos\phi - m_z\sin\phi \\
    -m_x\sin\theta + m_y\cos\theta\sin\phi + m_z\cos\theta\cos\phi
\end{bmatrix}
\end{array}
$

$
\begin{array}{rcl}
\psi &=& \mathrm{arctan2}(-b_y, b_x) \\
&=& \mathrm{arctan2}\big(m_z\sin\phi - m_y\cos\phi, \; m_x\cos\theta + \sin\theta(m_y\sin\phi + m_z\cos\phi)\big)
\end{array}
$

Combine into quaternion

$
\mathbf{q}_{am} =
\begin{pmatrix}q_w\\q_x\\q_y\\q_z\end{pmatrix} =
\begin{pmatrix}
    \cos\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) + \sin\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\
    \sin\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) - \cos\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\
    \cos\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big) + \sin\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) \\
    \cos\Big(\frac{\phi}{2}\Big)\cos\Big(\frac{\theta}{2}\Big)\sin\Big(\frac{\psi}{2}\Big) - \sin\Big(\frac{\phi}{2}\Big)\sin\Big(\frac{\theta}{2}\Big)\cos\Big(\frac{\psi}{2}\Big)
\end{pmatrix}
$

```python
m_θ = np.arctan2(a[:, 1], a[:, 2]) - np.pi
m_ϕ = np.arctan2(-a[:, 0], np.sqrt(a[:, 1]**2 + a[:, 2]**2))
m_ψ = np.zeros(n) #np.arctan2(ms[:, 2] * sin(m_ϕ) - ms[:, 1] * cos(m_ϕ), ms[:, 0] * cos(m_θ) + (ms[:, 1] * sin(m_ϕ) + ms[:, 2] * cos(m_ϕ)) * sin(m_θ))
```

```python
measured_angles = np.vstack((m_θ, m_ϕ, m_ψ)).T
q_am = quat.array.from_axis_angle(measured_angles)
```

```python
plt.plot(q_am.to_axis_angle)
```

## Filtering

$
\mathbf{q} = (1 - \alpha) \mathbf{q}_\omega + \alpha\mathbf{q}_{am}, \quad \alpha \in (0, 1)
$

```python
α = 0.4

q_filter = (1 - α) * q_gyro + α * q_am
```

```python
plt.plot(q_filter.to_axis_angle)
plt.ylim(-0.1, 0.4)
```

```python
α = 0.7

q_filter = (1 - α) * q_gyro + α * q_am
```

```python
plt.plot(q_filter.to_axis_angle)
plt.ylim(-0.1, 0.4)
```

```python

```