# Solving a mixed Neumann-Dirichlet Problem

### Background

With Bempp, it is possible to define operators only on segments of a given domain. This makes it possible to solve mixed Neumann-Dirichlet problems. In this tutorial, we solve the Laplace equation inside the unit cube with unit Dirichlet boundary conditions on two sides and unit Neumann boundary conditions on the other four sides.

Denote by $\Gamma_D$ the part of the boundary that holds the Dirichlet boundary conditions and by $\Gamma_N$ the boundary part that holds the Neumann boundary conditions. We denote by $t\in\Gamma_D$ the unknown Neumann data and by $u\in\Gamma_N$ the unknown Dirichlet data. The given Dirichlet data on $\Gamma_D$ is denoted by $g_D$ and the given Neumann data on $\Gamma_N$ is denoted by $g_N$.

From Green's representation theorem it follows that
$$
\begin{align}
\left[\mathsf{V}t\right] (\mathbf{x}) - \left[\mathsf{K}u\right] (\mathbf{x}) &= \left[\tfrac{1}{2}\mathsf{Id} + \mathsf{K}\right]g_D(\mathbf{x}) - \mathsf{V}g_N(\mathbf{x}),\quad \mathbf{x}\in\Gamma_D\\
\left[\mathsf{W}u\right] (\mathbf{x}) + \left[\mathsf{K}'t\right] (\mathbf{x}) &=\left[\tfrac{1}{2}\mathsf{Id} - \mathsf{K}'\right]g_N(\mathbf{x}) - \mathsf{W}g_D(\mathbf{x}),\quad \mathbf{x}\in\Gamma_N
\end{align}
$$
Here (as usual) $\mathsf{V}$, $\mathsf{K}$, $\mathsf{K}'$, $\mathsf{W}$ are the single layer, double layer, adjoint double layer and hypersingular boundary operators.

The difficulty in the implementation is the definition of the discrete function spaces and the treatment of degrees of freedom (dofs) that lie on the interface between $\Gamma_N$ and $\Gamma_D$. In the following, we will go through the implementation and point out how to correctly define all spaces involved.

### Implementation

We start with the usual imports. In addition we increase the integration order, as in this example we will be working with spaces of quadratic functions.

```python
import bempp.api
import numpy as np

bempp.api.global_parameters.quadrature.medium.double_order = 4
bempp.api.global_parameters.quadrature.far.double_order = 4
```

We now define the domain. We use a standard unit cube. In the corresponding function all sides of the cube are already associated with different domain indices. We associate the indices 1 and 3 with the Dirichlet boundary and the other indices with the neumann boundary.

```python
grid = bempp.api.shapes.cube()
dirichlet_segments = [1, 3]
neumann_segments = [2, 4, 5, 6]
```

We can now define the spaces. For the Neumann data, we use discontinuous polynomial basis functions of order 1. For the Dirichlet data, we use continuous basis functions of local polynomial order 2.

We need global spaces for the Dirichlet and Neumann data and suitable spaces on the segments. The space definitions are as follows:

* The ``neumann_space_dirichlet_segment`` space holds the unknown Neumann data $t$ on $\Gamma_D$. For $\Gamma_D$ we use the parameter ``closed=True``, meaning that all boundary edges and the associated dofs on the boundary edges are part of the space. The parameter ``element_on_segment=True`` implies that we restrict functions to elements that lie on elements associated with $\Gamma_D$. This is important for dofs on boundary edges and excludes associated functions that lie just outside $\Gamma_D$ on the other side of the boundary edge.

* The ``neumann_space_neumann_segment`` space is defined on $\Gamma_N$. $\Gamma_N$ is open: the boundary edges are not part of the space. We again restrict basis functions to $\Gamma_N$ by the parameter ``element_on_segment=True``. However, we also include all functions which are defined on elements of the space but whose reference points (i.e. the dof positions) are on the excluded boundary. This is achieved by the parameter ``reference_point_on_segment=False``. If it were set to ``True`` (default) it would only include dofs whose reference points lie in the segment and not on the excluded boundary.

* The ``dirichlet_space_dirichlet_segment`` space is a space of continuous basis functions that holds the Dirichlet data on $\Gamma_D$. The space is closed and by default basis functions are allowed to extend into the elements adjacent to $\Gamma_D$. This extension is necessary because of the definition of the underlying Sobolev space on the segment. To control this behavior for continuous spaces the option ``strictly_on_segment`` exists, which is by default set to ``False``.

* The ``dirichlet_space_neumann_segment`` is defined similarly to the ``dirichlet_space_dirichlet_segment`` but on the open segment $\Gamma_N$.

* For the discretisation of the Dirichlet data, we also need the space ``dual_dirichlet_space``. This is the correct dual space for projecting functions into the space of Dirichlet data.

```python
order_neumann = 1
order_dirichlet = 2

global_neumann_space = bempp.api.function_space(grid, "DP", order_neumann)
global_dirichlet_space = bempp.api.function_space(grid, "P", order_dirichlet)

neumann_space_dirichlet_segment = bempp.api.function_space(
    grid, "DP", order_neumann, domains=dirichlet_segments,
    closed=True, element_on_segment=True)

neumann_space_neumann_segment = bempp.api.function_space(
    grid, "DP", order_neumann, domains=neumann_segments,
    closed=False, element_on_segment=True, reference_point_on_segment=False)

dirichlet_space_dirichlet_segment = bempp.api.function_space(
    grid, "P", order_dirichlet, domains=dirichlet_segments, closed=True)

dirichlet_space_neumann_segment = bempp.api.function_space(
    grid, "P", order_dirichlet, domains=neumann_segments, closed=False)

dual_dirichlet_space = bempp.api.function_space(
    grid, "P", order_dirichlet, domains=dirichlet_segments,
    closed=True, strictly_on_segment=True)
```

In the following, we define all operators on the corresponding spaces and the overall blocked operator.

```python
slp_DD = bempp.api.operators.boundary.laplace.single_layer(
    neumann_space_dirichlet_segment,
    dirichlet_space_dirichlet_segment,
    neumann_space_dirichlet_segment)

dlp_DN = bempp.api.operators.boundary.laplace.double_layer(
    dirichlet_space_neumann_segment,
    dirichlet_space_dirichlet_segment,
    neumann_space_dirichlet_segment)

adlp_ND = bempp.api.operators.boundary.laplace.adjoint_double_layer(
    neumann_space_dirichlet_segment,
    neumann_space_neumann_segment,
    dirichlet_space_neumann_segment)

hyp_NN = bempp.api.operators.boundary.laplace.hypersingular(
    dirichlet_space_neumann_segment,
    neumann_space_neumann_segment,
    dirichlet_space_neumann_segment)

slp_DN = bempp.api.operators.boundary.laplace.single_layer(
    neumann_space_neumann_segment,
    dirichlet_space_dirichlet_segment,
    neumann_space_dirichlet_segment)

dlp_DD = bempp.api.operators.boundary.laplace.double_layer(
    dirichlet_space_dirichlet_segment,
    dirichlet_space_dirichlet_segment,
    neumann_space_dirichlet_segment)

id_DD = bempp.api.operators.boundary.sparse.identity(
    dirichlet_space_dirichlet_segment,
    dirichlet_space_dirichlet_segment,
    neumann_space_dirichlet_segment)

adlp_NN = bempp.api.operators.boundary.laplace.adjoint_double_layer(
    neumann_space_neumann_segment,
    neumann_space_neumann_segment,
    dirichlet_space_neumann_segment)

id_NN = bempp.api.operators.boundary.sparse.identity(
    neumann_space_neumann_segment,
    neumann_space_neumann_segment,
    dirichlet_space_neumann_segment)

hyp_ND = bempp.api.operators.boundary.laplace.hypersingular(
    dirichlet_space_dirichlet_segment,
    neumann_space_neumann_segment,
    dirichlet_space_neumann_segment)

blocked = bempp.api.BlockedOperator(2, 2)

blocked[0, 0] = slp_DD
blocked[0, 1] = -dlp_DN
blocked[1, 0] = adlp_ND
blocked[1, 1] = hyp_NN
```

Next, we define the functions of the Dirichlet and Neumann data and their discretisations on the corresponding segments.

```python
def dirichlet_data_fun(x):
    return 1

def dirichlet_data(x, n, domain_index, res):
    res[0] = dirichlet_data_fun(x)

def neumann_data_fun(x):
    return 1

def neumann_data(x, n, domain_index, res):
    res[0] = neumann_data_fun(x)

dirichlet_grid_fun = bempp.api.GridFunction(
    dirichlet_space_dirichlet_segment,
    fun=dirichlet_data, dual_space=dual_dirichlet_space)

neumann_grid_fun = bempp.api.GridFunction(
    neumann_space_neumann_segment,
    fun=neumann_data, dual_space=dirichlet_space_neumann_segment)

rhs_fun1 = (.5 * id_DD + dlp_DD) * dirichlet_grid_fun \
           - slp_DN * neumann_grid_fun
rhs_fun2 = - hyp_ND * dirichlet_grid_fun \
           + (.5 * id_NN - adlp_NN) * neumann_grid_fun
```

We can now discretise and solve the blocked operator system. We solve without preconditioner. This would cause problems if we were to further increase the degree of the basis functions.

```python
lhs = blocked.weak_form()
rhs = np.hstack([rhs_fun1.projections(neumann_space_dirichlet_segment),
                 rhs_fun2.projections(dirichlet_space_neumann_segment)])

from scipy.sparse.linalg import gmres
x, info = gmres(lhs, rhs)
```

Next, we split up the solution vector and define the grid functions associated with the computed Neumann and Dirichlet data.

```python
nx0 = neumann_space_dirichlet_segment.global_dof_count

neumann_solution = bempp.api.GridFunction(
    neumann_space_dirichlet_segment, coefficients=x[:nx0])
dirichlet_solution = bempp.api.GridFunction(
    dirichlet_space_neumann_segment, coefficients=x[nx0:])
```

We want to recombine the computed Dirichlet and Neumann data with the corresponding known data in order to get Dirichlet and Neumann grid functions defined on the whole grid. To achieve this we define identity operators from $\Gamma_N$ and $\Gamma_D$ into the global Dirichlet and Neumann spaces.

```python
neumann_imbedding_dirichlet_segment = \
    bempp.api.operators.boundary.sparse.identity(
        neumann_space_dirichlet_segment,
        global_neumann_space,
        global_neumann_space)

neumann_imbedding_neumann_segment = \
    bempp.api.operators.boundary.sparse.identity(
        neumann_space_neumann_segment,
        global_neumann_space,
        global_neumann_space)

dirichlet_imbedding_dirichlet_segment = \
    bempp.api.operators.boundary.sparse.identity(
        dirichlet_space_dirichlet_segment,
        global_dirichlet_space,
        global_dirichlet_space)

dirichlet_imbedding_neumann_segment = \
    bempp.api.operators.boundary.sparse.identity(
        dirichlet_space_neumann_segment,
        global_dirichlet_space,
        global_dirichlet_space)

dirichlet = (dirichlet_imbedding_dirichlet_segment * dirichlet_grid_fun +
             dirichlet_imbedding_neumann_segment * dirichlet_solution)

neumann = (neumann_imbedding_neumann_segment * neumann_grid_fun +
           neumann_imbedding_dirichlet_segment * neumann_solution)

dirichlet.plot()
```

