```python
# Required to load webpages
from IPython.display import IFrame
```

[Table of contents](../toc.ipynb)

# SymPy

* SymPy is a symbolic mathematics library for Python.
* It is a very powerful computer algebra system, which is easy to include in your Python scripts.
* Please find the documentation and a tutorial here [https://www.sympy.org/en/index.html](https://www.sympy.org/en/index.html)

## SymPy live

There is a very nice SymPy live shell in [https://live.sympy.org/](https://live.sympy.org/), where you can try SymPy without installation.

```python
IFrame(src='https://live.sympy.org/', width=1000, height=600)
```

## SymPy import and basics

```python
import sympy as sp

%matplotlib inline
```

Symbols can be defined with `sympy.symbols` like:

```python
x, y, t = sp.symbols('x, y, t')
```

These symbols and later equations are rendered with LaTeX, which makes pretty prints.

```python
display(x)
```

$\displaystyle x$

Expressions can be easily defined, and equations with left and right hand side are defined with `sympy.Eq` function.

```python
expr = x**2
expr
```

$\displaystyle x^{2}$

```python
eq = sp.Eq(3*x, -10)
eq
```

$\displaystyle 3 x = -10$

Plots are done with `sympy.plot` and the value range can be adjusted.

```python
sp.plot(expr, (x, -5, 5))
```

## Why should you consider symbolic math at all?

The power of symbolic computation is their precision. Just compare these two results.

```python
import math
math.sqrt(8)
```

    2.8284271247461903

```python
sp.sqrt(8)
```

$\displaystyle 2 \sqrt{2}$

You can simplify expressions and equations and also expand them.

```python
eq = sp.sin(x)**2 + sp.cos(x)**2
eq
```

$\displaystyle \sin^{2}{\left(x \right)} + \cos^{2}{\left(x \right)}$

```python
sp.simplify(eq)
```

$\displaystyle 1$

```python
eq = x*(x + y)
eq
```

$\displaystyle x \left(x + y\right)$

```python
sp.expand(eq)
```

$\displaystyle x^{2} + x y$

```python
sp.factor(eq)
```

$\displaystyle x \left(x + y\right)$

Differentiation and integration are built in of course.

```python
eq = sp.sin(x) * sp.exp(x)
eq
```

$\displaystyle e^{x} \sin{\left(x \right)}$

```python
sp.diff(eq, x)
```

$\displaystyle e^{x} \sin{\left(x \right)} + e^{x} \cos{\left(x \right)}$

```python
sp.integrate(eq, x)
```

$\displaystyle \frac{e^{x} \sin{\left(x \right)}}{2} - \frac{e^{x} \cos{\left(x \right)}}{2}$

Or define an explicit interval for the integration.

```python
sp.integrate(eq, (x, -10, 10))
```

$\displaystyle \frac{e^{10} \sin{\left(10 \right)}}{2} + \frac{\cos{\left(10 \right)}}{2 e^{10}} + \frac{\sin{\left(10 \right)}}{2 e^{10}} - \frac{e^{10} \cos{\left(10 \right)}}{2}$

We can also easily substitute one variable of an expression.

```python
eq.subs(x, 2)
```

$\displaystyle e^{2} \sin{\left(2 \right)}$

Solve one equation. $x^2 + \sin(x) = -10$.

```python
sp.solve(x**2 + 3*x - 10, x)
```

    [-5, 2]

## More advanced topics

Here, we will solve a linear system of equations.

```python
e1 = sp.Eq(3*x + 4*y, -20)
e2 = sp.Eq(4*y, -3)

system_of_eq = [e1, e2]

from sympy.solvers.solveset import linsolve

linsolve(system_of_eq, (x, y))
```

$\displaystyle \left\{\left( - \frac{17}{3}, \  - \frac{3}{4}\right)\right\}$

Also differential equations can be used. Let us solve $y'' - y = e^t$ for instance.

```python
y = sp.Function('y')
sp.dsolve(sp.Eq(y(t).diff(t, t) - y(t), sp.exp(t)), y(t))
```

$\displaystyle y{\left(t \right)} = C_{2} e^{- t} + \left(C_{1} + \frac{t}{2}\right) e^{t}$

Finally, we will have a short look at matrices.

```python
A = sp.Matrix([[0, 1],
               [1, 0]])
A
```

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

```python
A = sp.eye(3)
A
```

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

```python
A = sp.zeros(2, 3)
A
```

$\displaystyle \left[\begin{matrix}0 & 0 & 0\\0 & 0 & 0\end{matrix}\right]$

Inversion of a matrix is done with `**-1` or better readable with `.inv()`.

```python
A = sp.eye(2) * 2
A = A * 2
A.inv()
```

```python
A[-2] = x
A
```

$\displaystyle \left[\begin{matrix}4 & 0\\x & 4\end{matrix}\right]$

## To sum up

* SymPy is a very powerful computer algebra package!
* It is light, small, and easy to install through pip and conda.
* Simple to integrate in your Python project.


# Apollo Lunar Module

```python
import numpy as np
import sympy as sy
sy.init_printing()
```

```python
!pip install sympy
```

    Collecting sympy
      Downloading sympy-1.9-py3-none-any.whl (6.2 MB)
         |████████████████████████████████| 6.2 MB 522 kB/s eta 0:00:01
    Collecting mpmath>=0.19
      Downloading mpmath-1.2.1-py3-none-any.whl (532 kB)
         |████████████████████████████████| 532 kB 222 kB/s eta 0:00:01
    Installing collected packages: mpmath, sympy
    Successfully installed mpmath-1.2.1 sympy-1.9
    WARNING: You are using pip version 21.2.4; however, version 21.3 is available.
    You should consider upgrading via the '/home/kjartan/.pyenv/versions/3.8.11/envs/tug/bin/python3.8 -m pip install --upgrade pip' command.

```python
s = sy.symbols('s', real=False)
k1, k2 = sy.symbols('k_1, k_2')
k1
```

```python
A = sy.Matrix([[0,0,0],[1,0,0],[0,k2,0]])
B = sy.Matrix([[k1],[0],[0]])
A
```

```python
l0, l1, l2, l3 = sy.symbols('l_0, l_1, l_2, l_3')
L = sy.Matrix([[l1, l2, l3]])
L
```

```python
M = s*sy.eye(3) - (A-B*L)
M
```

```python
char_pol = sy.poly(sy.det(M), s)
```

## Desired closed-loop poles

```python
p1 = -np.sqrt(2)
p2 = complex(-1,1)
p3 = complex(-1, -1)
p3
```

    (-1-1j)

```python
char_pol_des = sy.poly((s-p1)*(s-p2)*(s-p3), s)
```

```python
char_pol_des
```

```python
eqns = [c_ - cd_ for c_, cd_ in zip(char_pol.coeffs(), char_pol_des.coeffs())]
eqns
```

```python
sol = sy.solve(eqns, [l1, l2, l3])
```

```python
sol
```

```python

```


```python
# 任意选一个你喜欢的整数，这能帮你得到稳定的结果
seed = 2333 # todo
```

# 欢迎来到线性回归项目

若项目中的题目有困难没完成也没关系，我们鼓励你带着问题提交项目，评审人会给予你诸多帮助。

所有选做题都可以不做，不影响项目通过。如果你做了，那么项目评审会帮你批改，也会因为选做部分做错而判定为不通过。

其中非代码题可以提交手写后扫描的 pdf 文件，或使用 Latex 在文档中直接回答。

# 1 矩阵运算

## 1.1 创建一个 4*4 的单位矩阵

```python
# 这个项目设计来帮你熟悉 python list 和线性代数
# 你不能调用任何NumPy以及相关的科学计算库来完成作业

# 本项目要求矩阵统一使用二维列表表示，如下：
A = [[1,2,3],
     [2,3,3],
     [1,2,5]]

B = [[1,2,3,5],
     [2,3,3,5],
     [1,2,5,1]]

# 向量也用二维列表表示
C = [[1],
     [2],
     [3]]

#TODO 创建一个 4*4 单位矩阵
I = [[1,0,0,0],
     [0,1,0,0],
     [0,0,1,0],
     [0,0,0,1]]
```

## 1.2 返回矩阵的行数和列数

```python
# TODO 返回矩阵的行数和列数
def shape(M):
    return len(M), len(M[0])
```

```python
# 运行以下代码测试你的 shape 函数
%run -i -e test.py LinearRegressionTestCase.test_shape
```

    .
    ----------------------------------------------------------------------
    Ran 1 test in 0.001s

    OK

## 1.3 每个元素四舍五入到特定小数数位

```python
# TODO 每个元素四舍五入到特定小数数位
# 直接修改参数矩阵，无返回值
def matxRound(M, decPts=4):
    for row in range(len(M)):
        for col in range(len(M[row])):
            M[row][col] = round(M[row][col], decPts)
```

```python
# 运行以下代码测试你的 matxRound 函数
%run -i -e test.py LinearRegressionTestCase.test_matxRound
```

    .
    ----------------------------------------------------------------------
    Ran 1 test in 0.010s

    OK

## 1.4 计算矩阵的转置

```python
# TODO 计算矩阵的转置
def transpose(M):
    rows, cols = shape(M)
    new = []
    for col in range(cols):
        new.append([])
        for row in range(rows):
            new[col].append(0)
    for row in range(rows):
        for col in range(cols):
            new[col][row] = M[row][col]
    return new

#     Pythonic approach
#     return [list(col) for col in zip(*M)]
```

```python
# 运行以下代码测试你的 transpose 函数
%run -i -e test.py LinearRegressionTestCase.test_transpose
```

    .
    ----------------------------------------------------------------------
    Ran 1 test in 0.030s

    OK

## 1.5 计算矩阵乘法 AB

```python
# TODO 计算矩阵乘法 AB，如果无法相乘则raise ValueError
def dot(a, b):
    return sum(i * j for i, j in zip(a, b))

def matxMultiply(A, B):
    n, m = shape(A)
    p, q = shape(B)
    if m != p:
        raise ValueError('dimension of matrices don\'t match')
    result = []
    for row in range(n):
        result.append([])
        for col in range(q):
            # ith row jth column = dot product of ith row of A and jth column of B
            # im not using [row][col] to index but rather just appending...
            # just because i can
            result[row].append(dot(A[row], [i[col] for i in B]))
    return result
```

```python
# 运行以下代码测试你的 matxMultiply 函数
%run -i -e test.py LinearRegressionTestCase.test_matxMultiply
```

    .
    ----------------------------------------------------------------------
    Ran 1 test in 0.124s

    OK

---

# 2 Gaussian Jordan 消元法

## 2.1 构造增广矩阵

$ A = \begin{bmatrix}
    a_{11}    & a_{12} & ... & a_{1n}\\
    a_{21}    & a_{22} & ... & a_{2n}\\
    a_{31}    & a_{22} & ... & a_{3n}\\
    ...    & ... & ... & ...\\
    a_{n1}    & a_{n2} & ... & a_{nn}\\
\end{bmatrix} , b = \begin{bmatrix}
    b_{1}  \\
    b_{2}  \\
    b_{3}  \\
    ...    \\
    b_{n}  \\
\end{bmatrix}$

返回 $ Ab = \begin{bmatrix}
    a_{11}    & a_{12} & ... & a_{1n} & b_{1}\\
    a_{21}    & a_{22} & ... & a_{2n} & b_{2}\\
    a_{31}    & a_{22} & ... & a_{3n} & b_{3}\\
    ...    & ... & ... & ...& ...\\
    a_{n1}    & a_{n2} & ... & a_{nn} & b_{n} \end{bmatrix}$

```python
# TODO 构造增广矩阵，假设A，b行数相同
def augmentMatrix(A, b):
#     assert n == shape(b)[0] # this is in the assumption!
    new = [A[row] + b[row] for row in range(len(A))]
    return new
```

```python
# 运行以下代码测试你的 augmentMatrix 函数
%run -i -e test.py LinearRegressionTestCase.test_augmentMatrix
```

    .
    ----------------------------------------------------------------------
    Ran 1 test in 0.004s

    OK

## 2.2 初等行变换
- 交换两行
- 把某行乘以一个非零常数
- 把某行加上另一行的若干倍：

```python
# TODO r1 <---> r2
# 直接修改参数矩阵，无返回值
def swapRows(M, r1, r2):
    if r1 != r2:
        M[r1], M[r2] = M[r2], M[r1]
```

```python
# 运行以下代码测试你的 swapRows 函数
%run -i -e test.py LinearRegressionTestCase.test_swapRows
```

    .
    ----------------------------------------------------------------------
    Ran 1 test in 0.004s

    OK

```python
## TODO r1 <--- r1 * scale
# scale为0是非法输入，要求 raise ValueError
# 直接修改参数矩阵，无返回值
def scaleRow(M, r, scale):
    if scale == 0:
        raise ValueError('cannot scale a matrix by zero')
    for col in range(len(M[r])):
        M[r][col] *= scale
```

```python
# 运行以下代码测试你的 scaleRow 函数
%run -i -e test.py LinearRegressionTestCase.test_scaleRow
```

    .
    ----------------------------------------------------------------------
    Ran 1 test in 0.005s

    OK

```python
# TODO r1 <--- r1 + r2*scale
# 直接修改参数矩阵，无返回值
def addScaledRow(M, r1, r2, scale):
    if scale == 0:
        raise ValueError('cannot scale a matrix by zero')
    for col in range(len(M[r1])):
        M[r1][col] += scale * M[r2][col]

```

```python
# 运行以下代码测试你的 addScaledRow 函数
%run -i -e test.py LinearRegressionTestCase.test_addScaledRow
```

    .
    ----------------------------------------------------------------------
    Ran 1 test in 0.002s

    OK

## 2.3  Gaussian Jordan 消元法求解 Ax = b

### 2.3.1 算法

步骤1 检查A，b是否行数相同

步骤2 构造增广矩阵Ab

步骤3 逐列转换Ab为化简行阶梯形矩阵 [中文维基链接](https://zh.wikipedia.org/wiki/%E9%98%B6%E6%A2%AF%E5%BD%A2%E7%9F%A9%E9%98%B5#.E5.8C.96.E7.AE.80.E5.90.8E.E7.9A.84-.7Bzh-hans:.E8.A1.8C.3B_zh-hant:.E5.88.97.3B.7D-.E9.98.B6.E6.A2.AF.E5.BD.A2.E7.9F.A9.E9.98.B5)

    对于Ab的每一列（最后一列除外）
        当前列为列c
        寻找列c中 对角线以及对角线以下所有元素（行 c~N）的绝对值的最大值
        如果绝对值最大值为0
            那么A为奇异矩阵，返回None (你可以在选做问题2.4中证明为什么这里A一定是奇异矩阵)
        否则
            使用第一个行变换，将绝对值最大值所在行交换到对角线元素所在行（行c）
            使用第二个行变换，将列c的对角线元素缩放为1
            多次使用第三个行变换，将列c的其他元素消为0

步骤4 返回Ab的最后一列

**注：** 我们并没有按照常规方法先把矩阵转化为行阶梯形矩阵，再转换为化简行阶梯形矩阵，而是一步到位。如果你熟悉常规方法的话，可以思考一下两者的等价性。

### 2.3.2 算法推演

为了充分了解Gaussian Jordan消元法的计算流程，请根据Gaussian Jordan消元法，分别手动推演矩阵A为***可逆矩阵***，矩阵A为***奇异矩阵***两种情况。

#### 推演示例

$Ab = \begin{bmatrix}
    -7 & 5 & -1 & 1\\
    1 & -3 & -8 & 1\\
    -10 & -2 & 9 & 1\end{bmatrix}$

$ --> $
$\begin{bmatrix}
    1 & \frac{1}{5} & -\frac{9}{10} & -\frac{1}{10}\\
    0 & -\frac{16}{5} & -\frac{71}{10} & \frac{11}{10}\\
    0 & \frac{32}{5} & -\frac{73}{10} & \frac{3}{10}\end{bmatrix}$

$ --> $
$\begin{bmatrix}
    1 & 0 & -\frac{43}{64} & -\frac{7}{64}\\
    0 & 1 & -\frac{73}{64} & \frac{3}{64}\\
    0 & 0 & -\frac{43}{4} & \frac{5}{4}\end{bmatrix}$

$ --> $
$\begin{bmatrix}
    1 & 0 & 0 & -\frac{3}{16}\\
    0 & 1 & 0 & -\frac{59}{688}\\
    0 & 0 & 1 & -\frac{5}{43}\end{bmatrix}$

#### 推演有以下要求:
1. 展示每一列的消元结果, 比如3*3的矩阵, 需要写三步
2. 用分数来表示
3. 分数不能再约分
4. 我们已经给出了latex的语法,你只要把零改成你要的数字(或分数)即可
5. 检查你的答案, 可以用[这个](http://www.math.odu.edu/~bogacki/cgi-bin/lat.cgi?c=sys), 或者后面通过单元测试后的`gj_Solve`

_你可以用python的 [fractions](https://docs.python.org/2/library/fractions.html) 模块辅助你的约分_

#### 以下开始你的尝试吧!

```python
# 不要修改这里！
from helper import *
A = generateMatrix(3,seed,singular=False)
b = np.ones(shape=(3,1),dtype=int) # it doesn't matter
Ab = augmentMatrix(A.tolist(),b.tolist()) # 请确保你的增广矩阵已经写好了
printInMatrixFormat(Ab,padding=3,truncating=0)
```

    -10,  9,  5 ||  1
     -4,  3, -4 ||  1
     -2,  3,  5 ||  1

请按照算法的步骤3，逐步推演***可逆矩阵***的变换。

在下面列出每一次循环体执行之后的增广矩阵。

要求：
1. 做分数运算
2. 使用`\frac{n}{m}`来渲染分数，如下：
 - $\frac{n}{m}$
 - $-\frac{a}{b}$

$ Ab = \begin{bmatrix}
    -10 & 9 & 5 & 1 \\
    -4 & 3 & -4 & 1 \\
    -2 & 3 & 5 & 1 \end{bmatrix}$

$ --> \begin{bmatrix}
    1 & -\frac{9}{10} & -\frac{5}{10} & -\frac{1}{10} \\
    0 & -\frac{3}{5} & -6 & \frac{3}{5} \\
    0 & \frac{6}{5} & 4 & \frac{4}{5} \end{bmatrix}$

$ --> \begin{bmatrix}
    1 & 0 & \frac{5}{2} & \frac{1}{2} \\
    0 & 1 & \frac{10}{3} & \frac{2}{3} \\
    0 & 0 & -4 & 1 \end{bmatrix}$

$ --> \begin{bmatrix}
    1 & 0 & 0 & \frac{9}{8} \\
    0 & 1 & 0 & \frac{3}{2} \\
    0 & 0 & 1 & -\frac{1}{4} \end{bmatrix}$

```python
# 不要修改这里！
A = generateMatrix(3,seed,singular=True)
b = np.ones(shape=(3,1),dtype=int)
Ab = augmentMatrix(A.tolist(),b.tolist()) # 请确保你的增广矩阵已经写好了
printInMatrixFormat(Ab,padding=3,truncating=0)
```

      6,  1,  9 ||  1
      0,  1,  1 ||  1
     -6,  1, -7 ||  1

请按照算法的步骤3，逐步推演***奇异矩阵***的变换。

在下面列出每一次循环体执行之后的增广矩阵。

要求：
1. 做分数运算
2. 使用`\frac{n}{m}`来渲染分数，如下：
 - $\frac{n}{m}$
 - $-\frac{a}{b}$

$ Ab = \begin{bmatrix}
    6 & 1 & 9 & 1 \\
    0 & 1 & 1 & 1 \\
    -6 & 1 & -7 & 1 \end{bmatrix}$

$ --> \begin{bmatrix}
    1 & \frac{1}{6} & \frac{3}{2} & \frac{1}{6} \\
    0 & 1 & 1 & 1 \\
    0 & 2 & 2 & 2 \end{bmatrix}$

$ --> \begin{bmatrix}
    1 & 0 & \frac{4}{3} & 0 \\
    0 & 1 & 1 & 0 \\
    0 & 0 & 0 & 0 \end{bmatrix}$

### 2.3.3 实现 Gaussian Jordan 消元法

```python
# TODO 实现 Gaussain Jordan 方法求解 Ax = b

""" Gaussian Jordan 方法求解 Ax = b.
    参数
        A: 方阵
        b: 列向量
        decPts: 四舍五入位数，默认为4
        epsilon: 判读是否为0的阈值，默认 1.0e-16

    返回列向量 x 使得 Ax = b
    返回None，如果 A，b 高度不同
    返回None，如果 A 为奇异矩阵
"""

from pprint import pprint

def gj_Solve(A, b, decPts=4, epsilon=1.0e-16):
    size = len(A)
    if size != len(b):
        return None
    C = augmentMatrix(A, b)
    for c in range(size):
        absCol = [abs(row[c]) for row in C[c:]]
        maxNum = max(absCol)
        if maxNum < epsilon: # singular matrix
            return None
        swapRows(C, absCol.index(maxNum) + c, c)
        scaleRow(C, c, 1 / C[c][c])
        for r in range(size):
            if r == c or abs(C[r][c]) < epsilon:
                continue
            addScaledRow(C, r, c, -C[r][c])
    solution = [[row[-1]] for row in C]
    return solution
```

```python
# 运行以下代码测试你的 gj_Solve 函数
%run -i -e test.py LinearRegressionTestCase.test_gj_Solve
```

    .
    ----------------------------------------------------------------------
    Ran 1 test in 4.122s

    OK

###### (选做) 2.4 算法正确判断了奇异矩阵：

在算法的步骤3 中，如果发现某一列对角线和对角线以下所有元素都为0，那么则断定这个矩阵为奇异矩阵。

我们用正式的语言描述这个命题，并证明为真。

证明下面的命题：

**如果方阵 A 可以被分为4个部分: **

$ A = \begin{bmatrix}
    I    & X \\
    Z    & Y \\
\end{bmatrix} , \text{其中 I 为单位矩阵，Z 为全0矩阵，Y 的第一列全0}$，

**那么A为奇异矩阵。**

提示：从多种角度都可以完成证明
- 考虑矩阵 Y 和 矩阵 A 的秩
- 考虑矩阵 Y 和 矩阵 A 的行列式
- 考虑矩阵 A 的某一列是其他列的线性组合

**TODO** 证明：

由于 $A,\ I$ 为方阵，则 $Y$ 为方阵，易知，$X$ 的行数与 $Z$ 的列数一致。
设 $n$ 为 $I$ 的大小，并设 $x_1,\ x_2,\dots,\ x_n$ 为矩阵 $X$ 的第一列的元素；设 $A_i$ 为矩阵 $A$ 的第 $i$ 列。

由于 $Y$ 的第一列均为 0，那么 $A_{n+1}$（$Y$ 所在的这一列）可以被表示为
$$ A_{n+1} = x_1 A_1 + x_2 A_2 + \cdots + x_n A_n = \sum_{i=1}^n x_i A_i $$

因此 $A_{n+1}$ 这一列并不是线性独立的（是 $A$ 前 $n$ 列的线性组合），又因 $A$ 为方阵，则得出 $A$ 不满秩，即 $A$ 是奇异矩阵，证毕。

# 3  线性回归

## 3.1 随机生成样本点

```python
# 不要修改这里！
# 运行一次就够了！
from helper import *
from matplotlib import pyplot as plt
%matplotlib inline

X,Y = generatePoints(seed,num=100)

## 可视化
plt.xlim((-5,5))
plt.xlabel('x',fontsize=18)
plt.ylabel('y',fontsize=18)
plt.scatter(X,Y,c='b')
plt.show()
```

## 3.2 拟合一条直线

### 3.2.1 猜测一条直线

```python
#TODO 请选择最适合的直线 y = mx + b
m1 = 2/5
b1 = 13.8

# 不要修改这里！
plt.xlim((-5,5))
x_vals = plt.axes().get_xlim()
y_vals = [m1*x+b1 for x in x_vals]
plt.plot(x_vals, y_vals, '-', color='r')

plt.xlabel('x',fontsize=18)
plt.ylabel('y',fontsize=18)
plt.scatter(X,Y,c='b')

plt.show()
```

### 3.2.2 计算平均平方误差 (MSE)

我们要编程计算所选直线的平均平方误差(MSE), 即数据集中每个点到直线的Y方向距离的平方的平均数，表达式如下：
$$
MSE = \frac{1}{n}\sum_{i=1}^{n}{(y_i - mx_i - b)^2}
$$

```python
# TODO 实现以下函数并输出所选直线的MSE

def calculateMSE(X,Y,m,b):
    error = 0
    for x, y in zip(X, Y):
        diff = (y - m * x - b) ** 2
        error += diff
    error /= len(X)
    return error

#     Pythonic approach
#     return sum([(y - m * x - b) ** 2 for x, y in zip(X, Y)]) / len(Y)

print(calculateMSE(X,Y,m1,b1))
```

    1.489379289068421

### 3.2.3 调整参数 $m, b$ 来获得最小的平方平均误差

你可以调整3.2.1中的参数 $m1,b1$ 让蓝点均匀覆盖在红线周围，然后微调 $m1, b1$ 让MSE最小。

## 3.3 (选做) 找到参数 $m, b$ 使得平方平均误差最小

**这一部分需要简单的微积分知识(  $ (x^2)' = 2x $ )。因为这是一个线性代数项目，所以设为选做。**

刚刚我们手动调节参数，尝试找到最小的平方平均误差。下面我们要精确得求解 $m, b$ 使得平方平均误差最小。

定义目标函数 $E$ 为
$$
E = \frac{1}{2}\sum_{i=1}^{n}{(y_i - mx_i - b)^2}
$$

因为 $E = \frac{n}{2}MSE$, 所以 $E$ 取到最小值时，$MSE$ 也取到最小值。要找到 $E$ 的最小值，即要找到 $m, b$ 使得 $E$ 相对于 $m$, $E$ 相对于 $b$ 的偏导数等于0.

因此我们要解下面的方程组。

$$
\begin{cases}
\displaystyle
\frac{\partial E}{\partial m} =0 \\
\\
\displaystyle
\frac{\partial E}{\partial b} =0 \\
\end{cases}
$$

### 3.3.1 计算目标函数相对于参数的导数
首先我们计算两个式子左边的值

证明/计算：
$$
\frac{\partial E}{\partial m} = \sum_{i=1}^{n}{-x_i(y_i - mx_i - b)}
$$

$$
\frac{\partial E}{\partial b} = \sum_{i=1}^{n}{-(y_i - mx_i - b)}
$$

**TODO** 证明:

设 $u_i = (y_i - mx_i - b)$，我们可以重写目标函数 $E$ 为
$$ E = \frac{1}{2} \sum_{i=1}^n u_i^2 $$
设 $E_i = \frac{1}{2} u_i^2$，我们可以再重写目标函数 $E$ 为（trivially）
$$ E = \sum_{i=1}^n E_i$$
则，我们有
$$ \frac{\partial E}{\partial m} = \sum_{i=1}^n \frac{\partial E_i}{\partial m} \quad\textrm{和}\quad \frac{\partial E}{\partial b} = \sum_{i=1}^n \frac{\partial E_i}{\partial b} $$

我们先求 $E_i$ 关于 $m$ 的偏导数，即 $\frac{\partial E_i}{\partial m}$，根据链式法则，我们有，对于任意的 $i \in \{1,2,\dots,n\}$
$$ \frac{\partial E_i}{\partial m} = \frac{\mathrm{d} E_i}{\mathrm{d} u_i} \cdot \frac{\partial u_i}{\partial m} $$
在 $u_i$ 中，只有一项 $-mx_i$ 是和 $m$ 相关的，因此 $\frac{\partial u_i}{\partial m} = -x_i$，即
$$ \frac{\partial E_i}{\partial m} = u_i\cdot (-x_i) = -x_i(y_i - mx_i - b) $$
即
$$ \frac{\partial E}{\partial m} = \sum_{i=1}^n -x_i(y_i - mx_i - b) $$

同理，我们可得
$$ \frac{\partial E_i}{\partial b} = \frac{\partial E_i}{\partial u_i}\cdot\frac{\partial u_i}{\partial b} = u_i\cdot (-1) = -(y_i - mx_i - b) $$
即
$$ \frac{\partial E}{\partial b} = \sum_{i=1}^n -(y_i - mx_i - b) $$
证毕。

### 3.3.2 实例推演

现在我们有了一个二元二次方程组

$$
\begin{cases}
\displaystyle
\sum_{i=1}^{n}{-x_i(y_i - mx_i - b)} =0 \\
\displaystyle
\sum_{i=1}^{n}{-(y_i - mx_i - b)} =0 \\
\end{cases}
$$

为了加强理解，我们用一个实际例子演练。

我们要用三个点 $(1,1),\ (2,2),\ (3,2)$ 来拟合一条直线 $y = mx + b$, 请写出

- 目标函数 $E$,
- 二元二次方程组，
- 并求解最优参数 $m, b$

**TODO** 写出目标函数，方程组和最优参数

1. 目标函数 $E$

\begin{align}
    E &= \frac{1}{2}\sum_{i=1}^n(y_i - mx_i - b)^2 \\
      &= \frac{1}{2}\left((1 - m - b)^2 + (2 - 2m - b)^2 + (3 - 2m - b)^2\right) \\
      &= \frac{1}{2}\left(9m^2 - 22m + 10mb - 12b + 3b^2 + 14\right) \\
      &= \frac{9}{2}m^2 - 11m + 5mb - 6b + \frac{3}{2}b^2 + 7
\end{align}

2. 二元二次方程组

$$\begin{cases}
    \displaystyle \sum_{i=1}^n -x_i(y_i - mx_i - b) = 0 \\
    \displaystyle \sum_{i=1}^n -(y_i - mx_i - b) = 0 \\
\end{cases} \; \Rightarrow \;
\begin{cases}
    \displaystyle -(1 - m - b) - 2(2 - 2m - b) - 3(2 - 3m - b) = 0 \\
    \displaystyle -(1 - m - b) - (2 - 2m - b) - (2 - 3m - b) = 0 \\
\end{cases} \; \Rightarrow \;
\begin{cases}
    \displaystyle -11 + 14m + 6b = 0 \\
    \displaystyle -5 + 8m + 3b = 0 \\
\end{cases}$$

3. 求解最优参数 $m,\ b$

\begin{cases}
    -11 + 14m + 6b = 0 \\
    -5 + 8m + 3b = 0 \\
\end{cases}

$$ (1) + -2 \times (2) $$
$$ \Downarrow $$

\begin{cases}
    \displaystyle -1 - 2m = 0 \quad \\
    \displaystyle -5 + 8m + 3b = 0 \quad \\
\end{cases}

$$ \therefore\ m = -\frac{1}{2} $$

$$ \therefore\ b = \frac{11 - 14m}{6} = \frac{18}{6} = 3 $$

$$\therefore\begin{cases}
    \displaystyle m = -\frac{1}{2}\\
    \displaystyle b = 3\\
\end{cases}$$

### 3.3.3 将方程组写成矩阵形式

我们的二元二次方程组可以用更简洁的矩阵形式表达，将方程组写成矩阵形式更有利于我们使用 Gaussian Jordan 消元法求解。

请证明
$$
\begin{bmatrix}
    \frac{\partial E}{\partial m} \\
    \frac{\partial E}{\partial b}
\end{bmatrix} = X^TXh - X^TY
$$

其中向量 $Y$, 矩阵 $X$ 和 向量 $h$ 分别为 :
$$
Y =  \begin{bmatrix}
    y_1 \\
    y_2 \\
    ... \\
    y_n
\end{bmatrix}
,
X =  \begin{bmatrix}
    x_1 & 1 \\
    x_2 & 1\\
    ... & ...\\
    x_n & 1 \\
\end{bmatrix},
h =  \begin{bmatrix}
    m \\
    b \\
\end{bmatrix}
$$

**TODO** 证明:

首先，
$$ X^\top X = \begin{bmatrix}
    \displaystyle \sum_{i=1}^n x_i^2 & \displaystyle \sum_{i=1}^n x_i \\
    \displaystyle \sum_{i=1}^n x_i & \displaystyle \sum_{i=1}^n 1
\end{bmatrix}$$
因此，
$$ X^\top Xh = \begin{bmatrix}
    \displaystyle \sum_{i=1}^n mx_i^2 + \sum_{i=1}^n bx_i \\
    \displaystyle \sum_{i=1}^n mx_i + \sum_{i=1}^n b
\end{bmatrix}$$
另外，
$$ X^\top Y = \begin{bmatrix}
    \displaystyle \sum_{i=1}^n x_i y_i \\
    \displaystyle \sum_{i=1}^n y_i
\end{bmatrix}$$
因此，
$$ X^\top Xh - X^\top Y = \begin{bmatrix}
    \displaystyle \sum_{i=1}^n mx_i^2 + \sum_{i=1}^n bx_i - \sum_{i=1}^n x_i y_i \\
    \displaystyle \sum_{i=1}^n mx_i + \sum_{i=1}^n b - \sum_{i=1}^n y_i
\end{bmatrix} = \begin{bmatrix}
    \displaystyle \sum_{i=1}^n (mx_i^2 + bx_i - x_i y_i) \\
    \displaystyle \sum_{i=1}^n (mx_i + b - y_i)
\end{bmatrix} = \begin{bmatrix}
    \displaystyle \sum_{i=1}^n -x_i(y_i - mx_i - b) \\
    \displaystyle \sum_{i=1}^n -(y_i - mx_i - b)
\end{bmatrix} = \begin{bmatrix}
    \displaystyle \frac{\partial E}{\partial m} \\
    \displaystyle \frac{\partial E}{\partial b}
\end{bmatrix}$$
证毕。

至此我们知道，通过求解方程 $X^TXh = X^TY$ 来找到最优参数。这个方程十分重要，他有一个名字叫做 **Normal Equation**，也有直观的几何意义。你可以在 [子空间投影](http://open.163.com/movie/2010/11/J/U/M6V0BQC4M_M6V2AJLJU.html) 和 [投影矩阵与最小二乘](http://open.163.com/movie/2010/11/P/U/M6V0BQC4M_M6V2AOJPU.html) 看到更多关于这个方程的内容。

### 3.4 求解 $X^TXh = X^TY$

在3.3 中，我们知道线性回归问题等价于求解 $X^TXh = X^TY$ (如果你选择不做3.3，就勇敢的相信吧，哈哈)

```python
# TODO 实现线性回归
'''
参数：X, Y 存储着一一对应的横坐标与纵坐标的两个一维数组
返回：m，b 浮点数
'''
def linearRegression(X,Y):
    X = [[i, 1] for i in X]
    Y = [[i] for i in Y]
    A = matxMultiply(transpose(X), X)
    b = matxMultiply(transpose(X), Y)
    x = gj_Solve(A, b)
    return x[0][0], x[1][0]

m2,b2 = linearRegression(X,Y)
assert isinstance(m2,float),"m is not a float"
assert isinstance(b2,float),"b is not a float"
print(m2,b2)
```

    0.30111301039028665 13.867374963487162

你求得的回归结果是什么？
请使用运行以下代码将它画出来。

```python
# 请不要修改下面的代码
x1,x2 = -5,5
y1,y2 = x1*m2+b2, x2*m2+b2

plt.xlim((-5,5))
plt.xlabel('x',fontsize=18)
plt.ylabel('y',fontsize=18)
plt.scatter(X,Y,c='b')
plt.plot((x1,x2),(y1,y2),'r')
plt.title('y = {m:.4f}x + {b:.4f}'.format(m=m2,b=b2))
plt.show()
```

你求得的回归结果对当前数据集的MSE是多少？

```python
print(calculateMSE(X,Y,m2,b2))
```

    1.402604878826932


# Reconstructing an *off-axis* hologram by Fresnel Approximation

Reference: Digital holography and wavefront sensing by Ulf Schnars, Claas Falldorf, John Watson, and Werner Jüptner, Springer-verlag Berlin an, 2016. (Section 3.2)

## Info about the digital hologram:
'ulf7.BMP' is a digital hologram created by recording an object at about 1 meter distance with HeNe laser (632.8 nm) and an image sensor with 6.8 µm pixel size.

```python
#Import libraries realted to matplotlib and mathematical operations
%matplotlib inline
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import numpy as np
```

```python
# Read the hologram image file
hologram = mpimg.imread('ulf7.BMP')
hologram = hologram.astype(float) #Convert into float type. Crucial for non integer based mathematical operations
# plot/view the hologram
imgplot = plt.imshow(hologram, cmap="viridis")
```

## Some equations from the book!

The *Fresnel-Kirchhoff* integral describing diffraction field beyond an aperture is given by the coherent superposition of the secondary waves (section 2.4)
\begin{equation}
\Gamma\left(\xi^{\prime}, \eta^{\prime}\right)=\frac{i}{\lambda} \int_{-\infty}^{\infty} \int_{-\infty}^{\infty} A(x, y) \frac{\exp \left(-i \frac{2 \pi}{\lambda} \rho^{\prime}\right)}{\rho^{\prime}} Q d x d y
\end{equation}
where, $A(x, y)$ is the complex amplitude in the plane of the diffracting aperture, $\rho^{\prime}$ is the distance between a point in the aperture plane and a point in the observation plane, and $Q$ is the inclination factor defined to take care of no backward propagation of the diffracted optical field. For holograms, $Q$ is approximately equal to 1.

A hologram $h(x,y)$ recorded by a reference light wave $E_{R}(x, y)$ can be reconstructed by a conjugate reference wave $E_{R}^{*}(x, y)$ as described by the following *Fresnel-Kirchhoff* integral
\begin{equation}
\Gamma(\xi, \eta)=\frac{i}{\lambda} \int_{-\infty}^{\infty} \int_{-\infty}^{\infty} h(x, y) E_{R}^{*}(x, y) \frac{\exp \left(-i \frac{2 \pi}{\lambda} \rho\right)}{\rho} d x d y
\end{equation}
with $\rho = \sqrt{ (x-\xi)^2 + (y-\eta)^2 + d^2 }$. Here $d$ is the distance between the object and hologram planes. Substituting the apprximated *Taylor* expansion of $\rho$ in above equation leads to the Fresnel reonstruction field relation (see section 3.2 of the book)
\begin{aligned} \Gamma(\xi, \eta)=& \frac{i}{\lambda d} \exp \left(-i \frac{2 \pi}{\lambda} d\right) \exp \left[-i \frac{\pi}{\lambda d}\left(\xi^{2}+\eta^{2}\right)\right] \times \int_{-\infty}^{\infty} \int_{-\infty}^{\infty} E_{R}^{*}(x, y) h(x, y) \exp \left[-i \frac{\pi}{\lambda d}\left(x^{2}+y^{2}\right)\right] \exp \left[i \frac{2 \pi}{\lambda d}(x \xi+y \eta)\right] d x d y \end{aligned}
Or, in a digital form by
\begin{aligned} \Gamma(m, n)=& \frac{i}{\lambda d} \exp \left(-i \frac{2 \pi}{\lambda} d\right) \exp \left[-i \pi \lambda d\left(\frac{m^{2}}{N^{2} \Delta x^{2}}+\frac{n^{2}}{N^{2} \Delta y^{2}}\right)\right] \times \sum_{k=0}^{N-1} \sum_{l=0}^{N-1} E_{R}^{*}(k, l) h(k, l) \exp \left[-i \frac{\pi}{\lambda d}\left(k^{2} \Delta x^{2}+l^{2} \Delta y^{2}\right)\right] \exp \left[i 2 \pi\left(\frac{k m}{N}+\frac{l n}{N}\right)\right] \\ =& C \times \sum_{k=0}^{N-1} \sum_{l=0}^{N-1} E_{R}^{*}(k, l) h(k, l) \exp \left[-i \frac{\pi}{\lambda d}\left(k^{2} \Delta x^{2}+l^{2} \Delta y^{2}\right)\right] \exp \left[i 2 \pi\left(\frac{k m}{N}+\frac{l n}{N}\right)\right] \end{aligned}
where, $h(k,l)$ is the hologram, $N$ is number of pixels in camera sensor (assumed number of rows = number of columns, if not, convert the hologram in such a way prior to the operations), $\lambda$ is wavelength, $\Delta x$ and $\Delta y$ are horizontal and vertical distance of neighboring sensor pixels, and $d$ is the distance of reconstruction. It is easy to see that the last term under discrete integral is actually an IFT (inverse Fourier transform) of a multiple of hologram function and an exponential factor term. C is just a complex constant which does not affect the reconstruction process and $E_{R}^{*}(k, l)$ gets simplified to unity for a plane wave as reconstruction/recording wave.

```python
# prepare the Fresnel operand for the hologram
Nr,Nc = np.shape(hologram) #number of rows and columns in the hologram
wavelength = 632.8e-9 #HeNe laser wavelength in SI units i.e. meters
dx = 6.8e-6 #sensor pixel size in meters
d = -1.054 #reconstruction distance in meters
Nr = np.linspace(0, Nr-1, Nr)-Nr/2
Nc = np.linspace(0, Nc-1, Nc)-Nc/2
k, l = np.meshgrid(Nc,Nr)
factor = np.multiply(hologram, np.exp(-1j*np.pi/(wavelength*d)*(np.multiply(k, k)*dx**2 + np.multiply(l, l)*dx**2)))
reconstructed_field = np.fft.ifftshift(np.fft.ifft2(np.fft.ifftshift(factor))) # Take inverse Fourier transform of the factor
```

```python
# save and plot
I = np.abs(reconstructed_field)/np.max(np.abs(reconstructed_field)) #normalized intensity profile
plt.imshow(I, cmap="hot", clim=(0.0, 0.2))
plt.colorbar()
mpimg.imsave('fresnel_reconstruction.png', I, cmap="hot", vmin=0.0, vmax=0.3) #save reconstruction matrix as image
```


# Modeling a Ball Channel Pendulum

Below is a video of a simple cardboard pendulum that has a metal ball in a semi-circular channel mounted above the pendulum's rotational joint. It is an interesting dynamic system that can be constructed and experimented with. This system seems to behave like a single degree of freedom system, i.e. that the ball's location is a kinematic function of the pendulum's angle. But this may not actually be the case. It depends on the nature of the motion of the ball with respect to the channel. If the ball rolls without slipping in the channel it is a single degree of freedom system. If the ball can slip and roll it is at a minimum a two degree of freedom system. In this notebook we will derive the equations of motion of the system considering the ball slips and doesn't roll in the channel.

```python
from IPython.display import YouTubeVideo
YouTubeVideo('3pJdkssUdfU', width=600, height=480)
```

# Imports and setup

```python
import sympy as sm
import numpy as np
```

```python
sm.init_printing()
```

```python
%matplotlib widget
```

# Free Body Diagram

Assumptions:

- Pendulum pivot is frictionless
- Pendulum is a simple pendulum
- Ball is a point mass that slides without friction in the pendulum channel

# Constants

Create a symbol for each of the system's contant parameters.

- $m_p$: mass of the pendulum
- $m_b$: mass of the ball
- $l$: length of the pendulum
- $r$: radius of the channel
- $g$: acceleration due to gravity

```python
mp, mb, r, l, g = sm.symbols('m_p, m_b, r, l, g', real=True, positive=True)
```

# Generalized Coordinates

Create functions of time for each generalized coordinate.

- $\theta(t)$: angle of the pendulum
- $\phi(t)$: angle of the line from the center of the channel semi-circle to the ball

```python
t = sm.symbols('t')
```

```python
theta = sm.Function('theta')(t)
phi = sm.Function('phi')(t)
```

```python
theta.diff()
```

```python
theta.diff(t)
```

Introduce two new variables for the generalized speeds:

$$
\alpha = \dot{\theta} \\
\beta = \dot{\phi}
$$

```python
alpha = sm.Function('alpha')(t)
beta = sm.Function('beta')(t)
```

# Kinetic Energy

Write the kinetic energy in terms of the generalized coordinates.

Pendulum:

```python
Tp = mp * (l * alpha)**2 / 2
Tp
```

Ball

```python
Tb = mb / 2 * ((-r * alpha * sm.cos(theta) + beta * r * sm.cos(theta + phi))**2 +
               (-r * alpha * sm.sin(theta) + beta * r * sm.sin(theta + phi))**2)
Tb
```

```python
T = Tp + Tb
T
```

# Potential Energy

Each particle (pendulum bob and the ball) has a potential energy associated with how high the mass rises.

```python
U = mp * g * (l - l * sm.cos(theta)) + mb * g * (r * sm.cos(theta) - r * sm.cos(theta + phi))
U
```

# Lagrange's equation of the second kind

There are two generalized coordinates with two degrees of freedom and thus two equations of motion.

$$
0 = f_\theta(\theta, \phi, \alpha, \beta, \dot{\alpha}, \dot{\beta}, t) \\
0 = f_\phi(\theta, \phi, \alpha, \beta, \dot{\alpha}, \dot{\beta}, t) \\
$$

```python
L = T - U
L
```

```python
gs_repl = {theta.diff(): alpha, phi.diff(): beta}
```

```python
f_theta = L.diff(alpha).diff(t).subs(gs_repl) - L.diff(theta)
f_theta = sm.trigsimp(f_theta)
f_theta
```

```python
f_phi = L.diff(beta).diff(t).subs(gs_repl) - L.diff(phi)
f_phi = sm.trigsimp(f_phi)
f_phi
```

```python
f = sm.Matrix([f_theta, f_phi])
f
```

The equations are motion are based on Newton's second law and Euler's equations, thus it is guaranteed that terms in $\mathbf{f}$ that include $\dot{\mathbf{u}}$ are linear with respect to $\dot{\mathbf{u}}$. So the equations of motion can be written in this matrix form:

$$
\mathbf{f}(\mathbf{c}, \mathbf{s}, \dot{\mathbf{s}}, t) = \mathbf{I}(\mathbf{c}, t)\dot{\mathbf{s}} + \mathbf{g}(\mathbf{c}, \mathbf{s}, t) = 0
$$

$\mathbf{I}$ is called the "mass matrix" of the nonlinear equations. If the derivatives of $\mathbf{f}$ with respect to $\dot{\mathbf{u}}$ are computed, i.e. the [Jacobian](https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant) of $\mathbf{f}$ with respect to $\dot{\mathbf{u}}$, then you can obtain the mass matrix.

```python
sbar = sm.Matrix([alpha, beta])
sbar
```

```python
Imat = f.jacobian(sbar.diff())
Imat
```

$$\mathbf{g} = \mathbf{f}|_{\dot{\mathbf{s}}=\mathbf{0}}$$

```python
gbar = f.subs({alpha.diff(t): 0, beta.diff(t): 0})
gbar
```

The explicit first order form has all of the $\dot{\mathbf{s}}$ on the left hand side. This requires solving the linear system of equations:

$$\mathbf{I}\dot{\mathbf{s}}=-\mathbf{g}$$

The mathematical solution is:

$$\dot{\mathbf{s}}=-\mathbf{I}^{-1}\mathbf{g}$$

```python
sdotbar = -Imat.inv() * gbar
sdotbar.simplify()
sdotbar
```

A better way to solve the system of linear equations is to use Guassian elmination. SymPy has a variety of methods for sovling linear systems. The LU decomposition method of Guassian elimination is a generally good choice for this and for large number of degrees of freedom this will provide reasonable computation time. For very large $n$ this should be done numerically instead of symbolically.

```python
sdotbar = -Imat.LUsolve(gbar)
sdotbar.simplify()
sdotbar
```

Note the differences in timing below. For systems with a large number of degrees of freedom, this gap in timing will increase significantly.

```python
%%timeit
-Imat.inv() * gbar
```

    874 ms ± 68.1 ms per loop (mean ± std. dev. of 7 runs, 1 loop each)

```python
%%timeit
-Imat.LUsolve(gbar)
```

    883 µs ± 148 µs per loop (mean ± std. dev. of 7 runs, 1000 loops each)

# Simulation of the nonlinear system

Resonance has a prepared system that is only missing the equations of motion.

```python
from resonance.nonlinear_systems import BallChannelPendulumSystem
```

```python
sys = BallChannelPendulumSystem()
```

```python
sys.constants
```

    {'mp': 0.012, 'mb': 0.0035, 'r': 0.1, 'l': 0.2, 'g': 9.81}

```python
sys.coordinates
```

    _CoordinatesDict([('theta', 0.17453292519943295), ('phi', -0.17453292519943295)])

```python
sys.speeds
```

    _CoordinatesDict([('alpha', 0.0), ('beta', 0.0)])

The full first order ordinary differential equations are:

$$
\dot{\theta} = \alpha \\
\dot{\phi} = \beta \\
\dot{\alpha} = f_{\alpha}(\theta, \phi, \alpha, \beta, t) \\
\dot{\beta} = f_{\beta}(\theta, \phi, \alpha, \beta, t)
$$

where:

$$
\dot{\mathbf{c}}=\begin{bmatrix}
\theta \\
\phi
\end{bmatrix} \\
\dot{\mathbf{s}}=-\mathbf{I}^{-1}\mathbf{g} =
\begin{bmatrix}
f_{\alpha}(\theta, \phi, \alpha, \beta, t) \\
f_{\beta}(\theta, \phi, \alpha, \beta, t)
\end{bmatrix}
$$

Introducing:

$$\mathbf{x} =
\begin{bmatrix}
\mathbf{c} \\
\mathbf{s}
\end{bmatrix}=
\begin{bmatrix}
\theta \\
\phi \\
\alpha \\
\beta
\end{bmatrix}
$$

we have equations for:

$$\dot{\mathbf{x}} = \begin{bmatrix}
\dot{\theta} \\
\dot{\phi} \\
\dot{\alpha} \\
\dot{\beta}
\end{bmatrix}
=
\begin{bmatrix}
\alpha \\
\beta \\
f_{\alpha}(\theta, \phi, \alpha, \beta, t) \\
f_{\beta}(\theta, \phi, \alpha, \beta, t)
\end{bmatrix}
$$

To find $\mathbf{x}$ we must integrate $\dot{\mathbf{x}}$ with respect to time:

$$
\mathbf{x} = \int_{t_0}^{t_f} \dot{\mathbf{x}} dt
$$

Resonance uses numerical integration behind the scenes to compute this integral. Numerical integration routines typicall require that you write a function that computes the right hand side of the first order form of the differential equations. This function takes the current state and time and computes the derivative of the states.

SymPy's `lambdify` function can convert symbolic expression into NumPy aware functions, i.e. Python functions that can accept NumPy arrays.

```python
eval_alphadot = sm.lambdify((phi, theta, alpha, beta, mp, mb, l, r, g), sdotbar[0])
```

```python
eval_alphadot(1, 2, 3, 4, 5, 6, 7, 8, 9)
```

```python
eval_betadot = sm.lambdify((phi, theta, alpha, beta, mp, mb, l, r, g), sdotbar[1])
```

```python
eval_betadot(1, 2, 3, 4, 5, 6, 7, 8, 9)
```

Now the right hand side (of the explicit ODEs) function can be written:

```python
def rhs(phi, theta, alpha, beta, mp, mb, l, r, g):
    theta_dot = alpha
    phi_dot = beta
    alpha_dot = eval_alphadot(phi, theta, alpha, beta, mp, mb, l, r, g)
    beta_dot = eval_betadot(phi, theta, alpha, beta, mp, mb, l, r, g)
    return theta_dot, phi_dot, alpha_dot, beta_dot
```

```python
rhs(1, 2, 3, 4, 5, 6, 7, 8, 9)
```

This function also works with numpy arrays:

```python
rhs(np.array([1, 2]), np.array([3, 4]), np.array([5, 6]), np.array([7, 8]), 9, 10, 11, 12, 13)
```

    (array([5, 6]),
     array([7, 8]),
     array([-27.2770872 , -36.73982421]),
     array([-13.91800374,  15.59186174]))

Add this function as the differential equation function of the system.

```python
sys.diff_eq_func = rhs
```

Now the `free_response` function can be called to simulation the nonlinear system.

```python
traj = sys.free_response(20, sample_rate=500)
```

```python
traj[['theta', 'phi']].plot(subplots=True);
```

    Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …

```python
sys.animate_configuration(fps=30, repeat=False)
```

    Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …

    <matplotlib.animation.FuncAnimation at 0x7f3885c36908>

# Equilibrium

This system four equilibrium points.

1. $\theta = \phi = \alpha = \beta = 0$
2. $\theta = \pi, \phi=\alpha=\beta=0$
3. $\theta = \pi, \phi = -\pi, \alpha=\beta=0$
4. $\theta = 0, \phi = \pi, \alpha=\beta=0$

If you set the velocities and accelerations equal to zero in the equations of motion you can then solve for the coordinates that make these equations equal to zero. This is the static force balance equations.

```python
static_repl = {alpha.diff(): 0, beta.diff(): 0, alpha: 0, beta: 0}
static_repl
```

```python
f_static = f.subs(static_repl)
f_static
```

```python
sm.solve(f_static, theta, phi)
```

Let's look at the simulation with the initial condition very close to an equilibrium point:

```python
sys.coordinates['theta'] = np.deg2rad(180.0000001)
sys.coordinates['phi'] = -np.deg2rad(180.00000001)
```

```python
traj = sys.free_response(20, sample_rate=500)
```

```python
sys.animate_configuration(fps=30, repeat=False)
```

    Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …

    <matplotlib.animation.FuncAnimation at 0x7f3885a377f0>

```python
traj[['theta', 'phi']].plot(subplots=True);
```

    Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …

This equlibrium point is an *unstable equlibrium*.

# Linearizing the system

The equations of motion can be linearized about one of the equilibrium points. This can be done by computing the linear terms of the multivariate Taylor Series expansion. This expansion can be expressed as:

$$
\mathbf{f}_{linear} = \mathbf{f}(\mathbf{v}_{eq}) + \mathbf{J}_{f,v}(\mathbf{v}_{eq}) (\mathbf{v} - \mathbf{v}_{eq})
$$

where $\mathbf{J}_f$ is the Jacobian of $\mathbf{f}$ with respect to $\mathbf{v}$ and:

$$
\mathbf{v} = \begin{bmatrix}
\theta\\
\phi \\
\alpha \\
\beta \\
\dot{\alpha} \\
\dot{\beta}
\end{bmatrix}
$$

In our case let's linearize about the static position where $\theta=\phi=0$.

```python
f
```

```python
v = sm.Matrix([theta, phi, alpha, beta, alpha.diff(), beta.diff()])
v
```

```python
veq = sm.zeros(len(v), 1)
veq
```

```python
v_eq_sub = dict(zip(v, veq))
v_eq_sub
```

The linear equations are then:

```python
f_lin = f.subs(v_eq_sub) + f.jacobian(v).subs(v_eq_sub) * (v - veq)
f_lin
```

Note that all of the terms that involve the coordinates, speeds, and their derivatives are linear terms, i.e. simple linear coefficients. These linear equations can be put into this canonical form:

$$\mathbf{M}\dot{\mathbf{s}} + \mathbf{C}\mathbf{s} + \mathbf{K} \mathbf{c} = \mathbf{F}$$

with:

- $\mathbf{M}$ as the mass matrix
- $\mathbf{C}$ as the damping matrix
- $\mathbf{K}$ as the stiffness matrix
- $\mathbf{F}$ as the forcing vector

The Jacobian can again be utlized to extract the linear coefficients.

```python
cbar = sm.Matrix([theta, phi])
cbar
```

```python
sbar
```

```python
M = f_lin.jacobian(sbar.diff())
M
```

```python
C = f_lin.jacobian(sbar)
C
```

```python
K = f_lin.jacobian(cbar)
K
```

```python
F = -f_lin.subs(v_eq_sub)
F
```

# Simulate the linear system

```python
from resonance.linear_systems import BallChannelPendulumSystem
```

```python
lin_sys = BallChannelPendulumSystem()
```

For linear systems, a function that calculates the canonical coefficient matrices should be created. Each of the canonical matrices should be created as 2 x 2 NumPy arrays.

```python
def canon_coeff_matrices(mp, mb, l, g, r):
    M = np.array([[mp * l**2 + mb * r**2, -mb * r**2],
                  [-mb * r**2, mb * r**2]])
    C = np.zeros((2, 2))
    K = np.array([[g * l * mp, g * mb * r],
                  [g * mb * r, g * mb * r]])
    return M, C, K
```

```python
lin_sys.canonical_coeffs_func = canon_coeff_matrices
```

```python
M_num, C_num, K_num = lin_sys.canonical_coefficients()
```

```python
M_num
```

    array([[ 5.15e-04, -3.50e-05],
           [-3.50e-05,  3.50e-05]])

```python
C_num
```

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

```python
K_num
```

    array([[0.023544 , 0.0034335],
           [0.0034335, 0.0034335]])

```python
lin_traj = lin_sys.free_response(20, sample_rate=500)
```

```python
lin_traj[['theta', 'phi']].plot(subplots=True);
```

    Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …

# Compare the nonlinear and linear simulations

```python
sys.coordinates['theta'] = np.deg2rad(10)
sys.coordinates['phi'] = np.deg2rad(-10)
```

```python
lin_sys.coordinates['theta'] = np.deg2rad(10)
lin_sys.coordinates['phi'] = np.deg2rad(-10)
```

```python
traj = sys.free_response(10.0)
```

```python
lin_traj = lin_sys.free_response(10.0)
```

```python
axes = traj[['theta', 'phi']].plot(subplots=True, color='red')
axes = lin_traj[['theta', 'phi']].plot(subplots=True, color='blue', ax=axes)
axes[0].legend([r'nonlin $\theta$', r'lin $\theta$'])
axes[1].legend([r'nonlin $\phi$', r'lin $\phi$']);
```

    Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …


# Gaussian Process Regression

Gaussian Process regression is a non-parametric approach to regression or data fitting that assumes that observed data points $y$ are generated by some unknown latent function $f(x)$.  The latent function $f(x)$ is modeled as being multivariate normally distributed (a Gaussian Process), and is commonly denoted

\begin{equation}
f(x) \sim \mathcal{GP}(m(x;\theta), \, k(x, x';\theta)) \,.
\end{equation}

$m(x ; \theta)$ is the *mean function*, and $k(x, x' ;\theta)$ is the covariance function.  In many applications, the mean function is set to $0$ because the data can still be fit well using just covariances.

$\theta$ is the set of *hyperparameters* for either the mean or covariance function.  These are the unknown variables.  They are usually found by maximizing the marginal likelihood.  This approach is much faster computationally than MCMC, but produces a point estimate, $\theta_{\mathrm{MAP}}$.

The data in the next two examples is generated by a GP with noise that is also gaussian distributed.  In sampling notation this is,

\begin{equation}
\begin{aligned}
y & = f(x) + \epsilon \\
f(x) & \sim \mathcal{GP}(0, \, k(x, x'; \theta)) \\
\epsilon & \sim \mathcal{N}(0, \sigma^2) \\
\sigma^2 & \sim \mathrm{Prior} \\
\theta & \sim \mathrm{Prior} \,.
\end{aligned}
\end{equation}

With Theano as a backend, PyMC3 is an excellent environment for developing fully Bayesian Gaussian Process models, particularly when a GP is component in a larger model.  The GP functionality of PyMC3 is meant to be lightweight, highly composable, and have a clear syntax.  This example is meant to give an introduction to how to specify a GP in PyMC3.

```python
%matplotlib inline
import matplotlib.pyplot as plt
import matplotlib.cm as cmap
cm = cmap.inferno

import numpy as np
import theano
import theano.tensor as tt
import theano.tensor.nlinalg
import sys
sys.path.insert(0, "../../..")
import pymc3 as pm
```

## Example 1: Non-Linear Regression

This is an example of a non-linear fit in a situation where there isn't much data.  Using optimization to find hyperparameters in this situation will greatly underestimate the amount of uncertainty if using the GP for prediction.  In PyMC3 it is easy to be fully Bayesian and use MCMC methods.

We generate 20 data points at random ```x``` values between 0 and 3.  The true values of the hyperparameters are hardcoded in this temporary ```model```.

```python
np.random.seed(200)
n = 20
X = np.sort(3*np.random.rand(n))[:,None]

with pm.Model() as model:
    # f(x)
    l_true = 0.3
    s2_f_true = 1.0
    cov = s2_f_true * pm.gp.cov.ExpQuad(1, l_true)

    # noise, epsilon
    s2_n_true = 0.05
    K_noise = s2_n_true * tt.eye(n)
    K = cov.K(X) + K_noise

# evaluate the covariance with the given hyperparameters
K = theano.function([], cov.K(X, X) + K_noise)()

# generate fake data from GP with white noise (with variance sigma2)
y = np.random.multivariate_normal(np.zeros(n), K)
```

```python
fig = plt.figure(figsize=(14,5)); ax = fig.add_subplot(111)
ax.plot(X, y, 'ok', ms=10);
ax.set_xlabel("x");
ax.set_ylabel("f(x)");
```

Since there isn't much data, there will likely be a lot of uncertainty in the hyperparameter values.

### The Model

- We assign prior distributions that are uniform in log space, suitable for variance-type parameters.  Each hyperparameter must at least be constrained to be positive valued by its prior.

- None of the covariance function objects have a scaling coefficient built in.  This is because random variables, such as ```s2_f```, can be multiplied directly with a covariance function object, ```gp.cov.ExpQuad```.

- The last line is the *marginal* likelihood.  Since the observed data $y$ is also assumed to be multivariate normally distributed, the marginal likelihood is also multivariate normal.  It is obtained by integrating out $f(x)$ from the product of the data likelihood $p(y \mid f, X)$ and the GP prior $p(f \mid X)$,

\begin{equation}
p(y \mid X) = \int p(y \mid f, X) p(f \mid X) df
\end{equation}

- The call in the last line ```f_cov.K(X)``` evaluates the covariance function across the inputs ```X```.  The result is a matrix.  The sum of this matrix and the diagonal noise term are used as the covariance matrix for the marginal likelihood.

```python
Z = np.linspace(0,3,100)[:,None]
with pm.Model() as model:
    # priors on the covariance function hyperparameters
    l = pm.Uniform('l', lower=1e-3, upper=10)

    # uninformative prior on the function variance
    log_s2_f = pm.Uniform('log_s2_f', lower=-10, upper=5)
    s2_f = pm.Deterministic('s2_f', tt.exp(log_s2_f))

    # uninformative prior on the noise variance
    log_s2_n = pm.Uniform('log_s2_n', lower=-10, upper=5)
    s2_n = pm.Deterministic('s2_n', tt.exp(log_s2_n))

    # covariance functions for the function f and the noise
    f_cov = s2_f * pm.gp.cov.ExpQuad(1, l)
    n_cov = s2_n * tt.eye(n)

    # the marginal likelihood
    y_obs = pm.MvNormal('y_obs', observed=y, mu=0.0,
                                 cov=f_cov.K(X) + n_cov)

    ## using deterministics for prediction
    K_inv = tt.nlinalg.matrix_inverse(K)
    K_s   = f_cov.K(X, Z)
    K_ss  = f_cov.K(Z, Z)

    f     = pm.Deterministic("f", tt.dot(tt.dot(K_s.T, K_inv), y))
    Sigma = pm.Deterministic("Sigma", K_ss - tt.dot(tt.dot(K_s.T, K_inv), K_s))
```

So far we've found that ```Slice``` and ```Metropolis``` work best for GP hyperparameters.

```python
with model:
    start = pm.find_MAP(fmin=sp.optimize.fmin_l_bfgs_b)
    trace = pm.sample(5000, step=pm.Slice(), start=start)
```

    100%|██████████| 5000/5000 [00:23<00:00, 216.93it/s]

```python
pm.traceplot(trace[1000:], varnames=['l', 's2_f', 's2_n'],
             lines={"l": l_true,
                    "s2_f": s2_f_true,
                    "s2_n": s2_n_true});
```

The results show that the hyperparameters were recovered pretty well, but definitely with a high degree of uncertainty. Lets look at the predicted fits and uncertainty next using samples from the full posterior.

```python
fig = plt.figure(figsize=(14,5)); ax = fig.add_subplot(111)

# plot random samples from the full predictive posterior
#   of the fit
idx = np.random.randint(1000,5000,1000)
for i, ix in enumerate(idx):
    S  = trace[ix]["Sigma"]
    mu = trace[ix]["f"]

    yz = np.random.multivariate_normal(mu, S)
    ax.plot(Z, yz, color=cm(0.3), alpha=0.05)

# overlay the observed data
ax.plot(X, y, 'ok', ms=10);
ax.set_xlabel("x");
ax.set_ylabel("f(x)");
ax.set_title("Posterior predictive distribution")
ax.set_ylim([-4,7]);
```

## Example 2: A periodic signal in non-white noise

This time let's pretend we have some more complex data that we would like to decompose.  For the sake of example, we simulate some data points from a function that
1. has a fainter periodic component
2. has a lower frequency drift away from periodicity
3. has additive white noise

As before, we generate the data using a throwaway PyMC3 ```model```.  We consider the sum of the drift term and the white noise to be "noise", while the periodic component is "signal".  In GP regression, the treatment of signal and noise covariance functions is identical, so the distinction between signal and noise is somewhat arbitrary.

```python
np.random.seed(200)
n = 150
X = np.sort(40*np.random.rand(n))[:,None]

# define gp, true parameter values
with pm.Model() as model:
    l_per_true = 2
    cov_per = pm.gp.cov.Cosine(1, l_per_true)

    l_drift_true = 4
    cov_drift = pm.gp.cov.Matern52(1, l_drift_true)

    s2_p_true = 0.3
    s2_d_true = 1.5
    s2_w_true = 0.1

    periodic_cov = s2_p_true * cov_per
    drift_cov    = s2_d_true * cov_drift

    signal_cov   = periodic_cov + drift_cov
    noise_cov    = s2_w_true * tt.eye(n)

K = theano.function([], signal_cov.K(X, X) + noise_cov)()
y = np.random.multivariate_normal(np.zeros(n), K)
```

In the plot of the observed data, the periodic component is barely distinguishable by eye.  It is plausible that there isn't a periodic component, and the observed data is just the drift component and white noise.

```python
fig = plt.figure(figsize=(12,5)); ax = fig.add_subplot(111)
ax.plot(X, y, '--', color=cm(0.4))
ax.plot(X, y, 'o', color="k", ms=10);
ax.set_xlabel("x");
ax.set_ylabel("f(x)");
```

Lets see if we can infer the correct values of the hyperparameters.

```python
with pm.Model() as model:
    # prior for periodic lengthscale, or frequency
    l_per = pm.Uniform('l_per', lower=1e-5, upper=10)

    # prior for the drift lengthscale hyperparameter
    l_drift  = pm.Uniform('l_drift', lower=1e-5, upper=10)

    # uninformative prior on the periodic amplitude
    log_s2_p = pm.Uniform('log_s2_p', lower=-10, upper=5)
    s2_p = pm.Deterministic('s2_p', tt.exp(log_s2_p))

    # uninformative prior on the drift amplitude
    log_s2_d = pm.Uniform('log_s2_d', lower=-10, upper=5)
    s2_d = pm.Deterministic('s2_d', tt.exp(log_s2_d))

    # uninformative prior on the white noise variance
    log_s2_w = pm.Uniform('log_s2_w', lower=-10, upper=5)
    s2_w = pm.Deterministic('s2_w', tt.exp(log_s2_w))

    # the periodic "signal" covariance
    signal_cov = s2_p * pm.gp.cov.Cosine(1, l_per)

    # the "noise" covariance
    drift_cov  = s2_d * pm.gp.cov.Matern52(1, l_drift)
    noise_cov  = drift_cov.K(X) + s2_w * tt.eye(n)

    # multivariate normal marginal likelihood
    y_obs = pm.MvNormal('y_obs', mu=0.0,
                        cov=signal_cov.K(X) + noise_cov,
                        observed=y)
```

```python
with model:
    start = pm.find_MAP(fmin=sp.optimize.fmin_l_bfgs_b)
    trace = pm.sample(5000, step=pm.Slice(), start=start)
```

    100%|██████████| 5000/5000 [06:45<00:00, 13.41it/s]

```python
pm.traceplot(trace[1000:], varnames=['l_per', 'l_drift', 's2_d', 's2_p', 's2_w'],
            lines={"l_per": l_per_true,
                   "l_drift": l_drift_true,
                   "s2_d":    s2_d_true,
                   "s2_p":    s2_p_true,
                   "s2_w":    s2_w_true});
```

Some large samples make the histogram of ```s2_p``` hard to read.  Below is a zoomed in histogram.

```python
fig = plt.figure(figsize=(12,6)); ax = fig.add_subplot(111)
ax.hist(trace['s2_p'], 100, range=(0,4), color=cm(0.3), ec='none');
ax.plot([0.3, 0.3], [0, 500], "k", lw=2);
ax.set_title("Histogram of s2_p");
ax.set_ylabel("Number of samples");
ax.set_xlabel("s2_p");
```

Comparing the histograms of the results to the true values, we can see that the PyMC3's MCMC methods did a good job estimating the true GP hyperparameters.  Although the periodic component is faintly apparent in the observed data, the GP model is able to extract it with high accuracy.


## Trajectory equations:

```python
%matplotlib inline
import matplotlib.pyplot as plt
```

```python
from sympy import *
init_printing()
```

```python
Bx, By, Bz, B = symbols("B_x, B_y, B_z, B")
x, y, z = symbols("x, y, z" )
x_0, y_0, z_0 = symbols("x_0, y_0, z_0")
vx, vy, vz, v = symbols("v_x, v_y, v_z, v")
vx_0, vy_0, vz_0 = symbols("v_x0, v_y0, v_z0")
t = symbols("t")
q, m = symbols("q, m")
c, eps0 = symbols("c, epsilon_0")
```

The equation of motion:
$$
\begin{gather*}
  m \frac{d^2 \vec{r} }{dt^2} = \frac{q}{c} [ \vec{v} \vec{B} ]
\end{gather*}
$$

For the case of a uniform magnetic field along the $z$-axis:
$$ \vec{B} = B_z = B, \quad B_x = 0, \quad B_y = 0 $$

In Cartesian coordinates:

```python
eq_x = Eq( Derivative(x(t), t, 2), q / c / m * Bz * Derivative(y(t),t) )
eq_y = Eq( Derivative(y(t), t, 2), - q / c / m * Bz * Derivative(x(t),t) )
eq_z = Eq( Derivative(z(t), t, 2), 0 )
display( eq_x, eq_y, eq_z )
```

Motion is uniform along the $z$-axis:

```python
z_eq = dsolve( eq_z, z(t) )
vz_eq = Eq( z_eq.lhs.diff(t), z_eq.rhs.diff(t) )
display( z_eq, vz_eq )
```

The constants of integration can be found from the initial conditions $z(0) = z_0$ and $v_z(0) = v_{z0}$:

```python
c1_c2_system = []
initial_cond_subs = [(t, 0), (z(0), z_0), (diff(z(t),t).subs(t,0), vz_0) ]
c1_c2_system.append( z_eq.subs( initial_cond_subs ) )
c1_c2_system.append( vz_eq.subs( initial_cond_subs ) )

c1, c2 = symbols("C1, C2")
c1_c2 = solve( c1_c2_system, [c1, c2] )
c1_c2
```

So that

```python
z_sol = z_eq.subs( c1_c2 )
vz_sol = vz_eq.subs( c1_c2 ).subs( [( diff(z(t),t), vz(t) ) ] )
display( z_sol, vz_sol )
```

For some reason I have not been able to solve the system of differential equations for $x$ and $y$ directly
with Sympy's `dsolve` function:

```python
#dsolve( [eq_x, eq_y], [x(t),y(t)] )
```

It is necessary to resort to the manual solution. The method is to differentiate one of them over
time and substitute the other. This will result in oscillator-type second-order equations for $v_y$ and $v_x$. Their solution is known. Integrating one more time, it is possible to obtain laws of motion $x(t)$ and $y(t)$.

```python
v_subs = [ (Derivative(x(t),t), vx(t)), (Derivative(y(t),t), vy(t)) ]
eq_vx = eq_x.subs( v_subs )
eq_vy = eq_y.subs( v_subs )
display( eq_vx, eq_vy )

eq_d2t_vx = Eq( diff(eq_vx.lhs,t), diff(eq_vx.rhs,t))
eq_d2t_vx = eq_d2t_vx.subs( [(eq_vy.lhs, eq_vy.rhs)] )
display( eq_d2t_vx )
```

The solution of the last equation is

```python
C1, C2, Omega = symbols( "C1, C2, Omega" )
vx_eq = Eq( vx(t), C1 * cos( Omega * t ) + C2 * sin( Omega * t ))
display( vx_eq )
omega_eq = Eq( Omega, Bz * q / c / m )
display( omega_eq )
```

where $\Omega$ is a cyclotron frequency.

```python
display( vx_eq )

vy_eq = Eq( vy(t), solve( Eq( diff(vx_eq.rhs,t), eq_vx.rhs ), ( vy(t) ) )[0] )
vy_eq = vy_eq.subs( [(Omega*c*m / Bz / q, omega_eq.rhs * c * m / Bz / q)]).simplify()
display( vy_eq )
```

For initial conditions $v_x(0) = v_{x0}, v_y(0) = v_{y0}$:

```python
initial_cond_subs = [(t,0), (vx(0), vx_0), (vy(0), vy_0) ]
vx0_eq = vx_eq.subs( initial_cond_subs )
vy0_eq = vy_eq.subs( initial_cond_subs )
display( vx0_eq, vy0_eq )

c1_c2 = solve( [vx0_eq, vy0_eq] )
c1_c2_subs = [ ("C1", c1_c2[c1]), ("C2", c1_c2[c2]) ]
vx_eq = vx_eq.subs( c1_c2_subs )
vy_eq = vy_eq.subs( c1_c2_subs )
display( vx_eq, vy_eq )
```

These equations can be integrated to obtain the laws of motion:

```python
x_eq = vx_eq.subs( vx(t), diff(x(t),t))
x_eq = dsolve( x_eq )
y_eq = vy_eq.subs( vy(t), diff(y(t),t))
y_eq = dsolve( y_eq ).subs( C1, C2 )
display( x_eq, y_eq )
```

$$x{\left (t \right )} = C_{1} + v_{x0} \left(\begin{cases} \frac{\sin{\left (\Omega t \right )}}{\Omega} & \text{for}\: \Omega \neq 0 \\t & \text{otherwise} \end{cases}\right) + v_{y0} \left(\begin{cases} - \frac{\cos{\left (\Omega t \right )}}{\Omega} & \text{for}\: \Omega \neq 0 \\0 & \text{otherwise} \end{cases}\right)$$

$$y{\left (t \right )} = C_{2} - v_{x0} \left(\begin{cases} - \frac{\cos{\left (\Omega t \right )}}{\Omega} & \text{for}\: \Omega \neq 0 \\0 & \text{otherwise} \end{cases}\right) + v_{y0} \left(\begin{cases} \frac{\sin{\left (\Omega t \right )}}{\Omega} & \text{for}\: \Omega \neq 0 \\t & \text{otherwise} \end{cases}\right)$$

For nonzero $\Omega$:

```python
x_eq = x_eq.subs( [(Omega, 123)] ).subs( [(123, Omega)] ).subs( [(Rational(1,123), 1/Omega)] )
y_eq = y_eq.subs( [(Omega, 123)] ).subs( [(123, Omega)] ).subs( [(Rational(1,123), 1/Omega)] )
display( x_eq, y_eq )
```

For initial conditions $x(0) = x_0, y(0) = y_0$:

```python
initial_cond_subs = [(t,0), (x(0), x_0), (y(0), y_0) ]
x0_eq = x_eq.subs( initial_cond_subs )
y0_eq = y_eq.subs( initial_cond_subs )
display( x0_eq, y0_eq )

c1_c2 = solve( [x0_eq, y0_eq] )
c1_c2_subs = [ ("C1", c1_c2[0][c1]), ("C2", c1_c2[0][c2]) ]
x_eq = x_eq.subs( c1_c2_subs )
y_eq = y_eq.subs( c1_c2_subs )
display( x_eq, y_eq )
```

```python
x_eq = x_eq.simplify()
y_eq = y_eq.simplify()
x_eq = x_eq.expand().collect(Omega)
y_eq = y_eq.expand().collect(Omega)
display( x_eq, y_eq )
```

Finally

```python
display( x_eq, y_eq, z_sol )
display( vx_eq, vy_eq, vz_sol )
display( omega_eq )
```

```python

```

examples/single_particle_in_magnetic_field/Single Particle in Uniform Magnetic Field.ipynb


```python
import sympy as sp
import Resources.script as rt

```

## Zadaca 1

```python
p = rt.hpoint3(8,2,5)
p*2
```

$\displaystyle \left[\begin{matrix}16\\4\\10\\2\end{matrix}\right]$

```python
rt.unit_vector(p,"homogeneous")
```

$\displaystyle \left[\begin{matrix}0.82956135578434\\0.207390338946085\\0.518475847365213\\0\end{matrix}\right]$

## Zadaca 4

```python
p = rt.hpoint3(3,5,7)
```

```python
rt.htranslation3(2, 3, 4)*p
```

$\displaystyle \left[\begin{matrix}5\\8\\11\\1\end{matrix}\right]$

## Zadaca 5

```python
b = sp.Matrix([[0, 1, 0, 2],
               [1, 0, 0, 4],
               [0, 0, -1, 6],
               [0, 0, 0, 1]])
```

$\displaystyle \left[\begin{matrix}0 & 1 & 0 & 2\\1 & 0 & 0 & 4\\0 & 0 & -1 & 6\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
rt.htranslation3(5, 2, 6)*b
```

$\displaystyle \left[\begin{matrix}0 & 1 & 0 & 7\\1 & 0 & 0 & 6\\0 & 0 & -1 & 12\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 6

```python
f = sp.Matrix([[0, 1, 0, 2],
               [1, 0, 0, 4],
               [0, 0, -1, 6],
               [0, 0, 0, 1]])
f
```

$\displaystyle \left[\begin{matrix}0 & 1 & 0 & 2\\1 & 0 & 0 & 4\\0 & 0 & -1 & 6\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
rt.htranslation3(5,2,6).inv() * f
```

$\displaystyle \left[\begin{matrix}0 & 1 & 0 & -3\\1 & 0 & 0 & 2\\0 & 0 & -1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 7

```python
f = sp.Matrix([[0, 1, 0, 0],
               [1, 0, 0, 4],
               [0, 0, -1, 2],
               [0, 0, 0, 1]])
f
```

$\displaystyle \left[\begin{matrix}0 & 1 & 0 & 0\\1 & 0 & 0 & 4\\0 & 0 & -1 & 2\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
rt.htranslation3(2,4,6).inv()*f
```

$\displaystyle \left[\begin{matrix}0 & 1 & 0 & -2\\1 & 0 & 0 & 0\\0 & 0 & -1 & -4\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 8

```python
x1, x2, x3 = sp.symbols('x1, x2, x3')
F = sp.Matrix([[0, x2, 0, -2],
               [1, 0, 0, 0],
               [x1, 0, x3, 4],
               [0, 0, 0, 1]])
F
```

$\displaystyle \left[\begin{matrix}0 & x_{2} & 0 & -2\\1 & 0 & 0 & 0\\x_{1} & 0 & x_{3} & 4\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
n, o, a = F[:3, 0], F[:3, 1], F[:3, 2]
```

```python
ndot, odot, adot = n.dot(n), o.dot(o), a.dot(a)
```

```python
sp.solveset(ndot - 1, x1)
```

$\displaystyle \left\{0\right\}$

```python
sp.solveset(odot - 1, x2)
```

$\displaystyle \left\{-1, 1\right\}$

```python
sp.solveset(adot - 1, x3)
```

$\displaystyle \left\{-1, 1\right\}$

```python
possible_solutions = []
possible_solutions.append(F.subs([(x1, 0), (x2, -1), (x3, -1)]))
possible_solutions.append(F.subs([(x1, 0), (x2, -1), (x3, 1)]))
possible_solutions.append(F.subs([(x1, 0), (x2, 1), (x3, -1)]))
possible_solutions.append(F.subs([(x1, 0), (x2, 1), (x3, 1)]))
possible_solutions[0]
```

$\displaystyle \left[\begin{matrix}0 & -1 & 0 & -2\\1 & 0 & 0 & 0\\0 & 0 & -1 & 4\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
for possible_solution in possible_solutions:
    if is_right_hand_coordinate_system3(possible_solution):
        sp.pprint(possible_solution)
        print()
```

    ⎡0  -1  0  -2⎤
    ⎢            ⎥
    ⎢1  0   0  0 ⎥
    ⎢            ⎥
    ⎢0  0   1  4 ⎥
    ⎢            ⎥
    ⎣0  0   0  1 ⎦

    ⎡0  1  0   -2⎤
    ⎢            ⎥
    ⎢1  0  0   0 ⎥
    ⎢            ⎥
    ⎢0  0  -1  4 ⎥
    ⎢            ⎥
    ⎣0  0  0   1 ⎦

## Zadaca 9

```python
x1, x2, x3 = sp.symbols('x1, x2, x3')
F = sp.Matrix([[0, x2, x3, 0],
               [x1, 0, -sp.sqrt(2)/2, -1],
               [sp.sqrt(2)/2, 0, sp.sqrt(2)/2, 6],
               [0, 0, 0, 1]])
F
```

$\displaystyle \left[\begin{matrix}0 & x_{2} & x_{3} & 0\\x_{1} & 0 & - \frac{\sqrt{2}}{2} & -1\\\frac{\sqrt{2}}{2} & 0 & \frac{\sqrt{2}}{2} & 6\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
n, o, a = F[:3, 0], F[:3, 1], F[:3, 2]
ndot, odot, adot = n.dot(n), o.dot(o), a.dot(a)
```

```python
sp.solveset(ndot - 1, x1)
```

$\displaystyle \left\{- \frac{\sqrt{2}}{2}, \frac{\sqrt{2}}{2}\right\}$

```python
sp.solveset(odot - 1, x2)
```

$\displaystyle \left\{-1, 1\right\}$

```python
sp.solveset(adot - 1, x3)
```

$\displaystyle \left\{0\right\}$

```python
possible_solutions = []
possible_solutions.append(F.subs([(x1, sp.sqrt(2)/2), (x2, 1), (x3, 0)]))
possible_solutions.append(F.subs([(x1, sp.sqrt(2)/2), (x2, -1), (x3, 0)]))
possible_solutions.append(F.subs([(x1, -sp.sqrt(2)/2), (x2, 1), (x3, 0)]))
possible_solutions.append(F.subs([(x1, -sp.sqrt(2)/2), (x2, -1), (x3, 0)]))
possible_solutions[0]
```

$\displaystyle \left[\begin{matrix}0 & 1 & 0 & 0\\\frac{\sqrt{2}}{2} & 0 & - \frac{\sqrt{2}}{2} & -1\\\frac{\sqrt{2}}{2} & 0 & \frac{\sqrt{2}}{2} & 6\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
for possible_solution in possible_solutions:
    if rt.is_right_hand_coordinate_system3(possible_solution):
        sp.pprint(possible_solution)
        print()
```

    ⎡0   -1   0    0 ⎤
    ⎢                ⎥
    ⎢√2      -√2     ⎥
    ⎢──  0   ────  -1⎥
    ⎢2        2      ⎥
    ⎢                ⎥
    ⎢√2       √2     ⎥
    ⎢──  0    ──   6 ⎥
    ⎢2        2      ⎥
    ⎢                ⎥
    ⎣0   0    0    1 ⎦

## Zadaca 10

```python
T1 = rt.hrotation3('x',sp.pi/2)
T2 = rt.hrotation3('z',sp.pi/2)
T1*T2
```

$\displaystyle \left[\begin{matrix}0 & -1 & 0 & 0\\0 & 0 & -1 & 0\\1 & 0 & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
rt.AnimationPlayground().play()
```

    VBox(children=(HBox(children=(ToggleButtons(options=('Ротирај', 'Придвижи се'), value='Ротирај'), BoundedFloat…

## Zadaca 14

```python
theta = sp.symbols('theta')
pox = rt.hrotation3('x', theta)
poy = rt.hrotation3('y', theta)
poz = rt.hrotation3('z', theta)
rt.is_right_hand_coordinate_system3(pox) and rt.is_right_hand_coordinate_system3(poz) and rt.is_right_hand_coordinate_system3(poy)
```

    True

## Zadaca 15

```python
theta = sp.symbols('theta')
pox = rt.hrotation3('x', theta)
poy = rt.hrotation3('y', theta)
poz = rt.hrotation3('z', theta)
sp.simplify(sp.det(pox) * sp.det(poy) * sp.det(poz))
```

$\displaystyle 1$

## Zadaca 17

```python
p = rt.hpoint3(2,3,4)
T = rt.hrotation3('x',sp.pi/4)
T*p
```

$\displaystyle \left[\begin{matrix}2\\- \frac{\sqrt{2}}{2}\\\frac{7 \sqrt{2}}{2}\\1\end{matrix}\right]$

## Zadaca 18

```python
p = rt.point3(2,3,4)
T = rt.rotation3('z',sp.pi/6)
T*p
```

$\displaystyle \left[\begin{matrix}- \frac{3}{2} + \sqrt{3}\\1 + \frac{3 \sqrt{3}}{2}\\4\end{matrix}\right]$

## Zadaca 19

```python
p = rt.point3(3, 5, 7)
rt.rotation3('x', 2*sp.pi/3) * p
```

$\displaystyle \left[\begin{matrix}3\\- \frac{7 \sqrt{3}}{2} - \frac{5}{2}\\- \frac{7}{2} + \frac{5 \sqrt{3}}{2}\end{matrix}\right]$

## Zadaca 20

```python
R = sp.Matrix([
    [1, -8, -4],
    [4, 4, 7],
    [8, -1, 4],
]) / 9
R
```

$\displaystyle \left[\begin{matrix}\frac{1}{9} & - \frac{8}{9} & - \frac{4}{9}\\\frac{4}{9} & \frac{4}{9} & \frac{7}{9}\\\frac{8}{9} & - \frac{1}{9} & \frac{4}{9}\end{matrix}\right]$

```python
theta = sp.acos((R.trace() - 1) / 2)
theta
```

$\displaystyle \frac{\pi}{2}$

```python
I =sp.Matrix([
    [R[2,1] - R[1,2]],
    [R[0,2] - R[2,0]],
    [R[1,0] - R[0,1]]
])/2*sp.sin(theta)
sp.pprint(I)
sp.pprint(-I)
```

    ⎡-4/9⎤
    ⎢    ⎥
    ⎢-2/3⎥
    ⎢    ⎥
    ⎣2/3 ⎦
    ⎡4/9 ⎤
    ⎢    ⎥
    ⎢2/3 ⎥
    ⎢    ⎥
    ⎣-2/3⎦

## Zadaca 21

```python
r = sp.Matrix([
    ['1/2', sp.sqrt(6)/4, -sp.sqrt(6)/4],
    [-sp.sqrt(6)/4,' 3/4', '1/4'],
    [sp.sqrt(6)/4, '1/4', '3/4'],
])
r
```

$\displaystyle \left[\begin{matrix}\frac{1}{2} & \frac{\sqrt{6}}{4} & - \frac{\sqrt{6}}{4}\\- \frac{\sqrt{6}}{4} & \frac{3}{4} & \frac{1}{4}\\\frac{\sqrt{6}}{4} & \frac{1}{4} & \frac{3}{4}\end{matrix}\right]$

```python
theta = sp.acos((r.trace() - 1) / 2)
theta
```

$\displaystyle \frac{\pi}{3}$

## Zadaca 22

```python
I =sp.Matrix([
    [r[2,1] - r[1,2]],
    [r[0,2] - r[2,0]],
    [r[1,0] - r[0,1]]
])/2*sp.sin(theta)
I
```

$\displaystyle \left[\begin{matrix}0\\- \frac{3 \sqrt{2}}{8}\\- \frac{3 \sqrt{2}}{8}\end{matrix}\right]$

## Zadaca 23

```python
playground = rt.AnimationPlayground()
playground.play(point=rt.hpoint3(3, 4, -5))
```

    VBox(children=(HBox(children=(ToggleButtons(options=('Ротирај', 'Придвижи се'), value='Ротирај'), BoundedFloat…

```python
t1 = rt.hrotation3('x',-sp.pi/6)
t2 = rt.htranslation3(y=3)
t3 = rt.hrotation3('a',sp.pi/4)
t4 = rt.htranslation3(6,9)
t = t2 * t1 * t3 * t4
t
```

$\displaystyle \left[\begin{matrix}\frac{\sqrt{2}}{2} & - \frac{\sqrt{2}}{2} & 0 & - \frac{3 \sqrt{2}}{2}\\\frac{\sqrt{6}}{4} & \frac{\sqrt{6}}{4} & \frac{1}{2} & 3 + \frac{15 \sqrt{6}}{4}\\- \frac{\sqrt{2}}{4} & - \frac{\sqrt{2}}{4} & \frac{\sqrt{3}}{2} & - \frac{15 \sqrt{2}}{4}\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
p = rt.hpoint3(3,4,-5)
(t*p).evalf()
```

$\displaystyle \left[\begin{matrix}-2.82842712474619\\13.9721935853075\\-12.1083016119742\\1.0\end{matrix}\right]$

## Zadaca 24

```python
t1 = rt.hrotation3('x',sp.pi/2)
t2 = rt.htranslation3(5,3,6)
t3 = rt.hrotation3('z',sp.pi/2)
t = t3*t2*t1
```

```python
p = rt.hpoint3(5,3,4)
(t*p).evalf()
```

$\displaystyle \left[\begin{matrix}1.0\\10.0\\9.0\\1.0\end{matrix}\right]$

## Zadaca 25

```python
t1 = rt.hrotation3('x',sp.pi/2)
t2 = rt.hrotation3('a',sp.pi/2)
t3 = rt.htranslation3(5,3,6)
t = t3 * t1 * t2
t
```

$\displaystyle \left[\begin{matrix}0 & -1 & 0 & 5\\0 & 0 & -1 & 3\\1 & 0 & 0 & 6\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
p = rt.hpoint3(2,3,5)
t*p
```

$\displaystyle \left[\begin{matrix}2\\-2\\8\\1\end{matrix}\right]$

## Zadaca 26

```python
t1 = rt.hrotation3('n',sp.pi/2)
t2 = rt.hrotation3('a',sp.pi/2)
t3 = rt.hrotation3('z',sp.pi/2)
t4 = rt.hrotation3('o',sp.pi/2)
t5 = rt.hrotation3('x',sp.pi/2)
t = t5 * t3 * t1 * t2 * t4
t
```

$\displaystyle \left[\begin{matrix}-1 & 0 & 0 & 0\\0 & 0 & -1 & 0\\0 & -1 & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
p = rt.hpoint3(2,1,0)
t*p
```

$\displaystyle \left[\begin{matrix}-2\\0\\-1\\1\end{matrix}\right]$

## Zadaca 27

```python
t1 = rt.hrotation3('x',-sp.pi/6)
t2 = rt.htranslation3(y=3)
t3 = rt.hrotation3('a',sp.pi/4)
t4 = rt.htranslation3(6,9)
t = t2 * t1 * t3 * t4
t
```

$\displaystyle \left[\begin{matrix}\frac{\sqrt{2}}{2} & - \frac{\sqrt{2}}{2} & 0 & - \frac{3 \sqrt{2}}{2}\\\frac{\sqrt{6}}{4} & \frac{\sqrt{6}}{4} & \frac{1}{2} & 3 + \frac{15 \sqrt{6}}{4}\\- \frac{\sqrt{2}}{4} & - \frac{\sqrt{2}}{4} & \frac{\sqrt{3}}{2} & - \frac{15 \sqrt{2}}{4}\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
p = rt.hpoint3(3,4,-5)
(t*p).evalf()
```

$\displaystyle \left[\begin{matrix}-2.82842712474619\\13.9721935853075\\-12.1083016119742\\1.0\end{matrix}\right]$

## Zadaca 31

```python
F13 = sp.Matrix([
    [0, 0, -1],
    [0, 1, 0],
    [1, 0, 0],
])
F12 = sp.Matrix([
    [1, 0, 0],
    [0, '1/2', -sp.sqrt(3)/2],
    [0, sp.sqrt(3)/2, '1/2'],
])

```

$\displaystyle \left[\begin{matrix}1 & 0 & 0\\0 & \frac{1}{2} & - \frac{\sqrt{3}}{2}\\0 & \frac{\sqrt{3}}{2} & \frac{1}{2}\end{matrix}\right]$

```python
F23 = F12.inv()*F13
F23
```

$\displaystyle \left[\begin{matrix}0 & 0 & -1\\\frac{\sqrt{3}}{2} & \frac{1}{2} & 0\\\frac{1}{2} & - \frac{\sqrt{3}}{2} & 0\end{matrix}\right]$

## Zadaca 32

```python
theta, phi = sp.symbols('theta, phi')
T1 = rt.hrotation3('y', theta)
T2 = rt.hrotation3('z', phi)
T1*T2
```

$\displaystyle \left[\begin{matrix}\cos{\left(\phi \right)} \cos{\left(\theta \right)} & - \sin{\left(\phi \right)} \cos{\left(\theta \right)} & \sin{\left(\theta \right)} & 0\\\sin{\left(\phi \right)} & \cos{\left(\phi \right)} & 0 & 0\\- \sin{\left(\theta \right)} \cos{\left(\phi \right)} & \sin{\left(\phi \right)} \sin{\left(\theta \right)} & \cos{\left(\theta \right)} & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 33

```python
gamma, beta, r = sp.symbols('gamma, beta, r')
sphere = rt.hrotation3('a', gamma) * rt.hrotation3('o', beta) * rt.htranslation3(z=r)
sphere
```

$\displaystyle \left[\begin{matrix}\cos{\left(\beta \right)} \cos{\left(\gamma \right)} & - \sin{\left(\gamma \right)} & \sin{\left(\beta \right)} \cos{\left(\gamma \right)} & r \sin{\left(\beta \right)} \cos{\left(\gamma \right)}\\\sin{\left(\gamma \right)} \cos{\left(\beta \right)} & \cos{\left(\gamma \right)} & \sin{\left(\beta \right)} \sin{\left(\gamma \right)} & r \sin{\left(\beta \right)} \sin{\left(\gamma \right)}\\- \sin{\left(\beta \right)} & 0 & \cos{\left(\beta \right)} & r \cos{\left(\beta \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
unrotated = sphere * rt.hrotation3('o', -beta) * rt.hrotation3('a', -gamma)
sp.simplify(unrotated)
```

$\displaystyle \left[\begin{matrix}1 & 0 & 0 & r \sin{\left(\beta \right)} \cos{\left(\gamma \right)}\\0 & 1 & 0 & r \sin{\left(\beta \right)} \sin{\left(\gamma \right)}\\0 & 0 & 1 & r \cos{\left(\beta \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 34

```python
x,y,z = 3.1375, 2.195, 3.214
```

```python
gama = sp.atan(x/y)
gama
```

$\displaystyle 0.960337799284207$

```python
r = sp.sqrt(x**2 + y**2 + z**2)
r
```

$\displaystyle 4.99917265655028$

```python
beta = sp.sqrt((x**2 + y**2) / z**2)
beta
```

$\displaystyle 1.19137838625981$

## Zadaca 35

```python
x,y,z = 1.5, 1.5, sp.sqrt(6)/2
```

```python
r, beta, gamma = sp.sqrt(x**2 + y**2 + z**2),sp.atan(sp.sqrt((x**2 + y**2) / z**2)), sp.atan(x/y)
```

```python
S = rt.hrotation3('a', gamma) * rt.hrotation3('o', beta) * rt.htranslation3(z=r)
S.evalf()
```

$\displaystyle \left[\begin{matrix}0.353553390593274 & -0.707106781186547 & 0.612372435695795 & 1.5\\0.353553390593274 & 0.707106781186548 & 0.612372435695794 & 1.5\\-0.866025403784439 & 0 & 0.5 & 1.22474487139159\\0 & 0 & 0 & 1.0\end{matrix}\right]$

## Zadaca 36

```python
theta_1, theta_2, theta_3 = sp.symbols('theta_1, theta_2, theta_3')
T1 = rt.hrotation3('a', theta_1)
T2 = rt.hrotation3('o', theta_2)
T3 = rt.hrotation3('a', theta_3)
T1 * T2 * T3
```

$\displaystyle \left[\begin{matrix}- \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{3} \right)} + \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} \right)} \cos{\left(\theta_{3} \right)} & - \sin{\left(\theta_{1} \right)} \cos{\left(\theta_{3} \right)} - \sin{\left(\theta_{3} \right)} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} \right)} & \sin{\left(\theta_{2} \right)} \cos{\left(\theta_{1} \right)} & 0\\\sin{\left(\theta_{1} \right)} \cos{\left(\theta_{2} \right)} \cos{\left(\theta_{3} \right)} + \sin{\left(\theta_{3} \right)} \cos{\left(\theta_{1} \right)} & - \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{3} \right)} \cos{\left(\theta_{2} \right)} + \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{3} \right)} & \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} \right)} & 0\\- \sin{\left(\theta_{2} \right)} \cos{\left(\theta_{3} \right)} & \sin{\left(\theta_{2} \right)} \sin{\left(\theta_{3} \right)} & \cos{\left(\theta_{2} \right)} & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 37

```python
T1 = rt.hrotation3('z', -sp.pi/2)
T2 = rt.hrotation3('y', sp.pi)
T3 = rt.htranslation3(x=3)
T = T3 * T2 * T1
T
```

$\displaystyle \left[\begin{matrix}0 & -1 & 0 & 3\\-1 & 0 & 0 & 0\\0 & 0 & -1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
euler_angles = rt.euler_angles(T, 'zyz')
euler_angles
```

$\displaystyle \left[\begin{matrix}\frac{\pi}{2}\\\pi\\0\end{matrix}\right]$

```python
rt.rotation_matrix_from_euler_angles(euler_angles,'zyz')
```

$\displaystyle \left[\begin{matrix}0 & -1 & 0 & 0\\-1 & 0 & 0 & 0\\0 & 0 & -1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 38

```python
t1 = rt.htranslation3(x=5)
t2 = rt.hrotation3('o', sp.pi/4)
t3 = rt.hrotation3('z', sp.pi/2)
t4 = rt.htranslation3(z=sp.sqrt(2))
t5 = rt.hrotation3('x', sp.pi/2)
T = t5 * t3 * t1 * t2 * t4
T
```

$\displaystyle \left[\begin{matrix}0 & -1 & 0 & 0\\\frac{\sqrt{2}}{2} & 0 & - \frac{\sqrt{2}}{2} & -1\\\frac{\sqrt{2}}{2} & 0 & \frac{\sqrt{2}}{2} & 6\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
euler_angles = rt.euler_angles(T, 'zyz')
euler_angles
```

$\displaystyle \left[\begin{matrix}- \frac{\pi}{2}\\\frac{\pi}{4}\\\pi\end{matrix}\right]$

## Zadaca 39-41

```python
r11, r12, r13, r21, r22, r23, r31, r32, r33 = sp.symbols('r11, r12, r13, r21, r22, r23, r31, r32, r33')
T = sp.Matrix([[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]])
T
```

$\displaystyle \left[\begin{matrix}r_{11} & r_{12} & r_{13}\\r_{21} & r_{22} & r_{23}\\r_{31} & r_{32} & r_{33}\end{matrix}\right]$

```python
sp.pprint(rt.euler_angles(T,'xyz'))
print()
sp.pprint(rt.euler_angles(T,'xzy'))
print('\n RPY se ojlerovi ZYX \n')
sp.pprint(rt.euler_angles(T,'zyx'))
```

    ⎡atan2(-r₂₃, r₃₃)⎤
    ⎢                ⎥
    ⎢   asin(r₁₃)    ⎥
    ⎢                ⎥
    ⎣atan2(-r₁₂, r₁₁)⎦

    ⎡atan2(r₃₂, r₂₂)⎤
    ⎢               ⎥
    ⎢  -asin(r₁₂)   ⎥
    ⎢               ⎥
    ⎣atan2(r₁₃, r₁₁)⎦

     RPY se ojlerovi ZYX

    ⎡atan2(r₂₁, r₁₁)⎤
    ⎢               ⎥
    ⎢  -asin(r₃₁)   ⎥
    ⎢               ⎥
    ⎣atan2(r₃₂, r₃₃)⎦

## Zadaca 42

```python
t1 = rt.htranslation3(x=5)
t2 = rt.hrotation3('o', sp.rad(60))
t3 = rt.hrotation3('z', sp.rad(30))
t4 = rt.htranslation3(z=3)
t5 = rt.hrotation3('x', sp.rad(45))
T = t5 * t3 * t1 * t2 * t4
T
```

$\displaystyle \left[\begin{matrix}\frac{\sqrt{3}}{4} & - \frac{1}{2} & \frac{3}{4} & \frac{9}{4} + \frac{5 \sqrt{3}}{2}\\\frac{\sqrt{2}}{8} + \frac{\sqrt{6}}{4} & \frac{\sqrt{6}}{4} & - \frac{\sqrt{2}}{4} + \frac{\sqrt{6}}{8} & \frac{\sqrt{2}}{2} + \frac{3 \sqrt{6}}{8}\\- \frac{\sqrt{6}}{4} + \frac{\sqrt{2}}{8} & \frac{\sqrt{6}}{4} & \frac{\sqrt{6}}{8} + \frac{\sqrt{2}}{4} & \frac{3 \sqrt{6}}{8} + 2 \sqrt{2}\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
sp.deg(rt.euler_angles(T,'zyx')).evalf()
```

$\displaystyle \left[\begin{matrix}61.246056618025\\25.8232078223648\\42.867573148363\end{matrix}\right]$

## Zadaca 43

```python
T = sp.Matrix([
    [0.527, -0.574, 0.628, 4],
    [0.369, 0.189, 0.439, 6],
    [-0.766, 0, 0.643, 9],
    [0, 0, 0, 1],
])
T
```

$\displaystyle \left[\begin{matrix}0.527 & -0.574 & 0.628 & 4\\0.369 & 0.189 & 0.439 & 6\\-0.766 & 0 & 0.643 & 9\\0 & 0 & 0 & 1\end{matrix}\right]$

```python
sp.deg(rt.euler_angles(T,'zyx')).evalf()
```

$\displaystyle \left[\begin{matrix}34.9993162356405\\49.996038663149\\0\end{matrix}\right]$

```python
rt.rotation_matrix_from_euler_angles(rt.euler_angles(T,'zyx'),'zyx')
```

$\displaystyle \left[\begin{matrix}0.52658856828585 & -0.573566660599361 & 0.627475709170925 & 0\\0.368711919729561 & 0.819158889257082 & 0.439352062019111 & 0\\-0.766 & 0 & 0.642840571214979 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zacada 47

```python
d1, d2 = sp.symbols('d1, d2')
robot = rt.SerialLinkRobot()
robot.add_prismatic_joint(0,d1,0,-sp.pi/2)
robot.add_prismatic_joint(0,d2,0,0)
robot.interact()
```

    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2'), value='Зглоб 1'), FloatSlider(value=1.0, descript…

```python
a1 = robot.get_dh_joint_to_joint(0,1)
a2 = robot.get_dh_joint_to_joint(1,2)
a1 * a2
```

$\displaystyle \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 0 & 1 & d_{2}\\0 & -1 & 0 & d_{1}\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 48

```python
d1, d2, d3 = sp.symbols('d1, d2, d3')
robot = rt.SerialLinkRobot()
robot.add_prismatic_joint(0,d1,0,-sp.pi/2)
robot.add_prismatic_joint(sp.pi/2,d2,0,-sp.pi/2)
robot.add_prismatic_joint(0,d3,0,0)
robot.interact()
```

    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2', 'Зглоб 3'), value='Зглоб 1'), FloatSlider(value=1.…

```python
a1 = robot.get_dh_joint_to_joint(0,1)
a2 = robot.get_dh_joint_to_joint(1,2)
a3 = robot.get_dh_joint_to_joint(2,3)
a1*a2*a3
```

$\displaystyle \left[\begin{matrix}0 & 0 & -1 & - d_{3}\\0 & -1 & 0 & d_{2}\\-1 & 0 & 0 & d_{1}\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 49

```python
theta1, theta2, l1, l2 = sp.symbols('theta1, theta2, l1, l2')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1,0,l1,0)
robot.add_revolute_joint(theta2,0,l2,-sp.pi/2)
robot.add_subs([(l1,1),(l2,1)])
robot.interact()
```

    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2'), value='Зглоб 1'), FloatSlider(value=0.0, descript…

```python
a1 = robot.get_dh_joint_to_joint(0,1)
a2 = robot.get_dh_joint_to_joint(1,2)
sp.simplify(a1*a2)
```

$\displaystyle \left[\begin{matrix}\cos{\left(\theta_{1} + \theta_{2} \right)} & 0 & - \sin{\left(\theta_{1} + \theta_{2} \right)} & l_{1} \cos{\left(\theta_{1} \right)} + l_{2} \cos{\left(\theta_{1} + \theta_{2} \right)}\\\sin{\left(\theta_{1} + \theta_{2} \right)} & 0 & \cos{\left(\theta_{1} + \theta_{2} \right)} & l_{1} \sin{\left(\theta_{1} \right)} + l_{2} \sin{\left(\theta_{1} + \theta_{2} \right)}\\0 & -1 & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 51

```python
theta1, theta2, theta3, l1, l2, l3 = sp.symbols('theta1, theta2, theta3, l1, l2, l3')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1,l1,0,-sp.pi/2)
robot.add_revolute_joint(theta2,0,l2,0)
robot.add_revolute_joint(theta3,0,l3,0)
robot.add_subs([(l1,1),(l2,1),(l3,1)])
robot.interact()
```

    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2', 'Зглоб 3'), value='Зглоб 1'), FloatSlider(value=0.…

```python
a1 = robot.get_dh_joint_to_joint(0,1)
a2 = robot.get_dh_joint_to_joint(1,2)
a3 = robot.get_dh_joint_to_joint(2,3)
sp.simplify(a1*a2*a3)
```

$\displaystyle \left[\begin{matrix}\cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)} & - \sin{\left(\theta_{2} + \theta_{3} \right)} \cos{\left(\theta_{1} \right)} & - \sin{\left(\theta_{1} \right)} & \left(l_{2} \cos{\left(\theta_{2} \right)} + l_{3} \cos{\left(\theta_{2} + \theta_{3} \right)}\right) \cos{\left(\theta_{1} \right)}\\\sin{\left(\theta_{1} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)} & - \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} + \theta_{3} \right)} & \cos{\left(\theta_{1} \right)} & \left(l_{2} \cos{\left(\theta_{2} \right)} + l_{3} \cos{\left(\theta_{2} + \theta_{3} \right)}\right) \sin{\left(\theta_{1} \right)}\\- \sin{\left(\theta_{2} + \theta_{3} \right)} & - \cos{\left(\theta_{2} + \theta_{3} \right)} & 0 & l_{1} - l_{2} \sin{\left(\theta_{2} \right)} - l_{3} \sin{\left(\theta_{2} + \theta_{3} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 52

```python
theta1, d2, theta3, d1, l3 = sp.symbols('theta1, d2, theta3, d1, l3')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1,0,0,-sp.pi/2)
robot.add_prismatic_joint(0,d1,0,sp.pi/2)
robot.add_revolute_joint(theta2,0,1,0)
robot.interact()
```

    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2', 'Зглоб 3'), value='Зглоб 1'), FloatSlider(value=0.…

```python
a1 = robot.get_dh_joint_to_joint(0,1)
a2 = robot.get_dh_joint_to_joint(1,2)
a3 = robot.get_dh_joint_to_joint(2,3)
sp.simplify(a1*a2*a3)
```

$\displaystyle \left[\begin{matrix}\cos{\left(\theta_{1} + \theta_{2} \right)} & - \sin{\left(\theta_{1} + \theta_{2} \right)} & 0 & - d_{1} \sin{\left(\theta_{1} \right)} + \cos{\left(\theta_{1} + \theta_{2} \right)}\\\sin{\left(\theta_{1} + \theta_{2} \right)} & \cos{\left(\theta_{1} + \theta_{2} \right)} & 0 & d_{1} \cos{\left(\theta_{1} \right)} + \sin{\left(\theta_{1} + \theta_{2} \right)}\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 53

```python
theta1, d1, d2 = sp.symbols('theta1, d1, d2')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1,0,0,0)
robot.add_prismatic_joint(0,d1,0,-sp.pi/2)
robot.add_prismatic_joint(0,d2,0,0)
robot.interact()
```

    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2', 'Зглоб 3'), value='Зглоб 1'), FloatSlider(value=0.…

```python
a1 = robot.get_dh_joint_to_joint(0,1)
a2 = robot.get_dh_joint_to_joint(1,2)
a3 = robot.get_dh_joint_to_joint(2,3)
sp.simplify(a1*a2*a3)
```

$\displaystyle \left[\begin{matrix}\cos{\left(\theta_{1} \right)} & 0 & - \sin{\left(\theta_{1} \right)} & - d_{2} \sin{\left(\theta_{1} \right)}\\\sin{\left(\theta_{1} \right)} & 0 & \cos{\left(\theta_{1} \right)} & d_{2} \cos{\left(\theta_{1} \right)}\\0 & -1 & 0 & d_{1}\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 54

```python
theta4, theta5, theta6 = sp.symbols('theta4, theta5, theta6')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta4,0,0,sp.pi/2)
robot.add_revolute_joint(theta5,0,0,-sp.pi/2)
robot.add_revolute_joint(theta6,1,0,0)
robot.interact()
```

    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2', 'Зглоб 3'), value='Зглоб 1'), FloatSlider(value=0.…

## Zadaca 57

```python
theta1, theta2, d1, d3 = sp.symbols('theta1, theta2, d1, d3')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1,d1,0,sp.pi/2)
robot.add_revolute_joint(theta2,0,0,-sp.pi/2)
robot.add_prismatic_joint(0,d3,0,0)
robot.add_subs([(d1,1)])
robot.interact()
```

    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2', 'Зглоб 3'), value='Зглоб 1'), FloatSlider(value=0.…

## Zadaca 58

```python
theta1, theta2, d1, l2, l3, d4 = sp.symbols('theta1, theta2, d1, l2, l3, d4')
robot = rt.SerialLinkRobot()
robot.add_prismatic_joint(0,d1,0,0)
robot.add_revolute_joint(theta1,0,l2,0)
robot.add_revolute_joint(theta2,0,l3,sp.pi)
robot.add_prismatic_joint(0,d4,0,0)
robot.add_subs([(l2,1),(l3,1)])
robot.interact()
```

    VBox(children=(ToggleButtons(options=('Зглоб 1', 'Зглоб 2', 'Зглоб 3', 'Зглоб 4'), value='Зглоб 1'), FloatSlid…

```python
a1 = robot.get_dh_joint_to_joint(0,1)
a2 = robot.get_dh_joint_to_joint(1,2)
a3 = robot.get_dh_joint_to_joint(2,3)
a4 = robot.get_dh_joint_to_joint(3,4)
sp.simplify(a1*a2*a3*a4)
```

$\displaystyle \left[\begin{matrix}\cos{\left(\theta_{1} + \theta_{2} \right)} & \sin{\left(\theta_{1} + \theta_{2} \right)} & 0 & l_{2} \cos{\left(\theta_{1} \right)} + l_{3} \cos{\left(\theta_{1} + \theta_{2} \right)}\\\sin{\left(\theta_{1} + \theta_{2} \right)} & - \cos{\left(\theta_{1} + \theta_{2} \right)} & 0 & l_{2} \sin{\left(\theta_{1} \right)} + l_{3} \sin{\left(\theta_{1} + \theta_{2} \right)}\\0 & 0 & -1 & d_{1} - d_{4}\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 60

```python
theta1, theta2, l1, l2 = sp.symbols('theta1, theta2, l1, l2')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1, 0, l1, 0)
robot.add_revolute_joint(theta2, 0, l2, 0)
robot.add_subs([(l1, 1), (l2, 1)])
T = robot.get_dh_matrix()
T
```

$\displaystyle \left[\begin{matrix}\cos{\left(\theta_{1} + \theta_{2} \right)} & - \sin{\left(\theta_{1} + \theta_{2} \right)} & 0 & l_{1} \cos{\left(\theta_{1} \right)} + l_{2} \cos{\left(\theta_{1} + \theta_{2} \right)}\\\sin{\left(\theta_{1} + \theta_{2} \right)} & \cos{\left(\theta_{1} + \theta_{2} \right)} & 0 & l_{1} \sin{\left(\theta_{1} \right)} + l_{2} \sin{\left(\theta_{1} + \theta_{2} \right)}\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

## Zadaca 61

```python
theta1, theta2, d3, d1 ,x,y,z= sp.symbols('theta1, theta2, d3, d1,x,y,z')
robot = rt.SerialLinkRobot()
robot.add_revolute_joint(theta1, d1, 0, -sp.pi/2)
robot.add_revolute_joint(theta2, 0, 0, sp.pi/2)
robot.add_prismatic_joint(0, d3, 0, 0)
robot.add_subs([(d1, 1)])
T = robot.get_dh_matrix()
T
```

$\displaystyle \left[\begin{matrix}\cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} \right)} & - \sin{\left(\theta_{1} \right)} & \sin{\left(\theta_{2} \right)} \cos{\left(\theta_{1} \right)} & d_{3} \sin{\left(\theta_{2} \right)} \cos{\left(\theta_{1} \right)}\\\sin{\left(\theta_{1} \right)} \cos{\left(\theta_{2} \right)} & \cos{\left(\theta_{1} \right)} & \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} \right)} & d_{3} \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} \right)}\\- \sin{\left(\theta_{2} \right)} & 0 & \cos{\left(\theta_{2} \right)} & d_{1} + d_{3} \cos{\left(\theta_{2} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

```python

```

```python
sp.solve([T[0,-1]-x,T[1,-1]-y,T[2,-1]-z],(theta1, theta2, d3, d1))
```

    [(-2*atan((x - sqrt(x**2 + y**2))/y),
      theta2,
      (-x**2 + x*sqrt(x**2 + y**2) - y**2)/((x - sqrt(x**2 + y**2))*sin(theta2)),
      y*cos(theta2)/(sin(theta2)*sin(2*atan((x - sqrt(x**2 + y**2))/y))) + z),
     (-2*atan((x + sqrt(x**2 + y**2))/y),
      theta2,
      -(x**2 + x*sqrt(x**2 + y**2) + y**2)/((x + sqrt(x**2 + y**2))*sin(theta2)),
      y*cos(theta2)/(sin(theta2)*sin(2*atan((x + sqrt(x**2 + y**2))/y))) + z)]

```python
x, y, z = sp.symbols('x, y, z')
eq1 = sp.Eq(x + y + z, 1)             # x + y + z  = 1
eq2 = sp.Eq(x + y + 2 * z, 3)         # x + y + 2z = 3
sp.solve((eq1, eq2), (x, y, z))
```

    {x: -y - 1, z: 2}

```python

```

$\displaystyle d_{3} \sin{\left(\theta_{2} \right)} \cos{\left(\theta_{1} \right)} - x$

```python

```


```python
import sympy as sp
```

# Orignal Matrix A

```python
A = sp.Matrix([[1,6], [4,3]])
A
```

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

```python
A.eigenvals()
```

    {-3: 1, 7: 1}

```python
A.eigenvects()
```

    [(-3, 1, [Matrix([
       [-3/2],
       [   1]])]), (7, 1, [Matrix([
       [1],
       [1]])])]

# Matrices P, D, and P-inverse

```python
P = sp.Matrix([[1,-3], [1,2]])
P
```

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

```python

```

```python
Pinv = P.inv()
Pinv
```

$\displaystyle \left[\begin{matrix}\frac{2}{5} & \frac{3}{5}\\- \frac{1}{5} & \frac{1}{5}\end{matrix}\right]$

```python
D = sp.Matrix([[7,0], [0,-3]])
D
```

$\displaystyle \left[\begin{matrix}7 & 0\\0 & -3\end{matrix}\right]$

```python
# Need to define k as a symbolic variable
k = sp.symbols('k')
D_k = D**k
D_k
```

$\displaystyle \left[\begin{matrix}7^{k} & 0\\0 & \left(-3\right)^{k}\end{matrix}\right]$

```python
# Formula for A to the power of k
A_k = P*D_k*Pinv
A_k
```

$\displaystyle \left[\begin{matrix}\frac{3 \left(-3\right)^{k}}{5} + \frac{2 \cdot 7^{k}}{5} & - \frac{3 \left(-3\right)^{k}}{5} + \frac{3 \cdot 7^{k}}{5}\\- \frac{2 \left(-3\right)^{k}}{5} + \frac{2 \cdot 7^{k}}{5} & \frac{2 \left(-3\right)^{k}}{5} + \frac{3 \cdot 7^{k}}{5}\end{matrix}\right]$

```python
# Diagonalize A directly:
P*(D**k)*(P.inv())
```

$\displaystyle \left[\begin{matrix}\frac{3 \left(-3\right)^{k}}{5} + \frac{2 \cdot 7^{k}}{5} & - \frac{3 \left(-3\right)^{k}}{5} + \frac{3 \cdot 7^{k}}{5}\\- \frac{2 \left(-3\right)^{k}}{5} + \frac{2 \cdot 7^{k}}{5} & \frac{2 \left(-3\right)^{k}}{5} + \frac{3 \cdot 7^{k}}{5}\end{matrix}\right]$

# 3x3 Diagonalization Example

```python
B = sp.Matrix([[319/118,-70/59,-49/118], [-105/59,-243/59,-147/59], [35/118,70/59,403/118]])
B
```

$\displaystyle \left[\begin{matrix}2.70338983050847 & -1.1864406779661 & -0.415254237288136\\-1.77966101694915 & -4.11864406779661 & -2.49152542372881\\0.296610169491525 & 1.1864406779661 & 3.41525423728814\end{matrix}\right]$

```python
#EigenValue
print(B.eigenvals())

```

    {-4.00000000000000: 1, 3.00000000000000: 1, 3.00000000000000: 1}

```python
Q = sp.Matrix([[1,-4,1], [-2,1,6], [5,0,-1]])
Q
```

$\displaystyle \left[\begin{matrix}1 & -4 & 1\\-2 & 1 & 6\\5 & 0 & -1\end{matrix}\right]$

```python
E = sp.Matrix([[3,0,0], [0,3,0], [0,0,-4]])
E
```

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

```python
# Exact fractions - many digits:
B_k = Q*(E**k)*(Q.inv())
B_k
```

$\displaystyle \left[\begin{matrix}\frac{5 \left(-4\right)^{k}}{118} + \frac{113 \cdot 3^{k}}{118} & \frac{10 \left(-4\right)^{k}}{59} - \frac{10 \cdot 3^{k}}{59} & \frac{7 \left(-4\right)^{k}}{118} - \frac{7 \cdot 3^{k}}{118}\\\frac{15 \left(-4\right)^{k}}{59} - \frac{15 \cdot 3^{k}}{59} & \frac{60 \left(-4\right)^{k}}{59} - \frac{3^{k}}{59} & \frac{21 \left(-4\right)^{k}}{59} - \frac{21 \cdot 3^{k}}{59}\\- \frac{5 \left(-4\right)^{k}}{118} + \frac{5 \cdot 3^{k}}{118} & - \frac{10 \left(-4\right)^{k}}{59} + \frac{10 \cdot 3^{k}}{59} & - \frac{7 \left(-4\right)^{k}}{118} + \frac{125 \cdot 3^{k}}{118}\end{matrix}\right]$

```python
# Easy decimal approcimations:
1.0*B_k
```

$\displaystyle \left[\begin{matrix}0.0423728813559322 \left(-4\right)^{k} + 0.957627118644068 \cdot 3^{k} & 0.169491525423729 \left(-4\right)^{k} - 0.169491525423729 \cdot 3^{k} & 0.0593220338983051 \left(-4\right)^{k} - 0.0593220338983051 \cdot 3^{k}\\0.254237288135593 \left(-4\right)^{k} - 0.254237288135593 \cdot 3^{k} & 1.01694915254237 \left(-4\right)^{k} - 0.0169491525423729 \cdot 3^{k} & 0.355932203389831 \left(-4\right)^{k} - 0.355932203389831 \cdot 3^{k}\\- 0.0423728813559322 \left(-4\right)^{k} + 0.0423728813559322 \cdot 3^{k} & - 0.169491525423729 \left(-4\right)^{k} + 0.169491525423729 \cdot 3^{k} & - 0.0593220338983051 \left(-4\right)^{k} + 1.05932203389831 \cdot 3^{k}\end{matrix}\right]$


```python
import px4tools
%pylab inline

d = px4tools.ulog.cached_log_processing(
    log='./logs/01-18-17-gyro-bias.ulg',
    msg_filter='sensor_gyro',
    processing_func=lambda x: x['sensor_gyro_0'].resample('1 s').agg('mean'),
    save_path='./logs/01-18-17-gyro-bias-sensor_gyro_0.pkl',
    force_processing=False)
d = d['10 min': '3 h 50 m']

def temp_fit(d, debias):
    temp = d.t_sensor_gyro_0__f_temperature.ffill()
    gx = d.t_sensor_gyro_0__f_x.ffill()
    gy = d.t_sensor_gyro_0__f_y.ffill()
    gz = d.t_sensor_gyro_0__f_z.ffill()

    if debias:
        gx -= gx.mean()
        gy -= gy.mean()
        gz -= gz.mean()

    Y = np.vstack([gx, gy, gz])
    c = np.ones(Y.shape[1])
    X = np.vstack([temp**3, temp**2, temp, c])
    # Y = Theta*X -> Theta = YX^+
    Theta = Y.dot(np.linalg.pinv(X))
    Theta

    print('X shape', X.shape)
    print('Y shape', Y.shape)
    print('Theta shape', Theta.shape)

    plt.plot(temp, 1e3*gx)
    plt.plot(temp, 1e3*gy)
    plt.plot(temp, 1e3*gz)
    plt.plot(temp, 1e3*Theta[0,:].dot(X))
    plt.plot(temp, 1e3*Theta[1,:].dot(X))
    plt.plot(temp, 1e3*Theta[2,:].dot(X))
    plt.xlabel('temp, C')
    plt.ylabel('bias, mrad/s')
    plt.grid()
    #plt.gca().set_ylim(-10, 10)
    return Theta
```

    attempting to monkey patch pandas timedelta series plotting
    monkey patch suceeded
    Populating the interactive namespace from numpy and matplotlib

```python
#data = px4tools.read_ulog('/home/jgoppert/logs/01-03-17-mhkabir-pixhawk2.ulg')
```

```python
Theta_debias = temp_fit(d, True)
```

```python
Theta = temp_fit(d, False)
```

# Recursive Algorithm

```python
import sympy
sympy.init_printing()

Theta = sympy.Matrix(sympy.symbols(
    'theta_0:3_0:4')).reshape(3,4)

def Y(n):
    return sympy.Matrix(sympy.symbols(
        'G_x:z_0:{:d}'.format(n+1))).T.reshape(3, n+1)

def C(n):
    return sympy.ones(n+1, 1)

def T(n):
    return sympy.Matrix(sympy.symbols('T_0:{:d}'.format(n+1)))

def T2(n):
    return T(n).multiply_elementwise(T(n))

def T3(n):
    return T2(n).multiply_elementwise(T(n))

def X(n):
    return C(n).row_join(T(n)).row_join(T2(n)).row_join(T3(n)).T
```

This let's us derive a recurisve form.

$Y = \Theta X$

$Y X^T = \Theta X X^T$

We accumulate $Y X^T$ and $X X^T$ since they are of fixed size
and there is a recursion relation as shown below.

At the end we perform the inverse. This is the same as a pseuod-inverse
solution but done recursively.

$\Theta = Y X^T (X X^T)^{-1}$

```python
X(0)*X(0).T
```

```python
X(1)*X(1).T
```

```python
dX = X(1)*X(1).T - X(0)*X(0).T
dX
```

```python
Y(0)*X(0).T
```

```python
Y(1)*X(1).T
```

```python
dYXT = Y(1)*X(1).T - Y(0)*X(0).T
dYXT
```