# Sympy codes to compute the inner expansion of Laplace and Helmholtz double-layer potentials on high aspect ratio ellipse

The expansions below are used in the manuscript

C. Carvalho, A. D. Kim, L. Lewis, and Z. Moitier, _Quadrature by Parity Asymptotic eXpansions (QPAX) for scattering by high aspect ratio particles_.

### Zoïs Moitier, Camille Carvalho (2021)

_Karlsruhe Institute of Technology, Germany_

_University of California Merced, USA_

```python
import sympy as sy
from IPython.display import display
```

```python
order = 3  # order of the expansion
```

```python
ε, δ, h = sy.symbols("varepsilon delta h", real=True, positive=True)
s, t, X = sy.symbols("s t X", real=True)
μ_term = sy.symbols(f"mu0:{order}", cls=sy.Function)
```

## The Kernel $K^L(s, t; \varepsilon)$ for Laplace's double-layer potential

```python
KL = -(ε) / (2 * sy.pi * (1 + ε ** 2 + (1 - ε ** 2) * sy.cos(s + t)))
display(KL)
```

$\displaystyle - \frac{\varepsilon}{2 \pi \left(\varepsilon^{2} + \left(1 - \varepsilon^{2}\right) \cos{\left(s + t \right)} + 1\right)}$

## Expansion of the kernel $K^L(s, \pi - s + \varepsilon X; \varepsilon)$

```python
KL_series = KL.subs(t, sy.pi - s + ε * X).series(x=ε, x0=0, n=order).expand()
display(KL_series)
```

$\displaystyle - \frac{1}{\pi X^{2} \varepsilon + 4 \pi \varepsilon} - \frac{X^{2} \varepsilon}{\pi X^{4} + 8 \pi X^{2} + 16 \pi} - \frac{X^{4} \varepsilon}{12 \pi X^{4} + 96 \pi X^{2} + 192 \pi} + O\left(\varepsilon^{3}\right)$

## Expansion of the function $\mu(\pi - s + \varepsilon X)$

```python
μ_series = sum((ε * X) ** i * μ_(sy.pi - s) for i, μ_ in enumerate(μ_term)) + sy.O(
    ε ** order
)
display(μ_series)
```

$\displaystyle \mu_{0}{\left(\pi - s \right)} + X \varepsilon \mu_{1}{\left(\pi - s \right)} + X^{2} \varepsilon^{2} \mu_{2}{\left(\pi - s \right)} + O\left(\varepsilon^{3}\right)$

## Expansion of the integrand $K^L(s, \pi-s+\varepsilon X; \varepsilon) \, \mu(\pi - s + \varepsilon X)\varepsilon$

```python
expr = (KL_series * μ_series * ε).expand()
display(expr)
```

$\displaystyle - \frac{\varepsilon \mu_{0}{\left(\pi - s \right)}}{\pi X^{2} \varepsilon + 4 \pi \varepsilon} - \frac{X \varepsilon^{2} \mu_{1}{\left(\pi - s \right)}}{\pi X^{2} \varepsilon + 4 \pi \varepsilon} - \frac{X^{2} \varepsilon^{2} \mu_{0}{\left(\pi - s \right)}}{\pi X^{4} + 8 \pi X^{2} + 16 \pi} - \frac{X^{2} \varepsilon^{3} \mu_{2}{\left(\pi - s \right)}}{\pi X^{2} \varepsilon + 4 \pi \varepsilon} - \frac{X^{4} \varepsilon^{2} \mu_{0}{\left(\pi - s \right)}}{12 \pi X^{4} + 96 \pi X^{2} + 192 \pi} + O\left(\varepsilon^{3}\right)$

## Inner expansion $\int_{-\frac{\delta}{2\varepsilon}}^{\frac{\delta}{2\varepsilon}} K^L(s, \pi-s+\varepsilon X; \varepsilon) \, \mu(\pi - s + \varepsilon X) \varepsilon\, \mathrm{d}X$

```python
int_expr = sy.O(ε ** 2)
for i in range(order):
    expr_term = expr.coeff(ε, i)
    for u_ in μ_term:
        expr_term_u = expr_term.coeff(u_(sy.pi - s), 1).factor()
        int_term = sy.integrate(expr_term_u, (X, -δ / (2 * ε), δ / (2 * ε))).expand()
        int_expr += (int_term * u_(sy.pi - s) * ε ** i).factor().expand()

display(int_expr)
```

$\displaystyle - \frac{\mu_{0}{\left(\pi - s \right)} \operatorname{atan}{\left(\frac{\delta}{4 \varepsilon} \right)}}{\pi} - \frac{\delta^{3} \varepsilon \mu_{0}{\left(\pi - s \right)}}{12 \pi \delta^{2} + 192 \pi \varepsilon^{2}} + O\left(\varepsilon^{2}\right)$

## The Kernel $K(s, t; \varepsilon) = K_1(s, t; \varepsilon)\log (4 \sin(\frac{s-t}{2})^2) + K_2(s, t; \varepsilon)$ for Helmholtz double-layer potential

```python
ε, δ, h, k = sy.symbols("varepsilon delta h k", real=True, positive=True)
s, t, X = sy.symbols("s t X", real=True)
u_term = sy.symbols(f"u0:{order}", cls=sy.Function)
```

```python
zε = (
    2
    * k
    * abs(sy.sin((s - t) / 2))
    * sy.sqrt(sy.cos((s + t) / 2) ** 2 + ε ** 2 * sy.sin((s + t) / 2) ** 2)
)
display(zε)
```

$\displaystyle 2 k \sqrt{\varepsilon^{2} \sin^{2}{\left(\frac{s}{2} + \frac{t}{2} \right)} + \cos^{2}{\left(\frac{s}{2} + \frac{t}{2} \right)}} \left|{\sin{\left(\frac{s}{2} - \frac{t}{2} \right)}}\right|$

```python
K1 = -(zε * KL * sy.besselj(1, zε)) / 2
display(K1)
```

$\displaystyle \frac{k \varepsilon \sqrt{\varepsilon^{2} \sin^{2}{\left(\frac{s}{2} + \frac{t}{2} \right)} + \cos^{2}{\left(\frac{s}{2} + \frac{t}{2} \right)}} \left|{\sin{\left(\frac{s}{2} - \frac{t}{2} \right)}}\right| J_{1}\left(2 k \sqrt{\varepsilon^{2} \sin^{2}{\left(\frac{s}{2} + \frac{t}{2} \right)} + \cos^{2}{\left(\frac{s}{2} + \frac{t}{2} \right)}} \left|{\sin{\left(\frac{s}{2} - \frac{t}{2} \right)}}\right|\right)}{2 \pi \left(\varepsilon^{2} + \left(1 - \varepsilon^{2}\right) \cos{\left(s + t \right)} + 1\right)}$

```python
log_term = sy.ln(4 * sy.sin((s - t) / 2) ** 2)
K2 = (
    -((sy.I * sy.pi * sy.hankel1(1, zε) + sy.besselj(1, zε) * log_term) * (KL * zε)) / 2
)
display(K2)
```

$\displaystyle \frac{k \varepsilon \sqrt{\varepsilon^{2} \sin^{2}{\left(\frac{s}{2} + \frac{t}{2} \right)} + \cos^{2}{\left(\frac{s}{2} + \frac{t}{2} \right)}} \left(\log{\left(4 \sin^{2}{\left(\frac{s}{2} - \frac{t}{2} \right)} \right)} J_{1}\left(2 k \sqrt{\varepsilon^{2} \sin^{2}{\left(\frac{s}{2} + \frac{t}{2} \right)} + \cos^{2}{\left(\frac{s}{2} + \frac{t}{2} \right)}} \left|{\sin{\left(\frac{s}{2} - \frac{t}{2} \right)}}\right|\right) + i \pi H^{(1)}_{1}\left(2 k \sqrt{\varepsilon^{2} \sin^{2}{\left(\frac{s}{2} + \frac{t}{2} \right)} + \cos^{2}{\left(\frac{s}{2} + \frac{t}{2} \right)}} \left|{\sin{\left(\frac{s}{2} - \frac{t}{2} \right)}}\right|\right)\right) \left|{\sin{\left(\frac{s}{2} - \frac{t}{2} \right)}}\right|}{2 \pi \left(\varepsilon^{2} + \left(1 - \varepsilon^{2}\right) \cos{\left(s + t \right)} + 1\right)}$

## Expansion of the kernel $K_1(s, \pi - s + \varepsilon X; \varepsilon)\log (4 \sin(\frac{s-t}{2})^2)u(s, \pi - s + \varepsilon X; \varepsilon)\varepsilon$

```python
def expansion(expr):
    return expr.subs(t, sy.pi - s + ε * X).series(x=ε, x0=0, n=order).expand()

dist = (
    sy.sqrt(2)
    * sy.Abs(sy.sin((s - t) / 2))
    * sy.sqrt(1 + ε ** 2 + (1 - ε ** 2) * sy.cos(s + t))
)
z_series = expansion(k * dist).subs(sy.cos(s) * sy.sign(sy.cos(s)), sy.Abs(sy.cos(s)))

J1 = sy.besselj(1, z_series.expand().removeO())
J1_series = J1.series(x=ε, x0=0, n=order)

u_series = sum((ε * X) ** i * u_(sy.pi - s) for i, u_ in enumerate(u_term)) + sy.O(
    ε ** order
)
log_term = sy.ln(1 + ε ** 2 + (1 - ε ** 2) * sy.cos(s + t))
log_series = expansion(log_term)

expr1 = (ε * z_series * u_series * J1_series * KL_series * log_series).expand()
display(expr1)
```

$\displaystyle - \frac{4 k^{2} \varepsilon^{3} \operatorname{u_{0}}{\left(\pi - s \right)} \log{\left(\varepsilon \right)} \cos^{2}{\left(s \right)}}{\pi X^{2} \varepsilon + 4 \pi \varepsilon} + \frac{4 k^{2} \varepsilon^{3} \operatorname{u_{0}}{\left(\pi - s \right)} \log{\left(2 \right)} \cos^{2}{\left(s \right)}}{2 \pi X^{2} \varepsilon + 8 \pi \varepsilon} - \frac{4 k^{2} \varepsilon^{3} \operatorname{u_{0}}{\left(\pi - s \right)} \log{\left(X^{2} + 4 \right)} \cos^{2}{\left(s \right)}}{2 \pi X^{2} \varepsilon + 8 \pi \varepsilon} - \frac{4 X k^{2} \varepsilon^{4} \operatorname{u_{1}}{\left(\pi - s \right)} \log{\left(\varepsilon \right)} \cos^{2}{\left(s \right)}}{\pi X^{2} \varepsilon + 4 \pi \varepsilon} - \frac{4 X k^{2} \varepsilon^{4} \operatorname{u_{0}}{\left(\pi - s \right)} \log{\left(\varepsilon \right)} \sin{\left(s \right)} \left|{\cos{\left(s \right)}}\right| \operatorname{sign}{\left(\cos{\left(s \right)} \right)}}{\pi X^{2} \varepsilon + 4 \pi \varepsilon} + \frac{4 X k^{2} \varepsilon^{4} \operatorname{u_{1}}{\left(\pi - s \right)} \log{\left(2 \right)} \cos^{2}{\left(s \right)}}{2 \pi X^{2} \varepsilon + 8 \pi \varepsilon} - \frac{4 X k^{2} \varepsilon^{4} \operatorname{u_{1}}{\left(\pi - s \right)} \log{\left(X^{2} + 4 \right)} \cos^{2}{\left(s \right)}}{2 \pi X^{2} \varepsilon + 8 \pi \varepsilon} + \frac{4 X k^{2} \varepsilon^{4} \operatorname{u_{0}}{\left(\pi - s \right)} \log{\left(2 \right)} \sin{\left(s \right)} \left|{\cos{\left(s \right)}}\right| \operatorname{sign}{\left(\cos{\left(s \right)} \right)}}{2 \pi X^{2} \varepsilon + 8 \pi \varepsilon} - \frac{4 X k^{2} \varepsilon^{4} \operatorname{u_{0}}{\left(\pi - s \right)} \log{\left(X^{2} + 4 \right)} \sin{\left(s \right)} \left|{\cos{\left(s \right)}}\right| \operatorname{sign}{\left(\cos{\left(s \right)} \right)}}{2 \pi X^{2} \varepsilon + 8 \pi \varepsilon} - \frac{X^{2} k^{2} \varepsilon^{3} \operatorname{u_{0}}{\left(\pi - s \right)} \log{\left(\varepsilon \right)} \cos^{2}{\left(s \right)}}{\pi X^{2} \varepsilon + 4 \pi \varepsilon} + \frac{X^{2} k^{2} \varepsilon^{3} \operatorname{u_{0}}{\left(\pi - s \right)} \log{\left(2 \right)} \cos^{2}{\left(s \right)}}{2 \pi X^{2} \varepsilon + 8 \pi \varepsilon} - \frac{X^{2} k^{2} \varepsilon^{3} \operatorname{u_{0}}{\left(\pi - s \right)} \log{\left(X^{2} + 4 \right)} \cos^{2}{\left(s \right)}}{2 \pi X^{2} \varepsilon + 8 \pi \varepsilon} - \frac{X^{3} k^{2} \varepsilon^{4} \operatorname{u_{1}}{\left(\pi - s \right)} \log{\left(\varepsilon \right)} \cos^{2}{\left(s \right)}}{\pi X^{2} \varepsilon + 4 \pi \varepsilon} - \frac{X^{3} k^{2} \varepsilon^{4} \operatorname{u_{0}}{\left(\pi - s \right)} \log{\left(\varepsilon \right)} \sin{\left(s \right)} \left|{\cos{\left(s \right)}}\right| \operatorname{sign}{\left(\cos{\left(s \right)} \right)}}{\pi X^{2} \varepsilon + 4 \pi \varepsilon} + \frac{X^{3} k^{2} \varepsilon^{4} \operatorname{u_{1}}{\left(\pi - s \right)} \log{\left(2 \right)} \cos^{2}{\left(s \right)}}{2 \pi X^{2} \varepsilon + 8 \pi \varepsilon} - \frac{X^{3} k^{2} \varepsilon^{4} \operatorname{u_{1}}{\left(\pi - s \right)} \log{\left(X^{2} + 4 \right)} \cos^{2}{\left(s \right)}}{2 \pi X^{2} \varepsilon + 8 \pi \varepsilon} + \frac{X^{3} k^{2} \varepsilon^{4} \operatorname{u_{0}}{\left(\pi - s \right)} \log{\left(2 \right)} \sin{\left(s \right)} \left|{\cos{\left(s \right)}}\right| \operatorname{sign}{\left(\cos{\left(s \right)} \right)}}{2 \pi X^{2} \varepsilon + 8 \pi \varepsilon} - \frac{X^{3} k^{2} \varepsilon^{4} \operatorname{u_{0}}{\left(\pi - s \right)} \log{\left(X^{2} + 4 \right)} \sin{\left(s \right)} \left|{\cos{\left(s \right)}}\right| \operatorname{sign}{\left(\cos{\left(s \right)} \right)}}{2 \pi X^{2} \varepsilon + 8 \pi \varepsilon} + O\left(\varepsilon^{4} \log{\left(\varepsilon \right)}\right)$