We can plot the solution using the command ``dirichlet.plot()``. The solution looks as follows.


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

```python
# basis vectors in x, y, and t
x=sp.Symbol('x')
y=sp.Symbol('y')
t=sp.Symbol('t')
#model and data amplitudes
Ad=sp.Symbol('A_d')
Am=sp.Symbol('A_m')

# domain width
L=sp.Symbol('L', positive=True, finite=True)
# domain time dinension
T=sp.Symbol('T', positive=True, finite=True)

# data errors:
s=sp.Symbol('\sigma_d')
# data density:
rd=sp.Symbol('rho', positive=True, finite=True)
# wavenumbers in the x, y, and t dimensions
kx=sp.Symbol('k_x', integer=True, positive=True)
ky=sp.Symbol('k_y', integer=True, positive=True)
kt=sp.Symbol('k_t', integer=True, positive=True)
# Weighting on d3z/dx2dt
Wxxt=sp.Symbol('W_{xxt}')
# Weighting on d2x/dt2
Wtt=sp.Symbol('W_{tt}')

```

```python
tau=sp.Symbol('tau')
lam=sp.Symbol('\lambda')
tau*lam
```

$\displaystyle \lambda \tau$

```python
# basis functions:
bf =sp.sin(2*kx*sp.pi*x/L) * sp.sin(2*ky*sp.pi*y/L) * sp.sin(2*kt*sp.pi*t/T)
# model is the basis function times Am
m=Am * bf
# data is basis function times Ad
d=Ad * bf
```

```python
# derivatives of the basis functions
D2xtm = sp.diff(sp.diff(sp.diff(m, x),x),t)
D2ytm = sp.diff(sp.diff(sp.diff(m, y),y),t)
Dxytm = sp.diff(sp.diff(sp.diff(m, x),y),t)
D2tm = sp.diff(sp.diff(m, t),t)
```

```python
# Combined residual:
R=sp.integrate(\
               sp.integrate(\
    sp.integrate( rd*((d-m)/s)**2 + (Wxxt*D2xtm)**2 + (Wxxt*D2ytm)**2 + 2*(Wxxt*Dxytm)**2+(Wtt*D2tm)**2, (x, 0, L)),\
    (y, 0, L)),
               (t, 0, T))
```

All of the terms inside the integral have units of $m^{-2} yr^{-1}$, so that R is unitless, so every quantity inside parenthesis has units of $m yr^{-1/2}$.
- $A_d$ and $A_m$ have units of $m$.
- $\rho_d$ has units of $m^{-2} yr^{-1}$ (points per meter squared per year), so $\rho_d(d-m)/\sigma$ has units of $m^{-2} yr^{-1}$.
- $\partial^3 z / \partial x^2 \partial t$ has units of $m^{-1} yr^{-1}$
- $W_{xxt}$ has units of $yr^{1/2}$
- $\partial^2 z /\partial t^2$ has units of $m yr^{-2}$
- $W_{tt}$ has units of $m^{-2} yr^{3/2}$
- $k_x$, $k_y$, and $k_t$ are all unitless

```python
R
```

$\displaystyle \frac{2 \pi^{4} A_{m}^{2} L^{2} W_{tt}^{2} k_{t}^{4}}{T^{3}} + \frac{8 \pi^{6} A_{m}^{2} W_{xxt}^{2} k_{t}^{2} k_{x}^{4}}{L^{2} T} + \frac{16 \pi^{6} A_{m}^{2} W_{xxt}^{2} k_{t}^{2} k_{x}^{2} k_{y}^{2}}{L^{2} T} + \frac{8 \pi^{6} A_{m}^{2} W_{xxt}^{2} k_{t}^{2} k_{y}^{4}}{L^{2} T} + \frac{\rho \left(\frac{A_{d}^{2} L^{2} T}{8} - \frac{A_{d} A_{m} L^{2} T}{4} + \frac{A_{m}^{2} L^{2} T}{8}\right)}{\sigma_{d}^{2}}$

```python
# solve for model amplitude that minimizes the combined residual:
A_best=sp.solve(sp.diff(R, Am), Am)[0]
A_best
```

$\displaystyle \frac{A_{d} L^{4} T^{4} \rho}{L^{4} T^{4} \rho + 16 \pi^{4} L^{4} W_{tt}^{2} \sigma_{d}^{2} k_{t}^{4} + 64 \pi^{6} T^{2} W_{xxt}^{2} \sigma_{d}^{2} k_{t}^{2} k_{x}^{4} + 128 \pi^{6} T^{2} W_{xxt}^{2} \sigma_{d}^{2} k_{t}^{2} k_{x}^{2} k_{y}^{2} + 64 \pi^{6} T^{2} W_{xxt}^{2} \sigma_{d}^{2} k_{t}^{2} k_{y}^{4}}$

It's not clear how best to simplify this expression, but working by hand gives this expression as equivalent to $A_{best}$

```python
sp.simplify(Ad*rd/s**2 /  (rd/s**2 + 16*sp.pi**4*(Wtt**2*kt**4/T**4 + 4*sp.pi**2*kt**2*Wxxt**2*(kx**2+ky**2)**2/(L**4*T**2))))
```

$\displaystyle \frac{A_{d} L^{4} T^{4} \rho}{L^{4} T^{4} \rho + 16 \pi^{4} \sigma_{d}^{2} k_{t}^{2} \left(L^{4} W_{tt}^{2} k_{t}^{2} + 4 \pi^{2} T^{2} W_{xxt}^{2} \left(k_{x}^{2} + k_{y}^{2}\right)^{2}\right)}$

...or better yet...

```python
Ad/(1+(16*sp.pi**4*s**2*kt**2/rd)*(Wtt**2*kt**2/T**4 + 4*sp.pi**2*T**2*Wxxt**2*(kx**2+ky**2)**2/(L**4*T**4)))
```

$\displaystyle \frac{A_{d}}{\frac{16 \pi^{4} \sigma_{d}^{2} k_{t}^{2} \left(\frac{W_{tt}^{2} k_{t}^{2}}{T^{4}} + \frac{4 \pi^{2} W_{xxt}^{2} \left(k_{x}^{2} + k_{y}^{2}\right)^{2}}{L^{4} T^{2}}\right)}{\rho} + 1}$

Writing this in terms of wavelength ($\lambda= L (k_x^2+k_y^2)^{-2}$) and period ($\tau = T/k_t$),

```python
Ad/(1+16*sp.pi**4*s**2/rd*(Wtt**2/tau**4 + 4*sp.pi**2*Wxxt**2/(lam**4*tau**2)))
```

$\displaystyle \frac{A_{d}}{\frac{16 \pi^{4} \sigma_{d}^{2} \left(\frac{W_{tt}^{2}}{\tau^{4}} + \frac{4 \pi^{2} W_{xxt}^{2}}{\lambda^{4} \tau^{2}}\right)}{\rho} + 1}$

To choose a value of $W_{tt}$, we look for the wavelength where $A_{best} = A_d/2$, or
$$\frac{16 \pi^4 \sigma_d^2}{\rho_d} \left(\frac{W_{tt}^2}{\tau^4} + \frac{4\pi^2W_{xxt}^2}{\lambda^4\tau^2}\right) =1\,$$

Solving for $W_{tt}$ gives:
$$ \frac{W_{tt}^2}{\tau^4} = \frac{\rho}{16 \pi^4 \sigma_d^2}$$

$$ W_{tt} = \frac{\tau^2\rho^{1/2}}{4 \pi^2 \sigma_d}$$

And solving for $W_{xxt}$ gives:
$$ \frac{4 \pi^2 W_{xxt}^2}{\lambda^4 \tau^2} = \frac{\rho}{16 \pi^4 \sigma_d^2}$$
$$ W_{xxt} = \frac{\rho^{1/2} \lambda^2 \tau}{8 \pi^3 \sigma_d} $$

## In 1D:

```python
# basis functions:
bf1 =sp.sin(2*kx*sp.pi*x/L)  * sp.sin(2*kt*sp.pi*t/T)
# model is the basis function times Am
m1=Am * bf1
# data is basis function times Ad
d1=Ad * bf1
```

```python
# derivatives of the basis functions
D2xtm1 = sp.diff(sp.diff(sp.diff(m1, x),x),t)
D2tm1 = sp.diff(sp.diff(m1, t),t)
```

```python
# Combined residual:
R1=sp.integrate(
    sp.integrate( rd*((d1-m1)/s)**2 + (Wxxt*D2xtm1)**2 ++(Wtt*D2tm1)**2, \
        (x, 0, L)),\
    (t, 0, T))
R1

```

$\displaystyle \frac{4 \pi^{4} A_{m}^{2} L W_{tt}^{2} k_{t}^{4}}{T^{3}} + \frac{16 \pi^{6} A_{m}^{2} W_{xxt}^{2} k_{t}^{2} k_{x}^{4}}{L^{3} T} + \frac{\rho \left(\frac{A_{d}^{2} L T}{4} - \frac{A_{d} A_{m} L T}{2} + \frac{A_{m}^{2} L T}{4}\right)}{\sigma_{d}^{2}}$

```python
A1_best=sp.solve(sp.diff(R1, Am), Am)[0]
A1_best
```

$\displaystyle \frac{A_{d} L^{4} T^{4} \rho}{L^{4} T^{4} \rho + 16 \pi^{4} L^{4} W_{tt}^{2} \sigma_{d}^{2} k_{t}^{4} + 64 \pi^{6} T^{2} W_{xxt}^{2} \sigma_{d}^{2} k_{t}^{2} k_{x}^{4}}$

Simplifying this (with $\tau = T/k_t$ and $\lambda = L/(k_x^2+k_y^2)^{1/2}$) gives:
$$
\frac{A_{best}}{A_d}=\frac{1}{1+\frac{16\pi^4\sigma_d^2}{\rho}\left(\frac{W_{tt}^2}{\tau^4} + \frac{4\pi^2W_{xxt}^2}{\lambda^4\tau^2}\right)}
$$

Solving for $W_{tt}$ when $A_{best}/A_d=1/2$:

$$
W_{tt} = \frac{\rho^{1/2}\tau^2}{4 \pi \sigma_d}
$$

Solving for $W_{xxt}$:

$$
W_{xxt} = \frac{\rho^{1/2} \lambda^2 \tau}{8 \pi^3 \sigma_d}
$$

```python

```

### Example cases

#### 1. Balancing systematic errors against temporal smoothing in a high-slope area
We only have one independent bias value every 10 km at Jakobshavn latitude, and per-track systematic errors might be as large as 10 m geolocation error * slope of 0.05 -> sigma of 0.5

```python
tau=0.125  # 1/8 year period
rho_tr=4/10000   # 4 measurements/year/10 km
si=0.5 # 0.5 m error
Wtti = (tau**2*rho_tr**0.5/(4*np.pi**2*si))
print(1/Wtti)
```

    63165.46816697189

With these parameters, the spectrum of attenuation at long wavelength should look like this:

