# Introduction
Play with sympy (Symbolic math)

```sh
%%sh
conda install --yes sympy pandas matplotlib
```

    Solving environment: ...working... done

    ## Package Plan ##

      environment location: /anaconda3/envs/jupyterlab

      added / updated specs:
        - matplotlib
        - pandas
        - sympy

    The following packages will be downloaded:

        package                    |            build
        ---------------------------|-----------------
        matplotlib-3.0.2           |   py37h54f8f79_0         6.5 MB
        numpy-base-1.15.4          |   py37h6575580_0         4.1 MB
        libgfortran-3.0.1          |       h93005f0_2         495 KB
        mkl_random-1.0.2           |   py37h27c97d8_0         358 KB
        numpy-1.15.4               |   py37hacdab7b_0          47 KB
        sympy-1.3                  |           py37_0         9.5 MB
        mkl-2019.1                 |              144       154.4 MB
        intel-openmp-2019.1        |              144         1.1 MB
        blas-1.0                   |              mkl           5 KB
        mkl_fft-1.0.6              |   py37h27c97d8_0         182 KB
        scipy-1.1.0                |   py37h1410ff5_2        14.9 MB
        mpmath-1.1.0               |           py37_0         957 KB
        ------------------------------------------------------------
                                               Total:       192.6 MB

    The following NEW packages will be INSTALLED:

        fastcache:       1.0.2-py37h1de35cc_2
        gmp:             6.1.2-hb37e062_1
        gmpy2:           2.0.8-py37h6ef4df4_2
        intel-openmp:    2019.1-144
        mkl:             2019.1-144
        mkl_fft:         1.0.6-py37h27c97d8_0
        mkl_random:      1.0.2-py37h27c97d8_0
        mpc:             1.1.0-h6ef4df4_1
        mpfr:            4.0.1-h3018a27_3
        mpmath:          1.1.0-py37_0
        numpy-base:      1.15.4-py37h6575580_0
        sympy:           1.3-py37_0

    The following packages will be UPDATED:

        libgfortran:     3.0.0-1                              conda-forge --> 3.0.1-h93005f0_2
        numpy:           1.15.4-py37_blas_openblashb06ca3d_0  conda-forge [blas_openblas] --> 1.15.4-py37hacdab7b_0
        openssl:         1.0.2p-h470a237_1                    conda-forge --> 1.1.1a-h1de35cc_0
        pandas:          0.23.4-py37hf8a1672_0                conda-forge --> 0.23.4-py37h6440ff4_0
        python:          3.7.1-h46c1a51_0                     conda-forge --> 3.7.1-haf84260_7

    The following packages will be DOWNGRADED:

        blas:            1.1-openblas                         conda-forge --> 1.0-mkl
        ca-certificates: 2018.11.29-ha4d7672_0                conda-forge --> 2018.03.07-0
        certifi:         2018.11.29-py37_1000                 conda-forge --> 2018.11.29-py37_0
        matplotlib:      3.0.2-py37_1                         conda-forge --> 3.0.2-py37h54f8f79_0
        scipy:           1.2.0-py37_blas_openblashb06ca3d_200 conda-forge [blas_openblas] --> 1.1.0-py37h1410ff5_2

    Downloading and Extracting Packages
    Preparing transaction: ...working... done
    Verifying transaction: ...working... done
    Executing transaction: ...working... done

    matplotlib-3.0.2     | 6.5 MB    | ########## | 100%
    numpy-base-1.15.4    | 4.1 MB    | ########## | 100%
    libgfortran-3.0.1    | 495 KB    | ########## | 100%
    mkl_random-1.0.2     | 358 KB    | ########## | 100%
    numpy-1.15.4         | 47 KB     | ########## | 100%
    sympy-1.3            | 9.5 MB    | ########## | 100%
    mkl-2019.1           | 154.4 MB  | ########## | 100%
    intel-openmp-2019.1  | 1.1 MB    | ########## | 100%
    blas-1.0             | 5 KB      | ########## | 100%
    mkl_fft-1.0.6        | 182 KB    | ########## | 100%
    scipy-1.1.0          | 14.9 MB   | ########## | 100%
    mpmath-1.1.0         | 957 KB    | ########## | 100%

```python
from sympy import *
from sympy.plotting import plot, plot_parametric
import math
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
```

```python
c, w, x, y, z = symbols('c w x y z')
```

```python
init_printing()
```

```python
eq1 = x*x + y*y*y
eq2 = w*w + z*z*z
eq = eq1 - eq2
```

```python
solve(eq)
```

```python
for c in range(1,20000):
    for w in range(1, c):
        for x in range (1, c):
            if c == w*w*w + x*x and w < x:
                for y in range (1, c):
                    for z in range (1, c):
                        if (c == y*y*y + z*z) and c == w*w*w + x*x and w < y and x != y and w != z and y != z and x != w and x != z:
                            print (c,w,x,y,z)
```

    (17, 1, 4, 2, 3)
    (89, 2, 9, 4, 5)
    (108, 2, 10, 3, 9)
    (145, 1, 12, 4, 9)
    (225, 5, 10, 6, 3)
    (233, 2, 15, 4, 13)
    (297, 2, 17, 6, 9)
    (316, 3, 17, 6, 10)
    (388, 3, 19, 4, 18)
    (449, 2, 21, 5, 18)
    (464, 4, 20, 7, 11)
    (505, 4, 21, 6, 17)
    (537, 2, 23, 8, 5)
    (548, 4, 22, 8, 6)
    (577, 1, 24, 6, 19)
    (593, 4, 23, 8, 9)
    (633, 2, 25, 8, 11)
    (737, 2, 27, 8, 15)
    (745, 6, 23, 9, 4)
    (792, 2, 28, 6, 24)
    (793, 4, 27, 9, 8)
    (801, 5, 26, 8, 17)
    (873, 8, 19, 9, 12)
    (1025, 1, 32, 4, 31)
    (1025, 1, 32, 5, 30)
    (1025, 1, 32, 10, 5)
    (1025, 4, 31, 5, 30)
    (1025, 4, 31, 10, 5)
    (1088, 4, 32, 8, 24)
    (1090, 1, 33, 9, 19)
    (1116, 3, 33, 6, 30)
    (1289, 4, 35, 10, 17)
    (1304, 2, 36, 7, 31)
    (1305, 6, 33, 9, 24)
    (1367, 7, 32, 11, 6)
    (1412, 8, 30, 11, 9)
    (1441, 6, 35, 10, 21)
    (1529, 2, 39, 10, 23)
    (1585, 4, 39, 6, 37)
    (1601, 1, 40, 8, 33)
    (1729, 10, 27, 12, 1)
    (1737, 6, 39, 8, 35)
    (1737, 6, 39, 12, 3)
    (1737, 8, 35, 12, 3)

```python
eq = x ** 3 - 2 * x - 1
solve(eq)
```

```python
plot(eq)
```

```python
range = 50
plot(cos(2*x) + sin(x/2), (x, -range, range))
```

```python
from sympy.plotting import plot3d
plot3d(x*y, -x*y, (x, -5, 5), (y, -5, 5))
```

```python
plot3d(1+x*x+y*y,(x, -5, 5), (y, -5, 5))
```

```python
from sympy import symbols, cos, sin
from sympy.plotting import plot3d_parametric_line
u = symbols('u')
r = 5*pi
plot3d_parametric_line(cos(u), sin(u), u, (u, -r, r))
```

```python
u,v = symbols('u v')
from sympy.plotting import plot3d_parametric_surface
plot3d_parametric_surface(cos(u + v), sin(u - v), u - v, (u, -5, 5), (v, -5, 5))
```

```python
from sympy import plot_implicit, symbols, Eq, And
plot_implicit(Eq(x**2 + y**2, 5))
```

```python
plot_implicit(And(y > x, y > -x))
```

```sh
%%sh
pip install pyglet
```

    Collecting pyglet
      Downloading pyglet-1.2.4-py2-none-any.whl (964kB)
    Installing collected packages: pyglet
    Successfully installed pyglet-1.2.4

```python
import pyglet
#from sympy import var, Plot
```

```python
from sympy import log, E
n = Symbol('n')
```

```python
plot(log(E**sin(x)))
```

```python
factorial(10)
```

```python
(pi, 2*pi) * 10
```

```python
Float(22./7.)
```

```python
pi.evalf(100)-66./21.
```

```python
E.evalf(10000)
```

```python
(pi+E).evalf(100)
```

```python
sin(pi)
```

```python
y = x**3
(y, Integral(y,x), integrate(y, x), diff(y))
```

```python

plot(erf( x ))
```

```python
y=(2-3*x)/(x-4)
plot(y)
```


```python
'''
Written By: Abhijeet Kulkarni (amkulk@udel.edu)
For: Research Project MEEG678
Title: Autonomous driving using Model Predictive ContouringControl.

#Run this file if .dill files are not present.
'''

import sympy as sm
import numpy as np
from IPython.core.interactiveshell import InteractiveShell
InteractiveShell.ast_node_interactivity = "all"
import dill
dill.settings['recurse'] = True
from sympy.utilities.lambdify import lambdify, implemented_function
from HelpingFunctions import *
#!which python
```

    '\nWritten By: Abhijeet Kulkarni (amkulk@udel.edu)\nFor: Research Project MEEG678\nTitle: Autonomous driving using Model Predictive ContouringControl.\n\n#Run this file if .dill files are not present.\n'

```python
M,I,Fx1,Fy1,Fx2,Fy2,a1,a2,C1,C2 = sm.symbols(r'M I F_{x1} F_{y1} F_{x2} F_{y2} a_1 a_2 C_1 C_2',real=True)
vx,vy,dtheta,theta,delta = sm.symbols(r'v_x v_y \dot{\theta} \theta \delta',real=True)
X,Y,dX,dY = sm.symbols(r'X Y \dot{X} \dot{Y}')
```

