```python
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
from scipy.optimize import root
import pandas as pd
```

```python
plt.style.use('fivethirtyeight')
```

# Problem 12.8 - Fogler - 5ed

The gas-phase reversible reaction

\begin{equation}
A \leftrightarrows B
\end{equation}

is carried out under high pressure in a packed-bed reactor with pressure drop. The feed consists of both inerts **I** and Species **A** with the ratio of inerts to the species A being 2 to 1. The entering molar flow rate of A is 5 mol/min at a temperature of 300 K and a concentration of 2 mol/dm3. **Work this problem in terms of volume.**

_Additional information:_

Parameter | Value | Parameter | Value
- | - | - | -
Fao | 5.0 mol/min | T0 | 300 K
CA0 | 2 mol/dm$^3$ | Ti | 300 K
CI | 2 CA0 | k (300 K) |  0.1 1/min
CpI |18 cal/mol/K | Ua | 150 cal/dm3/min/K
CPA |160 cal/mol/K | $T_{ao}$ | 300 K
E | 10,000 cal/mol | V | 40 dm$^3$
$\Delta H$Rx | −20,000 cal/mol | $\alpha \rho_b$ | 0.02 1/dm$^3$
KC(300 K) | 1,000 | |
CPB | 160 cal/mol/K | m_c | 50 mol/min
$\rho_B$ |1.2 kg/dm3 | Cpcool | 20 cal/mol/K

Plot X, Xe, p, T, and the rate of disappearance as a function of V up to V = 40 dm3. Explain why the curves look the way they do.

1. Co-current heat exchange
2. Countercurrent heat exchange
3. Constant heat-exchanger temperature, Ta
4. Compare and constrast each of the above results and the results for adiabatic operation
2. Vary the ratio of inerts to A (0 ≤ ΘI ≤ 10) and describe what you find.
3. Plot the Qr and Ta as function of V necessary to maintain isothermal operation.

```python
# Data

Fa0 = 5.0           # initial molar flowrate A, mol/min
CA0 = 2             # initial concentration A, mol/dm$^3$
CI0 = 2*CA0         # initial concentration Inert, mol/dm3

T0 = 300            # initial reactor temperature, K
Ti = 300            # inlet inert temperature, K
Ta0 = 300           # inlet coolant temperature, K

CPA = 160           # cal/mol/K
CPB = 160           # cal/mol/K
CPI = 18            # cal/mol/K
Cpcool = 20         # cal/mol/K
Ua = 150            # cal/dm3/min/K

k_300 = 0.1         # 1/min
KC_300 = 1000
Ea = 10000          # cal/mol
DHRx = -20000       # cal/mol

V = 40              # dm$^3$

rho_B = 1.2         # kg/dm3
alpha_rho_b = 0.02  # 1/dm$^3$

m_c = 50             # mol/min

Rgas = 1.987
```

# Preliminar calculations

```python
DCp = CPB - CPA - CPI
Vo = Fa0 / CA0           # inlet volumetric flow rate, dm3/min
Fi0 = Vo * CI0

Ft0 = Fa0 + Fi0

Ct0 = Ft0 / Vo          # Initial Total Concentration
```

# Mathematical Model for PFR

```python
def system(v, Y, parameters):
    """
      This function solves a generic case of a pfr.
      Input:  Y - dependent variable {x: fractional conversion of A,
                                      t: reactor temperature,
                                      p: ratio P/Po,
                                      ta: coolant temperature}
              v - independent variable { v: reactor volume}
              p - parameters {phi: ratio Fi0/FAo,
                              if_adiabatic: True if reactor is adiabatic
                              how_operates_HX: 'constant' if Ta is kept constant
                                               'cocurrent' if co-current flow
                                               'countercurrent' if counter-current flow}
      Output: dx_dv, dt_dv, dp_dv, dta_dv - solution for x, t, p, and ta
    """

    x, t, p, ta = Y
    phii, if_adiabatic, to, how_operates_HX, if_isothermal = parameters

    v = Vo * (1/p) * (t/to)   # total volumetric flowrate
    fa = Fa0 * (1 - x)        # molar flowrate of A
    fb = Fa0 * x              # molar flowrate of B
    fi = Fi0                  # molar flowrate of I

    ft = fa + fb + fi         # total molar flowrate

    ca = fa / v               # molar concentration of A
    cb = fb / v               # molar concentration of B

    # Kinetic parameters
    k = k_300 * np.exp(-Ea/Rgas * (1/t - 1/300))
    kc = KC_300 * np.exp(-DHRx/Rgas * (1/t - 1/300))
    ra = -k * (ca - cb / kc)

    if if_adiabatic:
        ua = 0
    else:
        ua = Ua

    # Energy balance inside the reactor
    qg = ra * DHRx
    qr = ua * (t - ta)

    if if_isothermal:
        qr = qg

    # Operation mode for HX
    if how_operates_HX == 'constant':
        qr = 0
    elif how_operates_HX == 'countercurrent':
        ua = -ua

    # System of ODEs
    dx_dv = -ra/Fa0
    dp_dv = -alpha_rho_b / (2 * p) * (t/to) * (ft/Ft0)
    dt_dv = (qg - qr) / (Fa0 * (CPA + phii * CPI + DCp * x))
    dta_dv = qr / (m_c * Cpcool)

    return [dx_dv, dt_dv, dp_dv, dta_dv]

def get_xe_ra(x, t, p, to):
    """
      This function solve the equilibrium conversion and the reaction rate.
      Input:  x: fractional conversion of A,
              t: reactor temperature,
              p: ratio P/Po,
      Output: Xe, -rA
    """
    v = Vo * (1/p) * (t/to)   # total volumetric flowrate
    fa = Fa0 * (1 - x)        # molar flowrate of A
    fb = Fa0 * x              # molar flowrate of B
    fi = Fi0                  # molar flowrate of I

    ft = fa + fb + fi         # total molar flowrate

    ca = fa / v               # molar concentration of A
    cb = fb / v               # molar concentration of B

    # Kinetic parameters
    k = k_300 * np.exp(-Ea/Rgas * (1/t - 1/300))
    kc = KC_300 * np.exp(-DHRx/Rgas * (1/t - 1/300))
    ra = -k * (ca - cb / kc)
    xe = kc / (1 + kc)
    return [xe, -ra]

def solve_scenario(parameters):
    fun = lambda v, Y: system(v, Y, parameters)
    vspan = [0, V]  #  independent variable range
    solode = solve_ivp(fun, vspan, iCs, t_eval=np.linspace(0, V), method='LSODA')
    vreactor = solode.t
    xa = solode.y[0]
    t = solode.y[1]
    p = solode.y[2]
    ta = solode.y[3]

    to = parameters[2]

    xe, ra = get_xe_ra(xa, t, p, to)

    fig, ((ax1, ax2), (ax3, ax4)) = plt.subplots(2, 2, figsize=(10, 10), sharex=True)

    ax1.plot(vreactor, xa, color='black', label='conversion')
    ax1.plot(vreactor, xe, color='#ed713a', ls=':', label='equilibrium')
    ax1.set_ylabel('Conversion, adim')
    ax1.legend(loc='best')

    ax2.plot(vreactor, p, ls='--')
    ax2.set_ylabel('P/P0, adim')

    ax3.plot(vreactor, t, color='red', ls='-.', label='Reactor')
    ax3.plot(vreactor, ta, color='green', label='Coolant')
    ax3.set_ylabel('Temperature, K')
    ax3.set_xlabel(r'Reactor volume, dm$^3$')
    ax3.legend(loc='best')

    ax4.plot(vreactor, ra, color='#79204c')
    ax4.set_xlabel(r'Reactor volume, dm$^3$')
    ax4.set_ylabel(r'-$r_A$, 1/min')

    plt.tight_layout()
    plt.show()
    return (vreactor, xa, t, p, ta, xe, ra)
```

# Part (a) Co-current heat exchange

```python
summary = []
iCs = [0, T0, 1, Ta0]
Phi_i = 2
Parameters = [Phi_i, False, T0, 'cocurrent', False]

Vreactor_a, Xa_a, Temp_a, p_a, Ta_a, Xe_a, rA_a = solve_scenario(Parameters)

summary.append([Xa_a[-1], Temp_a[-1], p_a[-1], Ta_a[-1], Xe_a[-1], rA_a[-1]])
```

# Part (b) Counter-current heat exchange

```python
def find_Ta0(tao):
    params = [Phi_i, False, T0, 'countercurrent', False]

    fun = lambda v, Y: system(v, Y, params)
    ics = [0, T0, 1, tao]

    solode = solve_ivp(fun, [0, V], ics, t_eval=np.linspace(0, V), method='LSODA')
    T_calc = solode.y[3][-1]
    return T_calc - Ta0

```

```python
to_ideal = root(find_Ta0, 250).x[0]
```

    /opt/homebrew/Caskroom/miniconda/base/lib/python3.8/site-packages/numpy/core/_asarray.py:102: VisibleDeprecationWarning: Creating an ndarray from ragged nested sequences (which is a list-or-tuple of lists-or-tuples-or ndarrays with different lengths or shapes) is deprecated. If you meant to do this, you must specify 'dtype=object' when creating the ndarray.
      return array(a, dtype, copy=False, order=order)

```python
iCs = [0, T0, 1, to_ideal]
params = [Phi_i, False, T0, 'countercurrent', False]
Vreactor_b, Xa_b, Temp_b, p_b, Ta_b, Xe_b, rA_b = solve_scenario(params)
summary.append([Xa_b[-1], Temp_b[-1], p_b[-1], Ta_b[-1], Xe_b[-1], rA_b[-1]])
```

# Part (c) Constant heat-exchanger temperature, Ta

```python
iCs = [0, T0, 1, Ta0]
Parameters = [Phi_i, False, T0, 'constant', False]

fun = lambda v, Y: system(v, Y, Parameters)

Vreactor_c, Xa_c, Temp_c, p_c, Ta_c, Xe_c, rA_c = solve_scenario(Parameters)
summary.append([Xa_c[-1], Temp_c[-1], p_c[-1], Ta_c[-1], Xe_c[-1], rA_c[-1]])
```

# Part (d) Adiabatic operation

```python
Parameters = [Phi_i, True, T0, 'cocurrent', False]

Vreactor_d, Xa_d, Temp_d, p_d, Ta_d, Xe_d, rA_d = solve_scenario(Parameters)
summary.append([Xa_d[-1], Temp_d[-1], p_d[-1], Ta_d[-1], Xe_d[-1], rA_d[-1]])
```

# Part (e). Vary the ratio of inerts to

Let's vary the ratio of inerts to A in the following values: [0, 2, 5]

**non-adiabatic and non-isothermal reactor**

```python
vs_reactor = []
xas = []
p_ratios = []
temps_reactor = []
temps_coolant = []

phis = [0, 1, 2, 5]

Vspan = [0, V]
for i in range(len(phis)):

    Parameters = [phis[i], False, T0, 'cocurrent', False]
    fun = lambda v, Y: system(v, Y, Parameters)
    solode = solve_ivp(fun, Vspan, iCs, t_eval=np.linspace(0, V), method='LSODA')

    vs_reactor.append(solode.t)
    xas.append(solode.y[0])
    temps_reactor.append(solode.y[1])
    p_ratios.append(solode.y[2])
    temps_coolant.append(solode.y[3])
```

```python
linestyles = ['solid', 'dotted', 'dashed', 'dashdot']
fig, ((ax1, ax2), (ax3, ax4)) = plt.subplots(2, 2, figsize=(12, 7), sharex=True)

for i in range(len(phis)):
    ax1.plot(vs_reactor[i], xas[i],
            linestyle=linestyles[i],
            label=(r'$\phi_i$= %s' % phis[i]))

    ax2.plot(vs_reactor[i], p_ratios[i],
        linestyle=linestyles[i],
        label=(r'$\phi_i$ = %s' % phis[i]))

    ax3.plot(vs_reactor[i], temps_reactor[i],
        linestyle=linestyles[i],
        label=(r'$\phi_i$= %s' % phis[i]))

    ax4.plot(vs_reactor[i], temps_coolant[i],
        linestyle=linestyles[i],
        label=(r'$\phi_i$ = %s' % phis[i]))

    plt.legend(loc='best')

ax1.set_ylabel('Conversion, adim')
ax2.set_ylabel('P/P0, adim')
ax3.set_xlabel(r'Reactor volume, dm$^3$')
ax3.set_ylabel('Reactor temperature, K')
ax4.set_xlabel(r'Reactor volume, dm$^3$')
ax4.set_ylabel('Coolant tempeture, K')
plt.tight_layout()
plt.show()
```