```python
Wtti = 1/50000
tau_0 = 10**np.arange(-2, 1, 0.1)
plt.figure()
for si in [0.03, 0.1, 0.5, 1]:
    atten=1/(1+16*np.pi**4*si**2/rho_tr*(Wtti**2/tau_0**4))
    plt.loglog(tau_0, atten, label=f'sigma={si:0.2f} m' )
plt.gca().set_ylim([10**-1.1, 10**0.25])
plt.plot(plt.gca().get_xlim(), 0.5*np.ones(2), 'k--')
plt.xlabel('period, yr')
plt.ylabel('attenuation')
plt.legend()
```

#### 2.  Balancing per-ref-point errors against spatio-temporal smoothing in a low-slope environment

```python
# if we want to suppress 1-km scale variations in dh/dt at a temporal scale of 0.125 yr:
lambda_i=1000
tau=0.25
si=0.01
rho_data=4/120 # 4 points/year every 120 m
Wxxti = lambda_i**2*tau*rho_data**0.5/(8*np.pi**3*si)
print(f'solution value:{1/Wxxti:2.2e}, ATL15 value: {0.00003:2.2e}')
#print(0.00003)
```

    solution value:5.43e-05, ATL15 value: 3.00e-05

The spectrum for these plots looks like this:

```python
plt.figure();
Wxxti=1/5.4e-5
si=0.01
rho_data=4/120
lambda_i = 10**np.arange(1., 4.5, 0.1)

for tau_0 in [0.125, 0.25, 0.5,  2]:
    atten=1/(1+16*np.pi**4*si**2/rho_data*(4*np.pi**2*Wxxti**2/(lambda_i**4*tau_0**2)))
    plt.loglog(lambda_i, atten, label=f"$\\tau$={tau_0:2.3f}yr")

plt.gca().set_xlim([100, 10**4.5])
plt.gca().set_ylim([0.05, 2])
plt.plot(plt.gca().get_xlim(), 0.5*np.ones(2), 'k--')

plt.legend()
plt.xlabel('$\lambda$, m')
plt.ylabel('attenuation')

```

```python

```


## Trajectory equations:

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

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

```python
Ex, Ey, Ez = symbols("E_x, E_y, E_z")
Bx, By, Bz, B = symbols("B_x, B_y, B_z, B")
x, y, z = symbols("x, y, z")
vx, vy, vz, v = symbols("v_x, v_y, v_z, v")
t = symbols("t")
q, m = symbols("q, m")
c, eps0 = symbols("c, epsilon_0")
```

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

In Cortesian coordinates:

```python
eq_x = Eq( diff(x(t), t, 2), q / m * Ex + q / c / m * (vy * Bz - vz * By) )
eq_y = Eq( diff(y(t), t, 2), q / m * Ey + q / c / m * (-vx * Bz + vz * Bx) )
eq_z = Eq( diff(z(t), t, 2), q / m * Ez + q / c / m * (vx * By - vy * Bx) )
display( eq_x, eq_y, eq_z )
```

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

```python
uni_mgn_subs = [ (Bx, 0), (By, 0), (Bz, B) ]

eq_x = eq_x.subs(uni_mgn_subs)
eq_y = eq_y.subs(uni_mgn_subs)
eq_z = eq_z.subs(uni_mgn_subs)
display( eq_x, eq_y, eq_z )
```

Assuming $E_z = 0$ and $E_y = 0$:

```python
zero_EyEz_subs = [ (Ey, 0), (Ez, 0) ]
eq_x = eq_x.subs(zero_EyEz_subs)
eq_y = eq_y.subs(zero_EyEz_subs)
eq_z = eq_z.subs(zero_EyEz_subs)
display( eq_x, eq_y, eq_z )
```

Motion is uniform along the $z$-axis:

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

The constants of integration can be found from the initial conditions $z(0) = 0$ and $v_z(0) = v$:

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

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

So that

```python
z_sol = z_eq.subs( c1_c2 )
vz_sol = vz_eq.subs( c1_c2 )
display( z_sol, vz_sol )
```

Now, the equation for $y$ can be integrated:

```python
v_as_diff = [ (vx, diff(x(t),t)), (vy, diff(y(t),t)), (vz, diff(z_sol.lhs,t)) ]
eq_y = eq_y.subs( v_as_diff )
eq_y = Eq( integrate( eq_y.lhs, (t, 0, t) ), integrate( eq_y.rhs, (t, 0, t) ) )
eq_y
```

$$\frac{d}{d t} y{\left (t \right )} - \left. \frac{d}{d t} y{\left (t \right )} \right|_{\substack{ t=0 }} = \frac{B q}{c m} x{\left (0 \right )} - \frac{B q}{c m} x{\left (t \right )}$$

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

```python
x_0 = Symbol('x_0')
vy_0 = 0
initial_cond_subs = [(x(0), x_0), (diff(y(t),t).subs(t,0), vy_0) ]
vy_sol = eq_y.subs( initial_cond_subs )
vy_sol
```

This equation can be substituted into the equation for $x$-coorditante:

```python
eq_x = eq_x.subs( vy, vy_sol.rhs )
eq_x = Eq( eq_x.lhs, collect( expand( eq_x.rhs ), B *q / c / m ) )
eq_x
```

An expression for $E_x$ can be taken from the example on ribbon beam in free space $E_x = \dfrac{ 2 \pi I_0 }{v}$:

```python
I0 = symbols('I_0')
Ex_subs = [ (Ex, 2 * pi * I0 / v) ]
eq_x = eq_x.subs( ex_subs )
eq_x
```

This is an oscillator-type equation
$$
x'' + a x + b = 0
$$
with $a$ and $b$ given by

```python
eq_a = Eq(a, eq_x.rhs.expand().coeff(x(t), 1))
eq_b = Eq( b, eq_x.rhs.expand().coeff(x(t), 0) )
display( eq_a , eq_b )
```

It's solution is given by:

```python
a, b, c = symbols("a, b, c")
osc_eqn = Eq( diff(x(t),t,2), - abs(a)*x(t) + b)
display( osc_eqn )
osc_eqn_sol = dsolve( osc_eqn )

osc_eqn_sol
```

From initial conditions $x(0) = x_0, v_0 = 0$:

```python
x_0 = symbols( 'x_0' )
v_0 = 0
c1_c2_system = []
initial_cond_subs = [(t, 0), (x(0), x_0), (diff(x(t),t).subs(t,0), v_0) ]
c1_c2_system.append( osc_eqn_sol.subs( initial_cond_subs ) )

osc_eqn_sol_diff = Eq( osc_eqn_sol.lhs.diff(t), osc_eqn_sol.rhs.diff(t) )
c1_c2_system.append( osc_eqn_sol_diff.subs( initial_cond_subs ) )

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

So that

```python
x_sol = osc_eqn_sol.subs( c1_c2 )
x_sol
```

Taking into account that
$$ \sqrt{|a|} = \omega_g = \frac{ q B }{mc } $$
where $\omega_g$ is the gyrofrequency, and since

```python
b_over_a = simplify( eq_b.rhs / abs( eq_a.rhs ).subs( abs( eq_a.rhs ), -eq_a.rhs ) )
Eq( b/abs(a), b_over_a )
```

It is possible to rewrite the solution as

```python
omega_g = symbols('omega_g')
eq_omega_g = Eq( omega_g, q * B / m / c )
A = symbols('A')
eq_A = Eq( A, b_over_a - x_0 )
subs_list = [ (b/abs(a), b_over_a), ( sqrt( abs(a) ), omega_g ), ( eq_A.rhs, eq_A.lhs) ]
x_sol = x_sol.subs( subs_list )
display( x_sol, eq_A, eq_omega_g )
```

From the laws of motion for $x(t)$ and $z(t)$

```python
display( x_sol, z_sol )
```

it is possible to obtain a trajectory equation:

```python
t_from_z = solve( z_sol.subs(z(t),z), t )[0]
x_z_traj = Eq( x_sol.lhs.subs( t, z ), x_sol.rhs.subs( [(t, t_from_z)] ) )
display( x_z_traj, eq_A, eq_omega_g )
```

```python

```

examples/ribbon_beam_in_magnetic_field_contour/Ribbon Beam Contour in Uniform Magnetic Field.ipynb


# Quicksort Algorithm

Distribution of the number of comparisons necessary to sort an array using the quicksort algorithm. Inspired by the materials belonging to:
https://mast.queensu.ca/~stat455/.

The algorithm works by recursively dividing the array into sub-arrays of elements that are larger or smaller than a randomly chosen entry (the "pivot").

```
def quicksort(x):
    l = len(x)

    if l == 0 or l == 1:
        return x  # no sorting needed
    else:
        pivot = pop(x) # choose & remove pivot
        H = []
        L = []
        for i in x:
            if i < pivot:
                L+=[i]
            else:
                H+=[i]

        H = quicksort(H) # recursion
        L = quicksort(L)

        return L+H
```

The code shown below has some extra lines to keep track of the number of "i<pivot" comparisons made.

### Expected Number of Comparisons

Because the pivot is randomly chosen, the amount of comparisons $X_n$ needed to sort an array of length $n$ is a random variable.

For arrays of length $n\in\{0,1,2\}$, the answer can be guessed:

\begin{equation}
\begin{array}{l}
\mathbb{E}[X_0] = 0\\
\mathbb{E}[X_1] = 0\\
\mathbb{E}[X_2] = 1\\
\end{array}
\end{equation}

For the general case, $\mathbb{E}[E_n]$, it is natural to condition on the *rank* of the pivot, $Y$. *Rank* meaning the size of the pivot relative to the other numbers in the array. $Y=1$ would mean that the pivot is the smallest number, $Y=5$ would mean the 5th smallest number etc.

\begin{equation}
\mathbb{E}[X_n] = \mathbb{E}[\mathbb{E}[X_n|Y=j]] = \sum_{j=1}^n \mathbb{E}[X_n|Y=j] \left(\frac{1}{n}\right)
\end{equation}

Where I made use of the fact that $p(Y=j) = \frac{1}{n}$. For a given choice of $Y=j$, the algorithm makes $n-1$ comparisons. It then calls quicksort again on two arrays of length $j-1$ and $n-j$ (the pivot was removed, so the two parts add up to a length of $n-1$). Hence the expectation for then number of comparison operations, conditioned on $j$, is:

\begin{equation}
\mathbb{E}[X_n|Y=j] = n-1 + \mathbb{E}[X_{j-1}] + \mathbb{E}[X_{n-j}]
\end{equation}

Taking the expectation with respect to the pivot:

\begin{equation}
\begin{array}{rl}
\mathbb{E}[X_n] &= \frac{1}{n}\sum_{j=1}^n \left(n-1 + \mathbb{E}[X_{j-1}] + \mathbb{E}[X_{n-j}]\right)\\
&= \frac{1}{n}\left[\underbrace{\sum_{j=1}^n (n-1)}_{n(n-1)} + 2\sum_{j=1}^{n-1}\mathbb{E}[X_j] \right]\\
\mathbb{E}[X_n]n - n(n-1)&= 2\sum_{j=1}^{n-1} \mathbb{E}[X_j]
\end{array}
\end{equation}

It's nicer to have an expression that depends on the previous term only, so the usual trick is applied:
\begin{equation}
\begin{array}{rl}
\mathbb{E}[X_{n+1}] (n+1) - \mathbb{E}[X_{n}] n &= n(n+1) - n(n-1) + 2\mathbb{E}[X_{n}] \\
\mathbb{E}[X_{n+1}]&= \frac{1}{n+1} \left[ 2n + \mathbb{E}[X_{n}] (n+2) \right]
\end{array}
\end{equation}

It's possible to calculate an expression that does not rely on recursion, but I will leave it at this.

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

def get_array(n):
    """
    Get a shuffled array of integers (without duplicates)
    """
    return list(np.random.permutation(list(range(n))))

def quicksort(x):
    """
    Returns sorted array and the number of comparisons made in sorting the array.
    """

    l = len(x)
    n = 0

    if l == 0 or l == 1: # no sorting needed
        return n,x

    else:
        pivot = x.pop() # randomly choose a pivot from array
        H = []
        L = []
        for i in x: # assign elements to H(igher) or L(ower) array depending on size relative to pivot
            n+=1
            if i > pivot:
                H+=[i]
            else:
                L+=[i]

        n_l, L = quicksort(L) # recursively call quicksort on H and L arrays.s
        n_h, H = quicksort(H)

        n = n+n_l+n_h

        return n,L+H

# simulation
n_trials = 10000
array_length = 10

n_comparisons = []
for i in range(n_trials):

    x = get_array(array_length)
    n,_ = quicksort(x)

    n_comparisons+=[n]

plt.figure(figsize=(12,8))
_ = plt.hist(n_comparisons,bins = list(range(array_length**2)),density=True)
plt.title('Quicksort - Number of Comparisons (array size: %i)' % array_length)
```