## Expansion of the kernel $K_2(s, \pi - s + \varepsilon X; \varepsilon)u(s, \pi - s + \varepsilon X; \varepsilon)\varepsilon$
In this case we expand the Hankel function of the first kind and first order using the NIST handbook of mathematical functions [https://dlmf.nist.gov/10.7]

```python
Y1approx = -2 / (sy.pi * z_series) + (2 / sy.pi) * sy.ln(z_series / 2) * J1_series
Y1_series = Y1approx.series(x=ε, x0=0, n=order)

expr2 = (
    ε
    * z_series
    * u_series
    * KL_series
    * (J1_series * log_series + sy.I * sy.pi * J1_series - sy.pi * Y1_series)
    / 2
).expand()
display(expr2)
```

$\displaystyle - \frac{\varepsilon \operatorname{u_{0}}{\left(\pi - s \right)}}{\pi X^{2} \varepsilon + 4 \pi \varepsilon} - \frac{X \varepsilon^{2} \operatorname{u_{1}}{\left(\pi - s \right)}}{\pi X^{2} \varepsilon + 4 \pi \varepsilon} + \frac{X \varepsilon^{2} \operatorname{u_{0}}{\left(\pi - s \right)} \sin{\left(s \right)} \left|{\cos{\left(s \right)}}\right| \operatorname{sign}{\left(\cos{\left(s \right)} \right)}}{2 \pi X^{2} \varepsilon \cos^{2}{\left(s \right)} + 8 \pi \varepsilon \cos^{2}{\left(s \right)}} - \frac{X \varepsilon^{2} \operatorname{u_{0}}{\left(\pi - s \right)} \sin{\left(s \right)} \operatorname{sign}{\left(\cos{\left(s \right)} \right)}}{2 \pi X^{2} \varepsilon \left|{\cos{\left(s \right)}}\right| + 8 \pi \varepsilon \left|{\cos{\left(s \right)}}\right|} + O\left(\varepsilon^{2}\right)$

## Inner expansion $\int_{-\frac{\delta}{2\varepsilon}}^{\frac{\delta}{2\varepsilon}} K_1(s, \pi-s+\varepsilon X; \varepsilon)\log (4 \sin(\frac{s-t}{2})^2) \, u(\pi - s + \varepsilon X) \varepsilon\, \mathrm{d}X$

```python
int_expr1 = sy.O(ε ** 2)
for i in range(order):
    expr1_term = expr1.coeff(ε, i)
    for u_ in u_term:
        expr1_term_u = expr1_term.coeff(u_(sy.pi - s), 1).factor()
        int_term1 = sy.integrate(expr1_term_u, (X, -δ / (2 * ε), δ / (2 * ε))).expand()
        int_expr1 += (int_term1 * u_(sy.pi - s) * ε ** i).factor().expand()

display(int_expr1)
```

$\displaystyle O\left(\varepsilon^{2}\right)$

## Inner expansion $\int_{-\frac{\delta}{2\varepsilon}}^{\frac{\delta}{2\varepsilon}} K_2(s, \pi-s+\varepsilon X; \varepsilon) u(\pi - s + \varepsilon X) \varepsilon\, \mathrm{d}X$

```python
int_expr2 = sy.O(ε ** 2)
for i in range(order):
    expr2_term = expr2.coeff(ε, i)
    for u_ in u_term:
        expr2_term_u = expr2_term.coeff(u_(sy.pi - s), 1).factor()
        int_term2 = sy.integrate(expr2_term_u, (X, -δ / (2 * ε), δ / (2 * ε))).expand()
        int_expr2 += (int_term2 * u_(sy.pi - s) * ε ** i).factor().expand()

display(int_expr2)
```

$\displaystyle - \frac{\operatorname{u_{0}}{\left(\pi - s \right)} \operatorname{atan}{\left(\frac{\delta}{4 \varepsilon} \right)}}{\pi} + O\left(\varepsilon^{2}\right)$


Non Linear Regression Analysis

If the data shows a curvy trend, then linear regression will not produce very accurate results when compared to a non-linear regression because, as the name implies, linear regression presumes that the data is linear.
Let's learn about non linear regressions and apply an example on python. In this notebook, we fit a non-linear model to the datapoints corrensponding to China's GDP from 1960 to 2014.

Importing required libraries

```python
import numpy as np
import math as m
import matplotlib.pyplot as plt
%matplotlib inline
```

Though Linear regression is very good to solve many problems, it cannot be used for all datasets. First recall how linear regression, could model a dataset. It models a linear relation between a dependent variable y and independent variable x. It had a simple equation, of degree 1, for example y = $2x$ + 3.

```python
x = np.arange(-5.0, 5.0, 0.1)

##You can adjust the slope and intercept to verify the changes in the graph
y = 2*(x) + 3
y_noise = 2 * np.random.normal(size=x.size)
ydata = y + y_noise
#plt.figure(figsize=(8,6))
plt.plot(x, ydata,  'bo')
plt.plot(x,y, 'r')
plt.ylabel('Dependent Variable')
plt.xlabel('Indepdendent Variable')
plt.show()
```

Non-linear regressions are a relationship between independent variables $x$ and a dependent variable $y$ which result in a non-linear function modeled data. Essentially any relationship that is not linear can be termed as non-linear, and is usually represented by the polynomial of $k$ degrees (maximum power of $x$).

$$ \ y = a x^3 + b x^2 + c x + d \ $$

Non-linear functions can have elements like exponentials, logarithms, fractions, and others. For example: $$ y = \log(x)$$

Or even, more complicated such as :
$$ y = \log(a x^3 + b x^2 + c x + d)$$

Let's take a look at a cubic function's graph.

```python
x = np.arange(-5.0, 5.0, 0.1)

##You can adjust the slope and intercept to verify the changes in the graph
y = 1*(x**3) + 1*(x**2) + 1*x + 3
y_noise = 20 * np.random.normal(size=x.size)
ydata = y + y_noise
plt.plot(x, ydata,  'bo')
plt.plot(x,y, 'r')
plt.ylabel('Dependent Variable')
plt.xlabel('Indepdendent Variable')
plt.show()
```

As you can see, this function has $x^3$ and $x^2$ as independent variables. Also, the graphic of this function is not a straight line over the 2D plane. So this is a non-linear function.

Some other types of non-linear functions are:

### Quadratic

$$ Y = X^2 $$

```python
x = np.arange(-5.0, 5.0, 0.1)

##You can adjust the slope and intercept to verify the changes in the graph

y = np.power(x,2)
y_noise = 2 * np.random.normal(size=x.size)
ydata = y + y_noise
plt.plot(x, ydata,  'bo')
plt.plot(x,y, 'r')
plt.ylabel('Dependent Variable')
plt.xlabel('Indepdendent Variable')
plt.show()
```

### Exponential

An exponential function with base c is defined by $$ Y = a + b c^X$$ where b ≠0, c > 0 , c ≠1, and x is any real number. The base, c, is constant and the exponent, x, is a variable.

```python
X = np.arange(-5.0, 5.0, 0.1)

##You can adjust the slope and intercept to verify the changes in the graph

Y= np.exp(X)

plt.plot(X,Y)
plt.ylabel('Dependent Variable')
plt.xlabel('Indepdendent Variable')
plt.show()
```

### Logarithmic

The response $y$ is a results of applying logarithmic map from input $x$'s to output variable $y$. It is one of the simplest form of __log()__: i.e. $$ y = \log(x)$$

Please consider that instead of $x$, we can use $X$, which can be polynomial representation of the $x$'s. In general form it would be written as
\begin{equation}
y = \log(X)
\end{equation}

```python
X = np.arange(-5.0, 5.0, 0.1)

Y = np.log(X)

plt.plot(X,Y)
plt.ylabel('Dependent Variable')
plt.xlabel('Indepdendent Variable')
plt.show()
```

### Sigmoidal/Logistic

$$ Y = a + \frac{b}{1+ c^{(X-d)}}$$

```python
X = np.arange(-5.0, 5.0, 0.1)

Y = 1/(1+np.power(m.e, -X))

plt.plot(X,Y)
plt.ylabel('Dependent Variable')
plt.xlabel('Indepdendent Variable')
plt.show()
```

# Non-Linear Regression example

For an example, we're going to try and fit a non-linear model to the datapoints corresponding to China's GDP from 1960 to 2014. We download a dataset with two columns, the first, a year between 1960 and 2014, the second, China's corresponding annual gross domestic income in US dollars for that year.

```python
import numpy as np
import pandas as pd

#downloading dataset
!wget -nv -O china_gdp.csv https://s3-api.us-geo.objectstorage.softlayer.net/cf-courses-data/CognitiveClass/ML0101ENv3/labs/china_gdp.csv

df = pd.read_csv("china_gdp.csv")
df.head(10)
```

    2020-04-14 14:10:36 URL:https://s3-api.us-geo.objectstorage.softlayer.net/cf-courses-data/CognitiveClass/ML0101ENv3/labs/china_gdp.csv [1218/1218] -> "china_gdp.csv" [1]

      Year
      Value

      0
      1960
      5.918412e+10

      1
      1961
      4.955705e+10

      2
      1962
      4.668518e+10

      3
      1963
      5.009730e+10

      4
      1964
      5.906225e+10

      5
      1965
      6.970915e+10

      6
      1966
      7.587943e+10

      7
      1967
      7.205703e+10

      8
      1968
      6.999350e+10

      9
      1969
      7.871882e+10

__Did you know?__ When it comes to Machine Learning, you will likely be working with large datasets. As a business, where can you host your data? IBM is offering a unique opportunity for businesses, with 10 Tb of IBM Cloud Object Storage: [Sign up now for free](http://cocl.us/ML0101EN-IBM-Offer-CC)

### Plotting the Dataset ###
This is what the datapoints look like. It kind of looks like an either logistic or exponential function. The growth starts off slow, then from 2005 on forward, the growth is very significant. And finally, it decelerate slightly in the 2010s.

```python
plt.figure(figsize=(8,5))
x_data, y_data = (df["Year"].values, df["Value"].values)
plt.plot(x_data, y_data, 'ro')
plt.ylabel('GDP')
plt.xlabel('Year')
plt.show()
```

### Choosing a model ###

From an initial look at the plot, we determine that the logistic function could be a good approximation,
since it has the property of starting with a slow growth, increasing growth in the middle, and then decreasing again at the end; as illustrated below:

```python
X = np.arange(-5.0, 5.0, 0.1)
Y = 1.0 / (1.0 + np.exp(-X))

plt.plot(X,Y)
plt.ylabel('Dependent Variable')
plt.xlabel('Indepdendent Variable')
plt.show()
```

The formula for the logistic function is the following:

$$ \hat{Y} = \frac1{1+e^{\beta_1(X-\beta_2)}}$$

$\beta_1$: Controls the curve's steepness,

$\beta_2$: Slides the curve on the x-axis.

### Building The Model ###
Now, let's build our regression model and initialize its parameters.

```python
def sigmoid(x, Beta_1, Beta_2):
     y = 1 / (1 + np.exp(-Beta_1*(x-Beta_2)))
     return y
```

Lets look at a sample sigmoid line that might fit with the data:

```python
beta_1 = 0.10
beta_2 = 1990.0

#logistic function
Y_pred = sigmoid(x_data, beta_1 , beta_2)

#plot initial prediction against datapoints
plt.plot(x_data, Y_pred*15000000000000.)
plt.plot(x_data, y_data, 'ro')
```

Our task here is to find the best parameters for our model. Lets first normalize our x and y:

```python
# Lets normalize our data
xdata =x_data/max(x_data)
ydata =y_data/max(y_data)
```

#### How we find the best parameters for our fit line?
we can use __curve_fit__ which uses non-linear least squares to fit our sigmoid function, to data. Optimal values for the parameters so that the sum of the squared residuals of sigmoid(xdata, *popt) - ydata is minimized.

popt are our optimized parameters.

```python
from scipy.optimize import curve_fit
popt, pcov = curve_fit(sigmoid, xdata, ydata)
#print the final parameters
print(" beta_1 = %f, beta_2 = %f" % (popt[0], popt[1]))
```

     beta_1 = 690.447527, beta_2 = 0.997207

Now we plot our resulting regression model.

```python
x = np.linspace(1960, 2015, 55)
x = x/max(x)
plt.figure(figsize=(8,5))
y = sigmoid(x, *popt)
plt.plot(xdata, ydata, 'ro', label='data')
plt.plot(x,y, linewidth=3.0, label='fit')
plt.legend(loc='best')
plt.ylabel('GDP')
plt.xlabel('Year')
plt.show()
```

## Practice
Can you calculate what is the accuracy of our model?

```python
# write your code here

# split data into train/test
msk = np.random.rand(len(df)) < 0.8

train_x = xdata[msk]
test_x = xdata[~msk]

train_y = ydata[msk]
test_y = ydata[~msk]

# build the model using train set
popt, pcov = curve_fit(sigmoid, train_x, train_y)

# predict using test set
y_hat = sigmoid(test_x, *popt)

# evaluation
print("Mean absolute error: %.2f" % np.mean(np.absolute(y_hat - test_y)))
print("Residual sum of squares (MSE): %.2f" % np.mean((y_hat - test_y) ** 2))
from sklearn.metrics import r2_score
print("R2-score: %.2f" % r2_score(y_hat , test_y) )
```

    Mean absolute error: 0.03
    Residual sum of squares (MSE): 0.00
    R2-score: 0.91

Double-click __here__ for the solution.

Want to learn more?

IBM SPSS Modeler is a comprehensive analytics platform that has many machine learning algorithms. It has been designed to bring predictive intelligence to decisions made by individuals, by groups, by systems – by your enterprise as a whole. A free trial is available through this course, available here: SPSS Modeler

Also, you can use Watson Studio to run these notebooks faster with bigger datasets. Watson Studio is IBM's leading cloud solution for data scientists, built by data scientists. With Jupyter notebooks, RStudio, Apache Spark and popular libraries pre-packaged in the cloud, Watson Studio enables data scientists to collaborate on their projects without having to install anything. Join the fast-growing community of Watson Studio users today with a free account at Watson Studio

Thanks for completing this lesson!

Author:  Saeed Aghabozorgi

Saeed Aghabozorgi, PhD is a Data Scientist in IBM with a track record of developing enterprise level applications that substantially increases clients’ ability to turn data into actionable knowledge. He is a researcher in data mining field and expert in developing advanced analytic methods like machine learning and statistical modelling on large datasets.

Copyright © 2018 Cognitive Class. This notebook and its source code are released under the terms of the MIT License.


```python
from __future__ import division, print_function
%matplotlib inline
```

```python
import sympy
from sympy import symbols, sin, cos, sqrt, diag
from sympy import Matrix, BlockMatrix, eye, ones, zeros
from sympy.physics.mechanics import *
from IPython.display import display
sympy.init_printing(use_latex='mathjax')

import numpy as np

```

# Quaternion Math

```python
def expq(n):
    n *= 0.5
    nNorm = n.norm()
    qn = Matrix([cos(nNorm),n/nNorm*sin(nNorm)])
    return qn

def quat2dcm(q):
    """
    Convert quaternion to DCM
    """

    # Extract components
    w = q[0]
    x = q[1]
    y = q[2]
    z = q[3]

    # Reduce repeated calculations
    ww = w*w
    xx = x*x
    yy = y*y
    zz = z*z
    wx = w*x
    wy = w*y
    wz = w*z
    xy = x*y
    xz = x*z
    yz = y*z

    # Build Direction Cosine Matrix (DCM)
    dcm = Matrix([
        [ww + xx - yy - zz,       2*(xy - wz),       2*(xz + wy)],
        [      2*(xy + wz), ww - xx + yy - zz,       2*(yz - wx)],
        [      2*(xz - wy),       2*(yz + wx), ww - xx - yy + zz]
    ])
    return dcm

def dcm2quat(dcm):
    """
    Determine quaternion corresponding to dcm using
    the stanley method.

    Flips sign to always return shortest path quaterion
    so w >= 0

    Converts the 3x3 DCM into the quaterion where the
    first component is the real part
    """

    tr = Matrix.trace(dcm)

    w = 0.25*(1+tr)
    x = 0.25*(1+2*dcm[0,0]-tr)
    y = 0.25*(1+2*dcm[1,1]-tr)
    z = 0.25*(1+2*dcm[2,2]-tr)

    #kMax = np.argmax([w,x,y,z])

    kMax = 0

    if kMax == 0:
        w = sqrt(w)
        x = 0.25*(dcm[1,2]-dcm[2,1])/w
        y = 0.25*(dcm[2,0]-dcm[0,2])/w
        z = 0.25*(dcm[0,1]-dcm[1,0])/w

    elif kMax == 1:
        x = sqrt(x)
        w = 0.25*(dcm[1,2]-dcm[2,1])/x
        if w<0:
            x = -x
            w = -w
        y = 0.25*(dcm[0,1]+dcm[1,0])/x
        z = 0.25*(dcm[2,0]+dcm[0,2])/x

    elif kMax == 2:
        y = sqrt(y)
        w = 0.25*(dcm[2,0]-dcm[0,2])/y
        if w<0:
            y = -y
            w = -w
        x = 0.25*(dcm[0,1]+dcm[1,0])/y
        z = 0.25*(dcm[1,2]+dcm[2,1])/y

    elif kMax == 3:
        z = sqrt(z)
        w = 0.25*(dcm[0,1]-dcm[1,0])/z
        if w<0:
            z = -z
            w = -w
        x = 0.25*(dcm[2,0]+dcm[0,2])/z
        y = 0.25*(dcm[1,2]+dcm[2,1])/z

    q = Matrix([w,x,y,z])

    return q

def skew3(v):
    vx,vy,vz = v
    out = Matrix([[  0, -vz,   vy],
                  [ vz,   0,  -vx],
                  [-vy,  vx,    0]])
    return out

def skew4Left(v):
    if len(v)==3:
        v = Matrix.vstack(zeros(1),v)
    w,x,y,z = v
    out = Matrix([
            [w, -x, -y, -z],
            [x,  w, -z,  y],
            [y,  z,  w, -x],
            [z, -y,  x,  w],
        ])
    return out

def skew4Right(v):
    if len(v)==3:
        v = Matrix.vstack(zeros(1),v)
    w,x,y,z = v
    out = Matrix([
            [w, -x, -y, -z],
            [x,  w,  z, -y],
            [y, -z,  w,  x],
            [z,  y, -x,  w],
        ])
    return out

def quatConj(q):
    q_out = Matrix(q[:])
    q_out = q_out.T*sympy.diag(1,-1,-1,-1)
    q_out = q_out.T

    return q_out

def qRot(q,v):
    qPrime = quatConj(q)
    v = Matrix.vstack(zeros(1),v)
    vout = skew4Left(q)*skew4Right(qPrime)*v
    return Matrix(vout[1:])

def dRotdq(q,v):
    qw,qx,qy,qz = q
    vx,vy,vz = v

    dRdq = Matrix([
        [2*qw*vx + 2*qy*vz - 2*qz*vy,  2*qx*vx + 2*qy*vy + 2*qz*vz,  2*qw*vz + 2*qx*vy - 2*qy*vx, -2*qw*vy + 2*qx*vz - 2*qz*vx],
        [2*qw*vy - 2*qx*vz + 2*qz*vx, -2*qw*vz - 2*qx*vy + 2*qy*vx,  2*qx*vx + 2*qy*vy + 2*qz*vz,  2*qw*vx + 2*qy*vz - 2*qz*vy],
        [2*qw*vz + 2*qx*vy - 2*qy*vx,  2*qw*vy - 2*qx*vz + 2*qz*vx, -2*qw*vx - 2*qy*vz + 2*qz*vy,  2*qx*vx + 2*qy*vy + 2*qz*vz]
    ])

    return dRdq

def dVdq(q,v):
    qw,qx,qy,qz = q
    vx,vy,vz = v

    dv = Matrix([
        [ 2*vx*qw + 2*vy*qz - 2*vz*qy, 2*vx*qx + 2*vy*qy + 2*vz*qz, -2*vx*qy + 2*vy*qx - 2*vz*qw, -2*vx*qz + 2*vy*qw + 2*vz*qx],
        [-2*vx*qz + 2*vy*qw + 2*vz*qx, 2*vx*qy - 2*vy*qx + 2*vz*qw,  2*vx*qx + 2*vy*qy + 2*vz*qz, -2*vx*qw - 2*vy*qz + 2*vz*qy],
        [ 2*vx*qy - 2*vy*qx + 2*vz*qw, 2*vx*qz - 2*vy*qw - 2*vz*qx,  2*vx*qw + 2*vy*qz - 2*vz*qy,  2*vx*qx + 2*vy*qy + 2*vz*qz],
    ])

    return dv

```

# Setup

```python
# Time step
t = symbols('T')

# States
qw,qx,qy,qz = symbols('q_w q_x q_y q_z')
rx,ry,rz = symbols('r_x r_y r_z')
vx,vy,vz = symbols('v_x v_y v_z')
abx,aby,abz = symbols('a_bx a_by a_bz')
gbx,gby,gbz = symbols('g_bx g_by g_bz')

r_L = Matrix([rx,ry,rz])
v_L = Matrix([vx,vy,vz])
q_toLfromB = Matrix([qw,qx,qy,qz])
gyroBias = Matrix([gbx,gby,gbz])
accelBias = Matrix([abx,aby,abz])

# Measurements
ax,ay,az = symbols('a_x a_y a_z') # measured
mx,my,mz = symbols('m_x m_y m_z') # measured
wx,wy,wz = symbols('w_x w_y w_z') # measured

accelMeas = Matrix([ax,ay,az])
magMeas = Matrix([mx,my,mz])
wMeas = Matrix([wx,wy,wz])

# Nav vectors
g = symbols('g')
bx,by,bz = symbols('b_x b_y b_z')

g_L = Matrix([0,0,g])
B_L = Matrix([mx,0,0])

# Measurment Uncertanties
eGyro, eAccel, eMag, eGPS = symbols('eGyro eAccel eMag eGPS')
ewx,ewy,ewz = symbols('e_wx e_wy e_wz')
eax,eay,eaz = symbols('e_ax e_ay e_az')
emx,emy,emz = symbols('e_mx e_my e_mz')

ew = Matrix([ewx,ewy,ewz])
ea = Matrix([eax,eay,eaz])
em = Matrix([emx,emy,emz])

eVec = Matrix([
    ew,
    ea,
    em,
])

esub0 = {ewx:0,ewy:0,ewz:0,
         eax:0,eay:0,eaz:0,
         emx:0,emy:0,emz:0}

#erx,ery,erz = symbols('e_wx e_wy w_wz')
#evx,evy,evz = symbols('e_ax e_ay w_az')
#ewx,ewy,ewz = symbols('e_wx e_wy w_wz')

#er = Matrix([erx,ery,erz])
#ev = Matrix([evx,evy,evz])
#ew = Matrix([ewx,ewy,ewz])

#eVec = Matrix([
#    er,
#    ev,
#    ew,
#])

#esub0 = {erx:0,ery:0,erz:0,
#         evx:0,evy:0,evz:0,
#         ewx:0,ewy:0,ewz:0}

wax,way,waz = symbols('w_ax w_ay w_az')
wa = symbols('w_a')
ww = symbols('w_w')
waB = symbols('w_aB')
wgB = symbols('w_gB')

```

# Validate quaternion math

```python
print('dcm')
display( quat2dcm(q_toLfromB) )
print('[q]L')
display( skew4Left(q_toLfromB) )
print('[q]R')
display( skew4Right(q_toLfromB) )
print('[wb]R')
display( skew4Right(wMeas) )
r_B = Matrix([rx,ry,rz])
print('d/dq R*r_B')
display(dRotdq(q_toLfromB, r_B))

```

    dcm

$\displaystyle \left[\begin{matrix}q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2} & - 2 q_{w} q_{z} + 2 q_{x} q_{y} & 2 q_{w} q_{y} + 2 q_{x} q_{z}\\2 q_{w} q_{z} + 2 q_{x} q_{y} & q_{w}^{2} - q_{x}^{2} + q_{y}^{2} - q_{z}^{2} & - 2 q_{w} q_{x} + 2 q_{y} q_{z}\\- 2 q_{w} q_{y} + 2 q_{x} q_{z} & 2 q_{w} q_{x} + 2 q_{y} q_{z} & q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\end{matrix}\right]$

    [q]L

$\displaystyle \left[\begin{matrix}q_{w} & - q_{x} & - q_{y} & - q_{z}\\q_{x} & q_{w} & - q_{z} & q_{y}\\q_{y} & q_{z} & q_{w} & - q_{x}\\q_{z} & - q_{y} & q_{x} & q_{w}\end{matrix}\right]$

    [q]R

$\displaystyle \left[\begin{matrix}q_{w} & - q_{x} & - q_{y} & - q_{z}\\q_{x} & q_{w} & q_{z} & - q_{y}\\q_{y} & - q_{z} & q_{w} & q_{x}\\q_{z} & q_{y} & - q_{x} & q_{w}\end{matrix}\right]$

    [wb]R

$\displaystyle \left[\begin{matrix}0 & - w_{x} & - w_{y} & - w_{z}\\w_{x} & 0 & w_{z} & - w_{y}\\w_{y} & - w_{z} & 0 & w_{x}\\w_{z} & w_{y} & - w_{x} & 0\end{matrix}\right]$

    d/dq R*r_B

$\displaystyle \left[\begin{matrix}2 q_{w} r_{x} + 2 q_{y} r_{z} - 2 q_{z} r_{y} & 2 q_{x} r_{x} + 2 q_{y} r_{y} + 2 q_{z} r_{z} & 2 q_{w} r_{z} + 2 q_{x} r_{y} - 2 q_{y} r_{x} & - 2 q_{w} r_{y} + 2 q_{x} r_{z} - 2 q_{z} r_{x}\\2 q_{w} r_{y} - 2 q_{x} r_{z} + 2 q_{z} r_{x} & - 2 q_{w} r_{z} - 2 q_{x} r_{y} + 2 q_{y} r_{x} & 2 q_{x} r_{x} + 2 q_{y} r_{y} + 2 q_{z} r_{z} & 2 q_{w} r_{x} + 2 q_{y} r_{z} - 2 q_{z} r_{y}\\2 q_{w} r_{z} + 2 q_{x} r_{y} - 2 q_{y} r_{x} & 2 q_{w} r_{y} - 2 q_{x} r_{z} + 2 q_{z} r_{x} & - 2 q_{w} r_{x} - 2 q_{y} r_{z} + 2 q_{z} r_{y} & 2 q_{x} r_{x} + 2 q_{y} r_{y} + 2 q_{z} r_{z}\end{matrix}\right]$

# Extended Kalman Filter Equations

## State Equations

For the Linear Kalman filter we have the following for the process and measurment models:

$$\begin{aligned}\dot{\mathbf x} &= \mathbf{Ax} + w_x\\
\mathbf z &= \mathbf{Hx} + w_z
\end{aligned}$$

Where $\mathbf A$ is the systems dynamic matrix. Using the state space methodsthese equations can be transformed into:
$$\begin{aligned}\bar{\mathbf x} &= \mathbf{Fx} \\
\mathbf z &= \mathbf{Hx}
\end{aligned}$$

For the nonlinear model the linear expression $\mathbf{Fx} + \mathbf{Bu}$ is replaced by a nonlinear function $f(\mathbf x, \mathbf u)$, and the linear expression $\mathbf{Hx}$ is replaced by a nonlinear function $h(\mathbf x)$:

$$\begin{aligned}\dot{\mathbf x} &= f(\mathbf x, \mathbf u) + w_x\\
\mathbf z &= h(\mathbf x) + w_z
\end{aligned}$$

The EKF doesn't alter the Kalman Filter's linear equations. Instead, it *linearizes* the nonlinear equations at the point of the current estimate. The system is linearized by using the jacobian.

$$
\begin{aligned}
\mathbf F
&= {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_t}} \\
\mathbf H &= \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t}
\end{aligned}
$$