# Part (f). Isothermal operation

In order to operate the reactor under isothermal conditions, the removed heat should be equal to the heat generated

\begin{align}
Q_g &= r_A \, DHRx \\
Q_r &= Ua \, (T - T_a)
\end{align}

Therefore, let's modify the system of ODEs to include this change. So,

\begin{equation}
qr = qg
\end{equation}

```python
Parameters = [Phi_i, False, T0, 'cocurrent', True]

Vreactor_f, Xa_f, Temp_f, p_f, Ta_f, Xe_f, rA_f = solve_scenario(Parameters)
summary.append( [Xa_f[-1], Temp_f[-1], p_f[-1], Ta_f[-1], Xe_f[-1], rA_f[-1]])
```

```python
Qr = -Ua * (Temp_f - Ta_f)
fig, ax = plt.subplots(figsize=(6, 6))
ax.plot(Vreactor_f, Qr,
       ls='-.')

ax.set_xlabel('Reactor volume, dm$^3$')
ax.set_ylabel('Heat removed, $Q_r$ [cal/dm$^3$/min]')
plt.tight_layout()
plt.show()
```

```python
index_name = ['(a) Co-current', '(b) Counter-current', '(c) Constant Ta', '(d) Adiabatic', '(f) Isothermal']
column_name = ['Xa', 'Temp', 'P/P0', 'Ta', 'Xe', '-rA']
```

```python
pd.DataFrame(summary, columns=column_name, index=index_name)
```

      Xa
      Temp
      P/P0
      Ta
      Xe
      -rA

      (a) Co-current
      0.914623
      346.852224
      0.291578
      346.706621
      0.914954
      1.759687e-04

      (b) Counter-current
      0.575420
      303.689160
      0.465618
      300.000000
      0.998499
      4.778921e-02

      (c) Constant Ta
      0.654819
      368.913768
      0.176776
      300.000000
      0.654819
      1.541716e-09

      (d) Adiabatic
      0.654819
      368.913768
      0.176776
      300.000000
      0.654819
      1.541716e-09

      (f) Isothermal
      0.702696
      300.000000
      0.446864
      370.269598
      0.999001
      2.650809e-02

```python

```


```python
%config InlineBackend.figure_format = 'retina'
%matplotlib inline

import numpy as np
import matplotlib.pyplot as plt

plt.style.use(['ggplot', 'assets/class.mplstyle'])

red = '#E24A33'
blue = '#348ABD'
purple = '#988ED5'
gray = '#777777'
yellow = '#FBC15E'
green = '#8EBA42'
pink = '#FFB5B8'
```

# AS4501

# Regression

# Bibliography: Chapter 8 Ivezic+

# Regression:

- Finding the relation between a dependent variable $y$ and a set of independent variables $\bf x$. It describes the expectation value of $y$ given $\bf x$, $E[y|{\bf x}]$.

- The dependent variable $y$ is also called **response variable**.

- The problem is to find the expectation of $y$ given $\bf x$:

$E[y|{\bf x}] = f({\bf x}, \theta) + \epsilon$

where $\epsilon$ is a random error.

- Note that we estimate $E[y | {\bf x}]$, the conditional expectation of $y$ given $\bf x$, rather than $p(y, {\bf x})$, the joint distribution of $y$ and $\bf x$, which is a much harder problem to solve. Regression is a simpler problem: instead of determining the multidimensional pdf, we would like to infer the expectation of $y$ given $\bf x$.

- **The goal of regression usually focuses on estimation of the parameters $\theta$ of the function and their confidence intervals.**

```python

```

## Metrics:

## Mean squared error (MSE)

The mean squared error (MSE) or mean squared deviation (MSD) of an estimator measures the average of the squares of the errors:

It is defined by (estimation of $y$):

## $MSE \equiv \frac{1}{n} \sum_i (y_i - \hat{y}_i)^2$

## Coefficient of determination (R$^2$)

It is the proportion of the variance in the dependent variable that is predictable from the independent variable(s).

It is defined as:

## $R^2 \equiv 1  - \frac{SS_{res}}{SS_{tot}}$,

where

### $ SS_{res} = \sum_i (y_i - \hat{y}_i)^2$

and

### $ SS_{tot} = \sum_i (y_i - \bar{y})^2$

```python

```

## Variance and bias

The variance of an estimator of the parameter $\theta$ will be:

## $Var_{\hat{\theta}} = E_{\hat{\theta}}\biggl[ \bigl(\hat{\theta} - E_{\hat{\theta}}[\hat{\theta}]\bigr)^2\biggr]$

The bias of an estimator will be:

## $Bias_{\hat{\theta}} = E_{\hat{\theta}} - \theta$

```python

```

## Relation between MSE, variance and bias

Note that when estimating a parameter the MSE equals the variance plus the bias squared

```python

```

# Linear regression

The one dimensional linear regression model is defined by

$f(x, \theta) = \theta_0 + \theta_1 x$

## Least squares regression for linear regression

## Ordinary least squares

Minimizes the residual sum of squares (RSS) between the observed response values $y_i$ and the model predictions

### $\min RSS = \min \sum\limits_{i=1}^{n} (y_i - \theta_0 - \theta_1 x_i)^2$

Differentiating w.r.t. to $\theta_0$ and $\theta_1$ and setting these derivatives to zero gives two equations which can be solved to give:

## $\hat{\theta}_{1, OLS} = \frac{\sum\limits_{i=1}^n (x_i - \bar{x}) (y_i - \bar{y})}{\sum\limits_{i=1}^n (x_i - \bar{x})^2} = \frac{S_{xy}}{S_{xx}}$

## $\hat{\theta}_{0, OLS} = \bar{y} - \hat{\theta}_{1,OLS} ~\bar{x}$

These estimators are also the maximum likelihood estimator (MLE) when the errors are normally distributed.

The OLS estimator of the error variance $\sigma^2$ is:

## $\hat{\sigma}^2 = S^2 = \frac{RSS}{n - 2}$

If the errors are i.i.d. random variables, the slope and intercept are unbiased and asymptotically normally distributed:

## $\hat{\theta}_{1, OLS} \sim N\biggl(\theta_1, \frac{\sigma^2}{S_{XX}}\biggr)$

## $\hat{\theta}_{0, OLS} \sim N\biggl(\theta_0, \sigma^2 \biggl(\frac{1}{n} + \frac{\bar{x}^2}{S_{XX}}\biggr)\biggr)$

If the errors are also normally distributed with zero mean and known variance $\sigma^2$, then the variance is $\chi^2$ distributed:

## $\hat{\sigma}^2 \sim \biggl(\frac{\sigma^2}{n - 2} \biggr) \chi_{n-2}^{2}$

However, $\sigma^2$ is rarely known, and only the sample variance $S^2$ is known, changing the statistical properties of the estimators.

Defining:

## $ T = \frac{\hat{\theta_1} - \theta_1}{SE}$

where $SE = \frac{S}{\sqrt{S_{XX}}}$ is also known as the **standard error**,

one can show that $T$ is t distributed with $n-2$ degrees of freedom.

Then, the $(1-\alpha)$ confidence interval for $\theta_1$ becomes $(\hat{\theta}_1 - t_{\alpha/2, n-2} ~ SE, \hat{\theta}_1 + t_{\alpha/2, n-2} ~ SE)$ and the confidence interval for $y$ at a given $x$ becomes:

## $y(x) = \hat{\theta}_0 + \hat{\theta}_1 x \pm t_{\alpha/2, n - 2} ~ S ~ \sqrt{1 + \frac{1}{n} \frac{(x - \bar{x})^2}{S_{xx}}}$

```python

```

### Maximum likelihood estimator (MLE)

Let us consider the probability of observing $\lbrace y_i \rbrace$ given the parameters $\theta$ and the set of independent variable $\lbrace x_i \rbrace$.

Here we assume that the observations are Gaussian with standard deviation $\sigma_i$:

## $p(\lbrace y_i \rbrace | \lbrace x_i \rbrace, \theta) = \prod\limits_{i = 1}^{N} \frac{1}{\sqrt{2 \pi} \sigma_i} \exp \biggl( \frac{- (y_i - (\theta_0 + \theta_1 x_i))^2}{2 \sigma_i^2} \biggr)$

Thus, we can maximize the log likelihood minus a constant to find the **maximum likelihood estimator (MLE)**

## $\log(L) = \log p(\lbrace y_i \rbrace | \lbrace x_i \rbrace, \theta) = \sum\limits_{i = 1}^{N}  \bigl( \frac{- (y_i - (\theta_0 + \theta_1 x_i))^2}{2 \sigma_i^2} \bigr)$

which is equivalent to minimizing the RSS

Using Bayes theorem $P(A|B) = P(B|A)~\dfrac{P(A)}{P(B)}$ we can also see that

## $P(\theta | \lbrace y_i \rbrace, \lbrace x_i \rbrace) = P(\lbrace y_i \rbrace | \lbrace x_i \rbrace, \theta) ~ \frac{p(\theta)}{p(y_i)}$

Therefore, this is also the **maximum a posteriori (MAP)** estimator assuming a flat prior for $\theta$.

```python

```

## Online estimation

This figure illustrates how the uncertainties in the parameters shrink as the number of observations grow.

```python

```