```python
def M_n(n):
    """
    Expected number of comparisons necessary for an array of length n
    """
    if n == 0 or n == 1:
        return 0

    else:
        M_n = 1
        for i in range(2,n):
            M_n = (1/(i+1))*(2*i+M_n*(i+2))

        return M_n

def M_n_simulated(array_length,n_trials=2000):
    """
    simulate expected number of comparisons
    """
    n_comparisons = []
    for i in range(n_trials):

        x = get_array(array_length)
        n_,_ = quicksort(x)

        n_comparisons+=[n_]

    return np.mean(n_comparisons)
```

```python
nn = list(range(10))

analytical = [M_n(n) for n in nn]
simulated = [M_n_simulated(n,n_trials=5000) for n in nn]

plt.figure(figsize=(12,8))
plt.plot(nn,analytical,'-',linewidth=2)
plt.plot(nn,simulated,'o',markersize=10)
plt.legend(['analytical','simulated'])

plt.title('Quicksort: Expected number of Comparisons')
plt.xlabel('array length',fontsize=15)
_ = plt.ylabel('expected comparisons',fontsize=15)
```


```python
from sympy import diag, var, simplify, collect, expand, solve, sin, cos, Matrix, eye, diff, Function, expand_power_base, exp
from sympy.physics.mechanics import mlatex, mechanics_printing
mechanics_printing()
```

```python
var("s t")
```

```python
A = Matrix([[2, 0, 0], [-1, 5, 1], [1, 1, 5]])
```

```python
exp(A*t)
```

```python
P = Matrix([[-2, 0, 0], [-1, 1, -1], [1, 1, 1]])
Ā = P.inv()*A*P
```

```python
P, P.inv(), Ā
```

```python
P*exp(Ā*t)*P.inv()
```

```python
exp(Ā*t)
```

```python
simplify(expand((s*eye(3) - A).inv()))
```

```python
eye(3), A, A**2
```

```python
Matrix([[1, 2, 4], [1, 4, 16], [1, 6, 36]]).inv()*8
```

```python
A = diag(1, 2, 3, 1, 2, 3, 4)
```

```python
((Matrix([[1, 2, 3, -1, 5, 1, 1]])*(s*eye(7) - A).inv()*Matrix([[1, -1, 1, -1, 1, -1, 1]]).T)[0]).factor().simplify()
```

```python
from numpy import array, matrix, kron, eye, zeros, vstack, hstack, linspace
from scipy.linalg import expm
```

```python
%precision 1
```

    '%.1f'

```python
A = matrix([[-9, 0, 1], [5, -7, 9], [-2, -5, -6]])
```

```python
Q = eye(3)
```

```python
matrix(-Q.flatten("F")).T
```

    matrix([[-1.],
            [-0.],
            [-0.],
            [-0.],
            [-1.],
            [-0.],
            [-0.],
            [-0.],
            [-1.]])

```python
kron(eye(3), A.T) + kron(A.T, eye(3)).I
```

    matrix([[ -9.1e+00,   5.0e+00,  -2.0e+00,  -1.5e-02,   0.0e+00,   0.0e+00,
               4.7e-02,   0.0e+00,   0.0e+00],
            [  0.0e+00,  -7.1e+00,  -5.0e+00,   0.0e+00,  -1.5e-02,   0.0e+00,
               0.0e+00,   4.7e-02,   0.0e+00],
            [  1.0e+00,   9.0e+00,  -6.1e+00,   0.0e+00,   0.0e+00,  -1.5e-02,
               0.0e+00,   0.0e+00,   4.7e-02],
            [  6.1e-03,   0.0e+00,   0.0e+00,  -9.1e+00,   5.0e+00,  -2.0e+00,
               5.5e-02,   0.0e+00,   0.0e+00],
            [  0.0e+00,   6.1e-03,   0.0e+00,   0.0e+00,  -7.1e+00,  -5.0e+00,
               0.0e+00,   5.5e-02,   0.0e+00],
            [  0.0e+00,   0.0e+00,   6.1e-03,   1.0e+00,   9.0e+00,  -6.1e+00,
               0.0e+00,   0.0e+00,   5.5e-02],
            [ -8.5e-03,   0.0e+00,   0.0e+00,  -1.0e-01,   0.0e+00,   0.0e+00,
              -9.1e+00,   5.0e+00,  -2.0e+00],
            [  0.0e+00,  -8.5e-03,   0.0e+00,   0.0e+00,  -1.0e-01,   0.0e+00,
               0.0e+00,  -7.1e+00,  -5.0e+00],
            [  0.0e+00,   0.0e+00,  -8.5e-03,   0.0e+00,   0.0e+00,  -1.0e-01,
               1.0e+00,   9.0e+00,  -6.1e+00]])

```python
(kron(eye(3), A.T) + kron(A.T, eye(3)).I)*(matrix(-Q.flatten("F")).T)
```

    matrix([[ 9.1],
            [ 0. ],
            [-1. ],
            [-5. ],
            [ 7.1],
            [-9.1],
            [ 2. ],
            [ 5.1],
            [ 6.1]])

```python

```


```
from sympy import *

x, a = symbols('x a')

integrate(exp(-x*x), (x, -oo, oo))
```


# SymPy

```python
%matplotlib inline

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

---
## Part I

$$ \Large {\displaystyle f(x)=3e^{-{\frac {x^{2}}{8}}}} \sin(x/3)$$

* Find the first four terms of the Taylor expansion of the above equation (at x = 0)
* Make a plot of the function
* Plot size 10 in x 4 in
* X limts -5, 5
* Y limits -2, 2
* On the same plot:
  * Over-plot the 1-term Taylor expansion using a different color/linetype/linewidth
  * Over-plot the 2-term Taylor expansion using a different color/linetype/linewidth
  * Over-plot the 3-term Taylor expansion using a different color/linetype/linewidth
  * Over-plot the 4-term Taylor expansion using a different color/linetype/linewidth

```python

```

```python

```

```python

```

---
## Part II

$$\Large {\displaystyle g(x)=x^{2} - 0.75}$$

#### What are the value(s) for `x` where f(x) = g(x)

```python

```

```python

```

```python

```

---
### Due Mon Mar 02 - 1 pm
- `File -> Download as -> HTML (.html)`
- `upload your .html file to the class Canvas page`

```python

```


# Determine derivatives of angular sequence to rotation matrix

Peter Corke 2021

SymPy code to evaluate the mappings from rotation angle sequences to rotation matrix, then compute the derivatives to determine the mapping from angular rates to angular velocity.

Adjust the next cell to choose the particular angle sequence.

```python
from spatialmath.base import *
from sympy import *
```

```python
# func = eul2r
# angle_names =  ('phi', 'theta', 'psi')

func = lambda Gamma: rpy2r(Gamma, order='yxz')
angle_names = ('alpha', 'beta', 'gamma')

```

Define a symbol for time

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

Define arrays of symbols for the angles, angle as function of time, time derivative of angle as a function of time

```python
angle = []  # names of angles, eg. theta
anglet = []  # angles as function of time, eg. theta(t)
angled = []  # derivative of above, eg. d theta(t) / dt
angledn = []  # symbol to represent above, eg. theta_dot
for i in angle_names:
    angle.append(symbols(i))
    anglet.append(Function(i)(t))
    angled.append(anglet[-1].diff(t))
    angledn.append(i + '_dot')
```

Compute the rotation matrix

```python
R = Matrix(func(anglet))
R
```

$\displaystyle \left[\begin{matrix}\sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} & - \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} + \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} & \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)}\\\sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} & \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} & - \sin{\left(\beta{\left(t \right)} \right)}\\\sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} & \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} + \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} & \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)}\end{matrix}\right]$

Compute its time derivative

```python
Rdot = Matrix(R).diff(t)
Rdot
```

$\displaystyle \left[\begin{matrix}\sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} & - \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} & - \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} + \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)}\\- \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} & - \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} & - \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)}\\- \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} + \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} & - \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} & - \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)}\end{matrix}\right]$

Get angular velocity vector in terms of angles and angle rates

```python
omega = Matrix(vex(Rdot * R.T))
omega
```

$\displaystyle \left[\begin{matrix}- \frac{\left(\sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} + \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)}\right) \left(- \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)}\right)}{2} - \frac{\left(\sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)}\right) \left(- \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)}\right)}{2} - \frac{\left(- \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)}\right) \sin{\left(\beta{\left(t \right)} \right)}}{2} + \frac{\left(- \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} + \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)}\right) \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)}}{2} + \frac{\left(- \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)}\right) \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)}}{2} + \frac{\cos^{2}{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)}}{2}\\\frac{\left(\sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} + \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)}\right) \left(- \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)}\right)}{2} - \frac{\left(- \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} + \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)}\right) \left(- \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)}\right)}{2} - \frac{\left(\sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)}\right) \left(- \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} + \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)}\right)}{2} + \frac{\left(\sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)}\right) \left(\sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)}\right)}{2} + \frac{\left(- \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} + \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)}\right) \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)}}{2} - \frac{\left(- \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)}\right) \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)}}{2}\\\frac{\left(- \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} + \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)}\right) \left(- \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)}\right)}{2} + \frac{\left(\sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)}\right) \left(- \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)}\right)}{2} + \frac{\left(- \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} + \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)}\right) \sin{\left(\beta{\left(t \right)} \right)}}{2} - \frac{\left(- \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)}\right) \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)}}{2} - \frac{\left(\sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)}\right) \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)}}{2} - \frac{\sin{\left(\gamma{\left(t \right)} \right)} \cos^{2}{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)}}{2}\end{matrix}\right]$

For each element of this 3x1 matrix get the coefficients of each angle derivative

```python
A = Matrix.zeros(3,3)
for i in range(3):
    e = omega[i,0].expand()
    for j in range(3):
        A[i, j] = e.coeff(angled[j])