```python
#Velocities
vx = dX*sm.cos(theta) + dY*sm.sin(theta)
vy = -dX*sm.sin(theta) + dY*sm.cos(theta)
#Forces
#Fy1 = -C1*sm.atan((vy+dtheta*a1)/vx)
#Fy2 = -C2*sm.atan((vy-dtheta*a2)/vx)
Fy = Fy2 + Fy1*sm.cos(delta)+Fx1*sm.sin(delta)
Fx = Fx2 + Fx1*sm.cos(delta) - Fy1*sm.sin(delta)
Tau = -Fy2*a2+a1*(Fy1*sm.cos(delta)+Fx1*sm.sin(delta))

#Accelerations in Global frame
ddX = (Fx*sm.cos(theta)-Fy*sm.sin(theta))/M
ddY = (Fx*sm.sin(theta)+Fy*sm.cos(theta))/M
ddtheta = Tau/I
ddX=sm.simplify(ddX)
ddY=sm.simplify(ddY)
ddtheta=sm.simplify(ddtheta)
ode=sm.simplify(sm.Matrix([dX, dY, dtheta, ddX, ddY, ddtheta]))
fode=lambdify([[X,Y,theta,dX,dY,dtheta],[delta,Fx1,Fx2],[Fy1,Fy2],M,I,a1,a2],ode,'numpy')
ode
dill.dump(fode, open("eta_dot.dill","wb"))
print(sm.latex(ode))
```

$\displaystyle \left[\begin{matrix}\dot{X}\\\dot{Y}\\\dot{\theta}\\\frac{F_{x1} \cos{\left(\delta + \theta \right)} + F_{x2} \cos{\left(\theta \right)} - F_{y1} \sin{\left(\delta + \theta \right)} - F_{y2} \sin{\left(\theta \right)}}{M}\\\frac{F_{x1} \sin{\left(\delta + \theta \right)} + F_{x2} \sin{\left(\theta \right)} + F_{y1} \cos{\left(\delta + \theta \right)} + F_{y2} \cos{\left(\theta \right)}}{M}\\\frac{- F_{y2} a_{2} + a_{1} \left(F_{x1} \sin{\left(\delta \right)} + F_{y1} \cos{\left(\delta \right)}\right)}{I}\end{matrix}\right]$

    \left[\begin{matrix}\dot{X}\\\dot{Y}\\\dot{\theta}\\\frac{F_{x1} \cos{\left(\delta + \theta \right)} + F_{x2} \cos{\left(\theta \right)} - F_{y1} \sin{\left(\delta + \theta \right)} - F_{y2} \sin{\left(\theta \right)}}{M}\\\frac{F_{x1} \sin{\left(\delta + \theta \right)} + F_{x2} \sin{\left(\theta \right)} + F_{y1} \cos{\left(\delta + \theta \right)} + F_{y2} \cos{\left(\theta \right)}}{M}\\\frac{- F_{y2} a_{2} + a_{1} \left(F_{x1} \sin{\left(\delta \right)} + F_{y1} \cos{\left(\delta \right)}\right)}{I}\end{matrix}\right]

```python
#linearizing contact forces
q = sm.Matrix([X,Y,theta,dX,dY,dtheta]) #  states
u = sm.Matrix([delta, Fx1,Fx2]) #  inputs

D1,D2,C1,C2,B1,B2 = sm.symbols(r'D_1 D_2 C_1 C_2 B_1 B_2',real =True)
vx = dX*sm.cos(theta) + dY*sm.sin(theta)
vy = -dX*sm.sin(theta) + dY*sm.cos(theta)
alpha1 = -delta + sm.atan2((vy+dtheta*a1),vx)
alpha2 = sm.atan2((vy-dtheta*a2),vx)
sFy1 = -D1*sm.sin(C1*sm.atan(B1*alpha1))
sFy2 = -D2*sm.sin(C2*sm.atan(B2*alpha2))
sm.Matrix([sFy1,sFy2])
print(sm.latex(sm.Matrix([sFy1,sFy2])))

```

$\displaystyle \left[\begin{matrix}- D_{1} \sin{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)}\\- D_{2} \sin{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)}\end{matrix}\right]$

    \left[\begin{matrix}- D_{1} \sin{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)}\\- D_{2} \sin{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)}\end{matrix}\right]

```python
# Linear approximation of alpha2
alpha2Grad = sm.Matrix([alpha2]).jacobian(q)
alpha2Grad@q
alpha2
alpha2C = sm.Matrix([alpha2]) - alpha2Grad@q
falpha2data=lambdify([[X,Y,theta,dX,dY,dtheta],a1,a2],[alpha2C,alpha2Grad],'numpy')
dill.dump(falpha2data, open("alpha2data.dill","wb"))
```

$\displaystyle \left[\begin{matrix}\dot{X} \left(- \frac{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \sin{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}} + \frac{\left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{2}\right) \cos{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}}\right) + \dot{Y} \left(\frac{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \cos{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}} + \frac{\left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{2}\right) \sin{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}}\right) - \frac{\dot{\theta} a_{2} \left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}} + \theta \left(\frac{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)}\right) \left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{2}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}} + \frac{\left(- \dot{X} \cos{\left(\theta \right)} - \dot{Y} \sin{\left(\theta \right)}\right) \left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}}\right)\end{matrix}\right]$

$\displaystyle \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}$

```python
# Linearizing
f = (ode.subs([(Fy1,sFy1),(Fy2,sFy2)]))
f
A = (f.jacobian(q))
2
B = (f.jacobian(u))
# dq ~ f(q_,u_) + A(q-q_) + B(u-u_) =  Aq + Bu + (f(q_,u_) - Aq_ - Bu_) = Aq + Bu + G

G = f - A@q -B@u

```

$\displaystyle \left[\begin{matrix}\dot{X}\\\dot{Y}\\\dot{\theta}\\\frac{D_{1} \sin{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)} \sin{\left(\delta + \theta \right)} + D_{2} \sin{\left(\theta \right)} \sin{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)} + F_{x1} \cos{\left(\delta + \theta \right)} + F_{x2} \cos{\left(\theta \right)}}{M}\\\frac{- D_{1} \sin{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)} \cos{\left(\delta + \theta \right)} - D_{2} \sin{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)} \cos{\left(\theta \right)} + F_{x1} \sin{\left(\delta + \theta \right)} + F_{x2} \sin{\left(\theta \right)}}{M}\\\frac{D_{2} a_{2} \sin{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)} + a_{1} \left(- D_{1} \sin{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)} \cos{\left(\delta \right)} + F_{x1} \sin{\left(\delta \right)}\right)}{I}\end{matrix}\right]$

    2

```python
#Discretizing
# (qk+1 - qk)/dt = Akqk + Bkuk  + Gk
# qk+1 = qk + (Akqk + Bkuk + Gk)dt = (I + Ak)qk + (Bkdt)uk + Gkdt = (Ak)qk + (Bk)uk + Gk
dt = sm.symbols(r'd\text{t}',real = True) #time step
Ak = (sm.eye(A.shape[0]) + A*dt)
Bk = (B*dt)
Gk = (G*dt)
Ak
DiscDynaMatrices=lambdify([dt,[X,Y,theta,dX,dY,dtheta],[delta,Fx1,Fx2],M,I,a1,a2,D1,D2,C1,C2,B1,B2],[Ak,Bk,Gk],'numpy')
dill.dump(DiscDynaMatrices, open("DiscDynaMatrices.dill","wb"))
```