### Diabetes dataset example
(https://scikit-learn.org/stable/auto_examples/linear_model/plot_ols.html#sphx-glr-auto-examples-linear-model-plot-ols-py)

```python
import matplotlib.pyplot as plt
import numpy as np
from sklearn import datasets, linear_model
from sklearn.metrics import mean_squared_error, r2_score

# Load the diabetes dataset
diabetes = datasets.load_diabetes()
print(diabetes.DESCR)
```

    .. _diabetes_dataset:

    Diabetes dataset
    ----------------

    Ten baseline variables, age, sex, body mass index, average blood
    pressure, and six blood serum measurements were obtained for each of n =
    442 diabetes patients, as well as the response of interest, a
    quantitative measure of disease progression one year after baseline.

    **Data Set Characteristics:**

      :Number of Instances: 442

      :Number of Attributes: First 10 columns are numeric predictive values

      :Target: Column 11 is a quantitative measure of disease progression one year after baseline

      :Attribute Information:
          - Age
          - Sex
          - Body mass index
          - Average blood pressure
          - S1
          - S2
          - S3
          - S4
          - S5
          - S6

    Note: Each of these 10 feature variables have been mean centered and scaled by the standard deviation times `n_samples` (i.e. the sum of squares of each column totals 1).

    Source URL:
    https://www4.stat.ncsu.edu/~boos/var.select/diabetes.html

    For more information see:
    Bradley Efron, Trevor Hastie, Iain Johnstone and Robert Tibshirani (2004) "Least Angle Regression," Annals of Statistics (with discussion), 407-499.
    (https://web.stanford.edu/~hastie/Papers/LARS/LeastAngle_2002.pdf)

```python
# Use only one feature (body mass index)
diabetes_X = diabetes.data[:, np.newaxis, 2]

# Split the data into training/testing sets
diabetes_X_train = diabetes_X[:-20]
diabetes_X_test = diabetes_X[-20:]

# Split the targets into training/testing sets
diabetes_y_train = diabetes.target[:-20]
diabetes_y_test = diabetes.target[-20:]

# Create linear regression object
regr = linear_model.LinearRegression()

# Train the model using the training sets
regr.fit(diabetes_X_train, diabetes_y_train)

# Make predictions using the testing set
diabetes_y_pred = regr.predict(diabetes_X_test)

# The coefficients
print('Coefficients: \n', regr.coef_)
# The mean squared error
print("Mean squared error (MSE): %.2f"
      % mean_squared_error(diabetes_y_test, diabetes_y_pred))
# Explained variance score: 1 is perfect prediction
print('Variance score (coefficient of determination, R2): %.2f' % r2_score(diabetes_y_test, diabetes_y_pred))
```

    Coefficients:
     [938.23786125]
    Mean squared error (MSE): 2548.07
    Variance score (coefficient of determination, R2): 0.47

```python
fig, ax = plt.subplots(figsize = (11, 7))

ax.scatter(diabetes_X_test, diabetes_y_test,  c = 'k')
ax.scatter(diabetes_X_train, diabetes_y_train, c = 'gray', alpha = 0.3)
ax.plot(diabetes_X_test, diabetes_y_pred, color=blue, linewidth=3)
ax.set_xlabel("BMI (scaled)")
ax.set_ylabel("Diabetes disease progression")
```

```python

```

## Least squares in matrix form

The previous equations can be derived in matrix form

In general one can define regression in terms of a design matrix $M$:

## $y = {\bf M} ~ \theta$

where $y$ is the $N$–dimensional vector of values $y_i$.

## $y = \begin{bmatrix} y_0 \\ y_1 \\ . \\ y_{N-1} \end{bmatrix}$

For straight--line regression, $\theta$ is a two--dimensional vector:

## $\theta = \begin{bmatrix} \theta_0 \\ \theta_1 \end{bmatrix}$

and $M$ is a $2 \times N$ matrix:

## ${\bf M} = \begin{bmatrix} 1 & x_0 \\ 1 & x_1 \\ ... & ... \\ 1 & x_{N-1} \\ \end{bmatrix}$

For the case of heteroscedastic uncertainties, we can define the covariance matrix $C$:

## $ C = \begin{bmatrix} \sigma_0^2 & 0 & . & 0 \\ 0 & \sigma_1^2 & . & 0 \\ . & . & . & . \\ 0 & 0 & . & \sigma_{N - 1}^2\\ \end{bmatrix}$

The MLE solution for this regression is:

## ${\bf \hat{\theta}} = ({\bf M}^T C^{-1} {\bf M})^{-1} ({\bf M}^T C^{-1} {\bf y})$

which is the also the solution that minimizes the equation

## $({\bf y} - {\bf M} ~ \theta)^{-1} C^{-1} ({\bf y} -  {\bf M} ~ \theta)$

The uncertainties on the regression coefficients $\bf \theta$ can be expressed as the following matrix (general case):

## $\Sigma_\theta = \begin{bmatrix} \sigma_{\theta_0}^2 & \dots & \sigma_{\theta_0 \theta_i} & \dots \\ \vdots & \ddots & \vdots & \vdots \\ \sigma_{\theta_0 \theta_i} & \dots & \sigma_{\theta_i}^2 \\  \vdots & \vdots & \vdots & \ddots \\ \end{bmatrix} = [{\bf M}^T C^{-1} {\bf M}]^{-1}$

In the general case, $C$ can be non--diagonal too, which indicates correlated noise.

```python

```

## Polynomial regression

Let us consider the case where we perform a polynomial representation of the data, e.g.:

## $y_i = \theta_0 + \theta_1 x_i + \theta_2 x_i^2 + \theta_3 x_i^3$

In this case the design matrix $M$ becomes:

## $ M = \begin{bmatrix}
1 & x_0 & x_0^2 & x_0^3 \\
1 & x_1 & x_1^2 & x_1^3 \\
. & . & . & . \\
1 & x_N & x_N^2 & x_N^3 \\
\end{bmatrix}$

And the previous formulae can be used to find the least squares solution since the coefficients are linear

```python

```

## Regularization

Sometimes we may want to trade an increase in bias for a reduction in variance (e.g. if the matrix $M$ is ill-conditioned the variance could be very large)

## Ridge regression

In this case we include the following regularization term in the least squares equation:

## $({\bf y} - M~ \theta)^{-1} C^{-1}({\bf y} - M~ \theta) + \lambda~ |\theta^T \theta|$

$\lambda$ is called the **regularization or smoothing parameter** and **$|\theta^T \theta|$** is called the **penalty function**

It can be shown that the new solution will be:

## ${\bf \hat{\theta}} = ({\bf M}^T C^{-1} {\bf M} + \lambda I)^{-1} ({\bf M}^T C^{-1} {\bf y})$

It is worth noting that even if $M^T C^{-1} M$ were singular, $M^T C^{-1} M + \lambda I$ may not be singular

### Bayes equivalent

In the Bayesian approach, it can be shown that the MAP solution is the same as the ridge solution if we assume a Gaussian prior for the parameters $\theta$:

## $ p(\theta) \propto \exp \biggl( \frac{- \lambda (\theta^T \theta)}{2} \biggr)$

Ridge regression effectively constrains the norm of the parameters $\theta$:

## $|{\bf \theta}|^2 < s$

```python

```

## LASSO regression

Another common regularization technique

In this case we include a different regularization term in the least squares equation:

## $({\bf y} - \theta {\bf X})^{-1}({\bf y} - \theta {\bf X}) + \lambda |\theta|$

Now we use the $\ell_1$ norm instead of the $\ell_2$ norm.

In this case there is no closed form, but it can be shown that the MAP solution is the same if an exponential prior for the parameters $\theta$ is assumed.

LASSO regression effectively constrains the $\ell_1$ norm of the parameters $\theta$:

## $|{\bf \theta}| < s$

```python

```

### Understanding Ridge and LASSO regression

```python

```

# Comparison between ordinary, ridge and LASSO regression

```python
from sklearn import linear_model

X_train = np.c_[.5, 1].T
y_train = [.5, 1]
X_test = np.c_[0, 2].T

np.random.seed(0)

classifiers = dict(ols=linear_model.LinearRegression(),
                   ridge=linear_model.Ridge(alpha=.05),
                    lasso=linear_model.Lasso(alpha=.02))

for name, clf in classifiers.items():
    fig, ax = plt.subplots(figsize=(4, 3))

    for _ in range(6):
        this_X = .1 * np.random.normal(size=(2, 1)) + X_train
        clf.fit(this_X, y_train)

        ax.plot(X_test, clf.predict(X_test), color='gray')
        ax.scatter(this_X, y_train, s=3, c='gray', marker='o', zorder=10)

    clf.fit(X_train, y_train)
    ax.plot(X_test, clf.predict(X_test), linewidth=2, color=blue)
    ax.scatter(X_train, y_train, s=30, c=red, marker='+', zorder=10)

    ax.set_title(name)
    ax.set_xlim(0, 2)
    ax.set_ylim((0, 1.6))
    ax.set_xlabel('X')
    ax.set_ylabel('y')

    fig.tight_layout()

plt.show()
```

```python

```

## Choosing the regularization parameter $\lambda$

One way to choose $\lambda$ is to do k-fold cross-validation on $\lambda$, defining the error:

## $ Err(\lambda) = k^{1} \sum_k N_k^{-1} \sum\limits_{i}^{N_k} \frac{[y_i - f(x_i | \theta)]^2}{\sigma_i^2}$

and then choose the $\lambda$ with the smallest error.

```python

```

## Outliers

## Huber loss

The Huber estimator minimizes

## $ \sum\limits_{i=1}^N e(y_i|y)$

where $e(y_i|y)$ is modeled as:

## $\begin{equation}
  \phi(t)=\left\{
  \begin{array}{@{}ll@{}}
    \frac{1}{2} t^2, & \text{if}\ |t|\le c \\
    c|t| - \frac{1}{2} c^2, & \text{otherwise}
  \end{array}\right.
\end{equation}$

With $t = y_i - \hat{y}_i$

## Bayesian Outlier Methods

Alternative to the Huber loss, we can modify the likelihood function by assuming a mixture model, where some points belong to an outlier distribution, which is modeled as a Gaussian with mean $\mu_b$ and variance $V_b$, and others belong to the main population, which follow the linear model.

1. Assume that the probability of belonging to the outlier population is $p_b$

2. Assume that each point can be tagged as either outlier ($g_i = 0$) or not ($g_i = 1$)

```python

```

```python
from sklearn.datasets import make_regression
from sklearn.linear_model import HuberRegressor, Ridge

# Generate toy data.
rng = np.random.RandomState(0)
X, y = make_regression(n_samples=20, n_features=1, random_state=0, noise=4.0,
                       bias=100.0)

# Add four strong outliers to the dataset.
X_outliers = rng.normal(0, 0.5, size=(4, 1))
y_outliers = rng.normal(0, 2.0, size=4)
X_outliers[:2, :] += X.max() + X.mean() / 4.
X_outliers[2:, :] += X.min() - X.mean() / 4.
y_outliers[:2] += y.min() - y.mean() / 4.
y_outliers[2:] += y.max() + y.mean() / 4.
X = np.vstack((X, X_outliers))
y = np.concatenate((y, y_outliers))
plt.plot(X, y, 'b.')

# Fit the huber regressor over a series of epsilon values.
colors = ['r-', 'b-', 'y-', 'm-']

x = np.linspace(X.min(), X.max(), 7)
epsilon_values = [1.35, 1.5, 1.75, 1.9]
for k, epsilon in enumerate(epsilon_values):
    huber = HuberRegressor(fit_intercept=True, alpha=0.0, max_iter=100,
                           epsilon=epsilon)
    huber.fit(X, y)
    coef_ = huber.coef_ * x + huber.intercept_
    plt.plot(x, coef_, colors[k], label="huber loss, %s" % epsilon)

# Fit a ridge regressor to compare it to huber regressor.
ridge = Ridge(fit_intercept=True, alpha=0.0, random_state=0, normalize=True)
ridge.fit(X, y)
coef_ridge = ridge.coef_
coef_ = ridge.coef_ * x + ridge.intercept_
plt.plot(x, coef_, 'g-', label="ridge regression")

plt.title("Comparison of HuberRegressor vs Ridge")
plt.xlabel("X")
plt.ylabel("y")
plt.legend(loc=0)
plt.show()
```

```python

```

## Cross-validation

Analogous to the classification problem, one can also do cross-validation during regression to determine the best hyperparameters. For example, choosing the regularization parameter for ridge or lasso regression, or choosing the polynomial degree of a polynomial fit.


# Equations
Here we are solving a set of coupled differential equations gives (from Chapman supplement and Arne notebook)

$$\begin{align}
    i \dot{\zeta}_1&= (p_1 B + q_1 B^2 + q\mu_1(t))\zeta_1+c\left[(\zeta_1^*\zeta_1+\zeta_0^*\zeta_0-\zeta_{-1}^*\zeta_{-1})\zeta_1+\zeta_0\zeta_0\zeta_{-1}^*)\right]\\
    i\dot{\zeta}_0&=(p_0 B + q_0 B^2 + q\mu_0(t))\zeta_0+c\left[(\zeta_1^*\zeta_1 + \zeta_{-1}^*\zeta_{-1})\zeta_0 + 2\zeta_1\zeta_{-1}\zeta_0^*\right]\\
    i \dot{\zeta}_{-1}&= (p_{-1} B + q_{-1} B^2 + q\mu_{-1}(t))\zeta_{-1}+c\left[(\zeta_{-1}^*\zeta_{-1}+\zeta_0^*\zeta_0-\zeta_{1}^*\zeta_{1})\zeta_{-1}+\zeta_0\zeta_0\zeta_{1}^*)\right]\\
\end{align}
$$

# Expectation Values
First we note that
$$ S_x = \frac{1}{\sqrt{2}}\begin{pmatrix}
0&1&0\\
1&0&1\\
0&1&0\\
\end{pmatrix}
$$
So to calculate its expectation value with a given wavefunction, we have
$$\psi^*S_x \psi =  (\psi_1^*,\psi_0^*,\psi_{-1}^*)\frac{1}{\sqrt{2}}\begin{pmatrix}
0&1&0\\
1&0&1\\
0&1&0\\
\end{pmatrix}
\begin{pmatrix}
\psi_1\\
\psi_0\\
\psi_{-1}
\end{pmatrix} = \frac{(\psi_1^*+\psi_{-1}^*)\psi_0 + \psi_0^*(\psi_{1}+\psi_{-1})}{\sqrt{2}}$$

# Inital State (Hamley thesis and Chapman supplement)
We can represent the initial state as a normal distribution in $S_x,S_y, N_{yz}, N_{xz}$, we can then derive starting conditions from these values and evolve the state.

```python
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import ode
from numpy.lib import scimath
import seaborn
import time
from tqdm import trange
%matplotlib inline
```

```python
#first define the system dy/dt = f(y,t)
def msqr(x):
    return x*np.conj(x)

def f(t,y,*args):
    """system of ode we want to solve"""
    z1i = y[0]
    z0i = y[1]
    zmi = y[2]
    #now define equations
    f0 = (q*z1i + c*((msqr(z1i) + msqr(z0i) - msqr(zmi))*z1i + z0i**2*np.conj(zmi)))*np.complex(0,-1)
    f1 = (c*((msqr(z1i) + msqr(zmi))*z0i) + 2*c*z1i*zmi*np.conj(z0i))*np.complex(0,-1)
    f2 = (q*zmi + c*((msqr(zmi) + msqr(z0i) - msqr(z1i))*zmi + z0i**2*np.conj(z1i)))*np.complex(0,-1)
    return np.asarray([f0,f1,f2])

class Operator(object):
    """class of operator for 3x3 matrices in this problem"""
    def __init__(self, mat,rep):
        self.rep = rep
        self.mat = mat

    def apply(self,ele):
        return np.dot(ele, np.dot(self.mat,np.conj(ele).T)).real
        #return np.dot(np.conj(ele), np.dot(self.mat,ele.T)).real

def find_phase(ele):
    """find the phase theta_1+theta_-1-2theta_0"""
    ans =  np.angle(ele)
    return ans[0] + ans[2] - 2 * ans[1]

def func_int(yn,t):
    return f(t,yn)

def ynplus1(func, yn,t,dt,**kwargs):
    """evolve Runge kutta with function func which takes two input arguments
    yn and t and possibly extra arguments
    :param yn: value at the previous iteration
    :param t: the time at current iteration
    :param dt: time step
    """
    k1 = func(yn,t,**kwargs)
    k2 =  func(yn+dt/2*k1,t+dt/2,**kwargs)
    k3 = func(yn+dt/2*k2,t+dt/2,**kwargs)
    k4 = func(yn+dt*k3,t+dt,**kwargs)
    a = k1+ 2 * k2 + 2 * k3 + k4
    return yn + dt*a/6
```

```python
S_x = Operator(np.array([[0,1,0],[1,0,1],[0,1,0]])*1/np.sqrt(2),r'$S_x$')
N_yz = Operator(1j/np.sqrt(2)* np.array([[0,-1,0],[1,0,1],[0,-1,0]]),r'$N_{yz}#')
rho_0 = Operator(np.array([[0,0,0],[0,1,0],[0,0,0]]),r'$\rho_0$')
```

```python
def generate_states(N1,N0,Nm1,theta,s):
    """generate quasi probability distribution"""
    N = N1 + N0 + Nm1
    r0 = N0/N
    r1 = N1/N
    rm1 = Nm1/N
    a = np.sqrt(2*r0*r1)
    b = np.sqrt(2*r0*rm1)
    sx_mean = np.cos(theta/2)*(a+b)
    sy_mean = np.sin(theta/2)*(b-a)
    nyz_mean = -np.sin(theta/2)*(a+b)
    nxz_mean = np.cos(theta/2)*(a-b)
    var_one = N*np.sqrt(r0**2-2*r0*np.sqrt(r1)*np.sqrt(rm1)*np.cos(theta)-r0*r1-r0*rm1+r1**(3/2)*np.sqrt(rm1)*np.cos(theta)+np.sqrt(r1)*rm1**(3/2)*np.cos(theta)+0.25*r1**2+0.5*r1*rm1*np.cos(2*theta) + r1*rm1 + 0.25*rm1**2)
    var_two = N*np.sqrt(r0**2+2*r0*np.sqrt(r1)*np.sqrt(rm1)*np.cos(theta)-r0*r1-r0*rm1-r1**(3/2)*np.sqrt(rm1)*np.cos(theta)-np.sqrt(r1)*rm1**(3/2)*np.cos(theta)+0.25*r1**2+0.5*r1*rm1*np.cos(2*theta) + r1*rm1 + 0.25*rm1**2)
    print(var_one,var_two)
    print(sx_mean,sy_mean,nyz_mean,nxz_mean)
    sx = np.random.normal(loc = sx_mean, scale = 1/np.sqrt(var_one), size = s)
    sy = np.random.normal(loc = sy_mean, scale = 1/np.sqrt(var_two), size = s)
    nyz = np.random.normal(loc = nyz_mean, scale = 1/np.sqrt(var_one), size = s)
    nxz = np.random.normal(loc = nxz_mean, scale = 1/np.sqrt(var_two), size = s)

    txip = np.where((sx+nxz)>0,np.arctan(-(sy + nyz)/(sx+ nxz)),np.arctan(-(sy + nyz)/(sx+ nxz))+np.pi)
    txim = np.where((sx-nxz)>0,np.arctan((sy-nyz)/(sx-nxz)),np.arctan((sy-nyz)/(sx-nxz))+np.pi)
    a = (sx+nxz)**2/(np.cos(txip))**2
    b = (sx-nxz)**2/(np.cos(txim))**2

    rho_0 = 1/2 + scimath.sqrt(1/4-(a+b)/8)
    m = 1/rho_0*(a-b)/8

    states = np.zeros((len(m),3),dtype = complex)

    states[:,0] = scimath.sqrt((1-rho_0+m)/2) * np.exp(txip*1j)
    states[:,1] = scimath.sqrt(rho_0)
    states[:,2] = scimath.sqrt((1-rho_0-m)/2) * np.exp(txim*1j)

    return states
#solve the sucker and plot
def solve_system_fixed(y0):
    dt = 0.0005
    t = []
    sol = []
    num_steps = int(tfinal/dt)
    for i in range(num_steps):
        sol.append(y0)
        t.append(i*dt)
        y0 = ynplus1(func_int,y0,i*dt,dt)
    t = np.asarray(t)
    sol = np.asarray(sol)
    return t, sol

def solve_system(y0):
    r = ode(f).set_integrator('zvode')
    r.set_initial_value(y0,0)
    sol = []
    t= []
    while r.successful() and r.t < tfinal:
        r.integrate(tfinal, step = True)
        sol.append(r.y)
        t.append(r.t)
    t = np.asarray([t])
    sol = np.asarray(sol)
    return np.concatenate((t.T,sol), axis = 1)

def get_exp_values(ans,step_size):
    """function to compute expectation values"""
    sol = ans[:,1:]
    #r_0 = np.asarray([rho_0.apply(i) for i in sol[::step_size]]).real
    r_0 = np.asarray([np.conj(i[1]) * i[1] for i in sol[::step_size]]).real
    sx_calc = np.asarray([S_x.apply(i) for i in sol[::step_size]]).real
    nyz_calc = np.asarray([N_yz.apply(i) for i in sol[::step_size]]).real
    phase = np.asarray([find_phase(i) for i in sol[::step_size]]).real
    return np.asarray([ans[:,0][::step_size].real,r_0, sx_calc, nyz_calc, phase]), len(ans[:,0][::step_size])

def solve_TWA():
    #alot array for computation
    step_size = 1
    ans = []
    t_max = 0
    t_index = 0
    #do calculation
    for i,state in enumerate(states):
        vals, t_num = get_exp_values(solve_system(state),step_size)
        ans.append(vals)
        if t_num > t_max:
            t_max = t_num
            t_index = i
    ans_1 = ans
    t_interp = ans_1[t_index][0]
    #alot array for interpolation
    ans = np.zeros((len(states),4,len(t_interp)))
    for i, vals in enumerate(ans_1):
        for j, row in enumerate(vals[1:]):
            ans[i,j] = np.interp(t_interp,vals[0],row)

    #output routine
    m = np.mean(ans[:,0],axis = 0)
    s = np.std(ans[:,0],axis = 0)
    return t_interp,m,s

def solve_TWA_fixed():
    #alot array for computation
    step_size = 1
    ans = []
    sols = []
    #do calculation
    for i in trange(len(states)):
        state = states[i]
        t, sol = solve_system_fixed(state)
        sols.append(sol)
        #get expectation values
        rho = np.asarray([i[1]*np.conj(i[1]) for i in sol]).real
        ans.append(rho)

    ans = np.asarray(ans)
    m = np.mean(ans,axis=0)
    std = np.std(ans,axis =0)
    return t, m,std, sols

```

```python
#define problem parameters
#dt = 2.1e-5
tfinal = .2
q = -5*2*np.pi
c = 2*np.pi*30
N = 10000
states = generate_states(2000,6000,2000,0,5000)
#print(np.mean(np.abs(states[:,1])**2))
ts = time.time()
#t,m,s = solve_TWA()
tm = time.time()
#states = generate_states(100,4800,100,0,500)
#t1,m1,s1,sol = solve_TWA_fixed()
te = time.time()
print(tm-ts)
print(te-tm)
plt.hist(np.abs(states)**2,bins=50);
```

```python
#sol1 = np.asarray(sol)
#plt.plot(t1,np.mean(msqr(sol1).real, axis = 0)[:,1],label='magfirst')
#plt.plot(t1,m1,label='old')
plt.plot(t,m,label='new')
plt.legend()

```

```python

```


[Table of Contents](http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/table_of_contents.ipynb)

# The Extended Kalman Filter

```python
#format the book
%matplotlib inline
from __future__ import division, print_function
from book_format import load_style
load_style()
```

<style>
@import url('http://fonts.googleapis.com/css?family=Source+Code+Pro');
@import url('http://fonts.googleapis.com/css?family=Vollkorn');
@import url('http://fonts.googleapis.com/css?family=Arimo');
@import url('http://fonts.googleapis.com/css?family=Fira+sans');

.CodeMirror pre {
    font-family: 'Source Code Pro', Consolas, monocco, monospace;
}
    div.cell{
        width: 900px;
        margin-left: 0% !important;
        margin-right: auto;
    }
    div.text_cell code {
        background: transparent;
        color: #000000;
        font-weight: 600;
        font-size: 12pt;
        font-style: bold;
        font-family:  'Source Code Pro', Consolas, monocco, monospace;
   }
    h1 {
        font-family: 'Open sans',verdana,arial,sans-serif;
	}

    div.input_area {
        background: #F6F6F9;
        border: 1px solid #586e75;
    }

    .text_cell_render h1 {
        font-weight: 200;
        font-size: 30pt;
        line-height: 100%;
        color:#c76c0c;
        margin-bottom: 0.5em;
        margin-top: 1em;
        display: block;
        white-space: wrap;
        text-align: left;
    }
    h2 {
        font-family: 'Open sans',verdana,arial,sans-serif;
        text-align: left;
    }
    .text_cell_render h2 {
        font-weight: 200;
        font-size: 16pt;
        font-style: italic;
        line-height: 100%;
        color:#c76c0c;
        margin-bottom: 0.5em;
        margin-top: 1.5em;
        display: block;
        white-space: wrap;
        text-align: left;
    }
    h3 {
        font-family: 'Open sans',verdana,arial,sans-serif;
    }
    .text_cell_render h3 {
        font-weight: 200;
        font-size: 14pt;
        line-height: 100%;
        color:#d77c0c;
        margin-bottom: 0.5em;
        margin-top: 2em;
        display: block;
        white-space: wrap;
        text-align: left;
    }
    h4 {
        font-family: 'Open sans',verdana,arial,sans-serif;
    }
    .text_cell_render h4 {
        font-weight: 100;
        font-size: 14pt;
        color:#d77c0c;
        margin-bottom: 0.5em;
        margin-top: 0.5em;
        display: block;
        white-space: nowrap;
    }
    h5 {
        font-family: 'Open sans',verdana,arial,sans-serif;
    }

    .text_cell_render h5 {
        font-weight: 200;
        font-style: normal;
        color: #1d3b84;
        font-size: 16pt;
        margin-bottom: 0em;
        margin-top: 0.5em;
        display: block;
        white-space: nowrap;
    }
    div.text_cell_render{
        /*font-family: 'Vollkorn', verdana,arial,sans-serif;*/
        line-height: 150%;
        font-size: 130%;
        text-align: justify;
        text-justify:inter-word;
    }
    div.output_subarea.output_text.output_pyout {
        overflow-x: auto;
        overflow-y: scroll;
        max-height: 50000px;
    }
    div.output_subarea.output_stream.output_stdout.output_text {
        overflow-x: auto;
        overflow-y: scroll;
        max-height: 50000px;
    }
    div.output_wrapper{
        margin-top:0.2em;
        margin-bottom:0.2em;
}

    code{
        font-size: 6pt;

    }
    .rendered_html code{
    background-color: transparent;
    }
    ul{
        margin: 2em;
    }
    ul li{
        padding-left: 0.5em;
        margin-bottom: 0.5em;
        margin-top: 0.5em;
    }
    ul li li{
        padding-left: 0.2em;
        margin-bottom: 0.2em;
        margin-top: 0.2em;
    }
    ol{
        margin: 2em;
    }
    ol li{
        padding-left: 0.5em;
        margin-bottom: 0.5em;
        margin-top: 0.5em;
    }
    ul li{
        padding-left: 0.5em;
        margin-bottom: 0.5em;
        margin-top: 0.2em;
    }
    a:link{
       font-weight: bold;
       color:#447adb;
    }
    a:visited{
       font-weight: bold;
       color: #1d3b84;
    }
    a:hover{
       font-weight: bold;
       color: #1d3b84;
    }
    a:focus{
       font-weight: bold;
       color:#447adb;
    }
    a:active{
       font-weight: bold;
       color:#447adb;
    }
    .rendered_html :link {
       text-decoration: underline;
    }
    .rendered_html :hover {
       text-decoration: none;
    }
    .rendered_html :visited {
      text-decoration: none;
    }
    .rendered_html :focus {
      text-decoration: none;
    }
    .rendered_html :active {
      text-decoration: none;
    }
    .warning{
        color: rgb( 240, 20, 20 )
    }
    hr {
      color: #f3f3f3;
      background-color: #f3f3f3;
      height: 1px;
    }
    blockquote{
      display:block;
      background: #fcfcfc;
      border-left: 5px solid #c76c0c;
      font-family: 'Open sans',verdana,arial,sans-serif;
      width:680px;
      padding: 10px 10px 10px 10px;
      text-align:justify;
      text-justify:inter-word;
      }
      blockquote p {
        margin-bottom: 0;
        line-height: 125%;
        font-size: 100%;
      }
</style>

We have developed the theory for the linear Kalman filter. Then, in the last two chapters we broached the topic of using Kalman filters for nonlinear problems. In this chapter we will learn the Extended Kalman filter (EKF). The EKF handles nonlinearity by linearizing the system at the point of the current estimate, and then the linear Kalman filter is used to filter this linearized system. It was one of the very first techniques used for nonlinear problems, and it remains the most common technique.

The EKF provides significant mathematical challenges to the designer of the filter; this is the most challenging chapter of the book. To be honest, I do everything I can to avoid the EKF in favor of other techniques that have been developed to filter nonlinear problems. However, the topic is unavoidable; all classic papers and a majority of current papers in the field use the EKF. Even if you do not use the EKF in your own work you will need to be familiar with the topic to be able to read the literature.

## Linearizing the Kalman Filter

The Kalman filter uses linear equations, so it does not work with nonlinear problems. Problems can be nonlinear in two ways. First, the process model might be nonlinear. An object falling through the atmosphere encounters drag which reduces its acceleration. The drag coefficient varies based on the velocity the object. The resulting behavior is nonlinear - it cannot be modeled with linear equations. Second, the measurements could be nonlinear. For example, a radar gives a range and bearing to a target. We use trigonometry, which is nonlinear, to compute the position of the target.

For the linear filter we have these equations for the process and measurement models:

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

For the nonlinear model these equations must be modified to read:

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

The linear expression $\mathbf{Ax} + \mathbf{Bu}$ is replaced by a nonlinear function $f(\mathbf x, \mathbf u)$, and the linear expression $\mathbf{Hx}$ is replaced by a nonlinear function $h(\mathbf x)$.

You might imagine that we proceed by finding a new set of Kalman filter equations that optimally solve these equations. But if you remember the charts in the **Nonlinear Filtering** chapter you'll recall that passing a Gaussian through a nonlinear function results in a probability distribution that is no longer Gaussian. So this will not work.

The EKF does not alter the Kalman filter's linear equations. Instead, it *linearizes* the nonlinear equations at the point of the current estimate, and uses this linearization in the linear Kalman filter.

*Linearize* means what it sounds like. We find a line that most closely matches the curve at a defined point. The graph below linearizes the parabola $f(x)=x^2−2x$ at $x=1.5$.

```python
import ekf_internal
ekf_internal.show_linearization()
```

If the curve above is the process model, then the dotted lines shows the linearization of that curve for the estimate $x=1.5$.

We linearize systems by taking the derivative, which finds an expression for the slope of the curve:

$$\begin{aligned}
f(x) &= x^2 -2x \\
\frac{df}{dx} &= 2x - 2
\end{aligned}$$

and then evaluating it at $x$:

$$\begin{aligned}m &= f'(x=1.5) \\&= 2(1.5) - 2 \\&= 1\end{aligned}$$

Linearizing systems of differential equations is more complicated. We linearize $f(\mathbf x, \mathbf u)$, and $h(\mathbf x)$ by taking the partial derivatives of each to evaluate $\mathbf A$ and $\mathbf H$ at the point $\mathbf x_t$ and $\mathbf u_t$. We call the partial derivative of a matrix the *Jacobian*. This gives us the the system dynamics matrix and measurement model matrix:

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

$h(\overline{\mathbf x})$ is computed with the prior, but I drop the bar on for notational convenience.

Finally, we find the discrete state transition matrix $\mathbf F$ by using the Taylor series expansion of $e^{\mathbf A \Delta t}$:

$$\mathbf F = e^{\mathbf A\Delta t} = \mathbf{I} + \mathbf A\Delta t  + \frac{(\mathbf A\Delta t)^2}{2!} + \frac{(\mathbf A\Delta t)^3}{3!} + ... $$

Alternatively, you can use one of the other techniques we learned in the **Kalman Math** chapter.

This leads to the following equations for the EKF. I placed them beside the equations for the linear Kalman filter, and put boxes around the changes:

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

We don't normally use $\mathbf{Fx}$ to propagate the state for the EKF as the linearization causes inaccuracies. It is typical to compute $\overline{\mathbf x}$ using a suitable numerical integration technique such as Euler or Runge Kutta. Thus I wrote $\mathbf{\overline x} = f(\mathbf x, \mathbf u)$. For the same reasons we don't use $\mathbf{H\overline{x}}$ in the computation for the residual, opting for the more accurate $h(\overline{\mathbf x})$.

I think the easiest way to understand the EKF is to start off with an example. Later you may want to come back and reread this section.

## Example: Tracking a Airplane

This example tracks an airplane using ground based radar. We implemented a UKF for this problem in the last chapter. Now we will implement an EKF for the same problem so we can compare both the filter performance and the level of effort required to implement the filter.

Radars work by emitting a beam of radio waves and scanning for a return bounce. Anything in the beam's path will reflects some of the signal back to the radar. By timing how long it takes for the reflected signal to get back to the radar the system can compute the *slant distance* - the straight line distance from the radar installation to the object.

The relationship between the radar's slant range distance and bearing with the horizontal position $x$ and altitude $y$ of the aircraft is illustrated in the figure below:

```python
import ekf_internal
ekf_internal.show_radar_chart()
```

This gives us the equalities:

$$\theta = \tan^{-1} \frac y x\\
r^2 = x^2 + y^2$$

### Design the State Variables

We want to track the position of an aircraft assuming a constant velocity and altitude, and measurements of the slant distance to the aircraft. That means we need 3 state variables - horizontal distance, horizonal velocity, and altitude:

$$\mathbf x = \begin{bmatrix}\mathtt{distance} \\\mathtt{velocity}\\ \mathtt{altitude}\end{bmatrix}=    \begin{bmatrix}x \\ \dot x\\ y\end{bmatrix}$$

### Design the Process Model

We assume a Newtonian, kinematic system for the aircraft. We've used this model in previous chapters, so by inspection you may recognize that we want

$$\mathbf F = \left[\begin{array}{cc|c} 1 & \Delta t & 0\\
0 & 1 & 0 \\ \hline
0 & 0 & 1\end{array}\right]$$

I've partioned the matrix into blocks to show the upper left block is a constant velocity model for $x$, and the lower right block is a constant position model for $y$.

However, let's practice finding these matrix for a nonlinear system. We model nonlinear systems with a set of differential equations. We need an equation in the form

$$\dot{\mathbf x} = \mathbf{Ax} + \mathbf{w}$$
where $\mathbf{w}$ is the system noise.

The variables $x$ and $y$ are independent so we can compute them separately. The differential equations for motion in one dimension are:

$$\begin{aligned}v &= \dot x \\
a &= \ddot{x} = 0\end{aligned}$$

Now we put the differential equations into state-space form. If this was a second or greater order differential system we would have to first reduce them to an equivalent set of first degree equations. The equations are first order, so we put them in state space matrix form as

$$\begin{aligned}\begin{bmatrix}\dot x \\ \ddot{x}\end{bmatrix} &= \begin{bmatrix}0&1\\0&0\end{bmatrix} \begin{bmatrix}x \\
\dot x\end{bmatrix} \\ \dot{\mathbf x} &= \mathbf{Ax}\end{aligned}$$
where $\mathbf A=\begin{bmatrix}0&1\\0&0\end{bmatrix}$.

Recall that $\mathbf A$ is the *system dynamics matrix*. It describes a set of linear differential equations. From it we must compute the state transition matrix $\mathbf F$. $\mathbf F$ describes a discrete set of linear equations which compute $\mathbf x$ for a discrete time step $\Delta t$.

A common way to compute $\mathbf F$ is to use the power series expansion of the matrix exponential:

$$\mathbf F(\Delta t) = e^{\mathbf A\Delta t} = \mathbf{I} + \mathbf A\Delta t  + \frac{(\mathbf A\Delta t)^2}{2!} + \frac{(\mathbf A \Delta t)^3}{3!} + ... $$

$\mathbf A^2 = \begin{bmatrix}0&0\\0&0\end{bmatrix}$, so all higher powers of $\mathbf A$ are also $\mathbf{0}$. Thus the power series expansion is:

$$
\begin{aligned}
\mathbf F &=\mathbf{I} + \mathbf At + \mathbf{0} \\
&= \begin{bmatrix}1&0\\0&1\end{bmatrix} + \begin{bmatrix}0&1\\0&0\end{bmatrix}\Delta t\\
\mathbf F &= \begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}
\end{aligned}$$