```

The result is a 3x3 matrix. Mapping from angle rates to angular velocity. We subsitute angle as a function of time to plain angle, then simplify.

```python
A = trigsimp(A.subs(a for a in zip(anglet, angle)))
A
```

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

Compute the inverse and simplify.  Mapping from angular velocity to angle rates.

```python
Ai = trigsimp(A.inv())
Ai
```

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

Render as code

```python
pycode(A).replace('ImmutableDenseMatrix', 'np.array')
```

    'np.array([[math.sin(gamma)*math.cos(beta), math.cos(gamma), 0], [-math.sin(beta), 0, 1], [math.cos(beta)*math.cos(gamma), -math.sin(gamma), 0]])'

```python
pycode(Ai).replace('ImmutableDenseMatrix', 'np.array')
```

    'np.array([[math.sin(gamma)/math.cos(beta), 0, math.cos(gamma)/math.cos(beta)], [math.cos(gamma), 0, -math.sin(gamma)], [math.sin(gamma)*math.tan(beta), 1, math.cos(gamma)*math.tan(beta)]])'

Compute the time derivative of `Ai`, from angular acceleration to angle acceleration

```python
Ai = Ai.subs(a for a in zip(angle, anglet))
Ai
```

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

```python
Ai_dot = trigsimp(Ai.diff(t).subs(a for a in zip(angled, angledn)).subs(a for a in zip(anglet, angle)))
Ai_dot
```

$\displaystyle \left[\begin{matrix}\frac{\frac{\beta_{dot} \sin{\left(\beta \right)} \sin{\left(\gamma \right)}}{\cos{\left(\beta \right)}} + \gamma_{dot} \cos{\left(\gamma \right)}}{\cos{\left(\beta \right)}} & 0 & \frac{\frac{\beta_{dot} \sin{\left(\beta \right)} \cos{\left(\gamma \right)}}{\cos{\left(\beta \right)}} - \gamma_{dot} \sin{\left(\gamma \right)}}{\cos{\left(\beta \right)}}\\- \gamma_{dot} \sin{\left(\gamma \right)} & 0 & - \gamma_{dot} \cos{\left(\gamma \right)}\\\frac{\beta_{dot} \sin{\left(\gamma \right)}}{\cos^{2}{\left(\beta \right)}} + \gamma_{dot} \cos{\left(\gamma \right)} \tan{\left(\beta \right)} & 0 & \frac{\beta_{dot} \cos{\left(\gamma \right)}}{\cos^{2}{\left(\beta \right)}} - \gamma_{dot} \sin{\left(\gamma \right)} \tan{\left(\beta \right)}\end{matrix}\right]$

```python
pycode(Ai_dot).replace('ImmutableDenseMatrix', 'np.array')
```

    'np.array([[(beta_dot*math.sin(beta)*math.sin(gamma)/math.cos(beta) + gamma_dot*math.cos(gamma))/math.cos(beta), 0, (beta_dot*math.sin(beta)*math.cos(gamma)/math.cos(beta) - gamma_dot*math.sin(gamma))/math.cos(beta)], [-gamma_dot*math.sin(gamma), 0, -gamma_dot*math.cos(gamma)], [beta_dot*math.sin(gamma)/math.cos(beta)**2 + gamma_dot*math.cos(gamma)*math.tan(beta), 0, beta_dot*math.cos(gamma)/math.cos(beta)**2 - gamma_dot*math.sin(gamma)*math.tan(beta)]])'

```python

```


```python
import math
import sympy as sp
from sympy.utilities.lambdify import lambdify
import numpy as np
import matplotlib.pyplot as plt

import openrtdynamics2.lang as dy
import openrtdynamics2.py_execute as dyexe

from vehicle_lib.vehicle_lib import *

```

# Kinematic bicycle model

The dynamic system equations are given by

$
\dot X
=
f(x,y,\psi)
=
\begin{pmatrix}
  \dot x \\
  \dot y \\
  \dot \psi
\end{pmatrix}
=
\begin{pmatrix}
  v \cos( \delta + \psi) \\
  v \sin( \delta + \psi) \\
  v / l_r \sin( \delta ), \\
\end{pmatrix}
$

with the state vector

$ X = [ x, y, \psi ]^T $.

Herein, $x$ and $y$ denote the coordinates of the vehicle front axle in cartesian space and $\psi$ the vehicle body orientation angle. The system inputs are the steering angle $\delta$ and the vehicle velocity $v$. Finally, the parameter $l_r$ denotes the wheelbase, which is the length in-between front and rear axle.

```python
x, y, v, delta, psi, l_r, T_s, n = sp.symbols('x y v delta psi l_r T_s n')

x_dot   = v * sp.cos( delta + psi )
y_dot   = v * sp.sin( delta + psi )
psi_dot = v / l_r * sp.sin( delta )

# system function f
f = sp.Matrix([ x_dot, y_dot, psi_dot ])

# state vector
X_bic = sp.Matrix( [ x, y, psi ])

# input vector
U_bic = sp.Matrix( [ delta, v ])
```

```python
f
```

$\displaystyle \left[\begin{matrix}v \cos{\left(\delta + \psi \right)}\\v \sin{\left(\delta + \psi \right)}\\\frac{v \sin{\left(\delta \right)}}{l_{r}}\end{matrix}\right]$

# Discretization of the continunous model

By applying Euler-forward discretization

$ {X}[k+1] = \underbrace{ {X}[k] + T_s  \dot{X} }_{f_{dscr}} $,

the continuous system is time-discretized with the sampling time $T_s$ yielding the discrete system funtion $f_{dscr}$.

```python
# apply Euler forward
f_dscr = sp.Matrix( [x,y,psi]) + T_s * f
```

```python
f_dscr
```

$\displaystyle \left[\begin{matrix}T_{s} v \cos{\left(\delta + \psi \right)} + x\\T_{s} v \sin{\left(\delta + \psi \right)} + y\\\frac{T_{s} v \sin{\left(\delta \right)}}{l_{r}} + \psi\end{matrix}\right]$

# Analytically compute the Jacobian matrices

A linearization of the non-linear system function around a dynamic set point is calculated by deriving the jacobian matrices w.r.t. the state vector $X$ and each system input $\delta$ and $v$:

continuous case
$
  A = \frac{ \partial f }{ \partial X},
  \qquad
  B = \frac{ \partial f }{ \partial [ \delta, v ]^T },
$

discrete-time case
$
  A_{dscr} = \frac{ \partial f_{dscr} }{ \partial X},
  \qquad
  B_{dscr} = \frac{ \partial f_{dscr} }{ \partial [ \delta, v ]^T },
$

```python
# continuous system matrices
A = f.jacobian(X_bic)
B = f.jacobian(U_bic)

# discrete system matrices
A_dscr = f_dscr.jacobian(X_bic)
B_dscr = f_dscr.jacobian(U_bic)
```

```python
A_dscr
```

$\displaystyle \left[\begin{matrix}1 & 0 & - T_{s} v \sin{\left(\delta + \psi \right)}\\0 & 1 & T_{s} v \cos{\left(\delta + \psi \right)}\\0 & 0 & 1\end{matrix}\right]$

```python
B_dscr
```

$\displaystyle \left[\begin{matrix}- T_{s} v \sin{\left(\delta + \psi \right)} & T_{s} \cos{\left(\delta + \psi \right)}\\T_{s} v \cos{\left(\delta + \psi \right)} & T_{s} \sin{\left(\delta + \psi \right)}\\\frac{T_{s} v \cos{\left(\delta \right)}}{l_{r}} & \frac{T_{s} \sin{\left(\delta \right)}}{l_{r}}\end{matrix}\right]$

# Create functions that generate the matrices A, B, and the system function f

Create python functions with which the symbolically dervied matrices and system function can be evaluated.

```python
variables = (T_s,l_r, x,y,psi,v,delta)

array2mat = [{'ImmutableDenseMatrix': np.matrix}, 'numpy']

A_dscr_fn = lambdify( variables, A_dscr, modules=array2mat)
B_dscr_fn = lambdify( variables, B_dscr, modules=array2mat)
f_dscr_fn = lambdify( variables, f_dscr, modules=array2mat)

```

```python
A_dscr_fn(0.01, 3.0, 0.1,0.2,0.4,10,0.1)
```

    matrix([[ 1.        ,  0.        , -0.04794255],
            [ 0.        ,  1.        ,  0.08775826],
            [ 0.        ,  0.        ,  1.        ]])

```python
B_dscr_fn(0.01, 3.0, 0.1,0.2,0.4,10,0.1)
```

    matrix([[-0.04794255,  0.00877583],
            [ 0.08775826,  0.00479426],
            [ 0.03316681,  0.00033278]])

```python
f_dscr_fn(0.01, 3.0, 0.1,0.2,0.4,10,0.1)
```

    matrix([[0.18775826],
            [0.24794255],
            [0.40332778]])

```python

```

# Run a simulation to generate test data

Set-up a simulation of a vehicle (bicycle model). The steering rate $\dot\delta$ is the input and integrated to obtain the steering angle $\delta$. A picewise constant (step signal) is applied to the input $\dot\delta$. The velocity $v$ is set to a constant value.

```python
system = dy.enter_system()

velocity               = dy.system_input( dy.DataTypeFloat64(1), name='velocity_',             default_value=5.0,    value_range=[0, 25],     title="vehicle velocity [m/s]")

s1                     = dy.system_input( dy.DataTypeFloat64(1), name='s1',                    default_value=0.0,    value_range=[-0.05, 0.05], title="steering rate 1 [rad/s]")
s2                     = dy.system_input( dy.DataTypeFloat64(1), name='s2',                    default_value=0.0,    value_range=[-0.05, 0.05], title="steering rate 2 [rad/s]")
s3                     = dy.system_input( dy.DataTypeFloat64(1), name='s3',                    default_value=0.0,    value_range=[-0.05, 0.05], title="steering rate 3 [rad/s]")
s4                     = dy.system_input( dy.DataTypeFloat64(1), name='s4',                    default_value=0.0,    value_range=[-0.05, 0.05], title="steering rate 4 [rad/s]")
s5                     = dy.system_input( dy.DataTypeFloat64(1), name='s5',                    default_value=0.0,    value_range=[-0.05, 0.05], title="steering rate 5 [rad/s]")

initial_steering       = dy.system_input( dy.DataTypeFloat64(1), name='initial_steering',      default_value=-0.0,   value_range=[-40, 40],   title="initial steering angle [degrees]")       * dy.float64(math.pi / 180.0)
initial_orientation    = dy.system_input( dy.DataTypeFloat64(1), name='initial_orientation',   default_value=0.0,    value_range=[-360, 360], title="initial orientation angle [degrees]")    * dy.float64(math.pi / 180.0)

# parameters
wheelbase = 3.0

# sampling time
Ts = 0.01

steering_rate = dy.float64(0)

cnt = dy.counter()

steering_rate = dy.conditional_overwrite(steering_rate, condition= cnt >= dy.int32(200), new_value=s1 )
steering_rate = dy.conditional_overwrite(steering_rate, condition= cnt >= dy.int32(400), new_value=s2 )
steering_rate = dy.conditional_overwrite(steering_rate, condition= cnt >= dy.int32(600), new_value=s3 )
steering_rate = dy.conditional_overwrite(steering_rate, condition= cnt >= dy.int32(800), new_value=s4 )
steering_rate = dy.conditional_overwrite(steering_rate, condition= cnt >= dy.int32(1000), new_value=s5 )