## Summary of Linear and Nonlinear Kalman Filter Equations

$$\begin{array}{l|l}
\text{Linear Kalman filter} & \text{EKF} \\
\hline
& \boxed{\mathbf F = {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_t}}} \\
\mathbf{\bar x} = \mathbf{Fx} + \mathbf{Bu} & \boxed{\mathbf{\bar x} = f(\mathbf x, \mathbf u)}  \\
\mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q  & \mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q \\
\hline
& \boxed{\mathbf H = \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t}} \\
\textbf{y} = \mathbf z - \mathbf{H \bar{x}} & \textbf{y} = \mathbf z - \boxed{h(\bar{x})}\\
\mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} & \mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} \\
\mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} & \mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} \\
\mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}} & \mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}}
\end{array}$$

## Measurment Model

The measurment model jacobian should be (N_measurments, N_states)

$$
\mathbf H = \frac{\partial{h(\bar{\mathbf x})}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t} =
\begin{bmatrix}
\frac{\partial h_1}{\partial x_1} & \frac{\partial h_1}{\partial x_2} &\dots \\
\frac{\partial h_2}{\partial x_1} & \frac{\partial h_2}{\partial x_2} &\dots \\
\vdots & \vdots
\end{bmatrix}
$$

# Quadcopter Model

Here we mode the system dynamics using a constant acceleration model where the inertial acceleration acts as the input to the inertial position and velocity functions. We also represent the body attitude using the quaternion representation.