$\displaystyle \left[\begin{matrix}1 & 0 & 0 & d\text{t} & 0 & 0\\0 & 1 & 0 & 0 & d\text{t} & 0\\0 & 0 & 1 & 0 & 0 & d\text{t}\\0 & 0 & \frac{d\text{t} \left(\frac{B_{1} C_{1} D_{1} \left(\frac{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)}\right) \left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{1}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}} + \frac{\left(- \dot{X} \cos{\left(\theta \right)} - \dot{Y} \sin{\left(\theta \right)}\right) \left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}}\right) \sin{\left(\delta + \theta \right)} \cos{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)}}{B_{1}^{2} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right)^{2} + 1} + \frac{B_{2} C_{2} D_{2} \left(\frac{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)}\right) \left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{2}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}} + \frac{\left(- \dot{X} \cos{\left(\theta \right)} - \dot{Y} \sin{\left(\theta \right)}\right) \left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}}\right) \sin{\left(\theta \right)} \cos{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)}}{B_{2}^{2} \operatorname{atan_{2}}^{2}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} + 1} + D_{1} \sin{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)} \cos{\left(\delta + \theta \right)} + D_{2} \sin{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)} \cos{\left(\theta \right)} - F_{x1} \sin{\left(\delta + \theta \right)} - F_{x2} \sin{\left(\theta \right)}\right)}{M} & 1 + \frac{d\text{t} \left(\frac{B_{1} C_{1} D_{1} \left(- \frac{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \sin{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}} + \frac{\left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{1}\right) \cos{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}}\right) \sin{\left(\delta + \theta \right)} \cos{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)}}{B_{1}^{2} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right)^{2} + 1} + \frac{B_{2} C_{2} D_{2} \left(- \frac{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \sin{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}} + \frac{\left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{2}\right) \cos{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}}\right) \sin{\left(\theta \right)} \cos{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)}}{B_{2}^{2} \operatorname{atan_{2}}^{2}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} + 1}\right)}{M} & \frac{d\text{t} \left(\frac{B_{1} C_{1} D_{1} \left(\frac{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \cos{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}} + \frac{\left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{1}\right) \sin{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}}\right) \sin{\left(\delta + \theta \right)} \cos{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)}}{B_{1}^{2} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right)^{2} + 1} + \frac{B_{2} C_{2} D_{2} \left(\frac{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \cos{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}} + \frac{\left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{2}\right) \sin{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}}\right) \sin{\left(\theta \right)} \cos{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)}}{B_{2}^{2} \operatorname{atan_{2}}^{2}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} + 1}\right)}{M} & \frac{d\text{t} \left(\frac{B_{1} C_{1} D_{1} a_{1} \left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \sin{\left(\delta + \theta \right)} \cos{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)}}{\left(B_{1}^{2} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right)^{2} + 1\right) \left(\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}\right)} - \frac{B_{2} C_{2} D_{2} a_{2} \left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \sin{\left(\theta \right)} \cos{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)}}{\left(B_{2}^{2} \operatorname{atan_{2}}^{2}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} + 1\right) \left(\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}\right)}\right)}{M}\\0 & 0 & \frac{d\text{t} \left(- \frac{B_{1} C_{1} D_{1} \left(\frac{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)}\right) \left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{1}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}} + \frac{\left(- \dot{X} \cos{\left(\theta \right)} - \dot{Y} \sin{\left(\theta \right)}\right) \left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}}\right) \cos{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)} \cos{\left(\delta + \theta \right)}}{B_{1}^{2} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right)^{2} + 1} - \frac{B_{2} C_{2} D_{2} \left(\frac{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)}\right) \left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{2}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}} + \frac{\left(- \dot{X} \cos{\left(\theta \right)} - \dot{Y} \sin{\left(\theta \right)}\right) \left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}}\right) \cos{\left(\theta \right)} \cos{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)}}{B_{2}^{2} \operatorname{atan_{2}}^{2}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} + 1} + D_{1} \sin{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)} \sin{\left(\delta + \theta \right)} + D_{2} \sin{\left(\theta \right)} \sin{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)} + F_{x1} \cos{\left(\delta + \theta \right)} + F_{x2} \cos{\left(\theta \right)}\right)}{M} & \frac{d\text{t} \left(- \frac{B_{1} C_{1} D_{1} \left(- \frac{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \sin{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}} + \frac{\left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{1}\right) \cos{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}}\right) \cos{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)} \cos{\left(\delta + \theta \right)}}{B_{1}^{2} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right)^{2} + 1} - \frac{B_{2} C_{2} D_{2} \left(- \frac{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \sin{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}} + \frac{\left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{2}\right) \cos{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}}\right) \cos{\left(\theta \right)} \cos{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)}}{B_{2}^{2} \operatorname{atan_{2}}^{2}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} + 1}\right)}{M} & 1 + \frac{d\text{t} \left(- \frac{B_{1} C_{1} D_{1} \left(\frac{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \cos{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}} + \frac{\left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{1}\right) \sin{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}}\right) \cos{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)} \cos{\left(\delta + \theta \right)}}{B_{1}^{2} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right)^{2} + 1} - \frac{B_{2} C_{2} D_{2} \left(\frac{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \cos{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}} + \frac{\left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{2}\right) \sin{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}}\right) \cos{\left(\theta \right)} \cos{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)}}{B_{2}^{2} \operatorname{atan_{2}}^{2}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} + 1}\right)}{M} & \frac{d\text{t} \left(- \frac{B_{1} C_{1} D_{1} a_{1} \left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \cos{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)} \cos{\left(\delta + \theta \right)}}{\left(B_{1}^{2} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right)^{2} + 1\right) \left(\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}\right)} + \frac{B_{2} C_{2} D_{2} a_{2} \left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \cos{\left(\theta \right)} \cos{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)}}{\left(B_{2}^{2} \operatorname{atan_{2}}^{2}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} + 1\right) \left(\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}\right)}\right)}{M}\\0 & 0 & \frac{d\text{t} \left(- \frac{B_{1} C_{1} D_{1} a_{1} \left(\frac{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)}\right) \left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{1}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}} + \frac{\left(- \dot{X} \cos{\left(\theta \right)} - \dot{Y} \sin{\left(\theta \right)}\right) \left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}}\right) \cos{\left(\delta \right)} \cos{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)}}{B_{1}^{2} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right)^{2} + 1} + \frac{B_{2} C_{2} D_{2} a_{2} \left(\frac{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)}\right) \left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{2}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}} + \frac{\left(- \dot{X} \cos{\left(\theta \right)} - \dot{Y} \sin{\left(\theta \right)}\right) \left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}}\right) \cos{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)}}{B_{2}^{2} \operatorname{atan_{2}}^{2}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} + 1}\right)}{I} & \frac{d\text{t} \left(- \frac{B_{1} C_{1} D_{1} a_{1} \left(- \frac{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \sin{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}} + \frac{\left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{1}\right) \cos{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}}\right) \cos{\left(\delta \right)} \cos{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)}}{B_{1}^{2} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right)^{2} + 1} + \frac{B_{2} C_{2} D_{2} a_{2} \left(- \frac{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \sin{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}} + \frac{\left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{2}\right) \cos{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}}\right) \cos{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)}}{B_{2}^{2} \operatorname{atan_{2}}^{2}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} + 1}\right)}{I} & \frac{d\text{t} \left(- \frac{B_{1} C_{1} D_{1} a_{1} \left(\frac{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \cos{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}} + \frac{\left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{1}\right) \sin{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}}\right) \cos{\left(\delta \right)} \cos{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)}}{B_{1}^{2} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right)^{2} + 1} + \frac{B_{2} C_{2} D_{2} a_{2} \left(\frac{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \cos{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}} + \frac{\left(\dot{X} \sin{\left(\theta \right)} - \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{2}\right) \sin{\left(\theta \right)}}{\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}}\right) \cos{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)}}{B_{2}^{2} \operatorname{atan_{2}}^{2}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} + 1}\right)}{I} & 1 + \frac{d\text{t} \left(- \frac{B_{1} C_{1} D_{1} a_{1}^{2} \left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \cos{\left(\delta \right)} \cos{\left(C_{1} \operatorname{atan}{\left(B_{1} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right) \right)} \right)}}{\left(B_{1}^{2} \left(- \delta + \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)}\right)^{2} + 1\right) \left(\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} + \dot{\theta} a_{1}\right)^{2}\right)} - \frac{B_{2} C_{2} D_{2} a_{2}^{2} \left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right) \cos{\left(C_{2} \operatorname{atan}{\left(B_{2} \operatorname{atan_{2}}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} \right)} \right)}}{\left(B_{2}^{2} \operatorname{atan_{2}}^{2}{\left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2},\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)} \right)} + 1\right) \left(\left(\dot{X} \cos{\left(\theta \right)} + \dot{Y} \sin{\left(\theta \right)}\right)^{2} + \left(- \dot{X} \sin{\left(\theta \right)} + \dot{Y} \cos{\left(\theta \right)} - \dot{\theta} a_{2}\right)^{2}\right)}\right)}{I}\end{matrix}\right]$

```python
# error contouring and lag
ql,qc,ta = sm.symbols(r'q_l q_c t_a',real = True)
Xrefta,Yrefta,dXrefta,dYrefta,ddXrefta,ddYrefta = sm.symbols(r'X^{ref} Y^{ref} \dot{X}^{ref} \dot{Y}^{ref} \ddot{X}^{ref} \ddot{Y}^{ref}',real = True)
phita = sm.atan2(dYrefta,dXrefta)
#phitaa = sm.symbols(r'\Phi(t_a)',real=True)
ec = sm.Matrix([sm.sin(phita)*(X-Xrefta) - sm.cos(phita)*(Y-Yrefta)]) #contouring error
el = sm.Matrix([-sm.cos(phita)*(X-Xrefta) - sm.sin(phita)*(Y-Yrefta)]) #lag error
e = sm.Matrix([[ec],[el]])
print(sm.latex(e))
# linearizing f(x)~f(x0)+Jf(x0)*(x-x0)=(f(x0)-Jf(x0)*x0)+Jf(x0)*x)=C+Grad*x

var = sm.Matrix([Xrefta,Yrefta,dXrefta,dYrefta])
# Chain differentiation. To not fix the bezier polynomial in symbolic generation
Gradenom = sm.simplify(e.jacobian(q))
Gradenom
# Gradient while condidering [Xrefta,Yrefta,dXrefta,dYrefta]
Graderef  = e.jacobian(var)
#Graderef
#Common jacobian for reference var to parameter ta
JacobRP =sm.Matrix([[dXrefta],[dYrefta],[ddXrefta],[ddYrefta]])

#Gradient while considering [ta]
Gradeta  = sm.simplify(Graderef@JacobRP)
Gradeta

#Constant part of linearization
C  = sm.simplify(e  - Gradenom@q - Gradeta@sm.Matrix([ta]))
C

```

    \left[\begin{matrix}- \frac{\dot{X}^{ref} \left(Y - Y^{ref}\right)}{\sqrt{\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}}} + \frac{\dot{Y}^{ref} \left(X - X^{ref}\right)}{\sqrt{\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}}}\\- \frac{\dot{X}^{ref} \left(X - X^{ref}\right)}{\sqrt{\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}}} - \frac{\dot{Y}^{ref} \left(Y - Y^{ref}\right)}{\sqrt{\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}}}\end{matrix}\right]

$\displaystyle \left[\begin{matrix}\frac{\dot{Y}^{ref}}{\sqrt{\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}}} & - \frac{\dot{X}^{ref}}{\sqrt{\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}}} & 0 & 0 & 0 & 0\\- \frac{\dot{X}^{ref}}{\sqrt{\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}}} & - \frac{\dot{Y}^{ref}}{\sqrt{\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}}} & 0 & 0 & 0 & 0\end{matrix}\right]$

$\displaystyle \left[\begin{matrix}\frac{- \ddot{X}^{ref} \left(\dot{X}^{ref} \left(\dot{X}^{ref} \left(- Y + Y^{ref}\right) + \dot{Y}^{ref} \left(X - X^{ref}\right)\right) + \left(Y - Y^{ref}\right) \left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right)\right) + \ddot{Y}^{ref} \left(\dot{Y}^{ref} \left(\dot{X}^{ref} \left(Y - Y^{ref}\right) + \dot{Y}^{ref} \left(- X + X^{ref}\right)\right) + \left(X - X^{ref}\right) \left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right)\right)}{\left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right)^{\frac{3}{2}}}\\\frac{\ddot{X}^{ref} \left(\dot{X}^{ref} \left(\dot{X}^{ref} \left(X - X^{ref}\right) + \dot{Y}^{ref} \left(Y - Y^{ref}\right)\right) + \left(- X + X^{ref}\right) \left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right)\right) + \ddot{Y}^{ref} \left(\dot{Y}^{ref} \left(\dot{X}^{ref} \left(X - X^{ref}\right) + \dot{Y}^{ref} \left(Y - Y^{ref}\right)\right) + \left(- Y + Y^{ref}\right) \left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right)\right) + \left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right)^{2}}{\left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right)^{\frac{3}{2}}}\end{matrix}\right]$