This is the same result used by the kinematic equations! This exercise was unnecessary other than to illustrate finding the state transition matrix from linear differential equations. We will conclude the chapter with an example that will require the use of this technique.

### Design the Measurement Model

The measurement function takes the state estimate of the prior $\overline{\mathbf x}$ and turn it into a measurement of the slant range distance. For notational convenience I will use $\mathbf x$, not $\overline{\mathbf x}$. We use the Pythagorean theorem to derive:

$$h(\mathbf x) = \sqrt{x^2 + y^2}$$

The relationship between the slant distance and the position on the ground is nonlinear due to the square root. We linearize it by evaluating its partial derivative at $\mathbf x_t$:

$$
\mathbf H = \frac{\partial{h(\mathbf x)}}{\partial{\mathbf x}}\biggr|_{\mathbf x_t}
$$

The partial derivative of a matrix is called a Jacobian, and takes the form

$$\frac{\partial \mathbf H}{\partial \mathbf x} =
\begin{bmatrix}
\frac{\partial h_1}{\partial x_1} & \frac{\partial h_1}{\partial x_2} &\dots \\
\frac{\partial h_2}{\partial x_1} & \frac{\partial h_2}{\partial x_2} &\dots \\
\vdots & \vdots
\end{bmatrix}
$$

In other words, each element in the matrix is the partial derivative of the function $h$ with respect to the $x$ variables. For our problem we have