The measurments consist of an imu+mag and gps. Together this provides body acceleration, body rates, estimated gravity vector, estimated north vector, inertial position (and possibly inertial velocity).

For now, the accel and gyro bias states will be excluded.

## State and Measurment Models

Constant acceleration model with acceleration as input and position sensor (GPS) measurements

```python
# State vector
X = Matrix([
    r_L,
    v_L,
    q_toLfromB,
])

# State transition model (const accel & angAccel)
a_B = accelMeas
a_L  = qRot(q_toLfromB, a_B) - g_L

fx_r = r_L + v_L*t + 1/2*a_L*t**2
fx_v = v_L + a_L*t

wb = wMeas
fx_q = (eye(4) + t/2*skew4Right(wb))*q_toLfromB

#expq_wt = expq(wMeas*t)
#fx_q = skew4Left(q_toLfromB) * expq_wt

fx = Matrix([
    fx_r,
    fx_v,
    fx_q
])

# Measurment Model (estimates measurment from states)
hx_gps = r_L # used for position
hx_accel = -qRot(quatConj(q_toLfromB), g_L) # used for attitude
hx_mag   =  qRot(quatConj(q_toLfromB), B_L) # used for attitude

hx = Matrix([
    hx_gps,
    hx_accel,
    hx_mag,
])

# Display
print('State vector (X)')
display(X,X.shape)
print('State transition model (fx)')
display(fx,fx.shape)
print('Measurment function (hx)')
display(hx,hx.shape)

```

    State vector (X)

$\displaystyle \left[\begin{matrix}r_{x}\\r_{y}\\r_{z}\\v_{x}\\v_{y}\\v_{z}\\q_{w}\\q_{x}\\q_{y}\\q_{z}\end{matrix}\right]$

$\displaystyle \left( 10, \  1\right)$

    State transition model (fx)

$\displaystyle \left[\begin{matrix}T^{2} \left(0.5 a_{x} \left(q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2}\right) + 0.5 a_{y} \left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + 0.5 a_{z} \left(2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\right) + T v_{x} + r_{x}\\T^{2} \left(0.5 a_{x} \left(2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + 0.5 a_{y} \left(q_{w}^{2} - q_{x}^{2} + q_{y}^{2} - q_{z}^{2}\right) + 0.5 a_{z} \left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\right)\right) + T v_{y} + r_{y}\\T^{2} \left(0.5 a_{x} \left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\right) + 0.5 a_{y} \left(2 q_{w} q_{x} + 2 q_{y} q_{z}\right) + 0.5 a_{z} \left(q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right) - 0.5 g\right) + T v_{z} + r_{z}\\T \left(a_{x} \left(q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2}\right) + a_{y} \left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + a_{z} \left(2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\right) + v_{x}\\T \left(a_{x} \left(2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + a_{y} \left(q_{w}^{2} - q_{x}^{2} + q_{y}^{2} - q_{z}^{2}\right) + a_{z} \left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\right)\right) + v_{y}\\T \left(a_{x} \left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\right) + a_{y} \left(2 q_{w} q_{x} + 2 q_{y} q_{z}\right) + a_{z} \left(q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right) - g\right) + v_{z}\\- \frac{T q_{x} w_{x}}{2} - \frac{T q_{y} w_{y}}{2} - \frac{T q_{z} w_{z}}{2} + q_{w}\\\frac{T q_{w} w_{x}}{2} + \frac{T q_{y} w_{z}}{2} - \frac{T q_{z} w_{y}}{2} + q_{x}\\\frac{T q_{w} w_{y}}{2} - \frac{T q_{x} w_{z}}{2} + \frac{T q_{z} w_{x}}{2} + q_{y}\\\frac{T q_{w} w_{z}}{2} + \frac{T q_{x} w_{y}}{2} - \frac{T q_{y} w_{x}}{2} + q_{z}\end{matrix}\right]$

$\displaystyle \left( 10, \  1\right)$

    Measurment function (hx)

$\displaystyle \left[\begin{matrix}r_{x}\\r_{y}\\r_{z}\\- g \left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\\- g \left(2 q_{w} q_{x} + 2 q_{y} q_{z}\right)\\- g \left(q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right)\\m_{x} \left(q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2}\right)\\m_{x} \left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\right)\\m_{x} \left(2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\end{matrix}\right]$

$\displaystyle \left( 9, \  1\right)$

## State and Measurment Jacobians

Determine the jacobian of the state and measurment models to implement the EKF for nonlinear systems.

```python
# State model jacobian
dfdx = fx.jacobian(X)

# Measurment model jacobian
dhdx = hx.jacobian(X)

# G (dfde)
#dfde = fx.jacobian(eVec)

# Display
print('State model jacobian (dfdx)')
display(dfdx,dfdx.shape)
print('Measurment model jacobian (dhdx)')
display(dhdx,dhdx.shape)

```

    State model jacobian (dfdx)

$\displaystyle \left[\begin{matrix}1 & 0 & 0 & T & 0 & 0 & T^{2} \left(1.0 a_{x} q_{w} - 1.0 a_{y} q_{z} + 1.0 a_{z} q_{y}\right) & T^{2} \left(1.0 a_{x} q_{x} + 1.0 a_{y} q_{y} + 1.0 a_{z} q_{z}\right) & T^{2} \left(- 1.0 a_{x} q_{y} + 1.0 a_{y} q_{x} + 1.0 a_{z} q_{w}\right) & T^{2} \left(- 1.0 a_{x} q_{z} - 1.0 a_{y} q_{w} + 1.0 a_{z} q_{x}\right)\\0 & 1 & 0 & 0 & T & 0 & T^{2} \left(1.0 a_{x} q_{z} + 1.0 a_{y} q_{w} - 1.0 a_{z} q_{x}\right) & T^{2} \left(1.0 a_{x} q_{y} - 1.0 a_{y} q_{x} - 1.0 a_{z} q_{w}\right) & T^{2} \left(1.0 a_{x} q_{x} + 1.0 a_{y} q_{y} + 1.0 a_{z} q_{z}\right) & T^{2} \left(1.0 a_{x} q_{w} - 1.0 a_{y} q_{z} + 1.0 a_{z} q_{y}\right)\\0 & 0 & 1 & 0 & 0 & T & T^{2} \left(- 1.0 a_{x} q_{y} + 1.0 a_{y} q_{x} + 1.0 a_{z} q_{w}\right) & T^{2} \left(1.0 a_{x} q_{z} + 1.0 a_{y} q_{w} - 1.0 a_{z} q_{x}\right) & T^{2} \left(- 1.0 a_{x} q_{w} + 1.0 a_{y} q_{z} - 1.0 a_{z} q_{y}\right) & T^{2} \left(1.0 a_{x} q_{x} + 1.0 a_{y} q_{y} + 1.0 a_{z} q_{z}\right)\\0 & 0 & 0 & 1 & 0 & 0 & T \left(2 a_{x} q_{w} - 2 a_{y} q_{z} + 2 a_{z} q_{y}\right) & T \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right) & T \left(- 2 a_{x} q_{y} + 2 a_{y} q_{x} + 2 a_{z} q_{w}\right) & T \left(- 2 a_{x} q_{z} - 2 a_{y} q_{w} + 2 a_{z} q_{x}\right)\\0 & 0 & 0 & 0 & 1 & 0 & T \left(2 a_{x} q_{z} + 2 a_{y} q_{w} - 2 a_{z} q_{x}\right) & T \left(2 a_{x} q_{y} - 2 a_{y} q_{x} - 2 a_{z} q_{w}\right) & T \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right) & T \left(2 a_{x} q_{w} - 2 a_{y} q_{z} + 2 a_{z} q_{y}\right)\\0 & 0 & 0 & 0 & 0 & 1 & T \left(- 2 a_{x} q_{y} + 2 a_{y} q_{x} + 2 a_{z} q_{w}\right) & T \left(2 a_{x} q_{z} + 2 a_{y} q_{w} - 2 a_{z} q_{x}\right) & T \left(- 2 a_{x} q_{w} + 2 a_{y} q_{z} - 2 a_{z} q_{y}\right) & T \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right)\\0 & 0 & 0 & 0 & 0 & 0 & 1 & - \frac{T w_{x}}{2} & - \frac{T w_{y}}{2} & - \frac{T w_{z}}{2}\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T w_{x}}{2} & 1 & \frac{T w_{z}}{2} & - \frac{T w_{y}}{2}\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T w_{y}}{2} & - \frac{T w_{z}}{2} & 1 & \frac{T w_{x}}{2}\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T w_{z}}{2} & \frac{T w_{y}}{2} & - \frac{T w_{x}}{2} & 1\end{matrix}\right]$

$\displaystyle \left( 10, \  10\right)$

    Measurment model jacobian (dhdx)

$\displaystyle \left[\begin{matrix}1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 2 g q_{y} & - 2 g q_{z} & 2 g q_{w} & - 2 g q_{x}\\0 & 0 & 0 & 0 & 0 & 0 & - 2 g q_{x} & - 2 g q_{w} & - 2 g q_{z} & - 2 g q_{y}\\0 & 0 & 0 & 0 & 0 & 0 & - 2 g q_{w} & 2 g q_{x} & 2 g q_{y} & - 2 g q_{z}\\0 & 0 & 0 & 0 & 0 & 0 & 2 m_{x} q_{w} & 2 m_{x} q_{x} & - 2 m_{x} q_{y} & - 2 m_{x} q_{z}\\0 & 0 & 0 & 0 & 0 & 0 & - 2 m_{x} q_{z} & 2 m_{x} q_{y} & 2 m_{x} q_{x} & - 2 m_{x} q_{w}\\0 & 0 & 0 & 0 & 0 & 0 & 2 m_{x} q_{y} & 2 m_{x} q_{z} & 2 m_{x} q_{w} & 2 m_{x} q_{x}\end{matrix}\right]$

$\displaystyle \left( 9, \  10\right)$

```python
# State model jacobian (analytic solution)
dfdx_q_1stOrderApprox = eye(4) + t/2*skew4Right(wb)
dfdx_q = skew4Right(expq(0.5*t/2*wb))

dfdx = Matrix(BlockMatrix([
    [  eye(3),     t*eye(3), 0.5*t**2*dRotdq(q_toLfromB, a_B)],
    [zeros(3),       eye(3),        t*dRotdq(q_toLfromB, a_B)],
    [zeros(4,3), zeros(4,3),                           dfdx_q],

]))

# Meaurment jacobian (analytical solution)
dhdx = Matrix(BlockMatrix([
    [  eye(3), zeros(3),             zeros(3,4)],
    [zeros(3), zeros(3), dVdq(q_toLfromB, -g_L)],
    [zeros(3), zeros(3), dVdq(q_toLfromB,  B_L)],
]))

# Display
print('State model jacobian (dfdx)')
display(dfdx,dfdx.shape)
print('Measurment model jacobian (dhdx)')
display(dhdx,dhdx.shape)
```

    State model jacobian (dfdx)