$\displaystyle \left[\begin{matrix}\frac{t_{a} \left(\ddot{X}^{ref} \left(- \dot{X}^{ref} \left(\dot{X}^{ref} \left(Y - Y^{ref}\right) - \dot{Y}^{ref} \left(X - X^{ref}\right)\right) + \left(Y - Y^{ref}\right) \left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right)\right) - \ddot{Y}^{ref} \left(\dot{Y}^{ref} \left(\dot{X}^{ref} \left(Y - Y^{ref}\right) - \dot{Y}^{ref} \left(X - X^{ref}\right)\right) + \left(X - X^{ref}\right) \left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right)\right)\right) + \left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right) \left(- X \dot{Y}^{ref} + Y \dot{X}^{ref} - \dot{X}^{ref} \left(Y - Y^{ref}\right) + \dot{Y}^{ref} \left(X - X^{ref}\right)\right)}{\left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right)^{\frac{3}{2}}}\\\frac{- t_{a} \left(\ddot{X}^{ref} \left(\dot{X}^{ref} \left(\dot{X}^{ref} \left(X - X^{ref}\right) + \dot{Y}^{ref} \left(Y - Y^{ref}\right)\right) - \left(X - X^{ref}\right) \left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right)\right) + \ddot{Y}^{ref} \left(\dot{Y}^{ref} \left(\dot{X}^{ref} \left(X - X^{ref}\right) + \dot{Y}^{ref} \left(Y - Y^{ref}\right)\right) - \left(Y - Y^{ref}\right) \left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right)\right) + \left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right)^{2}\right) + \left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right) \left(X \dot{X}^{ref} + Y \dot{Y}^{ref} - \dot{X}^{ref} \left(X - X^{ref}\right) - \dot{Y}^{ref} \left(Y - Y^{ref}\right)\right)}{\left(\left(\dot{X}^{ref}\right)^{2} + \left(\dot{Y}^{ref}\right)^{2}\right)^{\frac{3}{2}}}\end{matrix}\right]$

```python
ErrorLinMat=lambdify([[X,Y,theta,dX,dY,dtheta],ta,Xrefta,Yrefta,dXrefta,dYrefta,ddXrefta,ddYrefta],(C,Gradenom,Gradeta),'numpy')
dill.dump(ErrorLinMat, open("ErrorsLindata.dill","wb"))
#C0,gradenom0,gradeta0=ErrorLinMat(np.array([1,1,1,1,1,1]),1,1,2,3,4,5,6)
```

```python

```

    array([[ 0.336],
           [-2.752]])

    array([[ 0.8, -0.6,  0. ,  0. ,  0. ,  0. ],
           [-0.6, -0.8,  0. ,  0. ,  0. ,  0. ]])

    array([[0.064],
           [4.952]])

```python

```


# Preprocess ICRP-107 to create dataset for radioactivedecay
### Introduction
This notebook creates a decay dataset for `radioactivedecay` from the data in ICRP 107: Nuclear Decay Data for Dosimetric Calculations.

The data is saved into the following files:
- `decay_data.npz`: NumPy arrays containing the strings of the radionuclides in the decay dataset, the half-lives and their time units, the atomic masses, the progeny, branching fractions and decay modes of each radionuclide, and the days to year conversion number (NPZ NumPy compressed array format). Atomic data of the stable nuclides is included from the AME2020 database, with supplemental metastable isomer excitation energies from NuBase;
- `c_scipy.npz`: pre-calculated SciPy CSR sparse matrix *C* (Amaku et al. (2010)) (NPZ NumPy compressed array format);
- `c_inv_scipy.npz`: pre-calculated SciPy CSR sparse matrix *C-1* (inverse of *C*) (NPZ NumPy compressed array format);
- `c_sympy.pickle`: pre-calculated SymPy sparse matrix *C* for arbitrary-precision calculations (Python pickle format);
- `c_inv_sympy.pickle`: pre-calculated SymPy sparse matrix *C-1* for arbitrary-precision calculations (inverse of *C*) (Python pickle format);
- `atomic_masses_sympy.pickle`: SymPy matrix of atomic masses(g/mol)for arbitrary-precision calculations (Python pickle format);
- `decay_consts_sympy.pickle`: SymPy matrix of radionuclide decay constants (s-1) for arbitrary-precision calculations (Python pickle format);
- `year_conversion_sympy.pickle`: SymPy representation of the days to year conversion number (Python pickle format).

### Initial set up and read in ICRP-107 data into a DataFrame
First load the necessary Python modules.

```python
import itertools, pickle, re
import fortranformat as ff
import numpy as np
import pandas as pd
from scipy import sparse
from sympy import Integer, log, Matrix, nsimplify, S
from sympy.matrices import SparseMatrix
```

Now we need to download and read in the data from the ICRP-07.NDX data file provided as a supplement to ICRP-107. First read a prepared CSV file listing all elements, their symbols and atomic numbers.

```python
elements = pd.read_csv("element_list.csv", index_col="Symbol")[["Element","Z"]]
elements.head()
```

      Element
      Z

      Symbol

      H
      Hydrogen
      1

      He
      Helium
      2

      Li
      Lithium
      3

      Be
      Beryllium
      4

      B
      Boron
      5

Define functions to
* convert half-life in units of μs, ms, m, h, d, y into seconds;
* create a human-readable string of the half-life;
* return atomic number and mass number from a radionuclide string.

```python
year = 365.2422 # number of days in year used for conversion in ICRP-107
                # (see JAERI 1347 & JAEA-Data/Code 2007-021)
amu = 931494.10242 # number of keV in one amu (* c^2), from AME2020 Table A
num_nuclides = 1252

def convert_half_life(halflife, unit):
    units = {"ys":1.0E-24, "zs":1.0E-21, "as":1.0E-18, "fs":1.0E-15, "ps":1.0E-12, "ns":1.0E-9,
             "μs":1.0E-6, "us":1.0E-6, "ms":1.0E-3, "s":1.0, "m":60.0, "h":60.0*60.0,
             "d":60.0*60.0*24.0, "y":60.0*60.0*24.0*year, "ky":1.0E3*60.0*60.0*24.0*year,
             "My":1.0E6*60.0*60.0*24.0*year, "Gy":1.0E9*60.0*60.0*24.0*year,
             "Ty":1.0E12*60.0*60.0*24.0*year, "Py":1.0E15*60.0*60.0*24.0*year,
             "Ey":1.0E18*60.0*60.0*24.0*year, "Zy":1.0E21*60.0*60.0*24.0*year,
             "Yy":1.0E24*60.0*60.0*24.0*year}
    return float(halflife)*units[unit]

def readable_half_life(halflife, unit):
    # very hacky code to format some of the half lives in ICRP-107 into more readable time units
    if unit == "s":
        if halflife[-3:] == "E-7":
            halflife = "0." + halflife[:-3].replace(".", "")
            unit = "μs"
        elif halflife[-3:] == "E-6":
            halflife = halflife[:-3]
            unit = "μs"
        elif halflife[-3:] == "E-5":
            if len(halflife[:-3]) == 3:
                halflife = halflife[:-3].replace(".", "")
            else:
                halflife = halflife[:-3].replace(".", "")[:2] + '.' + halflife[:-3].replace(".", "")[2:]
            unit = "μs"
        elif halflife[-3:] == "E-4":
            halflife = "0." + halflife[:-3].replace(".", "")
            unit = "ms"
        elif halflife[-3:] == "E-3":
            halflife = halflife[:-3]
            unit = "ms"
        elif halflife[-3:] == "E-2":
            if len(halflife[:-3]) == 3:
                halflife = halflife[:-3].replace(".", "")
            else:
                halflife = halflife[:-3].replace(".", "")[:2] + '.' + halflife[:-3].replace(".", "")[2:]
            unit = "ms"

    elif unit == "y":
        if halflife[-3:-1] == "E1":
            halflife = halflife[:-2] + "+" + halflife[-2:]

        if halflife[-3:] == "E+3":
            halflife = halflife[:-3]
            unit = "ky"
        elif halflife[-3:] == "E+4":
            if len(halflife[:-3]) == 3:
                halflife = halflife[:-3].replace(".", "")
            else:
                halflife = halflife[:-3].replace(".", "")[:2] + '.' + halflife[:-3].replace(".", "")[2:]
            unit = "ky"
        elif halflife[-3:] == "E+5":
            halflife = "0." + halflife[:-3].replace(".", "")
            unit = "My"
        elif halflife[-3:] == "E+6":
            halflife = halflife[:-3]
            unit = "My"
        elif halflife[-3:] == "E+7":
            if len(halflife[:-3]) == 3:
                halflife = halflife[:-3].replace(".", "")
            else:
                halflife = halflife[:-3].replace(".", "")[:2] + '.' + halflife[:-3].replace(".", "")[2:]
            unit = "My"
        elif halflife[-3:] == "E+8":
            halflife = "0." + halflife[:-3].replace(".", "")
            unit = "By"
        elif halflife[-3:] == "E+9":
            halflife = halflife[:-3]
            unit = "By"
        elif halflife[-4:] == "E+10":
            if len(halflife[:-4]) == 3:
                halflife = halflife[:-4].replace(".", "")
            else:
                halflife = halflife[:-4].replace(".", "")[:2] + '.' + halflife[:-4].replace(".", "")[2:]
            unit = "By"
        elif halflife[-4:] == "E+11":
            halflife = "0." + halflife[:-4].replace(".", "")
            unit = "Ty"
        elif halflife[-4:] == "E+12":
            halflife = halflife[:-4]
            unit = "Ty"
        elif halflife[-4:] == "E+13":
            if len(halflife[:-4]) == 3:
                halflife = halflife[:-4].replace(".", "")
            else:
                halflife = halflife[:-4].replace(".", "")[:2] + '.' + halflife[:-4].replace(".", "")[2:]
            unit = "Ty"
        elif halflife[-4:] == "E+14":
            halflife = "0." + halflife[:-4].replace(".", "")
            unit = "Py"
        elif halflife[-4:] == "E+15":
            halflife = halflife[:-4]
            unit = "Py"
        elif halflife[-4:] == "E+16":
            if len(halflife[:-4]) == 3:
                halflife = halflife[:-4].replace(".", "")
            else:
                halflife = halflife[:-4].replace(".", "")[:2] + '.' + halflife[:-4].replace(".", "")[2:]
            unit = "Py"
        elif halflife[-4:] == "E+17":
            halflife = halflife[:-4].replace(".", "")
            unit = "Py"

    return halflife + " " + unit

def get_Z_A(radionuclide):
    [Z, A] = radionuclide.split("-")
    Z = elements.loc[Z, "Z"]
    if A[-1].isalpha():
        A = A[:-1]
    return Z, int(A)
```