$$\mathbf H = \begin{bmatrix}{\partial h}/{\partial x} & {\partial h}/{\partial \dot{x}} & {\partial h}/{\partial y}\end{bmatrix}$$

Solving each in turn:

$$\begin{aligned}
\frac{\partial h}{\partial x} &= \frac{\partial}{\partial x} \sqrt{x^2 + y^2} \\
&= \frac{x}{\sqrt{x^2 + y^2}}
\end{aligned}$$

and

$$\begin{aligned}
\frac{\partial h}{\partial \dot{x}} &=
\frac{\partial}{\partial \dot{x}} \sqrt{x^2 + y^2} \\
&= 0
\end{aligned}$$

and

$$\begin{aligned}
\frac{\partial h}{\partial y} &= \frac{\partial}{\partial y} \sqrt{x^2 + y^2} \\
&= \frac{y}{\sqrt{x^2 + y^2}}
\end{aligned}$$

giving us

$$\mathbf H =
\begin{bmatrix}
\frac{x}{\sqrt{x^2 + y^2}} &
0 &
&
\frac{y}{\sqrt{x^2 + y^2}}
\end{bmatrix}$$

This may seem daunting, so step back and recognize that all of this math is doing something very simple. We have an equation for the slant range to the airplane which is nonlinear. The Kalman filter only works with linear equations, so we need to find a linear equation that approximates $\mathbf H$. As we discussed above, finding the slope of a nonlinear equation at a given point is a good approximation. For the Kalman filter, the 'given point' is the state variable $\mathbf x$ so we need to take the derivative of the slant range with respect to $\mathbf x$. For the linear Kalman filter $\mathbf H$ was a constant that we computed prior to running the filter. For the EKF $\mathbf H$ is updated at each step as the evaluation point $\overline{\mathbf x}$ changes at each epoch.