# linearly increasing steering angle
delta = dy.euler_integrator( steering_rate, Ts, initial_state=initial_steering )
delta = dy.saturate(u=delta, lower_limit=-math.pi/2.0, upper_limit=math.pi/2.0)

# the model of the vehicle
x, y, psi, x_dot, y_dot, psi_dot = discrete_time_bicycle_model(delta, velocity, Ts, wheelbase, psi0=initial_orientation)

#
# outputs: these are available for visualization in the html set-up
#

dy.append_output(x, 'x')
dy.append_output(y, 'y')
dy.append_output(psi, 'psi')
dy.append_output(psi_dot, 'psi_dot')
dy.append_output(delta, 'steering')
dy.append_output(velocity*dy.float64(1.0), 'velocity')

# generate code for Web Assembly (wasm), requires emcc (emscripten) to build
code_gen_results = dy.generate_code(template=dy.TargetWasm(), folder="generated/trajectory_generation", build=True)

#
dy.clear()

```

    compiling system simulation (level 0)...
    Generated code will be written to generated/trajectory_generation .
    Running compiler: emcc --bind -s MODULARIZE=1 -s EXPORT_NAME="ORTD_simulator" generated/trajectory_generation/main.cpp -O2 -s -o generated/trajectory_generation/main.js
    Compilation result:  0

```python
compiled_system = dyexe.CompiledCode(code_gen_results)
```

Run the simulation given parameters to define the steering rates

```python
testsim = dyexe.SystemInstance(compiled_system)

N=1000
input_data = {'initial_steering': 0.0, 's1' : 0.1, 's2' : -0.15, 's3' : 0.2 }

# sim_results = run_batch_simulation(testsim, input_data, N )
sim_results = dyexe.run_batch_simulation(testsim, input_data, N,  output_keys=['x', 'y', 'psi', 'psi_dot', 'steering', 'velocity'] )
```

# Add noise to the model (sensing noise)

Simulate measurement noise which is, e.g., introduced by GPS.

```python
# X/Y positioning noise (normal distribution)
eta_x = np.random.normal(0, 0.1, N)
eta_y = np.random.normal(0, 0.1, N)

x_meas = eta_x + sim_results['x']
y_meas = eta_y + sim_results['y']
psi_meas = sim_results['psi']
psi_dot_meas = sim_results['psi_dot']
v_meas = sim_results['velocity']

plt.figure()
plt.plot(x_meas, y_meas, '+')
plt.plot(sim_results['x'], sim_results['y'])
plt.show()

plt.figure()
plt.plot(psi_dot_meas)
plt.show()
```

# Extended Kalman filter

The extended Kalman filter is applied to the linearized model descibed by the matrices $A_{dscr}$ and $B_{dscr}$ and given the system function $f_{dscr}$. The simulated data serves as measured data and is the input to the filter.

```python
f_dscr
```

$\displaystyle \left[\begin{matrix}T_{s} v \cos{\left(\delta + \psi \right)} + x\\T_{s} v \sin{\left(\delta + \psi \right)} + y\\\frac{T_{s} v \sin{\left(\delta \right)}}{l_{r}} + \psi\end{matrix}\right]$

```python
A_dscr
```

$\displaystyle \left[\begin{matrix}1 & 0 & - T_{s} v \sin{\left(\delta + \psi \right)}\\0 & 1 & T_{s} v \cos{\left(\delta + \psi \right)}\\0 & 0 & 1\end{matrix}\right]$

```python
B_dscr
```

$\displaystyle \left[\begin{matrix}- T_{s} v \sin{\left(\delta + \psi \right)} & T_{s} \cos{\left(\delta + \psi \right)}\\T_{s} v \cos{\left(\delta + \psi \right)} & T_{s} \sin{\left(\delta + \psi \right)}\\\frac{T_{s} v \cos{\left(\delta \right)}}{l_{r}} & \frac{T_{s} \sin{\left(\delta \right)}}{l_{r}}\end{matrix}\right]$

The implemented filter in form of a loop

```python
l_r = wheelbase

# allocate space to store the filter results
results = {'delta' : np.zeros(N), 'x' : np.zeros(N), 'y' : np.zeros(N), 'psi' : np.zeros(N) }

# the guess/estimate of the initial states
X = np.matrix([ [0.5], [0.5], [0.1] ])

P = np.matrix([  [0.1, 0,   0     ],
                 [0,   0.1, 0     ],
                 [0,   0,   0.1   ]  ])

# covariance of the noise w addtitive to the states
Q = 0.00001*np.matrix([  [1, 0, 0 ],
                         [0, 1, 0 ],
                         [0, 0, 1 ]  ])

# covariance of the noise v in the measured system output signal
R = np.matrix([  [0.1, 0   ],
                 [0  , 0.1 ]  ])

for i in range(0,N):

    # measured input signals
    v       = v_meas[i]
    x       = x_meas[i]
    y       = y_meas[i]
    psi_dot = psi_dot_meas[i]

    # compute steering angle by the inverse for the vehicle orientation change
    delta = math.asin( psi_dot * l_r / v )

    # system output vector (x, y)
    z = np.matrix([ [x], [y] ])

    # pridiction step using the non-linear model (f_dscr)
    # x(k-1|k-1) --> x(k|k-1)

    X[0] = X[0] + Ts * ( v * math.cos( X[2] + delta ) )
    X[1] = X[1] + Ts * ( v * math.sin( X[2] + delta ) )
    X[2] = X[2] + Ts * ( v / l_r * math.sin(delta) )

    # optionally use the auto-generated python function for evaluation
    # X = f_dscr_fn( Ts, l_r, float(X[0]), float(X[1]), float(X[2]), v, delta )

    # evaluate jacobi matrices A_dscr and B_dscr
    F = np.matrix([  [1, 0, -Ts*v*math.sin(delta+X[2])  ],
                     [0, 1,  Ts*v*math.cos(delta+X[2])  ],
                     [0, 0, 1                           ]   ])

    # optionally use the auto-generated python function for evaluation
    # F = A_dscr_fn( Ts, l_r, float(X[0]), float(X[1]), float(X[2]), v, delta )

    B = np.matrix([  [-Ts*v*math.sin(delta+X[2]), Ts*math.cos(delta+X[2])      ],
                     [ Ts*v*math.cos(delta+X[2]), Ts*math.sin(delta+X[2])      ],
                     [Ts*v/l_r * math.cos(delta), Ts/l_r * math.sin(delta)     ]   ])

    # optionally use the auto-generated python function for evaluation
    # B = B_dscr_fn( Ts, l_r, float(X[0]), float(X[1]), float(X[2]), v, delta )

    # the system output matrix: returns X and Y when multiplied with the state vector X
    # which are compared to the measurements
    H = np.matrix([  [1,0,0],
                     [0,1,0] ])

    # prdicted state covariance P(k|k-1)
    P = F*P*F.transpose() + Q

    # estimation output residual vector
    e = z - H*X

    # Kalman gain
    S = H*P*H.transpose() + R
    K = P*H.transpose() * np.linalg.inv( S )

    # post priori state X(k|k)
    X = X + K*e

    # post priori covariance
    P = (np.eye(3) - K*H) * P

    # store results
    results['delta'][i] = delta
    results['x'][i] = X[0]
    results['y'][i] = X[1]
    results['psi'][i] = X[2]

# show results
plt.figure()
#plt.plot(x_meas, y_meas, '+')
plt.plot(sim_results['x'], sim_results['y'], 'g')
plt.plot(results['x'], results['y'])
plt.show()

plt.figure()
plt.plot(results['psi'])
plt.plot(sim_results['psi'], 'g')
plt.show()
```


# The "kitchen sink" problem

```python
%matplotlib inline
import numpy as np
from scipy.optimize import fsolve
from scipy import integrate
import matplotlib.pyplot as plt
from clawpack import pyclaw
from clawpack import riemann
from clawpack.visclaw.ianimate import ianimate
import matplotlib
plt.style.use('seaborn-talk')
from IPython.display import HTML
```

Our next example is something you can experiment with in your own home.  Go to the kitchen sink, position the faucet over a flat part of the sink (not over the drain), and turn it on.  Carefully examine the flow of water in the sink.  You will see that near the jet of water coming from the faucet, there is a region of very shallow, very fast-flowing water.  Then at a certain distance, the water suddenly becomes deeper and slower moving.  This jump is relatively stable and will approximately form a circle if the bottom of the sink is flat around the area under the faucet.

Here's a demonstration in case you don't have a sink handy:

```python
HTML('')

```

This jump (known as a *hydraulic jump*) is a shock wave and the entire flow can be modeled as a sort of multidimensional Riemann problem.  Instead of left and right states, we have *inner* and *outer* states.  To investigate this phenomenon we'll again use the shallow water equations.  We'll assume the flow has cylindrical symmetry -- in other words, it depends on the distance away from the center (where water falls from the faucet), but not on the angular coordinate.

## Shallow water flow in cylindrical symmetry
The amount of water contained in an annular region $r_1< r <r_2$ is proportional to $r$ and to the depth, so in cylindrical coordinates the conserved mass is $rh$.  Similarly, the conserved momentum is $rhu$.  The conservation laws for these two quantities read
\begin{align}
    (rh)_t + (rhu)_r & = 0 \label{mass1} \\
    (rhu)_t + (rhu^2)_r + r \left(\frac{1}{2}gh^2\right)_r = 0. \label{mom1}
\end{align}

We have placed the coordinate $r$ inside the time derivative in order to emphasize what the conserved quantities are; of course, $r$ does not depend on $t$.  We can rewrite the equations above so that the left hand side is identical to the 1D shallow water equations, but at the cost of introducing geometric *source terms* on the right:
\begin{align}
    h_t + (hu)_r & = -\frac{hu}{r} \label{mass2} \\
    (hu)_t + \left(hu^2 + \frac{1}{2}gh^2\right)_r & = -\frac{hu^2}{r} \label{mom2}
\end{align}

### Steady profiles
Let us first look for time-independent solutions of the equations above.  Setting the time derivatives to zero, we obtain

\begin{align}
    (rhu)_r & = 0 \label{constant_mass} \\
    \left(hu^2 + \frac{1}{2}gh^2\right)_r & = -\frac{hu^2}{r} \label{constant_2}
\end{align}
Equation (\ref{constant_mass}) can be integrated to obtain $rhu = \beta$ where $\beta$ (evidently the flux through a circle) is an arbitrary constant.  Using this to eliminate $u$ in equation (\ref{constant_2}) leads to the ODE

\begin{align} \label{hdiff0}
    h'(r) = \frac{h}{\frac{g}{\beta^2} r^3 h^3 -r}.
\end{align}

Let us define the **Froude number**, which is a measure of the ratio of fluid velocity to gravitational effects:

$$
    F = \frac{|u|}{\sqrt{gh}}
$$

We say that the flow is *subcritical* if $F<1$, and *supercritical* if $F>1$.  Since the characteristic speeds for the system are $u \pm \sqrt{gh}$, in generic terms the flow is *subsonic* if $F<1$ and *supersonic* if $F>1$.  We can rewrite (\ref{hdiff0}) as

\begin{align} \label{hdiff}
    h'(r) = \frac{h}{r} \cdot \frac{F^2}{1-F^2}.
\end{align}
The sign of $h'(r)$ thus depends entirely on the Froude number.  Notice that in the limit $F\to\infty$ (i.e., in the absence of gravity), we have simply $h'(r) = -h/r$, with solution $h \propto 1/r$.  This corresponds to the purely geometric effect of water spreading as it flows outward at constant velocity.

Notice also that the RHS of \eqref{hdiff} blows up as $|F|$ approaches unity.  This means that a smooth steady flow must be either subsonic everywhere or supersonic everywhere; there is no smooth way to transition between the two.

```python
def steady_rhs(h,r,alpha,g=1.):
    return h/(g/alpha**2 * r**3 * h**3 - r)