Create a pandas DataFrame to hold AME2020 atomic data, needed for atomic masses of stable progeny. Pull data from the AME2020 `.txt` file (`mass_1.mas20.txt`).

```python
# read and parse AME2020 file for atomic masses
ame2020_format = ff.FortranRecordReader('(a1,a3,a5,a5,a5,1x,a3,a4,1x,a14,a12,a13,1x,a10,1x,a2,a13,a11,1x,a3,1x,a13,a12)')

with open('mass_1.mas20.txt','r') as ame2020:
    ame2020_lines = ame2020.readlines()

ame2020_df = pd.DataFrame()
isotope_dictionary_list = {}

for i in range(len(ame2020_lines)-37):
    # starting from the first isotope on line 38, read each line seperately
    isotope = ame2020_format.read(ame2020_lines[i+37])
    # name is combination of element and number of nucleons
    name = isotope[5].split()[0] + "-" + str(isotope[4]).split()[0]
    Z = int(isotope[3].split()[0])
    A = int(isotope[4].split()[0])
    # concatenate atomic number with decimal places, in μ amu
    # split by '#' to remove characters prior to float conversion
    # divide by 10e6, as AME2020 is in micro amu
    mass = (float(isotope[14].strip("# ")
           + isotope[15].strip("# ")))/10**6
    mass_str = isotope[14].strip("# ") + "." + (isotope[15].strip("# ")).replace(".", "")
    # build dictionary for specific isotope
    isotope_dictionary_list[name] = [Z, A, mass, mass_str]

# convert from dictionaries to DataFrame for max speed
ame2020_df = pd.DataFrame.from_dict(isotope_dictionary_list,
                                        orient='index')
ame2020_df.columns = ["Z", "A", "Mass", "Mass_str"]
```

Create a pandas DataFrame to hold NuBase atomic data, needed for the isomer excitation energy of the metastable states. Pull data from the associated file (`nubase_3.mas20.txt`).

```python
# read and parse NuBase file for isomer excitation energies
nubase_format = ff.FortranRecordReader('(a3,a1,a4,a3,a5,a1,a1,a13,a11,a12,a11,a2,a1,a1,a9,a2)')

with open('nubase_3.mas20.txt','r') as nubase:
    nubase_lines = nubase.readlines()

nubase_df = pd.DataFrame()
isotope_dictionary_list = {}
stable_dictionary_list = {}

for i in range(len(nubase_lines)-26):
    # starting from the first isotope on line 38, read each line seperately
    isotope = nubase_format.read(nubase_lines[i+26])
    # location in list  quantity        description
    # isotope[0]        AAA             Mass number
    #        [2]        ZZZi            Atomic number, i indicated isomer number
    #        [4]        ZZZAA           Nuclide name, mass number followed by element
    #        [5]        A               m,n (isomers); p,q (levels); r (resonance); i,j (IAS)
    #        [7]        ZZZZZZ.ZZZZZZZ  Mass excess in keV
    #        [8]        ZZZZ.ZZZZZZ     Mass excess uncertainty
    #        [9]        ZZZZZ.ZZZZZZ    Isomer excitation energy in keV
    #        [10]       ZZZZ.ZZZZZZ     Isomer excitation energy uncertainty
    #        [14]       ZZZZ.ZZZZ       Half life
    #        [15]       AA              Half life units
    #        [16]       ZZZ.ZZZ         Half life uncertainty

    meta_state = isotope[5].strip()
    # name is combination of element and number of nucleons
    element = isotope[4].strip(" 1234567890")
    A = isotope[0].lstrip("0")
    name = (element + "-" + A + meta_state)
    # split by '#' to remove characters prior to float conversion
    # convert isomer excitation energy to amu if nonzero
    isomer_excitation_energy = (
        float((isotope[9].strip()).split("#")[0]) / amu
        if isotope[9].strip(" non-exist") != ""
        else 0
        )
    # convert half-life to seconds
    half_life_raw = isotope[14].strip(" ~<>#").strip()
    # only include nuclides with half-life information
    if half_life_raw not in ["", "stbl", "p-unst"]:
        half_life = float(half_life_raw)
        half_life_units = isotope[15].strip()
        half_life_s = convert_half_life(half_life, half_life_units)
        # build dictionary for specific isotope
        isotope_dictionary_list[name] = [element, int(A), isomer_excitation_energy,
                                         half_life, half_life_units, half_life_s]
    elif name == "Ta-180m":
        # only stable isomer (no half life data, leave blank)
        isotope_dictionary_list["Ta-180m"] = ["Ta", 180, isomer_excitation_energy,
                                              np.nan, np.nan, np.nan]
    elif half_life_raw == "stbl":
        stable_dictionary_list[name] = [element, int(A), isomer_excitation_energy,
                                             np.nan, np.nan, np.nan]

# convert from dictionaries to DataFrame for max speed
nubase_df = pd.DataFrame.from_dict(isotope_dictionary_list, orient='index')
nubase_stable_df = pd.DataFrame.from_dict(stable_dictionary_list, orient='index')
nubase_df.columns = ["Element", "A", "Isomer_excitation_energy", "Half_life",
                     "Half_life_units", "Half_life_s"]
nubase_stable_df.columns = ["Element", "A", "Isomer_excitation_energy", "Half_life",
                     "Half_life_units", "Half_life_s"]
```

Define a function to compare half-lives accross matching isotopes in NuBase to pull correct isomer excitation energy data.

```python
def excitation_energy(name, elem, A, hl, compare_data):
    # build dataframe of matching isotope and its isomers
    compare_df = compare_data[((compare_data["Element"] == elem)
                                   & (compare_data["A"] == A))]
    # define name overrides, these instances are assigned erroneous data by
    # excitation_energy() otherwise (ref: radioactivedecay PR #33)
    overrides = {"Te-123": 0.0,
                 "Ra-219": compare_data.loc["Ra-219"].Isomer_excitation_energy,
                 "Pb-195m": compare_data.loc["Pb-195m"].Isomer_excitation_energy}
    if name in overrides.keys():
        isomer_excitation_energy = overrides[name]
    else:
        # pull isomer excitation energy of dataframe entry with closest matching half-life
        differences = [abs(hl-compare_hl) for compare_hl in compare_df["Half_life_s"]]
        min_index = differences.index(min(differences))
        isomer_excitation_energy = compare_df.iloc[min_index].Isomer_excitation_energy
    return isomer_excitation_energy
```

Define a function to convert a string representation of a number to a SymPy Rational. Hacky, but faster than using nsimplify().

```python
def to_rational(number):
    """
    Converts string representation of a number to a SymPy object.
    """

    if 'e' in number or 'E' in number:
        if 'e' in number:
            end = number.split('e')[1]
            number = number.split('e')[0]
        else:
            end = number.split('E')[1]
            number = number.split('E')[0]
        parts = number.split('.')
        if len(parts) == 1: parts.append('')
        if end[0] == '+':
            multiply = 1
            factor = S(10**int(end.lstrip('+')))
        else:
            multiply = 0
            factor = S(10**int(end.lstrip('-')))
        denom = S(10**len(parts[1]))
        parts[0] = parts[0].lstrip('0')
        if len(parts[0]) == 0: parts[1] = parts[1].lstrip('0')
        if multiply == 1:
            return S(parts[0]+parts[1])*factor/denom
        else: return S(parts[0]+parts[1])/(denom*factor)
    parts = number.split('.')
    if len(parts) == 1: parts.append('')
    denom = S(10**len(parts[1]))
    parts[0] = parts[0].lstrip('0')
    if len(parts[0]) == 0: parts[1] = parts[1].lstrip('0')
    return S(parts[0]+parts[1])/denom
```

Prepare a pandas DataFrame for the ICRP-107 decay data.

```python
icrp_col_names = ["Radionuclide", "Element", "Z", "A", "Metastable_state",
                  "Atomic_mass", "Atomic_mass_sympy", "Half_life", "Half_life_units",
                  "Half_life_s", "Half_life_readable","Num_decay_modes",
                  "Modes", "Branching_fractions", "Progeny"]
icrp = pd.DataFrame(columns=icrp_col_names)
```

Read data from ICRP-107 file (`ICRP-07.NDX`) line by line into the DataFrame. For atomic masses of metastable states, pull isomer excitation energy from NuBase dataframe.