To make this more concrete, let's now write a Python function that computes the Jacobian of $h$ for this problem.

```python
from math import sqrt
def HJacobian_at(x):
    """ compute Jacobian of H matrix at x """

    horiz_dist = x[0]
    altitude   = x[2]
    denom = sqrt(horiz_dist**2 + altitude**2)
    return array ([[horiz_dist/denom, 0., altitude/denom]])
```

Finally, let's provide the code for $h(\mathbf x)$

```python
def hx(x):
    """ compute measurement for slant range that
    would correspond to state x.
    """

    return (x[0]**2 + x[2]**2) ** 0.5
```

Now lets write a simulation for our radar.

```python
from numpy.random import randn
import math

class RadarSim(object):
    """ Simulates the radar signal returns from an object
    flying at a constant altityude and velocity in 1D.
    """

    def __init__(self, dt, pos, vel, alt):
        self.pos = pos
        self.vel = vel
        self.alt = alt
        self.dt = dt

    def get_range(self):
        """ Returns slant range to the object. Call once
        for each new measurement at dt time from last call.
        """

        # add some process noise to the system
        self.vel = self.vel  + .1*randn()
        self.alt = self.alt + .1*randn()
        self.pos = self.pos + self.vel*self.dt

        # add measurement noise
        err = self.pos * 0.05*randn()
        slant_dist = math.sqrt(self.pos**2 + self.alt**2)

        return slant_dist + err
```

### Design Process and Measurement Noise

The radar measures the range to a target. We will use $\sigma_{range}= 5$ meters for the noise. This gives us

$$\mathbf R = \begin{bmatrix}\sigma_{range}^2\end{bmatrix} = \begin{bmatrix}25\end{bmatrix}$$

The design of $\mathbf Q$ requires some discussion. The state $\mathbf x= \begin{bmatrix}x & \dot x & y\end{bmatrix}^\mathtt{T}$. The first two elements are position (down range distance) and velocity, so we can use `Q_discrete_white_noise` noise to compute the values for the upper left hand side of $\mathbf Q$. The third element of  $\mathbf x$ is altitude, which we are assuming is independent of the down range distance. That leads us to a block design of $\mathbf Q$ of:

$$\mathbf Q = \begin{bmatrix}\mathbf Q_\mathtt{x} & 0 \\ 0 & \mathbf Q_\mathtt{y}\end{bmatrix}$$

### Implementation

`FilterPy` provides the class `ExtendedKalmanFilter`. It works similarly to the `KalmanFilter` class we have been using, except that it allows you to provide a function that computes the Jacobian of $\mathbf H$ and the function $h(\mathbf x)$.

We start by importing the filter and creating it. The dimension of `x` is 3 and `z` has dimension 1.

```python
from filterpy.kalman import ExtendedKalmanFilter

rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)
```
We create the radar simulator:
```python
radar = RadarSim(dt, pos=0., vel=100., alt=1000.)
```
We will initialize the filter near the airplane's actual position:

```python
rk.x = array([radar.pos, radar.vel-10, radar.alt+100])
```

We assign the system matrix using the first term of the Taylor series expansion we computed above:

```python
dt = 0.05
rk.F = eye(3) + array([[0, 1, 0],
                       [0, 0, 0],
                       [0, 0, 0]])*dt
```

After assigning reasonable values to $\mathbf R$, $\mathbf Q$, and $\mathbf P$ we can run the filter with a simple loop. We pass the functions for computing the Jacobian of $\mathbf  H$ and $h(x)$ into the `update` method.

```python
for i in range(int(20/dt)):
    z = radar.get_range()
    rk.update(array([z]), HJacobian_at, hx)
    rk.predict()
```

Adding some boilerplate code to save and plot the results we get:

```python
from filterpy.common import Q_discrete_white_noise
from filterpy.kalman import ExtendedKalmanFilter
from numpy import eye, array, asarray
import numpy as np

dt = 0.05
rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)
radar = RadarSim(dt, pos=0., vel=100., alt=1000.)

# make an imperfect starting guess
rk.x = array([radar.pos-100, radar.vel+100, radar.alt+1000])

rk.F = eye(3) + array([[0, 1, 0],
                       [0, 0, 0],
                       [0, 0, 0]]) * dt

range_std = 5. # meters
rk.R = np.diag([range_std**2])
rk.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1)
rk.Q[2,2] = 0.1
rk.P *= 50

xs, track = [], []
for i in range(int(20/dt)):
    z = radar.get_range()
    track.append((radar.pos, radar.vel, radar.alt))

    rk.update(array([z]), HJacobian_at, hx)
    xs.append(rk.x)
    rk.predict()

xs = asarray(xs)
track = asarray(track)
time = np.arange(0, len(xs)*dt, dt)
ekf_internal.plot_radar(xs, track, time)
```

## Using SymPy to compute Jacobians

Depending on your experience with derivatives you may have found the computation of the Jacobian difficult. Even if you found it easy, a slightly more difficult problem easily leads to very difficult computations.

As explained in Appendix A, we can use the SymPy package to compute the Jacobian for us.

```python
import sympy
sympy.init_printing(use_latex=True)

x, x_vel, y = sympy.symbols('x, x_vel y')

H = sympy.Matrix([sympy.sqrt(x**2 + y**2)])

state = sympy.Matrix([x, x_vel, y])
H.jacobian(state)
```

This result is the same as the result we computed above, and with much less effort on our part!

## Robot Localization

It's time to try a real problem. I warn you that this section is difficult. However, most books choose simple, textbook problems with simple answers, and you are left wondering how to solve a real world problem.

We will consider the problem of robot localization. We already implemented this in the **Unscented Kalman Filter** chapter, and I recommend you read it now if you haven't already.  In this scenario we have a robot that is moving through a landscape using a sensor to detect landmarks. This could be a self driving car using computer vision to identify trees, buildings, and other landmarks. It might be one of those small robots that vacuum your house, or a robot in a warehouse.

The robot has 4 wheels in the same configuration used by automobiles. It maneuvers by pivoting the front wheels. This causes the robot to pivot around the rear axle while moving forward. This is nonlinear behavior which we will have to model.

The robot has a sensor that measures the range and bearing to known targets in the landscape. This is nonlinear because computing a position from a range and bearing requires square roots and trigonometry.

Both the process model and measurement models are nonlinear. The EKF accommodates both, so we provisionally conclude that the EKF is a viable choice for this problem.

### Robot Motion Model

At a first approximation an automobile steers by pivoting the front tires while moving forward. The front of the car moves in the direction that the wheels are pointing while pivoting around the rear tires. This simple description is complicated by issues such as slippage due to friction, the differing behavior of the rubber tires at different speeds, and the need for the outside tire to travel a different radius than the inner tire. Accurately modeling steering requires a complicated set of differential equations.

For lower speed robotic applications a simpler *bicycle model* has been found to perform well. This is a depiction of the model:

```python
ekf_internal.plot_bicycle()
```

In the **Unscented Kalman Filter** chapter we derived these equations:

$$\begin{aligned}
\beta &= \frac d w \tan(\alpha) \\
x &= x - R\sin(\theta) + R\sin(\theta + \beta) \\
y &= y + R\cos(\theta) - R\cos(\theta + \beta) \\
\theta &= \theta + \beta
\end{aligned}
$$

where $\theta$ is the robot's heading.

You do not need to understand this model in detail if you are not interested in steering models. The important thing to recognize is that our motion model is nonlinear, and we will need to deal with that with our Kalman filter.

### Design the State Variables

For our filter we will maintain the position $x,y$ and orientation $\theta$ of the robot:

$$\mathbf x = \begin{bmatrix}x \\ y \\ \theta\end{bmatrix}$$

Our control input $\mathbf u$ is the velocity $v$ and steering angle $\alpha$:

$$\mathbf u = \begin{bmatrix}v \\ \alpha\end{bmatrix}$$

### Design the System Model

We model our system as a nonlinear motion model plus noise.

$$\overline x = x + f(x, u) + \mathcal{N}(0, Q)$$

Using the motion model for a robot that we created above, we can expand this to

$$\overline{\begin{bmatrix}x\\y\\\theta\end{bmatrix}} = \begin{bmatrix}x\\y\\\theta\end{bmatrix} +
\begin{bmatrix}- R\sin(\theta) + R\sin(\theta + \beta) \\
R\cos(\theta) - R\cos(\theta + \beta) \\
\beta\end{bmatrix}$$

We find The $\mathbf F$ by taking the Jacobian of $f(x,u)$.

$$\mathbf F = \frac{\partial f(x, u)}{\partial x} =\begin{bmatrix}
\frac{\partial \dot x}{\partial x} &
\frac{\partial \dot x}{\partial y} &
\frac{\partial \dot x}{\partial \theta}\\
\frac{\partial \dot y}{\partial x} &
\frac{\partial \dot y}{\partial y} &
\frac{\partial \dot y}{\partial \theta} \\
\frac{\partial \dot{\theta}}{\partial x} &
\frac{\partial \dot{\theta}}{\partial y} &
\frac{\partial \dot{\theta}}{\partial \theta}
\end{bmatrix}
$$

When we calculate these we get

$$\mathbf F = \begin{bmatrix}
1 & 0 & -R\cos(\theta) + R\cos(\theta+\beta) \\
0 & 1 & -R\sin(\theta) + R\sin(\theta+\beta) \\
0 & 0 & 1
\end{bmatrix}$$

We can double check our work with SymPy.

```python
import sympy
from sympy.abc import alpha, x, y, v, w, R, theta
from sympy import symbols, Matrix
sympy.init_printing(use_latex="mathjax", fontsize='16pt')
time = symbols('t')
d = v*time
beta = (d/w)*sympy.tan(alpha)
r = w/sympy.tan(alpha)

fxu = Matrix([[x-r*sympy.sin(theta) + r*sympy.sin(theta+beta)],
              [y+r*sympy.cos(theta)- r*sympy.cos(theta+beta)],
              [theta+beta]])
F = fxu.jacobian(Matrix([x, y, theta]))

F
```

$$\left[\begin{matrix}1 & 0 & - \frac{w \cos{\left (\theta \right )}}{\tan{\left (\alpha \right )}} + \frac{w}{\tan{\left (\alpha \right )}} \cos{\left (\frac{t v}{w} \tan{\left (\alpha \right )} + \theta \right )}\\0 & 1 & - \frac{w \sin{\left (\theta \right )}}{\tan{\left (\alpha \right )}} + \frac{w}{\tan{\left (\alpha \right )}} \sin{\left (\frac{t v}{w} \tan{\left (\alpha \right )} + \theta \right )}\\0 & 0 & 1\end{matrix}\right]$$

That looks a bit complicated. We can use SymPy to substitute terms:

```python
# reduce common expressions
B, R = symbols('beta, R')
F = F.subs((d/w)*sympy.tan(alpha), B)
F.subs(w/sympy.tan(alpha), R)
```

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

This form  verifies that the computation of the Jacobian is correct.

Now we can turn our attention to the noise. Here, the noise is in our control input, so it is in *control space*. In other words, we command a specific velocity and steering angle, but we need to convert that into errors in $x, y, \theta$. In a real system this might vary depending on velocity, so it will need to be recomputed for every prediction. I will choose this as the noise model; for a real robot you will need to choose a model that accurately depicts the error in your system.