$\displaystyle \left[\begin{matrix}1 & 0 & 0 & T & 0 & 0 & 0.5 T^{2} \left(2 a_{x} q_{w} - 2 a_{y} q_{z} + 2 a_{z} q_{y}\right) & 0.5 T^{2} \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right) & 0.5 T^{2} \left(- 2 a_{x} q_{y} + 2 a_{y} q_{x} + 2 a_{z} q_{w}\right) & 0.5 T^{2} \left(- 2 a_{x} q_{z} - 2 a_{y} q_{w} + 2 a_{z} q_{x}\right)\\0 & 1 & 0 & 0 & T & 0 & 0.5 T^{2} \left(2 a_{x} q_{z} + 2 a_{y} q_{w} - 2 a_{z} q_{x}\right) & 0.5 T^{2} \left(2 a_{x} q_{y} - 2 a_{y} q_{x} - 2 a_{z} q_{w}\right) & 0.5 T^{2} \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right) & 0.5 T^{2} \left(2 a_{x} q_{w} - 2 a_{y} q_{z} + 2 a_{z} q_{y}\right)\\0 & 0 & 1 & 0 & 0 & T & 0.5 T^{2} \left(- 2 a_{x} q_{y} + 2 a_{y} q_{x} + 2 a_{z} q_{w}\right) & 0.5 T^{2} \left(2 a_{x} q_{z} + 2 a_{y} q_{w} - 2 a_{z} q_{x}\right) & 0.5 T^{2} \left(- 2 a_{x} q_{w} + 2 a_{y} q_{z} - 2 a_{z} q_{y}\right) & 0.5 T^{2} \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right)\\0 & 0 & 0 & 1 & 0 & 0 & T \left(2 a_{x} q_{w} - 2 a_{y} q_{z} + 2 a_{z} q_{y}\right) & T \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right) & T \left(- 2 a_{x} q_{y} + 2 a_{y} q_{x} + 2 a_{z} q_{w}\right) & T \left(- 2 a_{x} q_{z} - 2 a_{y} q_{w} + 2 a_{z} q_{x}\right)\\0 & 0 & 0 & 0 & 1 & 0 & T \left(2 a_{x} q_{z} + 2 a_{y} q_{w} - 2 a_{z} q_{x}\right) & T \left(2 a_{x} q_{y} - 2 a_{y} q_{x} - 2 a_{z} q_{w}\right) & T \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right) & T \left(2 a_{x} q_{w} - 2 a_{y} q_{z} + 2 a_{z} q_{y}\right)\\0 & 0 & 0 & 0 & 0 & 1 & T \left(- 2 a_{x} q_{y} + 2 a_{y} q_{x} + 2 a_{z} q_{w}\right) & T \left(2 a_{x} q_{z} + 2 a_{y} q_{w} - 2 a_{z} q_{x}\right) & T \left(- 2 a_{x} q_{w} + 2 a_{y} q_{z} - 2 a_{z} q_{y}\right) & T \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right)\\0 & 0 & 0 & 0 & 0 & 0 & \cos{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)} & - \frac{1.0 T w_{x} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & - \frac{1.0 T w_{y} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & - \frac{1.0 T w_{z} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}}\\0 & 0 & 0 & 0 & 0 & 0 & \frac{1.0 T w_{x} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & \cos{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)} & \frac{1.0 T w_{z} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & - \frac{1.0 T w_{y} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}}\\0 & 0 & 0 & 0 & 0 & 0 & \frac{1.0 T w_{y} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & - \frac{1.0 T w_{z} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & \cos{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)} & \frac{1.0 T w_{x} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}}\\0 & 0 & 0 & 0 & 0 & 0 & \frac{1.0 T w_{z} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & \frac{1.0 T w_{y} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & - \frac{1.0 T w_{x} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & \cos{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}\end{matrix}\right]$

$\displaystyle \left( 10, \  10\right)$

    Measurment model jacobian (dhdx)

$\displaystyle \left[\begin{matrix}1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 2 g q_{y} & - 2 g q_{z} & 2 g q_{w} & - 2 g q_{x}\\0 & 0 & 0 & 0 & 0 & 0 & - 2 g q_{x} & - 2 g q_{w} & - 2 g q_{z} & - 2 g q_{y}\\0 & 0 & 0 & 0 & 0 & 0 & - 2 g q_{w} & 2 g q_{x} & 2 g q_{y} & - 2 g q_{z}\\0 & 0 & 0 & 0 & 0 & 0 & 2 m_{x} q_{w} & 2 m_{x} q_{x} & - 2 m_{x} q_{y} & - 2 m_{x} q_{z}\\0 & 0 & 0 & 0 & 0 & 0 & - 2 m_{x} q_{z} & 2 m_{x} q_{y} & 2 m_{x} q_{x} & - 2 m_{x} q_{w}\\0 & 0 & 0 & 0 & 0 & 0 & 2 m_{x} q_{y} & 2 m_{x} q_{z} & 2 m_{x} q_{w} & 2 m_{x} q_{x}\end{matrix}\right]$

$\displaystyle \left( 9, \  10\right)$

## Process Noise Covariance $Q$

Unmodeled disturbances (wind) can cause a force to act on the body. This results in an acceleration disturbance and nees to be moddeled with the process noise covariance matrix $Q$.

$$Q = \begin{bmatrix}
    \sigma_{x}^2 & 0 & 0 & \sigma_{x \dot x} & 0 & 0\\
    0 & \sigma_{y}^2 & 0 & 0 & \sigma_{y \dot y} & 0 \\
    0 & 0 & \sigma_{z}^2 & 0 & 0 & \sigma_{z \dot z}\\
    \sigma_{\dot x x} & 0 & 0 & \sigma_{\dot x}^2 & 0  & 0 \\
    0 & \sigma_{\dot y y} & 0 & 0 & \sigma_{\dot y}^2  & 0 \\
    0 & 0 & \sigma_{\dot z z} & 0 & 0 & \sigma_{\dot z}^2 \\
   \end{bmatrix} \cdot \sigma_{a}$$

with 𝜎𝑎 as the magnitude of the standard deviation of the acceleration, which distrubs the quadcopter. Assume no cross correlation across x,y,z dimensions.

A matrix $G$ can act as an "actor" to help with this.

```python
G = Matrix([t**2/2, t]) # one dimension
display(G*G.T)
```

$\displaystyle \left[\begin{matrix}\frac{T^{4}}{4} & \frac{T^{3}}{2}\\\frac{T^{3}}{2} & T^{2}\end{matrix}\right]$

Get the following by inspection

```python
siga = symbols('\sigma_a') # std dev
sigw = symbols('\sigma_w') # std dev

# Q - Pos and Velocity
Qpv = Matrix([
    [t**4/4,      0,      0, t**3/2,      0,      0],
    [     0, t**4/4,      0,      0, t**3/2,      0],
    [     0,      0, t**4/4,      0,      0, t**3/2],
    [t**3/2,      0,      0,   t**2,      0,      0],
    [     0, t**3/2,      0,      0,   t**2,      0],
    [     0,      0, t**3/2,      0,      0,   t**2],
]) * siga**2

# Q - quaternion
Gq = Matrix(BlockMatrix([
    [zeros(1,3)],
    [eye(3)],
]))
Gq = -t/2*skew4Left(q_toLfromB)*Gq
Qq = Gq*sigw**2*eye(3)*Gq.T

Qq2  = Matrix([
    [1-qw**2,  -qx*qw,  -qy*qw,  -qz*qw],
    [ -qw*qx, 1-qx**2,  -qy*qx,  -qz*qx],
    [ -qw*qy,  -qx*qy, 1-qy**2,  -qz*qy],
    [ -qw*qz,  -qx*qz,  -qy*qz, 1-qz**2]
]) * sigw**2*t**2/4

# Q - Full
Q = Matrix(BlockMatrix([
    [Qpv,        zeros(6,4)],
    [zeros(4,6),         Qq],
]))

print('Q - Pos and Velocity')
display(Qpv, Qpv.shape)
print('Q - Quaternion')
display(Qq, Qq.shape)
print('Q - Full')
display(Q)

```

    Q - Pos and Velocity

$\displaystyle \left[\begin{matrix}\frac{T^{4} \sigma_{a}^{2}}{4} & 0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0\\0 & \frac{T^{4} \sigma_{a}^{2}}{4} & 0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0\\0 & 0 & \frac{T^{4} \sigma_{a}^{2}}{4} & 0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2}\\\frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & T^{2} \sigma_{a}^{2} & 0 & 0\\0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & T^{2} \sigma_{a}^{2} & 0\\0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & T^{2} \sigma_{a}^{2}\end{matrix}\right]$

$\displaystyle \left( 6, \  6\right)$

    Q - Quaternion

$\displaystyle \left[\begin{matrix}\frac{T^{2} \sigma_{w}^{2} q_{x}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{y}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{z}^{2}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{x}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{y}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{z}}{4}\\- \frac{T^{2} \sigma_{w}^{2} q_{w} q_{x}}{4} & \frac{T^{2} \sigma_{w}^{2} q_{w}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{y}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{z}^{2}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{y}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{z}}{4}\\- \frac{T^{2} \sigma_{w}^{2} q_{w} q_{y}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{y}}{4} & \frac{T^{2} \sigma_{w}^{2} q_{w}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{x}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{z}^{2}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{y} q_{z}}{4}\\- \frac{T^{2} \sigma_{w}^{2} q_{w} q_{z}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{z}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{y} q_{z}}{4} & \frac{T^{2} \sigma_{w}^{2} q_{w}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{x}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{y}^{2}}{4}\end{matrix}\right]$

$\displaystyle \left( 4, \  4\right)$

    Q - Full

$\displaystyle \left[\begin{matrix}\frac{T^{4} \sigma_{a}^{2}}{4} & 0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & 0 & 0 & 0 & 0\\0 & \frac{T^{4} \sigma_{a}^{2}}{4} & 0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & 0 & 0 & 0\\0 & 0 & \frac{T^{4} \sigma_{a}^{2}}{4} & 0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & 0 & 0\\\frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & T^{2} \sigma_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0\\0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & T^{2} \sigma_{a}^{2} & 0 & 0 & 0 & 0 & 0\\0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & T^{2} \sigma_{a}^{2} & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T^{2} \sigma_{w}^{2} q_{x}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{y}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{z}^{2}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{x}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{y}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{z}}{4}\\0 & 0 & 0 & 0 & 0 & 0 & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{x}}{4} & \frac{T^{2} \sigma_{w}^{2} q_{w}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{y}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{z}^{2}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{y}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{z}}{4}\\0 & 0 & 0 & 0 & 0 & 0 & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{y}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{y}}{4} & \frac{T^{2} \sigma_{w}^{2} q_{w}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{x}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{z}^{2}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{y} q_{z}}{4}\\0 & 0 & 0 & 0 & 0 & 0 & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{z}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{z}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{y} q_{z}}{4} & \frac{T^{2} \sigma_{w}^{2} q_{w}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{x}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{y}^{2}}{4}\end{matrix}\right]$

# Predict
## State Transition

```python
X = Matrix([
    r_L,
    v_L,
    q_toLfromB,
    gyroBias,
    accelBias
])

a_B = accelMeas-accelBias
a_L  = qRot(q_toLfromB, a_B) - g_L
a_L2 = Matrix([
    a_L[0]**2,
   a_L[1]**2,
   a_L[2]**2,
])

fx_r = r_L + v_L*t + a_L*t
fx_v = v_L + a_L*t
fx_q = (eye(4) + t/2*skew4Right(wMeas-gyroBias))*q_toLfromB
fx_gB = eye(3) * gyroBias
fx_aB = eye(3) * accelBias

fx = Matrix([
    fx_r,
    fx_v,
    fx_q,
    fx_gB,
    fx_aB
       ])

print('X')
display(X)
print('fx')
display(fx)

```

    X

$\displaystyle \left[\begin{matrix}r_{x}\\r_{y}\\r_{z}\\v_{x}\\v_{y}\\v_{z}\\q_{w}\\q_{x}\\q_{y}\\q_{z}\\g_{bx}\\g_{by}\\g_{bz}\\a_{bx}\\a_{by}\\a_{bz}\end{matrix}\right]$

    fx

$\displaystyle \left[\begin{matrix}T v_{x} + T \left(\left(- a_{bx} + a_{x}\right) \left(q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2}\right) + \left(- a_{by} + a_{y}\right) \left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + \left(- a_{bz} + a_{z}\right) \left(2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\right) + r_{x}\\T v_{y} + T \left(\left(- a_{bx} + a_{x}\right) \left(2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + \left(- a_{by} + a_{y}\right) \left(q_{w}^{2} - q_{x}^{2} + q_{y}^{2} - q_{z}^{2}\right) + \left(- a_{bz} + a_{z}\right) \left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\right)\right) + r_{y}\\T v_{z} + T \left(- g + \left(- a_{bx} + a_{x}\right) \left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\right) + \left(- a_{by} + a_{y}\right) \left(2 q_{w} q_{x} + 2 q_{y} q_{z}\right) + \left(- a_{bz} + a_{z}\right) \left(q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right)\right) + r_{z}\\T \left(\left(- a_{bx} + a_{x}\right) \left(q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2}\right) + \left(- a_{by} + a_{y}\right) \left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + \left(- a_{bz} + a_{z}\right) \left(2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\right) + v_{x}\\T \left(\left(- a_{bx} + a_{x}\right) \left(2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + \left(- a_{by} + a_{y}\right) \left(q_{w}^{2} - q_{x}^{2} + q_{y}^{2} - q_{z}^{2}\right) + \left(- a_{bz} + a_{z}\right) \left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\right)\right) + v_{y}\\T \left(- g + \left(- a_{bx} + a_{x}\right) \left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\right) + \left(- a_{by} + a_{y}\right) \left(2 q_{w} q_{x} + 2 q_{y} q_{z}\right) + \left(- a_{bz} + a_{z}\right) \left(q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right)\right) + v_{z}\\\frac{T q_{x} \left(g_{bx} - w_{x}\right)}{2} + \frac{T q_{y} \left(g_{by} - w_{y}\right)}{2} + \frac{T q_{z} \left(g_{bz} - w_{z}\right)}{2} + q_{w}\\\frac{T q_{w} \left(- g_{bx} + w_{x}\right)}{2} + \frac{T q_{y} \left(- g_{bz} + w_{z}\right)}{2} + \frac{T q_{z} \left(g_{by} - w_{y}\right)}{2} + q_{x}\\\frac{T q_{w} \left(- g_{by} + w_{y}\right)}{2} + \frac{T q_{x} \left(g_{bz} - w_{z}\right)}{2} + \frac{T q_{z} \left(- g_{bx} + w_{x}\right)}{2} + q_{y}\\\frac{T q_{w} \left(- g_{bz} + w_{z}\right)}{2} + \frac{T q_{x} \left(- g_{by} + w_{y}\right)}{2} + \frac{T q_{y} \left(g_{bx} - w_{x}\right)}{2} + q_{z}\\g_{bx}\\g_{by}\\g_{bz}\\a_{bx}\\a_{by}\\a_{bz}\end{matrix}\right]$