```python
file_NDX = open("ICRP-07.NDX", "r", encoding="ISO-8859-1")
file_NDX.readline()

# fortran format of data in the ICRP-107 Index File (Table 1 footnote, ICRP107)
ffline = ff.FortranRecordReader("(a7,a8,a2,a8,3i7,i6,1x,3(a7,i6,e11.0,1x),a7,i6,e11.0,f7.0,2f8.0,3i4,i5,i4,e11.0,e10.0,e9.0)")
rows = []
for i in range(0, num_nuclides):
    line = ffline.read(file_NDX.readline())
    line = [i.strip() if isinstance(i,str) else i for i in line]
    name = line[0]
    add = {"Radionuclide": name}
    add["Element"] = add["Radionuclide"].split("-")[0]
    add["Z"], add["A"] = get_Z_A(add["Radionuclide"])
    add["Half_life"] = line[1]
    unit = line[2]
    if unit == "us":
        unit = "μs"
    add["Half_life_units"] = unit
    add["Half_life_s"] = convert_half_life(line[1], unit)
    add["Half_life_readable"] = readable_half_life(line[1], unit)
    if add["Radionuclide"][-1].isalpha():
        add["Metastable_state"] = add["Radionuclide"][-1]
    ame_mass = ame2020_df.loc[name.strip("nm")]["Mass"]
    # set mass to AME2020 data plus isomer excitation energy matched from nubase
    isomer_excitation_energy = excitation_energy(name, add["Element"], add["A"],
                                                 add["Half_life_s"], nubase_df)
    add["Atomic_mass"] = ame_mass + isomer_excitation_energy
    ame_mass_str = ame2020_df.loc[name.strip("nm")]["Mass_str"]
    add["Atomic_mass_sympy"] = to_rational(ame_mass_str) + nsimplify(isomer_excitation_energy)
    # parse decay modes and progeny
    modes = re.findall(r"(A|B\-|ECB\+|EC|IT|SF)",line[3])
    j=0
    add["Modes"] = []
    add["Branching_fractions"] = []
    add["Progeny"] = []
    while j < 4 and line[8+j*3] != "":
        add["Branching_fractions"].append(str(line[8+2+j*3]))
        add["Progeny"].append(line[8+j*3])
        if add["Progeny"][-1] == "SF":
            add["Modes"].append("SF")
        else:
            Z, A = get_Z_A(add["Progeny"][-1])
            if add["Z"] == Z and add["A"] == A:
                add["Modes"].append("IT")
            elif add["Z"] - 2 == Z and add["A"] - 4 == A:
                add["Modes"].append("α")
            elif add["Z"] + 1 == Z and add["A"] == A:
                add["Modes"].append("β-")
            elif add["Z"] - 1 == Z and add["A"] == A:
                if "EC" in modes:
                    add["Modes"].append("EC")
                else:
                    add["Modes"].append("β+ & EC")
        j += 1
    add["Num_decay_modes"] = j

    rows.append(add)
icrp = icrp.append(rows, ignore_index=True)
file_NDX.close()
```

Add stable progeny to dataframe, using mass data from the AME2020 database. Set their half life to infinity, and their number of decay modes to 0.

```python
stable_dict = {}
stable_df = pd.DataFrame()
# ignore empty data, SF (spontanious fission)
stable_name_list = ["","SF"]
sf_key = 0
progeny_list = list(itertools.chain(*list(icrp.Progeny)))
stable_nonprogeny_list = [i for i in nubase_stable_df.index if i not in progeny_list]
stable_list = progeny_list + stable_nonprogeny_list

for n,i in enumerate(stable_list):
    if (i not in list(icrp.Radionuclide) and
        i not in stable_name_list and str(i) != "nan"):

        stable_progeny = []
        name = i

        if name == "Ta-180m": # only metastable stable nuclide
            # pull catagorical data from ame2020, isomer excitation energy from nubase
            ame_info = ame2020_df.loc["Ta-180"]
            stable_progeny  = [i, i.split("-")[0], int(ame_info["Z"]),
                               int(ame_info["A"]), "m", ame_info["Mass"]
                               + nubase_df.loc["Ta-180m"].Isomer_excitation_energy,
                               to_rational(ame_info["Mass_str"])
                               + nsimplify(nubase_df.loc["Ta-180m"].Isomer_excitation_energy)]
        else:
            # for elements, set element, Z, A, and mass
            ame_info = ame2020_df.loc[name]
            stable_progeny  = [i, i.split("-")[0], int(ame_info["Z"]),
                               int(ame_info["A"]), "", ame_info["Mass"], to_rational(ame_info["Mass_str"])]

        # add infinite half-life and stable description
        stable_progeny.append(np.inf)
        stable_progeny.append("s")
        stable_progeny.append(np.inf)
        stable_progeny.append("stable")
        stable_progeny.append(0)
        # set all progeny and progeny modes to NaN
        for m in range(3):
            stable_progeny.append([])

        # add new isotope to dictionary
        stable_dict[n] = stable_progeny
        stable_name_list.append(i)

#form dataframe of stable progeny from dictionary
stable_df = pd.DataFrame.from_dict(stable_dict, orient='index')
stable_df.columns = icrp_col_names

# add stable isotopes to icrp DataFrame
icrp = icrp.append(stable_df, ignore_index=True)
```

Remove NaN values, set DataFrame index to the radionuclide string, and check completed DataFrame. Export completed DataFrame to CSV file for analysis elsewhere.

```python
icrp = icrp.replace(np.nan, "", regex=True)
icrp.set_index("Radionuclide", inplace=True)
icrp.to_csv("icrp.csv", index=True)
icrp.head()
```

      Element
      Z
      A
      Metastable_state
      Atomic_mass
      Atomic_mass_sympy
      Half_life
      Half_life_units
      Half_life_s
      Half_life_readable
      Num_decay_modes
      Modes
      Branching_fractions
      Progeny

      Radionuclide

      Ac-223
      Ac
      89
      223

      223.019136
      111509567991/500000000
      2.10
      m
      1.260000e+02
      2.10 m
      1
      [α]
      [0.99]
      [Fr-219]

      Ac-224
      Ac
      89
      224

      224.021722
      224021722249/1000000000
      2.78
      h
      1.000800e+04
      2.78 h
      2
      [EC, α]
      [0.909, 0.091]
      [Ra-224, Fr-220]

      Ac-225
      Ac
      89
      225

      225.023229
      225023228601/1000000000
      10.0
      d
      8.640000e+05
      10.0 d
      1
      [α]
      [1.0]
      [Fr-221]

      Ac-226
      Ac
      89
      226

      226.026097
      226026096999/1000000000
      29.37
      h
      1.057320e+05
      29.37 h
      3
      [β-, EC, α]
      [0.83, 0.17, 6e-05]
      [Th-226, Ra-226, Fr-222]

      Ac-227
      Ac
      89
      227

      227.027751
      113513875297/500000000
      21.772
      y
      6.870574e+08
      21.772 y
      2
      [β-, α]
      [0.9862, 0.0138]
      [Th-227, Fr-223]

### Order ICRP DataFrame so progeny always come below their parent
The radionuclides need to be ordered so that the progeny (daughters) are always lower in the DataFrame than their parent. This is so the subsequent matrices that we create are lower triangular.

To achieve this we first count how many times each radioactive decay process occurs in the ICRP-107 dataset.

```python
modes = pd.Series(np.concatenate(icrp.Modes))
print("β+ or electron capture:", modes.value_counts()["β+ & EC"]
      + modes.value_counts()["EC"])
print("β-:", modes.value_counts()["β-"])
print("α:", modes.value_counts()["α"])
print("Isomeric Transition (IT):", modes.value_counts()["IT"])
print("Spontaneous Fission (SF):", (modes.value_counts()["SF"]/2).astype(np.int64))
```

    β+ or electron capture: 684
    β-: 539
    α: 183
    Isomeric Transition (IT): 178
    Spontaneous Fission (SF): 14

The outcomes of these decay processes are as follows:
- β+ or electron capture (EC): $\mathrm{^{A}_{Z}X} \rightarrow \mathrm{^{A}_{Z-1}Y}$
- β- decay: $\mathrm{^{A}_{Z}X} \rightarrow \mathrm{^{A}_{Z+1}Y}$
- α decay: $\mathrm{^{A}_{Z}X} \rightarrow \mathrm{^{A-4}_{Z-2}Y}$
- IT decay: $\mathrm{^{Am}_{Z}X} \rightarrow \mathrm{^{A}_{Z}X}$ or $\mathrm{^{An}_{Z}X} \rightarrow \mathrm{^{A}_{Z}X}$
- SF decay: The ICRP-107 dataset does not contain data for the outcomes (progeny) from spontaneous fission decays

We order by decreasing mass number (A), followed by decreasing atomic number (Z) (as there are more Beta+ and EC decays than Beta- decays), then by decreasing isomer index (n, m, ground state).

```python
icrp.sort_values(by=["A", "Z", "Metastable_state"], inplace=True, ascending=[False, False, False])
icrp.head()
```

      Element
      Z
      A
      Metastable_state
      Atomic_mass
      Atomic_mass_sympy
      Half_life
      Half_life_units
      Half_life_s
      Half_life_readable
      Num_decay_modes
      Modes
      Branching_fractions
      Progeny

      Radionuclide

      Fm-257
      Fm
      100
      257

      257.095105
      257095105419/1000000000
      100.5
      d
      8683200.0
      100.5 d
      2
      [α, SF]
      [0.9979, 0.0021]
      [Cf-253, SF]

      Fm-256
      Fm
      100
      256

      256.091772
      256091771699/1000000000
      157.6
      m
      9456.0
      157.6 m
      2
      [α, SF]
      [0.081, 0.919]
      [Cf-252, SF]

      Es-256
      Es
      99
      256

      256.093597
      256093597/1000000
      25.4
      m
      1524.0
      25.4 m
      1
      [β-]
      [1.0]
      [Fm-256]

      Fm-255
      Fm
      100
      255

      255.089963
      51017992699/200000000
      20.07
      h
      72252.0
      20.07 h
      2
      [α, SF]
      [1.0, 2.3e-07]
      [Cf-251, SF]

      Es-255
      Es
      99
      255

      255.090274
      7971571047/31250000
      39.8
      d
      3438720.0
      39.8 d
      3
      [β-, α, SF]
      [0.92, 0.08, 4.5e-05]
      [Fm-255, Bk-251, SF]

Now it is necessary to correct the positions of the remaining radionuclides that are still incorrectly ordered. This is achieved by looping over all the radionuclides in the table, and checking if their progeny are located lower in the table or not. If not, the parent and progeny row positions are switched. This takes a few passes until all progeny are correctly located below their parents.