$$\mathbf{M} = \begin{bmatrix}\sigma_{vel}^2 & 0 \\ 0 & \sigma_\alpha^2\end{bmatrix}$$

If this was a linear problem we would convert from control space to state space using the by now familiar $\mathbf{FMF}^\mathsf T$ form. Since our motion model is nonlinear we do not try to find a closed form solution to this, but instead linearize it with a Jacobian which we will name $\mathbf{V}$.

$$\mathbf{V} = \frac{\partial f(x, u)}{\partial u} \begin{bmatrix}
\frac{\partial \dot x}{\partial v} & \frac{\partial \dot x}{\partial \alpha} \\
\frac{\partial \dot y}{\partial v} & \frac{\partial \dot y}{\partial \alpha} \\
\frac{\partial \dot{\theta}}{\partial v} & \frac{\partial \dot{\theta}}{\partial \alpha}
\end{bmatrix}$$

These partial derivatives become very difficult to work with. Let's compute them with SymPy.

```python
V = fxu.jacobian(Matrix([v, alpha]))
V = V.subs(sympy.tan(alpha)/w, 1/R)
V = V.subs(time*v/R, B)
V = V.subs(time*v, 'd')
V
```

$$\left[\begin{matrix}t \cos{\left (\beta + \theta \right )} & \frac{d \cos{\left (\beta + \theta \right )}}{\tan{\left (\alpha \right )}} \left(\tan^{2}{\left (\alpha \right )} + 1\right) - \frac{w \sin{\left (\theta \right )}}{\tan^{2}{\left (\alpha \right )}} \left(- \tan^{2}{\left (\alpha \right )} - 1\right) + \frac{w \sin{\left (\beta + \theta \right )}}{\tan^{2}{\left (\alpha \right )}} \left(- \tan^{2}{\left (\alpha \right )} - 1\right)\\t \sin{\left (\beta + \theta \right )} & \frac{d \sin{\left (\beta + \theta \right )}}{\tan{\left (\alpha \right )}} \left(\tan^{2}{\left (\alpha \right )} + 1\right) + \frac{w \cos{\left (\theta \right )}}{\tan^{2}{\left (\alpha \right )}} \left(- \tan^{2}{\left (\alpha \right )} - 1\right) - \frac{w \cos{\left (\beta + \theta \right )}}{\tan^{2}{\left (\alpha \right )}} \left(- \tan^{2}{\left (\alpha \right )} - 1\right)\\\frac{t}{R} & \frac{d}{w} \left(\tan^{2}{\left (\alpha \right )} + 1\right)\end{matrix}\right]$$

This should give you an appreciation of how quickly the EKF become mathematically intractable.

This gives us the final form of our prediction equations:

$$\begin{aligned}
\mathbf{\overline x} &= \mathbf x +
\begin{bmatrix}- R\sin(\theta) + R\sin(\theta + \beta) \\
R\cos(\theta) - R\cos(\theta + \beta) \\
\beta\end{bmatrix}\\
\mathbf{\overline P} &=\mathbf{FPF}^{\mathsf T} + \mathbf{VMV}^{\mathsf T}
\end{aligned}$$

This form of linearization is not the only way to predict $\mathbf x$. For example, we could use a numerical integration technique such as *Runge Kutta* to compute the movement
of the robot. This will be required if the time step is relatively large. Things are not as cut and dried with the EKF as for the Kalman filter. For a real problem you have to carefully model your system with differential equations and then determine the most appropriate way to solve that system. The correct approach depends on the accuracy you require, how nonlinear the equations are, your processor budget, and numerical stability concerns.

### Design the Measurement Model

The robot's sensor provides a noisy bearing and range measurement to multiple known locations in the landscape. The measurement model must convert the state $\begin{bmatrix}x & y&\theta\end{bmatrix}^\mathsf T$ into a range and bearing to the landmark. If $\mathbf p$
is the position of a landmark, the range $r$ is

$$r = \sqrt{(p_x - x)^2 + (p_y - y)^2}$$

The sensor provides bearing relative to the orientation of the robot, so we must subtract the robot's orientation from the bearing to get the sensor reading, like so:

$$\phi = \arctan(\frac{p_y - y}{p_x - x}) - \theta$$

Thus our measurement model $h$ is

$$\begin{aligned}
\mathbf z& = h(\bar{\mathbf x}, \mathbf p) &+ \mathcal{N}(0, R)\\
&= \begin{bmatrix}
\sqrt{(p_x - x)^2 + (p_y - y)^2} \\
\arctan(\frac{p_y - y}{p_x - x}) - \theta
\end{bmatrix} &+ \mathcal{N}(0, R)
\end{aligned}$$

This is clearly nonlinear, so we need linearize $h$  at $\mathbf x$ by taking its Jacobian. We compute that with SymPy below.

```python
px, py = symbols('p_x, p_y')
z = Matrix([[sympy.sqrt((px-x)**2 + (py-y)**2)],
            [sympy.atan2(py-y, px-x) - theta]])
z.jacobian(Matrix([x, y, theta]))
```

$$\left[\begin{matrix}\frac{- p_{x} + x}{\sqrt{\left(p_{x} - x\right)^{2} + \left(p_{y} - y\right)^{2}}} & \frac{- p_{y} + y}{\sqrt{\left(p_{x} - x\right)^{2} + \left(p_{y} - y\right)^{2}}} & 0\\- \frac{- p_{y} + y}{\left(p_{x} - x\right)^{2} + \left(p_{y} - y\right)^{2}} & - \frac{p_{x} - x}{\left(p_{x} - x\right)^{2} + \left(p_{y} - y\right)^{2}} & -1\end{matrix}\right]$$

Now we need to write that as a Python function. For example we might write:

```python
from math import sqrt

def H_of(x, landmark_pos):
    """ compute Jacobian of H matrix where h(x) computes
    the range and bearing to a landmark for state x """

    px = landmark_pos[0]
    py = landmark_pos[1]
    hyp = (px - x[0, 0])**2 + (py - x[1, 0])**2
    dist = sqrt(hyp)

    H = array(
        [[-(px - x[0, 0]) / dist, -(py - x[1, 0]) / dist, 0],
         [ (py - x[1, 0]) / hyp,  -(px - x[0, 0]) / hyp, -1]])
    return H
```

We also need to define a function that converts the system state into a measurement.

```python
from math import atan2

def Hx(x, landmark_pos):
    """ takes a state variable and returns the measurement
    that would correspond to that state.
    """
    px = landmark_pos[0]
    py = landmark_pos[1]
    dist = sqrt((px - x[0, 0])**2 + (py - x[1, 0])**2)

    Hx = array([[dist],
                [atan2(py - x[1, 0], px - x[0, 0]) - x[2, 0]]])
    return Hx
```

### Design Measurement Noise

It is reasonable to assume that the noise of the range and bearing measurements are independent, hence

$$\mathbf R=\begin{bmatrix}\sigma_{range}^2 & 0 \\ 0 & \sigma_{bearing}^2\end{bmatrix}$$

### Implementation

We will use `FilterPy`'s `ExtendedKalmanFilter` class to implement the filter. Its `predict()` method uses the standard linear equations for the process model. Our's is nonlinear, so we will have to override `predict()` with our own implementation.  I'll want to also use this class to simulate the robot, so I'll add a method `move()` that computes the position of the robot which both `predict()` and my simulation can call.

The matrices for the prediction step are quite large. While writing this code I made several errors before I finally got it working. I only found my errors by using SymPy's `evalf` function. `evalf` evaluates a SymPy `Matrix` with specific values for the variables. I decided to demonstrate this technique to you, and used `evalf` in the Kalman filter code. You'll need to understand a couple of points.

First, `evalf` uses a dictionary to specify the values. For example, if your matrix contains an `x` and `y`, you can write

```python
    M.evalf(subs={x:3, y:17})
```

to evaluate the matrix for `x=3` and `y=17`.

Second, `evalf` returns a `sympy.Matrix` object. Use `numpy.array(M).astype(float)` to convert it to a NumPy array. `numpy.array(M)` creates an array of  type `object`, which is not what you want.

Here is the code for the EKF:

```python
from filterpy.kalman import ExtendedKalmanFilter as EKF
from numpy import dot, array, sqrt
class RobotEKF(EKF):
    def __init__(self, dt, wheelbase, std_vel, std_steer):
        EKF.__init__(self, 3, 2, 2)
        self.dt = dt
        self.wheelbase = wheelbase
        self.std_vel = std_vel
        self.std_steer = std_steer

        a, x, y, v, w, theta, time = symbols(
            'a, x, y, v, w, theta, t')
        d = v*time
        beta = (d/w)*sympy.tan(a)
        r = w/sympy.tan(a)

        self.fxu = Matrix(
            [[x-r*sympy.sin(theta)+r*sympy.sin(theta+beta)],
             [y+r*sympy.cos(theta)-r*sympy.cos(theta+beta)],
             [theta+beta]])

        self.F_j = self.fxu.jacobian(Matrix([x, y, theta]))
        self.V_j = self.fxu.jacobian(Matrix([v, a]))

        # save dictionary and it's variables for later use
        self.subs = {x: 0, y: 0, v:0, a:0,
                     time:dt, w:wheelbase, theta:0}
        self.x_x, self.x_y, = x, y
        self.v, self.a, self.theta = v, a, theta

    def predict(self, u=0):
        self.x = self.move(self.x, u, self.dt)

        self.subs[self.theta] = self.x[2, 0]
        self.subs[self.v] = u[0]
        self.subs[self.a] = u[1]

        F = array(self.F_j.evalf(subs=self.subs)).astype(float)
        V = array(self.V_j.evalf(subs=self.subs)).astype(float)

        # covariance of motion noise in control space
        M = array([[self.std_vel*u[0]**2, 0],
                   [0, self.std_steer**2]])

        self.P = dot(F, self.P).dot(F.T) + dot(V, M).dot(V.T)

    def move(self, x, u, dt):
        hdg = x[2, 0]
        vel = u[0]
        steering_angle = u[1]
        dist = vel * dt

        if abs(steering_angle) > 0.001: # is robot turning?
            beta = (dist / self.wheelbase) * tan(steering_angle)
            r = self.wheelbase / tan(steering_angle) # radius

            dx = np.array([[-r*sin(hdg) + r*sin(hdg + beta)],
                           [r*cos(hdg) - r*cos(hdg + beta)],
                           [beta]])
        else: # moving in straight line
            dx = np.array([[dist*cos(hdg)],
                           [dist*sin(hdg)],
                           [0]])
        return x + dx
```

Now we have another issue to handle. The residual is notionally computed as $y = z - h(x)$ but this will not work because our measurement contains an angle in it. Suppose z has a bearing of $1^\circ$ and $h(x)$ has a bearing of $359^\circ$. Naively subtracting them would yield a angular difference of $-358^\circ$, whereas the correct value is $2^\circ$. We have to write code to correctly compute the bearing residual.

```python
def residual(a, b):
    """ compute residual (a-b) between measurements containing
    [range, bearing]. Bearing is normalized to [-pi, pi)"""
    y = a - b
    y[1] = y[1] % (2 * np.pi)    # force in range [0, 2 pi)
    if y[1] > np.pi:             # move to [-pi, pi)
        y[1] -= 2 * np.pi
    return y
```

The rest of the code runs the simulation and plots the results, and shouldn't need too much comment by now. I create a variable `landmarks` that contains the landmark coordinates. I update the simulated robot position 10 times a second, but run the EKF only once per second. This is for two reasons. First, we are not using Runge Kutta to integrate the differental equations of motion, so a narrow time step allows our simulation to be more accurate. Second, it is fairly normal in embedded systems to have limited processing speed. This forces you to run your Kalman filter only as frequently as absolutely needed.