## State Transition Jacobian

```python
dfdx = fx.jacobian(X)

print('dfdx')
display(dfdx)
```

    dfdx

$\displaystyle \left[\begin{array}{cccccccccccccccc}1 & 0 & 0 & T & 0 & 0 & T \left(2 q_{w} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & T \left(2 q_{w} \left(- a_{bz} + a_{z}\right) + 2 q_{x} \left(- a_{by} + a_{y}\right) - 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(- 2 q_{w} \left(- a_{by} + a_{y}\right) + 2 q_{x} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & 0 & 0 & 0 & T \left(- q_{w}^{2} - q_{x}^{2} + q_{y}^{2} + q_{z}^{2}\right) & T \left(2 q_{w} q_{z} - 2 q_{x} q_{y}\right) & T \left(- 2 q_{w} q_{y} - 2 q_{x} q_{z}\right)\\0 & 1 & 0 & 0 & T & 0 & T \left(2 q_{w} \left(- a_{by} + a_{y}\right) - 2 q_{x} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & T \left(- 2 q_{w} \left(- a_{bz} + a_{z}\right) - 2 q_{x} \left(- a_{by} + a_{y}\right) + 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & T \left(2 q_{w} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & 0 & 0 & 0 & T \left(- 2 q_{w} q_{z} - 2 q_{x} q_{y}\right) & T \left(- q_{w}^{2} + q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right) & T \left(2 q_{w} q_{x} - 2 q_{y} q_{z}\right)\\0 & 0 & 1 & 0 & 0 & T & T \left(2 q_{w} \left(- a_{bz} + a_{z}\right) + 2 q_{x} \left(- a_{by} + a_{y}\right) - 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(2 q_{w} \left(- a_{by} + a_{y}\right) - 2 q_{x} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & T \left(- 2 q_{w} \left(- a_{bx} + a_{x}\right) - 2 q_{y} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & 0 & 0 & 0 & T \left(2 q_{w} q_{y} - 2 q_{x} q_{z}\right) & T \left(- 2 q_{w} q_{x} - 2 q_{y} q_{z}\right) & T \left(- q_{w}^{2} + q_{x}^{2} + q_{y}^{2} - q_{z}^{2}\right)\\0 & 0 & 0 & 1 & 0 & 0 & T \left(2 q_{w} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & T \left(2 q_{w} \left(- a_{bz} + a_{z}\right) + 2 q_{x} \left(- a_{by} + a_{y}\right) - 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(- 2 q_{w} \left(- a_{by} + a_{y}\right) + 2 q_{x} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & 0 & 0 & 0 & T \left(- q_{w}^{2} - q_{x}^{2} + q_{y}^{2} + q_{z}^{2}\right) & T \left(2 q_{w} q_{z} - 2 q_{x} q_{y}\right) & T \left(- 2 q_{w} q_{y} - 2 q_{x} q_{z}\right)\\0 & 0 & 0 & 0 & 1 & 0 & T \left(2 q_{w} \left(- a_{by} + a_{y}\right) - 2 q_{x} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & T \left(- 2 q_{w} \left(- a_{bz} + a_{z}\right) - 2 q_{x} \left(- a_{by} + a_{y}\right) + 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & T \left(2 q_{w} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & 0 & 0 & 0 & T \left(- 2 q_{w} q_{z} - 2 q_{x} q_{y}\right) & T \left(- q_{w}^{2} + q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right) & T \left(2 q_{w} q_{x} - 2 q_{y} q_{z}\right)\\0 & 0 & 0 & 0 & 0 & 1 & T \left(2 q_{w} \left(- a_{bz} + a_{z}\right) + 2 q_{x} \left(- a_{by} + a_{y}\right) - 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(2 q_{w} \left(- a_{by} + a_{y}\right) - 2 q_{x} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & T \left(- 2 q_{w} \left(- a_{bx} + a_{x}\right) - 2 q_{y} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & 0 & 0 & 0 & T \left(2 q_{w} q_{y} - 2 q_{x} q_{z}\right) & T \left(- 2 q_{w} q_{x} - 2 q_{y} q_{z}\right) & T \left(- q_{w}^{2} + q_{x}^{2} + q_{y}^{2} - q_{z}^{2}\right)\\0 & 0 & 0 & 0 & 0 & 0 & 1 & \frac{T \left(g_{bx} - w_{x}\right)}{2} & \frac{T \left(g_{by} - w_{y}\right)}{2} & \frac{T \left(g_{bz} - w_{z}\right)}{2} & \frac{T q_{x}}{2} & \frac{T q_{y}}{2} & \frac{T q_{z}}{2} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T \left(- g_{bx} + w_{x}\right)}{2} & 1 & \frac{T \left(- g_{bz} + w_{z}\right)}{2} & \frac{T \left(g_{by} - w_{y}\right)}{2} & - \frac{T q_{w}}{2} & \frac{T q_{z}}{2} & - \frac{T q_{y}}{2} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T \left(- g_{by} + w_{y}\right)}{2} & \frac{T \left(g_{bz} - w_{z}\right)}{2} & 1 & \frac{T \left(- g_{bx} + w_{x}\right)}{2} & - \frac{T q_{z}}{2} & - \frac{T q_{w}}{2} & \frac{T q_{x}}{2} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T \left(- g_{bz} + w_{z}\right)}{2} & \frac{T \left(- g_{by} + w_{y}\right)}{2} & \frac{T \left(g_{bx} - w_{x}\right)}{2} & 1 & \frac{T q_{y}}{2} & - \frac{T q_{x}}{2} & - \frac{T q_{w}}{2} & 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 & 1 & 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 & 1 & 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 & 1\end{array}\right]$

```python

```

```python
qv = Matrix([q_toLfromB[1:]]).T
QF = Matrix([
    [qv, qw*eye(3)+skew3(qv)]
])
dvdq = 2*QF*Matrix([
                [zeros(1),a_B.T],
                [a_B, -skew3(a_B)]
])

C_toLfromB = quat2dcm(q_toLfromB)
box = Matrix([
    [-qv.T],
    [qw*eye(3) + skew3(qv)]
])

dfdx = Matrix([
    [zeros(3), eye(3), zeros(3,4),zeros(3),zeros(3)],
    [zeros(3), zeros(3), dvdq, zeros(3), -C_toLfromB ],
    [zeros(4,3), zeros(4,3), 1/2*skew4Right(wMeas-gyroBias), -1/2*box, zeros(4,3)],
    [zeros(3),zeros(3),zeros(3,4),zeros(3),zeros(3)],
    [zeros(3),zeros(3),zeros(3,4),zeros(3),zeros(3)]
])
dfdx = dfdx * t + eye(16)

print('dfdx')
display(dfdx)
mprint(dfdx)
print(dfdx.shape)

```

    dfdx

$\displaystyle \left[\begin{array}{cccccccccccccccc}1 & 0 & 0 & T & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & T & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & T & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 1 & 0 & 0 & T \left(2 q_{w} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) - 2 q_{z} \left(a_{bz} - a_{z}\right)\right) & T \left(2 q_{w} \left(- a_{bz} + a_{z}\right) + 2 q_{x} \left(- a_{by} + a_{y}\right) + 2 q_{y} \left(a_{bx} - a_{x}\right)\right) & T \left(2 q_{w} \left(a_{by} - a_{y}\right) + 2 q_{x} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & 0 & 0 & 0 & T \left(- q_{w}^{2} - q_{x}^{2} + q_{y}^{2} + q_{z}^{2}\right) & T \left(2 q_{w} q_{z} - 2 q_{x} q_{y}\right) & T \left(- 2 q_{w} q_{y} - 2 q_{x} q_{z}\right)\\0 & 0 & 0 & 0 & 1 & 0 & T \left(2 q_{w} \left(- a_{by} + a_{y}\right) - 2 q_{x} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & T \left(2 q_{w} \left(a_{bz} - a_{z}\right) - 2 q_{x} \left(- a_{by} + a_{y}\right) + 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(- 2 q_{x} \left(a_{bx} - a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & T \left(2 q_{w} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(a_{by} - a_{y}\right)\right) & 0 & 0 & 0 & T \left(- 2 q_{w} q_{z} - 2 q_{x} q_{y}\right) & T \left(- q_{w}^{2} + q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right) & T \left(2 q_{w} q_{x} - 2 q_{y} q_{z}\right)\\0 & 0 & 0 & 0 & 0 & 1 & T \left(2 q_{w} \left(- a_{bz} + a_{z}\right) + 2 q_{x} \left(- a_{by} + a_{y}\right) - 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(2 q_{w} \left(- a_{by} + a_{y}\right) + 2 q_{x} \left(a_{bz} - a_{z}\right) + 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & T \left(2 q_{w} \left(a_{bx} - a_{x}\right) - 2 q_{y} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) - 2 q_{y} \left(a_{by} - a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & 0 & 0 & 0 & T \left(2 q_{w} q_{y} - 2 q_{x} q_{z}\right) & T \left(- 2 q_{w} q_{x} - 2 q_{y} q_{z}\right) & T \left(- q_{w}^{2} + q_{x}^{2} + q_{y}^{2} - q_{z}^{2}\right)\\0 & 0 & 0 & 0 & 0 & 0 & 1 & T \left(0.5 g_{bx} - 0.5 w_{x}\right) & T \left(0.5 g_{by} - 0.5 w_{y}\right) & T \left(0.5 g_{bz} - 0.5 w_{z}\right) & 0.5 T q_{x} & 0.5 T q_{y} & 0.5 T q_{z} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & T \left(- 0.5 g_{bx} + 0.5 w_{x}\right) & 1 & T \left(- 0.5 g_{bz} + 0.5 w_{z}\right) & T \left(0.5 g_{by} - 0.5 w_{y}\right) & - 0.5 T q_{w} & 0.5 T q_{z} & - 0.5 T q_{y} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & T \left(- 0.5 g_{by} + 0.5 w_{y}\right) & T \left(0.5 g_{bz} - 0.5 w_{z}\right) & 1 & T \left(- 0.5 g_{bx} + 0.5 w_{x}\right) & - 0.5 T q_{z} & - 0.5 T q_{w} & 0.5 T q_{x} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & T \left(- 0.5 g_{bz} + 0.5 w_{z}\right) & T \left(- 0.5 g_{by} + 0.5 w_{y}\right) & T \left(0.5 g_{bx} - 0.5 w_{x}\right) & 1 & 0.5 T q_{y} & - 0.5 T q_{x} & - 0.5 T q_{w} & 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 & 1 & 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 & 1 & 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 & 1\end{array}\right]$

    Matrix([
    [1, 0, 0, T, 0, 0,                                                                   0,                                                                  0,                                                                   0,                                                                  0,          0,          0,          0,                                      0,                                      0,                                      0],
    [0, 1, 0, 0, T, 0,                                                                   0,                                                                  0,                                                                   0,                                                                  0,          0,          0,          0,                                      0,                                      0,                                      0],
    [0, 0, 1, 0, 0, T,                                                                   0,                                                                  0,                                                                   0,                                                                  0,          0,          0,          0,                                      0,                                      0,                                      0],
    [0, 0, 0, 1, 0, 0, T*(2*q_w*(-a_bx + a_x) + 2*q_y*(-a_bz + a_z) - 2*q_z*(-a_by + a_y)), T*(2*q_x*(-a_bx + a_x) + 2*q_y*(-a_by + a_y) - 2*q_z*(a_bz - a_z)),  T*(2*q_w*(-a_bz + a_z) + 2*q_x*(-a_by + a_y) + 2*q_y*(a_bx - a_x)), T*(2*q_w*(a_by - a_y) + 2*q_x*(-a_bz + a_z) - 2*q_z*(-a_bx + a_x)),          0,          0,          0, T*(-q_w**2 - q_x**2 + q_y**2 + q_z**2),              T*(2*q_w*q_z - 2*q_x*q_y),             T*(-2*q_w*q_y - 2*q_x*q_z)],
    [0, 0, 0, 0, 1, 0, T*(2*q_w*(-a_by + a_y) - 2*q_x*(-a_bz + a_z) + 2*q_z*(-a_bx + a_x)), T*(2*q_w*(a_bz - a_z) - 2*q_x*(-a_by + a_y) + 2*q_y*(-a_bx + a_x)), T*(-2*q_x*(a_bx - a_x) + 2*q_y*(-a_by + a_y) + 2*q_z*(-a_bz + a_z)), T*(2*q_w*(-a_bx + a_x) + 2*q_y*(-a_bz + a_z) + 2*q_z*(a_by - a_y)),          0,          0,          0,             T*(-2*q_w*q_z - 2*q_x*q_y), T*(-q_w**2 + q_x**2 - q_y**2 + q_z**2),              T*(2*q_w*q_x - 2*q_y*q_z)],
    [0, 0, 0, 0, 0, 1, T*(2*q_w*(-a_bz + a_z) + 2*q_x*(-a_by + a_y) - 2*q_y*(-a_bx + a_x)), T*(2*q_w*(-a_by + a_y) + 2*q_x*(a_bz - a_z) + 2*q_z*(-a_bx + a_x)),  T*(2*q_w*(a_bx - a_x) - 2*q_y*(-a_bz + a_z) + 2*q_z*(-a_by + a_y)), T*(2*q_x*(-a_bx + a_x) - 2*q_y*(a_by - a_y) + 2*q_z*(-a_bz + a_z)),          0,          0,          0,              T*(2*q_w*q_y - 2*q_x*q_z),             T*(-2*q_w*q_x - 2*q_y*q_z), T*(-q_w**2 + q_x**2 + q_y**2 - q_z**2)],
    [0, 0, 0, 0, 0, 0,                                                                   1,                                             T*(0.5*g_bx - 0.5*w_x),                                              T*(0.5*g_by - 0.5*w_y),                                             T*(0.5*g_bz - 0.5*w_z),  0.5*T*q_x,  0.5*T*q_y,  0.5*T*q_z,                                      0,                                      0,                                      0],
    [0, 0, 0, 0, 0, 0,                                             T*(-0.5*g_bx + 0.5*w_x),                                                                  1,                                             T*(-0.5*g_bz + 0.5*w_z),                                             T*(0.5*g_by - 0.5*w_y), -0.5*T*q_w,  0.5*T*q_z, -0.5*T*q_y,                                      0,                                      0,                                      0],
    [0, 0, 0, 0, 0, 0,                                             T*(-0.5*g_by + 0.5*w_y),                                             T*(0.5*g_bz - 0.5*w_z),                                                                   1,                                            T*(-0.5*g_bx + 0.5*w_x), -0.5*T*q_z, -0.5*T*q_w,  0.5*T*q_x,                                      0,                                      0,                                      0],
    [0, 0, 0, 0, 0, 0,                                             T*(-0.5*g_bz + 0.5*w_z),                                            T*(-0.5*g_by + 0.5*w_y),                                              T*(0.5*g_bx - 0.5*w_x),                                                                  1,  0.5*T*q_y, -0.5*T*q_x, -0.5*T*q_w,                                      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,          1,          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,                                      1,                                      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,                                      1]])
    (16, 16)

## Process Noise

```python
Q_r  = (wa*t**2)**2 * eye(3)
Q_v  = (wa*t)**2 * eye(3)
Q_q  = (ww*t/2)**2 *Matrix([
    [1-qw**2,  -qx*qw,  -qy*qw,  -qz*qw],
    [ -qw*qx, 1-qx**2,  -qy*qx,  -qz*qx],
    [ -qw*qy,  -qx*qy, 1-qy**2,  -qz*qy],
    [ -qw*qz,  -qx*qz,  -qy*qz, 1-qz**2]
])
Q_gB = (wgB*t)**2 * eye(3)
Q_aB = (waB*t)**2 * eye(3)

Q = Matrix([
    [Q_r, zeros(3), zeros(3,4), zeros(3), zeros(3)],
    [zeros(3), Q_v, zeros(3,4), zeros(3), zeros(3)],
    [zeros(4,3), zeros(4,3), Q_q, zeros(4,3), zeros(4,3)],
    [zeros(3), zeros(3), zeros(3,4), Q_gB, zeros(3)],
    [zeros(3), zeros(3), zeros(3,4), zeros(3), Q_aB],

])

print('Q')
display(Q)
print(Q.shape)

#print('Q_r')
#display(Q_r)
#print('Q_v')
#display(Q_v)
#print('Q_q')
#display(Q_q)
#print('Q_gB')
#display(Q_gB)
#print('Q_aB')
#display(Q_aB)
```

    Q

$\displaystyle \left[\begin{array}{cccccccccccccccc}T^{4} w_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & T^{4} w_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & T^{4} w_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & T^{2} w_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & T^{2} w_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & T^{2} w_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T^{2} w_{w}^{2} \left(1 - q_{w}^{2}\right)}{4} & - \frac{T^{2} q_{w} q_{x} w_{w}^{2}}{4} & - \frac{T^{2} q_{w} q_{y} w_{w}^{2}}{4} & - \frac{T^{2} q_{w} q_{z} w_{w}^{2}}{4} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & - \frac{T^{2} q_{w} q_{x} w_{w}^{2}}{4} & \frac{T^{2} w_{w}^{2} \left(1 - q_{x}^{2}\right)}{4} & - \frac{T^{2} q_{x} q_{y} w_{w}^{2}}{4} & - \frac{T^{2} q_{x} q_{z} w_{w}^{2}}{4} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & - \frac{T^{2} q_{w} q_{y} w_{w}^{2}}{4} & - \frac{T^{2} q_{x} q_{y} w_{w}^{2}}{4} & \frac{T^{2} w_{w}^{2} \left(1 - q_{y}^{2}\right)}{4} & - \frac{T^{2} q_{y} q_{z} w_{w}^{2}}{4} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & - \frac{T^{2} q_{w} q_{z} w_{w}^{2}}{4} & - \frac{T^{2} q_{x} q_{z} w_{w}^{2}}{4} & - \frac{T^{2} q_{y} q_{z} w_{w}^{2}}{4} & \frac{T^{2} w_{w}^{2} \left(1 - q_{z}^{2}\right)}{4} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & T^{2} w_{gB}^{2} & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & T^{2} w_{gB}^{2} & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & T^{2} w_{gB}^{2} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & T^{2} w_{aB}^{2} & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & T^{2} w_{aB}^{2} & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & T^{2} w_{aB}^{2}\end{array}\right]$

    (16, 16)

# Update

## Measurment Function

```python
hx_accel = -qRot(quatConj(q_toLfromB), g_L)
hx_mag   =  qRot(quatConj(q_toLfromB), B_L)
hx_gps   = r_L
hx = Matrix([
    hx_accel,
    hx_mag,
    hx_gps
])

print('hx')
display(hx)
```

    hx

$\displaystyle \left[\begin{matrix}- g \left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\\- g \left(2 q_{w} q_{x} + 2 q_{y} q_{z}\right)\\- g \left(q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right)\\m_{x} \left(q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2}\right)\\m_{x} \left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\right)\\m_{x} \left(2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\\r_{x}\\r_{y}\\r_{z}\end{matrix}\right]$

## Measurment Function Jacobian

```python
dhdx = hx.jacobian(X)

print('dhdx')
display(dhdx)
mprint(dhdx)

```

    dhdx

$\displaystyle \left[\begin{array}{cccccccccccccccc}0 & 0 & 0 & 0 & 0 & 0 & 2 g q_{y} & - 2 g q_{z} & 2 g q_{w} & - 2 g q_{x} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & - 2 g q_{x} & - 2 g q_{w} & - 2 g q_{z} & - 2 g q_{y} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & - 2 g q_{w} & 2 g q_{x} & 2 g q_{y} & - 2 g q_{z} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 2 m_{x} q_{w} & 2 m_{x} q_{x} & - 2 m_{x} q_{y} & - 2 m_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & - 2 m_{x} q_{z} & 2 m_{x} q_{y} & 2 m_{x} q_{x} & - 2 m_{x} q_{w} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 2 m_{x} q_{y} & 2 m_{x} q_{z} & 2 m_{x} q_{w} & 2 m_{x} q_{x} & 0 & 0 & 0 & 0 & 0 & 0\\1 & 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 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]$

    Matrix([
    [0, 0, 0, 0, 0, 0,    2*g*q_y,  -2*g*q_z,    2*g*q_w,   -2*g*q_x, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0,   -2*g*q_x,  -2*g*q_w,   -2*g*q_z,   -2*g*q_y, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0,   -2*g*q_w,   2*g*q_x,    2*g*q_y,   -2*g*q_z, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0,  2*m_x*q_w, 2*m_x*q_x, -2*m_x*q_y, -2*m_x*q_z, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0, -2*m_x*q_z, 2*m_x*q_y,  2*m_x*q_x, -2*m_x*q_w, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0,  2*m_x*q_y, 2*m_x*q_z,  2*m_x*q_w,  2*m_x*q_x, 0, 0, 0, 0, 0, 0],
    [1, 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, 1, 0, 0, 0,          0,         0,          0,          0, 0, 0, 0, 0, 0, 0]])

```python

```

```python

```

```python

```

```python

```

```python

```


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

from tectosaur2.nb_config import setup
setup()
from tectosaur2 import integrate_term, refine_surfaces, gauss_rule
from tectosaur2.mesh import pts_grid
from tectosaur2.laplace2d import double_layer, hypersingular
```

```python
t = sp.var('t')
theta = 0.5 * t * sp.pi + 0.5 * sp.pi
x = sp.cos(theta)
y = sp.sin(theta) - 0.5
#x, y = t, sp.sin((t + 1) * 2 * sp.pi)
src = refine_surfaces(
    [(t, x, y)],
    gauss_rule(12),
    max_curvature=2.0
)
src.n_pts
```

    24

```python
nobs = 300
# zoomx = [0.4, 0.9]
# zoomy = [0, 0.5]
zoomx = [-2.0, 2.0]
zoomy = [-2.0, 2.0]
xs = np.linspace(*zoomx, nobs)
ys = np.linspace(*zoomy, nobs)
obs_pts = pts_grid(xs, ys)
```

```python

# plt.plot(slip)
# plt.show()
# for K in [double_layer,]
# K = double_layer
for name, latex_name, K, c in [
    ("sigma_xz", "$\sigma_{xz}$", hypersingular, 0),
    ("sigma_yz", "$\sigma_{yz}$", hypersingular, 1),
    ("disp", "$u$", double_layer, 0)
]:
    slip = np.cos(np.pi * src.quad_pts) + 1.0
    I_direct = K.direct(obs_pts, src)[:,:,:,0].dot(slip)
    M_qbx, report = integrate_term(K, obs_pts, src, return_report=True)
    # from tectosaur2.debug import plot_centers
    # plot_centers(report, zoomx, zoomy, failed=True)
    I_qbx = M_qbx[:,:,:,0].dot(slip)
    obsx = obs_pts[:,0].reshape((nobs, nobs))
    obsy = obs_pts[:,1].reshape((nobs, nobs))
    direct = I_direct[:,c].reshape((nobs, nobs))
    qbx = I_qbx[:,c].reshape((nobs, nobs))

    levels = np.linspace(-1, 1, 11)
    plt.figure(figsize=(9, 5))
    plt.subplot(1, 2, 1)
    plt.title(f"Direct integration: {latex_name}")
    cntf = plt.contourf(obsx, obsy, direct, levels=levels, extend="both")
    plt.contour(
        obsx,
        obsy,
        direct,
        colors="k",
        linestyles="-",
        linewidths=0.5,
        levels=levels,
        extend="both",
    )
    #plt.plot(src.pts[:,0], src.pts[:,1], "k-", linewidth=1.5)
    plt.xlim(zoomx)
    plt.ylim(zoomy)
    plt.axis('scaled')

    plt.subplot(1, 2, 2)
    plt.title('Quadrature by expansion')
    cntf = plt.contourf(obsx, obsy, qbx, levels=levels, extend="both")
    plt.contour(
        obsx,
        obsy,
        qbx,
        colors="k",
        linestyles="-",
        linewidths=0.5,
        levels=levels,
        extend="both",
    )
    #plt.colorbar(cntf)
    #plt.plot(src.pts[:,0], src.pts[:,1], "k-", linewidth=1.5)
    plt.axis('scaled')
    plt.xlim(zoomx)
    #plt.ylim(zoomy)
    plt.tight_layout()
    plt.savefig(f'qbx_vs_direct_{name}.png', bbox_inches="tight", pad_inches=0.01, dpi=300)
    plt.show()
```


# Symbolic Hamiltonian and Transformations

In this notebook, I will show how to systematically construct the symbolic Hamiltonian of fermionic and spin models. I will introduce the Jordan-Wigner transformation and Majorana fermions. The code implementation is based on the open source frameworks, [OpenFermion](https://quantumai.google/openfermion) and [SymPy](https://www.sympy.org/en/index.html).

## Function Definition

```python
from openfermion import *
from sympy import *

def spin_latex(H_spin_dict):
    """Convert the spin operators into LaTeX expression via SymPy

    Parameters
    ----------
    H_spin_dict : dict
        contains spin operators and their coefficients

    Returns
    -------
    LaTeX expression
        output the symbolic expression in LaTeX form
    """
    expr = []
    for key, value in H_spin_dict.items():
        coefficient = nsimplify(simplify(value))
        if isinstance(coefficient, Integer):
            coefficient = sign(
                coefficient)*UnevaluatedExpr(factorint(abs(coefficient), visual=True))
        operator = []
        for i_key in key:
            operator.append(
                symbols(i_key[1]+'_'+str(i_key[0]), commutative=False))
        expr.append(coefficient*prod(operator))
    return sum(expr)

def fermion_latex(H_fermion_dict):
    """Convert the fermionic operators into LaTeX expression via SymPy

    Parameters
    ----------
    H_fermion_dict : dict
        contains fermionic operators and their coefficients

    Returns
    -------
    LaTeX expression
        output the symbolic expression in LaTeX form
    """
    expr = []
    for key, value in H_fermion_dict.items():
        coefficient = nsimplify(simplify(value))
        if isinstance(coefficient, Integer):
            coefficient = sign(
                coefficient)*UnevaluatedExpr(factorint(abs(coefficient), visual=True))
        if key != ():
            operator = []
            for i_key in key:
                if i_key[1] == 1:
                    operator.append(
                        symbols('c^\dagger_'+str(i_key[0]), commutative=False))
                else:
                    operator.append(
                        symbols('c_'+str(i_key[0]), commutative=False))
        else:
            operator = [1]
        expr.append(coefficient*prod(operator))
    return sum(expr)

def majorana_latex(H_majorana_dict):
    """Convert the Majorana operators into LaTeX expression via SymPy

    Parameters
    ----------
    H_majorana_dict : dict
        contains Majorana operators and their coefficients

    Returns
    -------
    LaTeX expression
        output the symbolic expression in LaTeX form
    """
    H_majorana_dict_nonzero = {key: value for key,
                               value in H_majorana_dict.items() if value != 0}
    expr = []
    for key, value in H_majorana_dict_nonzero.items():
        coefficient = nsimplify(simplify(value))
        if isinstance(coefficient/I, Integer):
            coefficient = sign(
                coefficient)*UnevaluatedExpr(factorint(abs(coefficient), visual=True))
        if key != ():
            operator = []
            for i_key in key:
                operator.append(
                    symbols('\gamma_'+str(i_key), commutative=False))
        else:
            operator = [1]
        expr.append(coefficient*prod(operator))
    return sum(expr)

```

## Spin Chain

The general anisotropic 1D spin-1/2 chain of 4 sites with nearest-neighbor exchange couplings under the open boundary condition is described by the Hamiltonian in Equation (1.12).

```python
n_qubits = 4
alpha, beta_, g = symbols('alpha, beta, g')
H_spin = QubitOperator()
for j in range(n_qubits-1):
    H_spin += QubitOperator(((j, 'Z')), -Indexed(g, j))
    H_spin += QubitOperator(((j, 'X'), (j+1, 'X')), -1)
    H_spin += QubitOperator(((j, 'Y'), (j+1, 'Y')), -Indexed(alpha, j))
    H_spin += QubitOperator(((j, 'Z'), (j+1, 'Z')), -Indexed(beta_, j))
H_spin += QubitOperator(((n_qubits-1, 'Z')), -Indexed(g, n_qubits-1))
spin_latex(H_spin.terms)

```

$\displaystyle - {\alpha}_{0} Y_{0} Y_{1} - {\alpha}_{1} Y_{1} Y_{2} - {\alpha}_{2} Y_{2} Y_{3} - {\beta}_{0} Z_{0} Z_{1} - {\beta}_{1} Z_{1} Z_{2} - {\beta}_{2} Z_{2} Z_{3} - {g}_{0} Z_{0} - {g}_{1} Z_{1} - {g}_{2} Z_{2} - {g}_{3} Z_{3} - 1 X_{0} X_{1} - 1 X_{1} X_{2} - 1 X_{2} X_{3}$

## The $p$-wave superconductor

The one-dimensional tight-binding $p$-wave superconductor, whose Hamiltonian in Equation (1.16) is quadratic.

```python
n_sites = 4
t, Delta, mu = symbols('t, Delta, mu')
H_fermion = FermionOperator()
for j in range(n_sites-1):
    H_fermion += FermionOperator(((j, 1), (j, 0)), -
                                 Indexed(mu, j)) + Rational(1/2)*Indexed(mu, j)
    H_fermion += FermionOperator(((j, 1), (j+1, 0)), -Indexed(t, j))
    H_fermion += FermionOperator(((j+1, 1), (j, 0)), -
                                 Indexed(t, j).conjugate())
    H_fermion += FermionOperator(((j, 1), (j+1, 1)), -Indexed(Delta, j))
    H_fermion += FermionOperator(((j+1, 0), (j, 0)), -
                                 Indexed(Delta, j).conjugate())
H_fermion += FermionOperator(((n_sites-1, 1), (n_sites-1, 0)), -
                             Indexed(mu, n_sites-1)) + Rational(1/2)*Indexed(mu, n_sites-1)
fermion_latex(H_fermion.terms)

```

$\displaystyle - \overline{{\Delta}_{0}} c_{1} c_{0} - \overline{{\Delta}_{1}} c_{2} c_{1} - \overline{{\Delta}_{2}} c_{3} c_{2} - \overline{{t}_{0}} c^{\dagger}_{1} c_{0} - \overline{{t}_{1}} c^{\dagger}_{2} c_{1} - \overline{{t}_{2}} c^{\dagger}_{3} c_{2} - {\Delta}_{0} c^{\dagger}_{0} c^{\dagger}_{1} - {\Delta}_{1} c^{\dagger}_{1} c^{\dagger}_{2} - {\Delta}_{2} c^{\dagger}_{2} c^{\dagger}_{3} + \frac{{\mu}_{0}}{2} - {\mu}_{0} c^{\dagger}_{0} c_{0} + \frac{{\mu}_{1}}{2} - {\mu}_{1} c^{\dagger}_{1} c_{1} + \frac{{\mu}_{2}}{2} - {\mu}_{2} c^{\dagger}_{2} c_{2} + \frac{{\mu}_{3}}{2} - {\mu}_{3} c^{\dagger}_{3} c_{3} - {t}_{0} c^{\dagger}_{0} c_{1} - {t}_{1} c^{\dagger}_{1} c_{2} - {t}_{2} c^{\dagger}_{2} c_{3}$

We assign some numerical value for the coupling parameters

```python
n_sites = 4
t, Delta, mu = 3, 5, 7
H_fermion = FermionOperator()
for j in range(n_sites-1):
    H_fermion += FermionOperator(((j, 1), (j, 0)), -mu) + 1/2*mu
    H_fermion += FermionOperator(((j, 1), (j+1, 0)), -t)
    H_fermion += FermionOperator(((j+1, 1), (j, 0)), -t)
    H_fermion += FermionOperator(((j, 1), (j+1, 1)), -Delta)
    H_fermion += FermionOperator(((j+1, 0), (j, 0)), -Delta)
H_fermion += FermionOperator(((n_sites-1, 1), (n_sites-1, 0)), -mu) + 1/2*mu
fermion_latex(H_fermion.terms).doit()

```

$\displaystyle 14 - 5 c^{\dagger}_{0} c^{\dagger}_{1} - 7 c^{\dagger}_{0} c_{0} - 3 c^{\dagger}_{0} c_{1} - 5 c^{\dagger}_{1} c^{\dagger}_{2} - 3 c^{\dagger}_{1} c_{0} - 7 c^{\dagger}_{1} c_{1} - 3 c^{\dagger}_{1} c_{2} - 5 c^{\dagger}_{2} c^{\dagger}_{3} - 3 c^{\dagger}_{2} c_{1} - 7 c^{\dagger}_{2} c_{2} - 3 c^{\dagger}_{2} c_{3} - 3 c^{\dagger}_{3} c_{2} - 7 c^{\dagger}_{3} c_{3} - 5 c_{1} c_{0} - 5 c_{2} c_{1} - 5 c_{3} c_{2}$

The Hermitian part $S = S^\dagger$ of the Bogoliubov-de Gennes Hamiltonian is

```python
get_quadratic_hamiltonian(H_fermion).hermitian_part.real

```

    array([[-7., -3.,  0.,  0.],
           [-3., -7., -3.,  0.],
           [ 0., -3., -7., -3.],
           [ 0.,  0., -3., -7.]])

The anti-symmetric part $A = - A^T$ of the Bogoliubov-de Gennes Hamiltonian is

```python
get_quadratic_hamiltonian(H_fermion).antisymmetric_part.real

```

    array([[ 0., -5.,  0.,  0.],
           [ 5.,  0., -5.,  0.],
           [ 0.,  5.,  0., -5.],
           [ 0.,  0.,  5.,  0.]])

## Majorana Representation

We can transform the Hamiltonian of the $p$-wave superconducting model from fermionic representation to Majorana representation (we set $t_i=t^*_i=\Delta_i=\Delta^*_i=3$ and $\mu_i = 2$ for demonstration):

```python
n_sites = 4
t, Delta, mu = 3, 3, 2
H_fermion = FermionOperator()
for j in range(n_sites-1):
    H_fermion += FermionOperator(((j, 1), (j, 0)), -mu) + 1/2*mu
    H_fermion += FermionOperator(((j, 1), (j+1, 0)), -t)
    H_fermion += FermionOperator(((j+1, 1), (j, 0)), -t)
    H_fermion += FermionOperator(((j, 1), (j+1, 1)), -Delta)
    H_fermion += FermionOperator(((j+1, 0), (j, 0)), -Delta)
H_fermion += FermionOperator(((n_sites-1, 1), (n_sites-1, 0)), -mu) + 1/2*mu
fermion_latex(H_fermion.terms).doit()

```

$\displaystyle 4 - 3 c^{\dagger}_{0} c^{\dagger}_{1} - 2 c^{\dagger}_{0} c_{0} - 3 c^{\dagger}_{0} c_{1} - 3 c^{\dagger}_{1} c^{\dagger}_{2} - 3 c^{\dagger}_{1} c_{0} - 2 c^{\dagger}_{1} c_{1} - 3 c^{\dagger}_{1} c_{2} - 3 c^{\dagger}_{2} c^{\dagger}_{3} - 3 c^{\dagger}_{2} c_{1} - 2 c^{\dagger}_{2} c_{2} - 3 c^{\dagger}_{2} c_{3} - 3 c^{\dagger}_{3} c_{2} - 2 c^{\dagger}_{3} c_{3} - 3 c_{1} c_{0} - 3 c_{2} c_{1} - 3 c_{3} c_{2}$

Here is an instance of Equation (1.51).

```python
H_majorana_dict = get_majorana_operator(H_fermion).terms
majorana_latex(H_majorana_dict).simplify()

```

$\displaystyle i \left(- \gamma_{0} \gamma_{1} + 3 \gamma_{1} \gamma_{2} - \gamma_{2} \gamma_{3} + 3 \gamma_{3} \gamma_{4} - \gamma_{4} \gamma_{5} + 3 \gamma_{5} \gamma_{6} - \gamma_{6} \gamma_{7}\right)$

## Jordan-Wigner Transformation

Here we explicitly how to transform the spin Hamiltonian in Equation (1.12) into fermionic Hamiltonian in Equation (1.70). We remark that OpenFermion adopts the transformation in the quantum chemistry community, which differs a minus sign of the couplings $g$ along the $z$-direction

```python
n_qubits = 4
alpha, beta_, g = 3, 5, -7
H_spin = QubitOperator()
for j in range(n_qubits-1):
    H_spin += QubitOperator(((j, 'Z')), -g)
    H_spin += QubitOperator(((j, 'X'), (j+1, 'X')), -1)
    H_spin += QubitOperator(((j, 'Y'), (j+1, 'Y')), -alpha)
    H_spin += QubitOperator(((j, 'Z'), (j+1, 'Z')), -beta_)
H_spin += QubitOperator(((n_qubits-1, 'Z')), -g)
spin_latex(H_spin.terms)

```

$\displaystyle - 1 X_{0} X_{1} - 1 X_{1} X_{2} - 1 X_{2} X_{3} - 3^{1} Y_{0} Y_{1} - 3^{1} Y_{1} Y_{2} - 3^{1} Y_{2} Y_{3} - 5^{1} Z_{0} Z_{1} - 5^{1} Z_{1} Z_{2} - 5^{1} Z_{2} Z_{3} + 7^{1} Z_{0} + 7^{1} Z_{1} + 7^{1} Z_{2} + 7^{1} Z_{3}$

Apply the Jordan-Wigner transformation, one can obtain the fermionic representation.

```python
H_fermion = normal_ordered(reverse_jordan_wigner(H_spin))
fermion_latex(H_fermion.terms)

```

$\displaystyle 13^{1} - 2^{1} c^{\dagger}_{1} c^{\dagger}_{0} - 2^{1} c^{\dagger}_{2} c^{\dagger}_{1} - 2^{1} c^{\dagger}_{3} c^{\dagger}_{2} + 2^{1} c_{1} c_{0} + 2^{1} c_{2} c_{1} + 2^{1} c_{3} c_{2} - 2^{2} c^{\dagger}_{0} c_{0} - 2^{2} c^{\dagger}_{0} c_{1} - 2^{2} c^{\dagger}_{1} c_{0} - 2^{2} c^{\dagger}_{1} c_{2} - 2^{2} c^{\dagger}_{2} c_{1} - 2^{2} c^{\dagger}_{2} c_{3} - 2^{2} c^{\dagger}_{3} c_{2} - 2^{2} c^{\dagger}_{3} c_{3} + 2^{1} \cdot 3^{1} c^{\dagger}_{1} c_{1} + 2^{1} \cdot 3^{1} c^{\dagger}_{2} c_{2} + 2^{2} \cdot 5^{1} c^{\dagger}_{1} c^{\dagger}_{0} c_{1} c_{0} + 2^{2} \cdot 5^{1} c^{\dagger}_{2} c^{\dagger}_{1} c_{2} c_{1} + 2^{2} \cdot 5^{1} c^{\dagger}_{3} c^{\dagger}_{2} c_{3} c_{2}$

We can also verify the transformation from Equation (4.1) to Equation (4.2).

```python
n_qubits = 4
J, t, gamma_, delta, g = 1, 7/2, 1/2, 5, -13
H_spin = QubitOperator()
for j in range(n_qubits-1):
    H_spin += QubitOperator(((j, 'Z')), -J*g)
    H_spin += QubitOperator(((j, 'X'), (j+1, 'X')), -J*(t + gamma_))
    H_spin += QubitOperator(((j, 'Y'), (j+1, 'Y')), -J*(t - gamma_))
    H_spin += QubitOperator(((j, 'Z'), (j+1, 'Z')), -J*delta)
H_spin += QubitOperator(((n_qubits-1, 'Z')), -J*g)
spin_latex(H_spin.terms)

```

$\displaystyle 13^{1} Z_{0} + 13^{1} Z_{1} + 13^{1} Z_{2} + 13^{1} Z_{3} - 2^{2} X_{0} X_{1} - 2^{2} X_{1} X_{2} - 2^{2} X_{2} X_{3} - 3^{1} Y_{0} Y_{1} - 3^{1} Y_{1} Y_{2} - 3^{1} Y_{2} Y_{3} - 5^{1} Z_{0} Z_{1} - 5^{1} Z_{1} Z_{2} - 5^{1} Z_{2} Z_{3}$

```python
H_fermion = normal_ordered(reverse_jordan_wigner(H_spin))
fermion_latex(H_fermion.terms)

```

$\displaystyle 1 c^{\dagger}_{1} c^{\dagger}_{0} + 1 c^{\dagger}_{2} c^{\dagger}_{1} + 1 c^{\dagger}_{3} c^{\dagger}_{2} - 1 c_{1} c_{0} - 1 c_{2} c_{1} - 1 c_{3} c_{2} - 2^{4} c^{\dagger}_{0} c_{0} - 2^{4} c^{\dagger}_{3} c_{3} + 37^{1} - 7^{1} c^{\dagger}_{0} c_{1} - 7^{1} c^{\dagger}_{1} c_{0} - 7^{1} c^{\dagger}_{1} c_{2} - 7^{1} c^{\dagger}_{2} c_{1} - 7^{1} c^{\dagger}_{2} c_{3} - 7^{1} c^{\dagger}_{3} c_{2} - 2^{1} \cdot 3^{1} c^{\dagger}_{1} c_{1} - 2^{1} \cdot 3^{1} c^{\dagger}_{2} c_{2} + 2^{2} \cdot 5^{1} c^{\dagger}_{1} c^{\dagger}_{0} c_{1} c_{0} + 2^{2} \cdot 5^{1} c^{\dagger}_{2} c^{\dagger}_{1} c_{2} c_{1} + 2^{2} \cdot 5^{1} c^{\dagger}_{3} c^{\dagger}_{2} c_{3} c_{2}$