```python
nuclide_list = list(icrp.index)
swapping = 1
while swapping >= 1:
    swaps = 0
    for parent in nuclide_list:
        for progeny, mode, bf in zip(icrp.at[parent, "Progeny"],
                                     icrp.at[parent, "Modes"],
                                     icrp.at[parent, "Branching_fractions"]):
            if (icrp.at[parent, "Num_decay_modes"] == 0): continue
            if (progeny not in nuclide_list): continue
            j = nuclide_list.index(parent)
            k = nuclide_list.index(progeny)
            if  j > k:
                nuclide_list[j], nuclide_list[k] = nuclide_list[k], nuclide_list[j]
                icrp = icrp.reindex(index=nuclide_list)
                swaps +=1
    print("Iteration", swapping, "number of swaps:", swaps)
    swapping += 1
    if swaps == 0: swapping = 0
```

    Iteration 1 number of swaps: 516
    Iteration 2 number of swaps: 201
    Iteration 3 number of swaps: 54
    Iteration 4 number of swaps: 17
    Iteration 5 number of swaps: 3
    Iteration 6 number of swaps: 0

The sorted DataFrame looks like this. Note this is just one of many possible solutions for sorting the DataFrame.

```python
icrp.head()
```

      Element
      Z
      A
      Metastable_state
      Atomic_mass
      Atomic_mass_sympy
      Half_life
      Half_life_units
      Half_life_s
      Half_life_readable
      Num_decay_modes
      Modes
      Branching_fractions
      Progeny

      Radionuclide

      Fm-257
      Fm
      100
      257

      257.095105
      257095105419/1000000000
      100.5
      d
      8683200.0
      100.5 d
      2
      [α, SF]
      [0.9979, 0.0021]
      [Cf-253, SF]

      Es-256
      Es
      99
      256

      256.093597
      256093597/1000000
      25.4
      m
      1524.0
      25.4 m
      1
      [β-]
      [1.0]
      [Fm-256]

      Fm-256
      Fm
      100
      256

      256.091772
      256091771699/1000000000
      157.6
      m
      9456.0
      157.6 m
      2
      [α, SF]
      [0.081, 0.919]
      [Cf-252, SF]

      Cf-255
      Cf
      98
      255

      255.091046
      127545523/500000
      85
      m
      5100.0
      85 m
      1
      [β-]
      [1.0]
      [Es-255]

      Es-255
      Es
      99
      255

      255.090274
      7971571047/31250000
      39.8
      d
      3438720.0
      39.8 d
      3
      [β-, α, SF]
      [0.92, 0.08, 4.5e-05]
      [Fm-255, Bk-251, SF]

### Make the *Λ* matrix
Now we make the sparse lower triangular matrix *Λ*, which captures the decay pathways and branching relations between the radionuclides. _Λ_ is set up based on Eq. (6) in Amaku et al. (2010). The diagonal elements are all *-λjj*, i.e. negative decay constant for each radionuclide. The off-diagonal elements are all of the form *BFij×λjj* for *i* > *j*, where *BFij* is the branching fraction from parent *j* to progeny *i*. The non-zero elements beneath the *jj*th element in each column are first progeny of radionuclide *j*.

```python
rows = np.array([], dtype=np.int64)
cols = np.array([], dtype=np.int64)
data = np.array([], dtype=np.float64)
ln2 = np.log(2)

for parent in nuclide_list:
    j = nuclide_list.index(parent)
    rows = np.append(rows, [j])
    cols = np.append(cols, [j])
    half_life = icrp.at[parent, "Half_life_s"]
    if half_life == np.inf:
        lambd = 0.0
    else:
        lambd = ln2/half_life
    data = np.append(data, -lambd)
    for progeny, mode, bf in zip(icrp.at[parent, "Progeny"],
                                 icrp.at[parent, "Modes"],
                                 icrp.at[parent, "Branching_fractions"]):
        if (mode in ["stable", "SF"]): continue
        if (progeny not in nuclide_list): continue
        i = nuclide_list.index(progeny)
        rows = np.append(rows, [i])
        cols = np.append(cols, [j])
        data = np.append(data, [lambd*float(bf)])

lambda_mat = sparse.csc_matrix((data, (rows, cols)))
```

### Calculate the matrices *C* and _C-1_
We now need to make the sparse matrices *C* and *C-1*, which are given by Eqs. (10) and (13) in Amaku et al. (2010), respectively. The diagonal elements of both matrices are 1. *C* and *C-1* differ from *Λ* in that there are non-zero elements beneath the *jj*th element in each column for all progeny of *j*, i.e. everything in its full decay chain, not just the immediate daughters.

Therefore we have to find all the progeny in the decay chain of each radionuclide. We do this by looping backwards over each column in *Λ* to build up lists of the radionuclides in the decay chain of each parent. We then set up the basic structure (i.e. define the non-zero elements) of sparse matrices *C* and *C-1*.

```python
num_nuclides = len(nuclide_list)
# dictionary of decay chain members by index
rows_dict = {}
for i in range(num_nuclides-1, -1, -1):
    a,_ = lambda_mat[:,i].nonzero()
    b = a
    for j in a:
        if j > i:
            b = np.unique(np.concatenate((b,rows_dict[j])))
    rows_dict[i] = b

rows_c = np.array([], dtype=np.int32)
cols_c = np.array([], dtype=np.int32)
for i in range(0, num_nuclides):
    # row of C initialized as array of decay chain members
    rows_c = np.concatenate((rows_c,rows_dict[i]))
    # column of C initialized as array of index with length of chain size
    cols_c = np.concatenate((cols_c,np.array([i]*len(rows_dict[i]), dtype=np.int32)))

# initialize c and c^(-1) as sparse csc matrices with ones on diagonal
c = sparse.eye(num_nuclides, num_nuclides, dtype=np.float64, format="csc")
c_inv = sparse.eye(num_nuclides, num_nuclides, dtype=np.float64, format="csc")
```

Now calculate *C* and *C-1*. Note that only the non-zero elements of *Ckj* and *C-1kj*  need to be considered for the sums in Eqs. (10) and (13) of Amaku et al. (2010).

Calculate *C*, and highlight any cases where the relative difference of the decay constants of two radionuclides in the same decay chain is less than 0.001 (as this could lead to numerical precision issues):

```python
for index in range(0, rows_c.size):
    i = rows_c[index]
    j = cols_c[index]
    if i == j: continue
    sigma = 0.0
    for k in rows_dict[j]:
        if k == i: break
        sigma += lambda_mat[i,k]*c[k,j]
    c[i,j] = sigma/(lambda_mat[j,j]-lambda_mat[i,i])

    if abs((lambda_mat[j,j]-lambda_mat[i,i])/lambda_mat[j,j]) < 1E-3: print(nuclide_list[i], nuclide_list[j])
```

    /Users/lemointm/opt/anaconda3/lib/python3.9/site-packages/scipy/sparse/_index.py:82: SparseEfficiencyWarning: Changing the sparsity structure of a csc_matrix is expensive. lil_matrix is more efficient.
      self._set_intXint(row, col, x.flat[0])

There are no cases where radionuclides in the same decay chain have decay constants that are too similar (for the radionuclides in ICRP-107). Now proceed to calculate *C-1*:

```python
for index in range(0, rows_c.size):
    i = rows_c[index]
    j = cols_c[index]
    if i == j: continue
    sigma = 0.0
    for k in rows_dict[j]:
        if k == i: break
        sigma -= c[i,k]*c_inv[k,j]
    c_inv[i,j] = sigma
```

### Calculate SymPy versions of the matrices for arbitrary-precision calculations
We now calculate SymPy versions of *C* and *C-1* for arbitrary-precision calculations. First define some functions for processing the data into SymPy objects:

```python
year_sympy = S(3652422)/10000

def convert_half_life_sympy(halflife, unit):
    """
    Conversion of SymPy half-life into seconds.
    """

    units = {'μs':S(1)/1000000,
             'ms':S(1)/1000,
             's':S(1),
             'm':S(60),
             'h':S(3600),
             'd':S(86400),
             'y':S(86400)*year_sympy
            }
    return halflife*units[unit]
```

Now make a SymPy version of the *Λ* matrix:

```python
lambda_mat_sympy = SparseMatrix.zeros(num_nuclides, num_nuclides)
lambdas_sympy = Matrix.zeros(num_nuclides, 1)
masses_sympy = Matrix.zeros(num_nuclides, 1)

for parent in nuclide_list:
    j = nuclide_list.index(parent)
    if icrp.at[parent, 'Half_life'] == np.inf:
        lambd = Integer(0)
        lambda_mat_sympy[j, j] = Integer(0)
        lambdas_sympy[j] = Integer(0)
    else:
        hl_sympy = convert_half_life_sympy(to_rational(icrp.at[parent, 'Half_life']),
                                        icrp.at[parent, 'Half_life_units'])
        lambd = log(2)/hl_sympy
        lambda_mat_sympy[j, j] = -lambd
        lambdas_sympy[j] = lambd
    for progeny, mode, bf in zip(icrp.at[parent, "Progeny"],
                                 icrp.at[parent, "Modes"],
                                 icrp.at[parent, "Branching_fractions"]):
        if (mode in ['stable', 'SF']): continue
        if (progeny not in nuclide_list): continue
        i = nuclide_list.index(progeny)
        lambda_mat_sympy[i, j] = lambd*to_rational(bf)
    masses_sympy[j] = icrp.at[parent, 'Atomic_mass_sympy']
```

Now make a SymPy version of the *C* and *C-1* matrix:

```python
c_sympy = SparseMatrix.eye(num_nuclides)
c_inv_sympy = SparseMatrix.eye(num_nuclides)

for index in range(0, rows_c.size):
    i = rows_c[index]
    j = cols_c[index]
    if i == j: continue
    sigma = Integer(0)
    for k in rows_dict[j]:
        if k == i: break
        sigma += lambda_mat_sympy[i,k]*c_sympy[k,j]
    c_sympy[i,j] = sigma/(lambda_mat_sympy[j,j]-lambda_mat_sympy[i,i])

for index in range(0, rows_c.size):
    i = rows_c[index]
    j = cols_c[index]
    if i == j: continue
    sigma = Integer(0)
    for k in rows_dict[j]:
        if k == i: break
        sigma -= c_sympy[i,k]*c_inv_sympy[k,j]
    c_inv_sympy[i,j] = sigma
```