```python
from filterpy.stats import plot_covariance_ellipse
from math import sqrt, tan, cos, sin, atan2
import matplotlib.pyplot as plt

dt = 1.0

def z_landmark(lmark, sim_pos, std_rng, std_brg):
    x, y = sim_pos[0, 0], sim_pos[1, 0]
    d = np.sqrt((lmark[0] - x)**2 + (lmark[1] - y)**2)
    a = atan2(lmark[1] - y, lmark[0] - x) - sim_pos[2, 0]
    z = np.array([[d + randn()*std_rng],
                  [a + randn()*std_brg]])
    return z

def ekf_update(ekf, z, landmark):
    ekf.update(z, HJacobian=H_of, Hx=Hx,
               residual=residual,
               args=(landmark), hx_args=(landmark))

def run_localization(landmarks, std_vel, std_steer,
                     std_range, std_bearing,
                     step=10, ellipse_step=20, ylim=None):
    ekf = RobotEKF(dt, wheelbase=0.5, std_vel=std_vel,
                   std_steer=std_steer)
    ekf.x = array([[2, 6, .3]]).T # x, y, steer angle
    ekf.P = np.diag([.1, .1, .1])
    ekf.R = np.diag([std_range**2, std_bearing**2])

    sim_pos = ekf.x.copy() # simulated position
    # steering command (vel, steering angle radians)
    u = array([1.1, .01])

    plt.scatter(landmarks[:, 0], landmarks[:, 1],
                marker='s', s=60)

    track = []
    for i in range(200):
        sim_pos = ekf.move(sim_pos, u, dt/10.) # simulate robot
        track.append(sim_pos)

        if i % step == 0:
            ekf.predict(u=u)

            if i % ellipse_step == 0:
                plot_covariance_ellipse(
                    (ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2],
                     std=6, facecolor='k', alpha=0.3)

            x, y = sim_pos[0, 0], sim_pos[1, 0]
            for lmark in landmarks:
                z = z_landmark(lmark, sim_pos,
                               std_range, std_bearing)
                ekf_update(ekf, z, lmark)

            if i % ellipse_step == 0:
                plot_covariance_ellipse(
                    (ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2],
                    std=6, facecolor='g', alpha=0.8)
    track = np.array(track)
    plt.plot(track[:, 0], track[:,1], color='k', lw=2)
    plt.axis('equal')
    plt.title("EKF Robot localization")
    if ylim is not None: plt.ylim(*ylim)
    plt.show()
    return ekf
```

```python
landmarks = array([[5, 10], [10, 5], [15, 15]])

ekf = run_localization(
    landmarks, std_vel=0.1, std_steer=np.radians(1),
    std_range=0.3, std_bearing=0.1)
print('Final P:', ekf.P.diagonal())
```

I have plotted the landmarks as solid squares. The path of the robot is drawn with a black line. The covariance ellipses for the predict step are light gray, and the covariances of the update are shown in green. To make them visible at this scale I have set the ellipse boundary at 6$\sigma$.

We can see that there is a lot of uncertainty added by our motion model, and that most of the error in in the direction of motion. We determine that from the shape of the blue ellipses. After a few steps we can see that the filter incorporates the landmark measurements and the errors improve.

I used the same initial conditions and landmark locations in the UKF chapter. The UKF achieves much better accuracy in terms of the error ellipse. Both perform roughly as well as far as their estimate for $\mathbf x$ is concerned.

Now lets add another landmark.

```python
landmarks = array([[5, 10], [10, 5], [15, 15], [20, 5]])

ekf = run_localization(
    landmarks, std_vel=0.1, std_steer=np.radians(1),
    std_range=0.3, std_bearing=0.1)
plt.show()
print('Final P:', ekf.P.diagonal())
```

The uncertainly in the estimates near the end of the track are smaller.  We can see the effect that multiple landmarks have on our uncertainty by only using the first two landmarks.

```python
ekf = run_localization(
    landmarks[0:2], std_vel=1.e-10, std_steer=1.e-10,
    std_range=1.4, std_bearing=.05)
print('Final P:', ekf.P.diagonal())
```

The estimate quickly diverges from the robot's path after passing the landmarks. The covariance also grows quickly. Let's see what happens with only one landmark:

```python
ekf = run_localization(
    landmarks[0:1], std_vel=1.e-10, std_steer=1.e-10,
    std_range=1.4, std_bearing=.05)
print('Final P:', ekf.P.diagonal())
```

As you probably suspected, one landmark produces a very bad result. Conversely, a large number of landmarks allows us to make very accurate estimates.

```python
landmarks = array([[5, 10], [10,  5], [15, 15], [20,  5], [15, 10],
                   [10,14], [23, 14], [25, 20], [10, 20]])

ekf = run_localization(
    landmarks, std_vel=0.1, std_steer=np.radians(1),
    std_range=0.3, std_bearing=0.1, ylim=(0, 21))
print('Final P:', ekf.P.diagonal())
```

### Discussion

I said that this was a real problem, and in some ways it is. I've seen alternative presentations that used robot motion models that led to simpler Jacobians. On the other hand, my model of the movement is also simplistic in several ways. First, it uses a bicycle model. A real car has two sets of tires, and each travels on a different radius. The wheels do not grip the surface perfectly. I also assumed that the robot responds instantaneously to the control input. Sebastian Thrun writes in *Probabilistic Robots* that simplified models are justified because the filters perform well when used to track real vehicles. The lesson here is that while you have to have a reasonably accurate nonlinear model, it does not need to be perfect to operate well. As a designer you will need to balance the fidelity of your model with the difficulty of the math and the CPU time required to perform the linear algebra.

Another way in which this problem was simplistic is that we assumed that we knew the correspondance between the landmarks and measurements. But suppose we are using radar - how would we know that a specific signal return corresponded to a specific building in the local scene? This question hints at SLAM algorithms - simultaneous localization and mapping. SLAM is not the point of this book, so I will not elaborate on this topic.

## UKF vs EKF

In the last chapter I used the UKF to solve this problem. The difference in implementation should be very clear. Computing the Jacobians for the state and measurement models was not trivial despite a rudimentary motion model. A different problem could result in a Jacobian which is difficult or impossible to derive analytically. In contrast, the UKF only requires you to provide a function that computes the system motion model and another for the measurement model.

There are many cases where the Jacobian cannot be found analytically. The details are beyond the scope of this book, but you will have to use numerical methods to compute the Jacobian. That undertaking is not trivial, and you will spend a significant portion of a master's degree at a STEM school learning techniques to handle such situations. Even then you'll likely only be able to solve problems related to your field - an aeronautical engineer learns a lot about Navier Stokes equations, but not much about modelling chemical reaction rates.

So, UKFs are easy. Are they accurate? In practice they often perform better than the EKF. You can find plenty of research papers that prove that the UKF outperforms the EKF in various problem domains. It's not hard to understand why this would be true. The EKF works by linearizing the system model and measurement model at a single point, and the UKF uses $2n+1$ points.

Let's look at a specific example. Take $f(x) = x^3$ and pass a Gaussian distribution through it. I will compute an accurate answer using a monte carlo simulation. I generate 50,000 points randomly distributed according to the Gaussian, pass each through $f(x)$, then compute the mean and variance of the result.

The EKF linearizes the function by taking the derivative to find the slope at the evaluation point $x$. This slope becomes the linear function that we use to transform the Gaussian. Here is a plot of that.

```python
import nonlinear_plots
nonlinear_plots.plot_ekf_vs_mc()
```

The EKF computation is rather inaccurate. In contrast, here is the performance of the UKF:

```python
nonlinear_plots.plot_ukf_vs_mc(alpha=0.001, beta=3., kappa=1.)
```

Here we can see that the computation of the UKF's mean is accurate to 2 decimal places. The standard deviation is slightly off, but you can also fine tune how the UKF computes the distribution by using the $\alpha$, $\beta$, and $\gamma$ parameters for generating the sigma points. Here I used $\alpha=0.001$, $\beta=3$, and $\gamma=1$. Feel free to modify them to see the result. You should be able to get better results than I did. However, avoid over-tuning the UKF for a specific test. It may perform better for your test case, but worse in general.


# ***Introduction to Radar Using Python and MATLAB***
## Andy Harrison - Copyright (C) 2019 Artech House
<br/>

# Infinite Cylinder with Oblique Incidence
***

Referring to Section 7.4.1.3, studies the scattering from infinite length cylinders with oblique incidence, as shown in Figure 7.8.
The two-dimensional bistatic radar cross section for the TMz polarization is given by (Equation 7.28)

$$
    \sigma_{2D} = \frac{2\lambda}{\pi\sin\theta_i}\Bigg| \sum\limits_{n=0}^{\infty} \epsilon_n\frac{J_n(ka\sin\theta_i)}{H_n^{(2)}(ka\sin\theta_i)} \cos(n\, \phi_o)\Bigg|^2 \hspace{0.5in} \text{(m)},
$$

The two-dimensional bistatic radar cross section for the TEz polarization is given by (Equation 7.30)

$$
    \sigma_{2D} = \frac{2\lambda}{\pi\sin\theta_i}\Bigg| \sum\limits_{n=0}^{\infty} \epsilon_n\frac{J_n^\prime(ka\sin\theta_i)}{H_n^{(2)\prime}(ka\sin\theta_i)} \cos(n\, \phi_o)\Bigg|^2 \hspace{0.5in} \text{(m)}.
$$

As in the case of normal incidence, an approximation of the three-dimensional radar cross section from the two-dimensional radar cross section is (Equations 7.31 and 7.32)

\begin{align}
    \sigma_{TM} \approx \frac{4L^2}{\pi}\frac{\sin^2\theta_o}{\sin\theta_i}\, &\Bigg| \sum\limits_{n=0}^{\infty} \epsilon_n\frac{J_n(ka\sin\theta_i)}{H_n^{(2)}(ka\sin\theta_i)} \cos(n\, \phi_o)\Bigg|^2 \nonumber \\ \nonumber \\ &\times \Bigg[ \frac{\sin\big(0.5kL(\cos\theta_i + \cos\theta_o)\big)}{0.5kL(\cos\theta_i + \cos\theta_o)} \Bigg]^2\hspace{0.5in} \text{(m}^2\text{)},
\end{align}

and

\begin{align}
    \sigma_{TE} \approx \frac{4L^2}{\pi}\sin\theta_i\, &\Bigg| \sum\limits_{n=0}^{\infty} \epsilon_n\frac{J_n^\prime(ka\sin\theta_i)}{H_n^{(2)\prime}(ka\sin\theta_i)} \cos(n\, \phi_o)\Bigg|^2 \nonumber \\ \nonumber \\ &\times \Bigg[ \frac{\sin\big(0.5kL(\cos\theta_i + \cos\theta_o)\big)}{0.5kL(\cos\theta_i + \cos\theta_o)} \Bigg]^2\hspace{0.5in} \text{(m}^2\text{)}.
\end{align}
***

Begin by getting the library path

```python
import lib_path
```

Set the operating frequency (Hz), the radius (m), the length (for 3D RCS) (m), the number of modes and the incident angle (from the z-axis) (degrees)

```python
frequency = 10e9

radius = 0.5

length = 20.0

number_of_modes = 30

incident_angle = 20.0
```

Select either 2D or 3D RCS

```python
mode = '2D'
```

Set the observation angles with the `linspace` routine from `scipy`

```python
from numpy import linspace

observation_angle = linspace(-90, 90, 1801)
```

Calculate the radar cross section based on the mode

```python
from Libs.rcs.infinite_cylinder_oblique import radar_cross_section, radar_cross_section_3d

if mode == '2D':

    rcs_te, rcs_tm = radar_cross_section(frequency, radius, incident_angle, observation_angle, number_of_modes)

else:

    rcs_te, rcs_tm = radar_cross_section_3d(frequency, radius, incident_angle, observation_angle, number_of_modes, length)
```

Display the RCS for the infinite cylinder using the `matplotlib` routines

```python
from matplotlib import pyplot as plt

from numpy import log10, degrees

# Set the figure size

plt.rcParams["figure.figsize"] = (15, 10)

# Display the results

plt.plot(observation_angle, 10.0 * log10(rcs_te), '', label='TE$^z$')

plt.plot(observation_angle, 10.0 * log10(rcs_tm), '--', label='TM$^z$')

# Set the plot title and labels

plt.title('RCS vs Bistatic Angle', size=14)

plt.ylabel('RCS (dBsm)', size=12)

plt.xlabel('Observation Angle (deg)', size=12)

plt.ylim(min(10.0 * log10(rcs_te)) - 3, max(10.0 * log10(rcs_te)) + 3)

# Set the tick label size

plt.tick_params(labelsize=12)

# Turn on the grid

plt.grid(linestyle=':', linewidth=0.5)

# Set the legend

plt.legend(loc='upper right', prop={'size': 10})
```

```python

```