r = np.linspace(0.5, 10)
h0 = 1.; u0 = 1.; alpha = r[0]*h0*u0
h = np.squeeze(integrate.odeint(steady_rhs,h0,r,args=(alpha,0.)))  # Zero gravity
plt.plot(r,h);  plt.title('$F=\infty$'); plt.xlabel('r'); plt.ylabel('h');
```

### Supercritical flow ($F>1$)
Suppose we have a steady flow that is supercritical (everywhere, by the argument above).
In the presence of gravity, $h$ is still a decreasing function of $r$ if $F>1$.  We see that the depth $h$ falls off somewhat faster than $1/r$.  Since $rhu$ is constant, this means that the velocity $u=1/(hr)$ must increase with $r$.  Hence the flow becomes shallower and faster as it moves outward; the Froude number increases.  Asymptotically, the falloff in depth approaches the $1/r$ rate and the velocity approaches a constant value.

```python
h0 = 1.; u0 = 2.; alpha = r[0]*h0*u0; g=1.
h = np.squeeze(integrate.odeint(steady_rhs,h0,r,args=(alpha,g)));
u = alpha/(h*r)
plt.figure(figsize=(12,4));
plt.subplot(1,3,1); plt.title('Depth');
plt.xlabel('r'); plt.ylabel('h');
plt.plot(r,h);
plt.subplot(1,3,2);  plt.title('Velocity');
plt.xlabel('r'); plt.ylabel('u');
plt.plot(r,alpha/(r*h));
plt.subplot(1,3,3); plt.title('Froude number');
plt.xlabel('r'); plt.ylabel('$F$');
plt.plot(r,alpha/(r*h*np.sqrt(g*h)));
plt.tight_layout();
```

### Subcritical flow ($F<1$)

Meanwhile, if the flow is subsonic then $h(r)$ is increasing and the steady profile is subsonic everywhere.

```python
h0 = 1.; u0 = 0.5; alpha = r[0]*h0*u0; g=1.
h = np.squeeze(integrate.odeint(steady_rhs,h0,r,args=(alpha,g)));
u = alpha/(h*r)
plt.figure(figsize=(12,4));
plt.subplot(1,3,1); plt.title('Depth');
plt.xlabel('r'); plt.ylabel('h');
plt.plot(r,h);
plt.subplot(1,3,2);  plt.title('Velocity');
plt.xlabel('r'); plt.ylabel('u');
plt.plot(r,alpha/(r*h));
plt.subplot(1,3,3); plt.title('Froude number');
plt.xlabel('r'); plt.ylabel('$F$');
plt.plot(r,alpha/(r*h*np.sqrt(g*h)));
plt.tight_layout();
```

A different and complementary approach to deriving steady profiles (see (Zobeyer 2013) is to recognize that in such a solution, the energy $gh^2 + \frac{hu^2}{2}$ is constant.  More simply

$$
    h + \frac{u^2}{2g} = \gamma,
$$
for some constant $\gamma$.  Combining this with conservation of mass yields a cubic equation for the depth:
$$
h^3 -  \beta h^2 + \frac{\alpha^2}{2gr^2}=0.
$$
In non-dimensionalized coordinates, with $H=h/h_0$, $R = r/r_0$, and letting $F_0$ denote the Froude number at $r_0$, this becomes simply
$$
H^3 - \left(1+\frac{1}{2}F_0^2\right)H^2 + \frac{F_0^2}{2R^2} = 0.
$$
This can also be solved to obtain the depth as a function of radius; the result, of course, agrees with that obtained from the differential equation above.  The supercritical and subcritical flows correspond to different roots of the cubic.

## The hydraulic jump
To transition from supersonic to subsonic, the flow must *jump* over the pole of the ODE above, through the presence of a hydraulic jump.  This is a standing shock wave; since we have an outward-oriented flow, this jump must be a 1-shock (in order to be stationary).

### Rankine-Hugoniot jump conditions
The jump condition arising from the continuity equation is the same as that obtained for shallow water in one dimension, since the value of $r$ at the shock location simply divides out:

$$
    s (h_r - h_l) = h_r u_r - h_l u_l.
$$

The momentum equation (\ref{mom1}) seems harder to deal with.  We might try to derive appropriate conditions from (\ref{mom2}) by using some averaged values of $h$ and $u$ to model the source term as a delta function (this makes sense for the shallow water equations with a bathymetric source term).  A simpler tactic is to return to (\ref{mom1}), which yields the jump condition

$$
    s (r h_r u_r - r h_l u_l) = r h_r u_r^2 + \frac{gr}{2}h_r^2 - r h_l u_l^2 - \frac{gr}{2}h_l^2.
$$
Again, we can divide through by $r$ to obtain the same jump condition that is familiar from the one-dimensional shallow water equations:
$$
    s (h_r u_r - h_l u_l) = h_r u_r^2 - h_l u_l^2 + \frac{g}{2}(h_r^2 - h_l^2).
$$
It makes sense that the jump conditions for the cylindrical SW system are the same as those for the 1D SW system, since a cylindrical shock occurs at a single value of $r$.

Unlike the 1D case, however, it does not make sense to consider a Riemann problem in which the left and right states are uniform in space, since those are not temporally steady states of the system.  Instead, we can consider two steady profiles with a jump between them.  For infinitesimal times, in a neighborhood of the initial jump, the solution would then be close to the solution of the 1D problem; at later times it could be much more complex, as the waves from the Riemann problem interact with the structure of the steady states.  But if the Riemann solution consists of a single stationary shock, then the solution will be steady for all time.  This is just the kind of solution that is relevant to our kitchen sink experiment.

### A stationary 1-shock
We know from the analysis above that the hydraulic jump we are looking for is a stationary 1-shock (if it were a 2-shock it would necessarily move outward, since the fluid velocity $u>0$).  In this case the shock speed $s=0$ and the first jump condition is simply

$$
    h_r u_r = h_l u_l.
$$
From (LeVeque, pp. 265-266, Eqn. (13.18)), we have that for 1-shocks,

$$
    h_r u_r = h_l u_l + \alpha\left[ u_l - \sqrt{gh_l\left(1+\frac{\alpha}{h_l}\right)\left(1+\frac{\alpha}{2h_l}\right)}\right],
$$
where $\alpha = h_r - h_l$.  We can find a shock that satisfies the jump condition either by setting $\alpha=0$ (which is the uninteresting case where there is no jump) or by setting the quantity in brackets equal to zero.  The latter condition yields
$$
    \alpha = \frac{-3h_l \pm \sqrt{h_l^2 + 8 h_l u_l^2/g}}{2}
$$
Since we know the depth should increase (from left to right) at the hydraulic jump, we take the plus sign.  Then the value above can be written in terms of the Froude number as

\begin{align}
    \alpha = \frac{3h_l}{2}\left(\sqrt{1+\frac{8}{9}(F_l^2-1)}-1\right), \label{depth_jump}
\end{align}
where $F_l = u_l/\sqrt{gh_l}$ is the Froude number of the left state.

## A steady solution with a hydraulic jump
To find a steady solution with a hydraulic jump, we impose steady supercritical flow for $r<r_0$, a jump defined by \eqref{depth_jump} at $r=r_0$ (with $h_l$ and $F_l$ being the depth and Froude number just inside the jump radius), and steady subcritical flow for $r>r_0$.  The steady flow profiles can be obtained by numerically integrating \eqref{hdiff0}.

The code below implements this solution.  The parameters $h_{in}, u_{in}$ are the depth and velocity at the inner radius of the domain.  The function returns the depth and velocity at equispaced points in $r$, as well as values of $h$ and $u$ that will be needed to impose appropriate boundary conditions via ghost cells in a finite volume simulation.

```python
def initial_and_boundary_data(r_jump = 1.,r_inner = 0.5,r_outer = 5.,
                              num_cells = 501,g=1.,h_in=1.,u_in=2.):

    r = pyclaw.Dimension(r_inner, r_outer, num_cells, name='r')
    rc = r.centers
    i_jump = np.argmin(np.abs(rc-r_jump))

    # Left boundary
    h_inner = h_in
    u_inner = u_in
    beta_inner = r_inner*h_inner*u_inner

    h = 0*rc
    u = 0*rc

    d = r.delta
    rvals = np.insert(rc[:i_jump+1],(0),[rc[0]-2*d,rc[0]-d])
    beta = rvals[0]*h_inner*u_inner

    hh = integrate.odeint(steady_rhs,h_inner,rvals,args=(beta,g))
    hh = np.squeeze(hh)
    uu = beta/(hh*rvals)
    h[:i_jump+1] = np.squeeze(hh[2:])
    u[:i_jump+1] = uu[2:]
    lower_bc_data = [hh[:2], uu[:2]]

    # Jump in h
    # Left side of jump
    h_m = h[i_jump]; u_m = u[i_jump]
    aleph = (-3*h_m+np.sqrt(h_m**2+8*h_m*u_m**2/g))/2.
    # Right side of jump
    h_p = h_m + aleph; u_p = h_m*u_m/h_p
    h[i_jump+1] = h_p; u[i_jump+1] = u_p

    # Outer part of solution
    beta_outer = rc[i_jump+1]*h[i_jump+1]*u[i_jump+1]
    rvals = np.append(rc[i_jump+1:],[rc[-1]+d,rc[-1]+2*d])

    hh = integrate.odeint(steady_rhs,h_p,rvals,args=(beta_outer,g))
    hh = np.squeeze(hh)
    uu = beta_outer/(rvals*hh)
    h[i_jump+1:] = hh[:-2]
    u[i_jump+1:] = uu[:-2]
    upper_bc_data = [hh[-2:],uu[-2:]]

    return h, u, upper_bc_data, lower_bc_data, rc
```

Here's an example of a solution:

```python
h, u, _, _, rc = initial_and_boundary_data()
plt.plot(rc, h)
plt.xlim(0.5,5)
```

## Finite volume simulation

To check that the solution we've obtained is truly steady, we set up a finite volume simulation using PyClaw.

```python
def step_radial_src(solver,state,dt):
    """
    Geometric source terms for SW equations with cylindrical symmetry.
    Integrated using a 2-stage, 2nd-order Runge-Kutta method.
    This is a Clawpack-style source term routine, which approximates
    the integral of the source terms over a step.
    """
    dt2 = dt/2.

    q = state.q
    rad = state.grid.r.centers

    h = q[0,:]
    u = q[1,:]/h

    qstar = np.empty(q.shape)

    qstar[0,:] = q[0,:] - dt2/rad * h*u
    qstar[1,:] = q[1,:] - dt2/rad * h*u*u

    h = qstar[0,:]
    u = qstar[1,:]/h

    q[0,:] = q[0,:] - dt/rad * h*u
    q[1,:] = q[1,:] - dt/rad * h*u*u