### Save the outputs
Write out file containing NumPy arrays with the radionuclide names, the half-lives, their time units, human-readable half-life strings, the progeny, branching fractions and decay modes of each radionuclide, the atomic masses of both unstable and stable nuclides, and the days to year conversion number. Write output files containing *C* and *C-1* in the SciPy and SymPy sparse formats. Write out the SymPy versions of the decay constants.

```python
hldata = np.array([(np.float64(a),b,c) for a, b, c in zip(icrp["Half_life"], icrp["Half_life_units"], icrp["Half_life_readable"])], dtype=object)

progeny = [[] for _ in range(0, len(nuclide_list))]
bfs = [[] for _ in range(0, len(nuclide_list))]
modes = [[] for _ in range(0, len(nuclide_list))]
for i in range(0, len(nuclide_list)):
    prog, bf, mode = [], [], []
    for d in range(0, icrp.at[nuclide_list[i], "Num_decay_modes"]):
        prog.append(icrp.at[nuclide_list[i], "Progeny"][d])
        bf.append(float(icrp.at[nuclide_list[i], "Branching_fractions"][d]))
        mode.append(icrp.at[nuclide_list[i], "Modes"][d])
    if len(mode) > 0:
        # sort by decreasing branching fraction
        triple = list(zip(bf, prog, mode))
        triple.sort(key=lambda t: (-t[0], t[1], t[2]))
        bfs[i], progeny[i], modes[i] = map(list, zip(*triple))

np.savez_compressed("./decay_data.npz", nuclides=np.asarray(nuclide_list),
                    masses=icrp["Atomic_mass"].values,
                    hldata=hldata, progeny=np.asarray(progeny, dtype=object),
                    bfs=np.asarray(bfs, dtype=object), modes=np.asarray(modes, dtype=object),
                    year_conv=np.float64(year))

# Write out SciPy sparse matrices (convert to CSR format)
sparse.save_npz("./c_scipy.npz", c.tocsr())
sparse.save_npz("./c_inv_scipy.npz", c_inv.tocsr())

import pkg_resources, sympy
if pkg_resources.parse_version(sympy.__version__) >= pkg_resources.parse_version('1.9'):
    pickle_type = '1.9'
else:
    pickle_type = '1.8'

# Write out SymPy objects to pickle files
with open(f"c_sympy_{pickle_type}.pickle", "wb") as outfile:
    outfile.write(pickle.dumps(c_sympy))
with open(f"c_inv_sympy_{pickle_type}.pickle", "wb") as outfile:
    outfile.write(pickle.dumps(c_inv_sympy))
with open(f"atomic_masses_sympy_{pickle_type}.pickle", "wb") as outfile:
    outfile.write(pickle.dumps(masses_sympy))
with open(f"decay_consts_sympy_{pickle_type}.pickle", "wb") as outfile:
    outfile.write(pickle.dumps(lambdas_sympy))
with open(f"year_conversion_sympy_{pickle_type}.pickle", "wb") as outfile:
    outfile.write(pickle.dumps(year_sympy))
```


# Dynamical System Approximation
This notebook aims at learning a functional correlation based on given snapshots. The data is created through the following ODE which is called the Fermi Pasta model:
\begin{align}
\frac{d^2}{dt^2} x_i = (x_{i+1} - 2x_i + x_{i-1}) + 0.7((x_{i+1} - x_i)^3 - (x_i-x_{i-1})^3) + \mathcal{N}(0,\sigma)
\end{align}

```python
import numpy as np
import xerus
import matplotlib.pyplot as plt
from matplotlib.colors import LogNorm
import time
from itertools import chain
import helpers as hp
import pandas as pd

%precision  4
```

    '%.4f'

```python
#Construction of the exact solution in the choosen basis.
#Transform implements the basis transformation for given b
#For the exact solution we also factor out the kernel, by use of the Pseudoinverse.
def transform(X):
    M = np.zeros([4,4])
    M[0,0] = 1   #1
    M[1,1] = 1   #1
    M[2,2] = 1.5 #1.5
    M[3,3] = 2.5 #2.5
    M[0,2] = -0.5 #-0.5
    M[1,3] = -1.5 #-1.5
    t = xerus.Tensor.from_ndarray(np.linalg.inv(M))
    a1,a2,a3,a4,b1,b2,b3,b4 = xerus.indices(8)
    for eq in range(noo):
        tmp = X.get_component(eq)
        tmp2 = C2list[eq]
        tmp(a1,a2,a3,a4) <<  tmp(a1,b2,a3,a4)* t(a2,b2)
        X.set_component(eq,tmp)
    return X

def project(X):
    dim = ([(3 if i == 0 or i == noo -1 else 4) for i in range(0,noo)])
    dim.extend(dim)
    C2T = xerus.TTOperator(dim)
    for eq in range(noo):
        idx = [0 for i in range(noo)]
        if eq == 0:
            idx[0] = 2
            idx[1] = 3
        elif eq == noo -1:
            idx[noo-2] = 1
            idx[noo-1] = 1
        elif eq == noo -2:
            idx[eq-1] = 1
            idx[eq]   = 2
            idx[eq+1] = 2
        else:
            idx[eq-1] = 1
            idx[eq]   = 2
            idx[eq+1] = 3
        idx.extend(idx)
        C2T += xerus.TTOperator.dirac(dim,idx)
    C2T.round(1e-12)
    i1,i2,i3,i4,i5,i6,j1,j2,j3,j4,k1,k2,k3 = xerus.indices(13)

    X(i1^noo,j1^noo) << X(i1^noo,k1^noo) * C2T(k1^noo,j1^noo)
    X.round(1e-12)
    return X

def exact(noo,p):
    beta = 0.7
    C1ex = hp.construct_exact_fermit_pasta(noo,p,beta)
    C1ex = transform(C1ex)
    C1ex = project(C1ex)
    return C1ex

```

### Recovery algorithm
We want to recover the exact solution with the help of a regularized ALS.

```python
# initialize simulation
def initialize(p,noo):
    rank = 4 #fix rank
    dim = [p for i in range(0,noo)]
    dim.extend([4 for i in range(0,noo)])
    dim[noo] = 3
    dim[2*noo-1]=3
    C = xerus.TTOperator.random(dim,[rank for i in range(0,noo-1)]) # initalize randomly
    C.move_core(0,True)
    return C
```

```python
# We choose different pairstriples  of dimensions samplesizes and noise level to run the algoirthm for.
data_noo_nos = [(12,4500,1e-8),(12,4500,1e-7),(12,4500,1e-6),(12,4500,1e-5),(12,4500,1e-4)\
                ,(12,4500,1e-3),(12,4500,1e-2),(12,4500,1e-1),(12,4500,1)]
#                                                       pairs used in simulations in the paper
#                                                       uncomment to simulate but is computational intensive
data_noo_nos = [(12,4500,1e-2),(12,4500,1e-1),(12,4500,1)]
#data_noo_nos = [(8,3000)] #specify pairs to simulate for
#runs = 1 #specify number of runs for each pair (10 in the paper)
runs = 3
max_iter = 30 # specify number of sweeps
output = 'data.csv' # specify name of output file

# build data structure to store solution
tuples = []
for data in data_noo_nos:
    noo = data[0]
    nos = data[1]
    sigma = data[2]
    for r in range(0,runs):
        tuples.append((noo,nos,sigma, r))
index = pd.MultiIndex.from_tuples(tuples, names=['d', 'm','sigma','runs'])

# The results of each optimization is store in a Dataframe
col = ["data norm", "noise norm"]
col.extend([i for i in range(1,max_iter+1)])
df = pd.DataFrame(np.zeros([len(tuples),max_iter+2]), index=index,columns=col)
print(len(index))
df["data norm"]
```

    9

    d   m     sigma  runs
    12  4500  0.01   0       0.0
                     1       0.0
                     2       0.0
              0.10   0       0.0
                     1       0.0
                     2       0.0
              1.00   0       0.0
                     1       0.0
                     2       0.0
    Name: data norm, dtype: float64

```python
np.set_printoptions(precision=4)
#loop over all pairs of samples, calls hp.run_als for the solution
lam = 1 #regularization parameter
#Master iteration
psi = hp.basis(0) # get basis functions, Legendre
p = len(psi)
for data in data_noo_nos:
    noo = data[0]
    nos = data[1]
    sigma = data[2]
    print( "(noo,nos,sigma) = (" + str(noo) +',' + str(nos) +',' + str(sigma) + ')' )
    C2list = hp.build_choice_tensor2(noo) # build selection tensor as list of pxnos matrices
    C1ex = exact(noo,p) # construct exact solution
    print("C1ex frob_norm: " +str(C1ex.frob_norm()))
    for r in range(runs):
        [x, y] = hp.fermi_pasta_ulam(noo, nos) # create samples and labels
        df["data norm"].loc[(noo,nos,sigma,r)] = np.linalg.norm(y)
        noise = np.random.normal(0,sigma,size=y.shape)
        df["noise norm"].loc[(noo,nos,sigma,r)] = np.linalg.norm(noise)
        y = y + noise
        Alist = hp.build_data_tensor_list2(noo,x,nos,psi,p) # build the dictionary tensor for the given samples x
        Y = xerus.Tensor.from_ndarray(y)
        C1 = initialize(p,noo) # initialize the als randomly
        errors = hp.run_als(noo,nos,C1,C2list,Alist,C1ex,Y,max_iter,lam) # run the regularized ALS iteration
        #post processing, store data in dataframe
        for i in range(1,len(errors)):
            df[i].loc[(noo,nos,sigma,r)] = errors[i-1]

        print("Run: " +str(r) + " finished result = " + str(errors) + " data norm = " + str( df["data norm"].loc[(noo,nos,sigma,r)])+ " noise norm = " + str( df["noise norm"].loc[(noo,nos,sigma,r)]))
        df.to_csv(output)
```

    L2(-1,1) orthogonal polynomials
    (noo,nos,sigma) = (12,4500,0.01)
    C1ex frob_norm: 19.885773809434728

```python

```

```python

```