def inner_state(state,dim,t,qbc,auxbc,num_ghost):
    h =  state.problem_data['lower_bc_data'][0]
    u =  state.problem_data['lower_bc_data'][1]
    qbc[0,:num_ghost] = h
    qbc[1,:num_ghost] = h*u

def outer_state(state,dim,t,qbc,auxbc,num_ghost):
    h =  state.problem_data['upper_bc_data'][0]
    u =  state.problem_data['upper_bc_data'][1]
    qbc[0,-num_ghost:] = h
    qbc[1,-num_ghost:] = h*u

def setup(r_jump=1.,r_inner=0.5,r_outer=3.,num_cells=501,g=1.):
    r = pyclaw.Dimension(r_inner, r_outer, num_cells=num_cells, name='r')
    h, u, upper_bc_data, lower_bc_data, _ = \
            initial_and_boundary_data(r_jump=r_jump,g=g, r_inner=r_inner,
                                      r_outer=r_outer, num_cells=num_cells)

    solver = pyclaw.ClawSolver1D(riemann_solver=riemann.shallow_roe_with_efix_1D)
    solver.bc_lower[0] = pyclaw.BC.custom
    solver.user_bc_lower = inner_state
    solver.bc_upper[0] = pyclaw.BC.custom
    solver.user_bc_upper = outer_state
    solver.step_source = step_radial_src
    domain = pyclaw.Domain([r])
    state = pyclaw.State(domain,solver.num_eqn)
    state.problem_data['grav'] = g
    state.problem_data['lower_bc_data'] = lower_bc_data
    state.problem_data['upper_bc_data'] = upper_bc_data

    state.q[0,:] = h
    state.q[1,:] = h*u

    claw = pyclaw.Controller()
    claw.solver = solver
    claw.solution = pyclaw.Solution(state,domain)
    claw.tfinal = 15.0
    claw.keep_copy = True
    claw.num_output_times = 50
    return claw
```

```python
claw = setup()
claw.verbosity=0
claw.run()

ianimate(claw)
```

### Dirichlet BCs with transition from super- to subsonic flow generically create a hydraulic jump

Although the result above took a lot of work, it is not very impressive -- it doesn't do anything!  You might be wondering whether this steady solution is dynamically stable -- i.e., whether the flow will converge to this state if it is initially different.  The answer is yes; in fact, any boundary data that implies a transition from supercritical to subcritical flow will lead to a hydraulic jump.  In the example below, we impose such boundary data but initialize $h$ and $u$ with constant values, to show the emergence of the jump.

```python
def setup_constant_initial_data(r_jump=1.,r_inner=0.5,r_outer=3.,
                                num_cells=501,g=1.):
    r = pyclaw.Dimension(r_inner, r_outer, num_cells=num_cells, name='r')

    solver = pyclaw.ClawSolver1D(riemann_solver=riemann.shallow_roe_with_efix_1D)
    solver.bc_lower[0] = pyclaw.BC.custom
    solver.user_bc_lower = inner_state
    solver.bc_upper[0] = pyclaw.BC.custom
    solver.user_bc_upper = outer_state
    solver.step_source = step_radial_src
    domain = pyclaw.Domain([r])
    state = pyclaw.State(domain,solver.num_eqn)
    state.problem_data['grav'] = g

    hl = 0.5; hul = 3.
    hr = 2.; hur = 0.1
    state.problem_data['lower_bc_data'] = np.array([[hl,hl],[hul,hul]])
    state.problem_data['upper_bc_data'] = np.array([[hr,hr],[hur,hur]])

    state.q[0,:] = 1.
    state.q[1,:] = 0.

    claw = pyclaw.Controller()
    claw.solver = solver
    claw.solution = pyclaw.Solution(state,domain)
    claw.tfinal = 15.0
    claw.keep_copy = True
    claw.num_output_times = 50
    return claw
```

```python
claw = setup_constant_initial_data()
claw.verbosity=0
claw.run()

ianimate(claw)
```

## Comparison with experimental results
We may ask how well our model corresponds to reality.  There are many approximations made in deriving the shallow water equations used here; perhaps most notably, we have completely neglected viscosity and surface tension.  Viscosity in particular is believed to be very important in the very shallow flow just inside the jump radius.

### Inflow conditions
Experimentally it is difficult to measure the depth near the jet.  We can eliminate that dependence (and the apparent dependence on our choice of inner radius) by considering the radius of the vertical jet, which we denote by $a$, and the  flow rate, denoted by $Q$.  Then

$$
Q = u_{jet} \pi a^2 = 2 \pi r u(r) h(r)
$$
The first expression comes from considering flow in the jet, while the second comes from considering flow through a circle anywhere outside the jet (centered on the jet).  If we suppose that $u$ is approximately constant (recall that it actually increases somewhat with $r$) then we have $u(r)\approx u_{jet}$, and consequently
$$
h(r) = a^2/(2r).
$$
Using these equations, with a specified flow rate $Q$ and jet radius $a$, along with a chosen inner radius $r_0\ge a$, we can determine the correct values of $h_0$ and $u_0.$  It can be shown that the results obtained in this way are only very weakly sensitive to our choice of $r_0$.

```python
def compute_inner_values(Q,a,r0):
    """
        Q: flow rate
        a: jet radius
        r0: inner domain radius
    """
    assert r0 >= a
    h0 = a**2/(2*r0)
    u0 = Q/(2*np.pi*r0*h0)
    return h0, u0
```

### Locating the jump
In the examples above, we selected the boundary data based on a prescribed jump location.  But in practice we can't choose where the jump is -- we'd like to predict that!

We can predict the location of the jump based on prescribed inflow conditions ($r_0, h_0, u_0$ and a prescribed far-field depth $h_\infty$) as follows:

1.  Set $\beta = r_0 h_0 u_0$.  Choose a finite outer radius $r_\infty \gg r_0$.  Set $u_\infty$ so that $r_\infty h_\infty u_\infty = \beta$.
2.  Integrate (\ref{hdiff}) outward from $(r_0,h_0)$ to obtain a profile $h_\text{outward}$.
3.  Integrate (\ref{hdiff}) inward from $(r_\infty, h_\infty)$ to obtain a profile $h_\text{inward}$.
4.  Compute $\phi(r) = h_\text{outward} - h_\text{inward}$ and determine the value of $r$ such that $\phi(r) = \alpha$, with $\alpha$ given by (\ref{depth_jump}).

```python
def jump_location(Q,r_jet,h_inf,r_inf=100.,g=1.,r0=None,tol=1./10000):
    r"""Predict location of hydraulic jump for given inner
        radius flow and asymptotic depth."""

    if r0 == None:
        r0 = r_jet
    h0, u0 = compute_inner_values(Q,r_jet,r0)

    F_in = u0/np.sqrt(g*h0)  # Inflow Froude number
    assert F_in > 1  # Initial flow must be supercritical

    r = np.linspace(r0,r_inf,int(round(1./tol)))
    beta = r0 * h0 * u0
    u_inf = u0 * (r0/r_inf) * (h0/h_inf)

    F_inf = u_inf/np.sqrt(g*h_inf)  # Far-field Froude number
    assert F_inf < 1  # Far field flow must be subcritical

    # Integrate outward
    hh_in = np.squeeze(integrate.odeint(steady_rhs,h0,r,args=(beta,g)))
    uu_in = beta/(r*hh_in)

    hh_out = np.squeeze(integrate.odeint(steady_rhs,h_inf,-r[::-1],args=(beta,g)))
    hh_out = hh_out[::-1]

    F_l = uu_in/np.sqrt(g*hh_in) # Froude number for left state
    phi = hh_in - hh_out + 1.5*hh_in*(np.sqrt(1.+8./9.*(F_l**2-1.))-1)

    jump_loc = np.argmin(np.abs(phi))
    profile = 0*r
    profile[:jump_loc] = hh_in[:jump_loc]
    profile[jump_loc:] = hh_out[jump_loc:]
    return r[jump_loc], r, profile
```

```python
r_jump, r, profile = jump_location(Q=200.,r_jet=1.,h_inf=1.,g=980.,tol=1.e-6)
print('Jump radius: '+str(r_jump)+' cm')
plt.clf()
plt.plot(r,profile)
plt.xlim(r[0],10);
```

### Watson's experiment
Here we use data from an experiment conducted in (Watson, 1964); see p. 496 therein.  The unit of length therein is feet; here we have converted everything to centimeters.  The experimental jump location is at a radius of about 17 cm.  Let's see what our model gives.

```python
Q = 202. # Flow rate (in cm^3/s)
r_jet = 0.3175 # Nozzle radius (in cm)
h_inf = 0.343 # Depth at infinity (in cm)
g = 980. # Gravity (in cm/s^2)
r_jump, r, profile = jump_location(Q,r_jet,h_inf,r_inf=500.,g=980.,tol=1.e-6)
print('Predicted jump radius: '+str(r_jump)+' cm')
print('Measured jump radius: 17 cm')
plt.plot(r,profile)
plt.xlim(r[0],r[-1]);
```

Clearly, some of the effects we have ignored must be important!  In particular, as Watson (and others) argue, viscosity or friction becomes very significant in the shallow flow before the jump, causing the jump to emerge much closer to the jet than this inviscid model predicts.

### Experiments of Craik et. al.
Here we compare with two more experiments; see Table 1 of (Craik et. al., 1981).

```python
Q = 4.48
r_jet = 0.1
h_inf = 0.18
r_jump, r, profile = jump_location(Q,r_jet,h_inf,r_inf=50.,g=980.,tol=1.e-6)
print('Predicted jump radius: '+str(r_jump)+' cm')
print('Measured jump radius: 1.2 cm')
plt.plot(r,profile)
plt.xlim(r[0],r[-1]);
```

```python
Q = 26.
r_jet = 0.215
h_inf = 0.33
r_jump, r, profile = jump_location(Q,r_jet,h_inf,r_inf=200.,g=980.,tol=1.e-6)
print('Predicted jump radius: '+str(r_jump)+' cm')
print('Measured jump radius: 2.3 cm')
plt.plot(r,profile)
plt.xlim(r[0],r[-1]);
```

The difference is less extreme, but still very substantial.  It is worth noting also that if we consider water flowing onto an infinite flat plate, the purely hyperbolic model (with no viscosity or friction) doesn't predict any jump at all, because there is no mechanism forcing the flow to transition to a subsonic state.  We can observe this in the simulation if we set the boundary condition at the outer radius to outflow.

In the simulation below, we start with a steady-state solution involving a hydraulic jump, but allow outflow at the outer boundary ((here this is imposed approximately using zero-order extrapolation).

```python
claw = setup()
claw.solver.bc_upper[0] = pyclaw.BC.extrap
claw.verbosity = 0
claw.run()

ianimate(claw)
```