[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=Lora');

//@import url('http://fonts.googleapis.com/css?family=Open+Sans');
//@import url('http://fonts.googleapis.com/css?family=Vollkorn');
//@import url('http://fonts.googleapis.com/css?family=Karla');
//@import url('http://fonts.googleapis.com/css?family=Poppins');
//@import url('http://fonts.googleapis.com/css?family=Arimo');
//@import url('http://fonts.googleapis.com/css?family=Roboto');
//@import url('http://fonts.googleapis.com/css?family=Lato');
//@import url('http://fonts.googleapis.com/css?family=Domine');
//@import url('http://fonts.googleapis.com/css?family=Chivo');
//@import url('http://fonts.googleapis.com/css?family=Cardo');
//@import url('http://fonts.googleapis.com/css?family=Arvo');
//@import url('http://fonts.googleapis.com/css?family=Crimson+Text');
//@import url('http://fonts.googleapis.com/css?family=Ubuntu');
//@import url('http://fonts.googleapis.com/css?family=Fontin');
//@import url('http://fonts.googleapis.com/css?family=Raleway');
//@import url('http://fonts.googleapis.com/css?family=Merriweather');

.CodeMirror pre {
    font-family: 'Source Code Pro', Consolas, monocco, monospace;
}
    div.cell{
        //width: 950px;
        margin-left: 0% !important;
        margin-right: auto;
    }
    div.text_cell_render{
        font-family: 'Lora';
        //font-family: 'Open Sans';
        //font-family: 'Karla',verdana,arial,sans-serif;
        //font-family: 'Roboto',verdana,arial,sans-serif;
        //font-family: 'Lato',verdana,arial,sans-serif;
        //font-family: 'Domine',verdana,arial,sans-serif;
        //font-family: 'Chivo',verdana,arial,sans-serif;
        //font-family: 'Cardo',verdana,arial,sans-serif;
        //font-family: 'Arvo',verdana,arial,sans-serif;
        //font-family: 'Poppins',verdana,arial,sans-serif;
        //font-family: 'Ubuntu',verdana,arial,sans-serif;
        //font-family: 'Fontin',verdana,arial,sans-serif;
        //font-family: 'Raleway',verdana,arial,sans-serif;
        //font-family: 'Merriweather',verdana,arial,sans-serif;
        //font-family: 'Crimson Text', verdana,arial,sans-serif;
        //font-family: verdana,arial,sans-serif;
        //font-family: arial,sans-serif;
        line-height: 125%;
        font-size: 130%;
        text-align: justify;
        text-justify:inter-word;
    }
    div.text_cell code {
        background: transparent;
        color: #000000;
        font-weight: 400;
        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.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{
       color:#447adb;
    }
    a:visited{
       color: #1d3b84;
    }
    a:hover{
       color: #1d3b84;
    }
    a:focus{
       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. 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}\dot{\mathbf x} &= \mathbf{Ax} + w_x\\
\mathbf z &= \mathbf{Hx} + w_z
\end{aligned}$$

Where $\mathbf A$ is the systems dynamic matrix. Using the state space methods covered in the **Kalman Filter Math** chapter these equations can be tranformed into
$$\begin{aligned}\bar{\mathbf x} &= \mathbf{Fx} \\
\mathbf z &= \mathbf{Hx}
\end{aligned}$$

where $\mathbf F$ is the *fundamental matrix*. The noise $w_x$ and $w_z$ terms are incorporated into the matrices $\mathbf R$ and $\mathbf Q$. This form of the equations allow us to compute the state at step $k$ given a measurement at step $k$ and the state estimate at step $k-1$. In earlier chapters I built your intuition and minimized the math by using problems describable with Newton's equations. We know how to design $\mathbf F$ based on high school physics.

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

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

You might imagine that we could 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 code.ekf_internal as 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 the slope of a 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 similar. 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*](https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant). 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(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t}
\end{aligned}
$$

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 put boxes around the differences from the linear filter:

$$\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{\bar x} = \mathbf{Fx} + \mathbf{Bu} & \boxed{\mathbf{\bar x} = f(\mathbf x, \mathbf u)}  \\
\mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q  & \mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q \\
\hline
& \boxed{\mathbf H = \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t}} \\
\textbf{y} = \mathbf z - \mathbf{H \bar{x}} & \textbf{y} = \mathbf z - \boxed{h(\bar{x})}\\
\mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} & \mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} \\
\mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} & \mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} \\
\mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}} & \mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}}
\end{array}$$

We don't normally use $\mathbf{Fx}$ to propagate the state for the EKF as the linearization causes inaccuracies. It is typical to compute $\bar{\mathbf x}$ using a suitable numerical integration technique such as Euler or Runge Kutta. Thus I wrote $\mathbf{\bar x} = f(\mathbf x, \mathbf u)$. For the same reasons we don't use $\mathbf{H\bar{x}}$ in the computation for the residual, opting for the more accurate $h(\bar{\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 $r$ and elevation angle $\epsilon$ with the horizontal position $x$ and altitude $y$ of the aircraft is illustrated in the figure below:

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

This gives us the equalities:

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

### 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 $\bar{\mathbf x}$ and turn it into a measurement of the slant range distance. We use the Pythagorean theorem to derive:

$$h(\bar{\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(\bar{\mathbf x})}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t}
$$

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

$$\frac{\partial \mathbf H}{\partial \bar{\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 $\bar{\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(\bar{\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 let's 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.

$$\bar 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

$$\bar{\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{\bar x} &= \mathbf x +
\begin{bmatrix}- R\sin(\theta) + R\sin(\theta + \beta) \\
R\cos(\theta) - R\cos(\theta + \beta) \\
\beta\end{bmatrix}\\
\mathbf{\bar 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. Ours 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.figure()
    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 let's 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 this simplified model is 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 code.nonlinear_plots as 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.


## First Assignment

#### 1) Apply the appropriate string methods to the **x** variable (as '.upper') to change it exactly to: "$Dichlorodiphenyltrichloroethane$".

```python
x = "DiClOrod  IFeNi  lTRicLOr oETaNo  DiChlorod iPHeny lTrichL oroEThaNe"
x = x.upper()
x = x.capitalize()
x = x.replace("  ", "")
x = x.replace(" ", "")
print(x)
```

    Diclorodifeniltricloroetanodichlorodiphenyltrichloroethane

#### 2) Assign respectively the values: 'word', 15, 3.14 and 'list' to variables A, B, C and D in a single line of code. Then, print them in that same order on a single line separated by a space, using only one print statement.

```python
A = "word"; B = 15; C = 3.14; D = "list"
print(A + " " + str(B) + " " + str(C) + " " + D)
```

    word 15 3.14 list

#### 3) Use the **input()** function to receive an input in the form **'68.4 1.71'**, that is, two floating point numbers in a line separated by space. Then, assign these numbers to the variables **w** and **h** respectively, which represent an individual's weight and height (hint: take a look at the '.split()' method). With this data, calculate the individual's Body Mass Index (BMI) from the following relationship:

\begin{equation}
BMI = \dfrac{weight}{height^2}
\end{equation}

```python
print("Enter weight and height (seperated by space):")
w, h = input().split()
w = (float(w))
h = (float(h))
bmi = w / (h**2)
print("\nThe resulting BMI is " + str(bmi))
```

    Enter weight and height (seperated by space):
    68.4 1.71

    The resulting BMI is 23.39181286549708

#### This value can also be classified according to ranges of values, following to the table below. Use conditional structures to classify and print the classification assigned to the individual.

<\center>

(source: https://healthtravelguide.com/bmi-calculator/)

```python
print("Enter weight and height (seperated by space):")
w, h = input().split()
w = (float(w))
h = (float(h))
bmi = w / (h**2)

if bmi < 18.5 :
  print("Underweight")
elif bmi >= 18.5 and bmi <= 24.9 :
  print("Normal weight")
elif bmi >= 25 and bmi <= 29.9 :
  print("Pre-obesity")
elif bmi >= 30 and bmi <= 34.9 :
  print("Obesity class I")
elif bmi >= 35 and bmi <= 39.9 :
  print("Obesity class II")
else :
  print("Obesity class III")
```

    Enter weight and height (seperated by space):
    68.4 1.71
    Normal weight

#### 4) Receive an integer as an input and, using a loop, calculate the factorial of this number, that is, the product of all the integers from one to the number provided.

```python
print("Enter any number to calculate its factorial:")
zahl = int(input())
factorial = 1

if zahl > 0 :
  for i in range(1, zahl+1):
    factorial = factorial*i
    print("calculating..." + "\n" + "The factorial of " + str(zahl) + " is " + str(factorial))
else :
  print ("calculation of factorial not possible")

```

    Enter any number to calculate its factorial:
    4
    calculating...
    The factorial of 4 is 1
    calculating...
    The factorial of 4 is 2
    calculating...
    The factorial of 4 is 6
    calculating...
    The factorial of 4 is 24

#### 5) Using a while loop and the input function, read an indefinite number of integers until the number read is -1. Present the sum of all these numbers in the form of a print, excluding the -1 read at the end.

```python
print("Enter a number of integers:")
a_num = int(input())
total = 0

while a_num != -1:
  total = total + a_num
  a_num = int(input())
print(total)
```

    Enter a number of integers:
    45
    76
    4
    8
    3
    -1
    136

#### 6) Read the **first name** of an employee, his **amount of hours worked** and the **salary per hour** in a single line separated by commas. Next, calculate the **total salary** for this employee and show it to two decimal places.

```python
pers_x = {"first name":"Max Müller", "amount of hours worked":520, "salary per hour":12.45}
total_salary = pers_x["amount of hours worked"] * pers_x["salary per hour"]
ts_fo = "{:.2f}".format(total_salary)
print("The total salary of " + pers_x["first name"] + " is " + str(ts_fo))
```

    The total salary of Max Müller is 6474.00

#### 7) Read three floating point values **A**, **B** and **C** respectively. Then calculate itens a, b, c, d and e:

```python
print("Enter values to define A, B and C:")
A = float(input())
B = float(input())
C = float(input())
```

    Enter values to define A, B and C:
    4
    8
    5

    a) the area of the triangle rectangle with A as the base and C as the height.

```python
tri = (C*A)/2
print("The area of the triangle is " + str(tri))
```

    The area of the triangle is 10.0

    b) the area of the circle of radius C. (pi = 3.14159)

```python
circ = 3.14159*(C**2)
print("The area of the circle is " + str(circ))
```

    The area of the circle is 78.53975

    c) the area of the trapezoid that has A and B for bases and C for height.

```python
trape = ((A+B)/2)*C
print("The area of the trapezoid is " + str(trape))
```

    The area of the trapezoid is 30.0

    d) the area of the square that has side B.

```python
squ = B**2
print("The area of the square is " + str(squ))
```

    The area of the square is 64.0

    e) the area of the rectangle that has sides A and B.

```python
rec = A * B
print("The area of the rectangle is " + str(rec))
```

    The area of the rectangle is 32.0

#### 8) Read **the values a, b and c** and calculate the **roots of the second degree equation** $ax^{2}+bx+c=0$ using [this formula](https://en.wikipedia.org/wiki/Quadratic_equation). If it is not possible to calculate the roots, display the message **“There are no real roots”**.

```python
print("Enter values to define a, b and c:")
a = float(input())
b = float(input())
c = float(input())

import cmath
discr = (b**2) - (4*a*c)

root1 = (-b-cmath.sqrt(discr))/(2*a)
root2 = (-b+cmath.sqrt(discr))/(2*a)

if discr > 0 :
  print("The roots are " + str(root1) + " " + str(root2))
else :
  print("There are no real roots")
```

    Enter values to define a, b and c:
    1
    7
    7
    The roots are (-5.7912878474779195+0j) (-1.20871215252208+0j)

#### 9) Read four floating point numerical values corresponding to the coordinates of two geographical coordinates in the cartesian plane. Each point will come in a line with its coordinates separated by space. Then calculate and show the distance between these two points.

(obs: $d=\sqrt{(x_1-x_2)^2 + (y_1-y_2)^2}$)

```python
print("Enter four coordinates for two geographical points:")
x_1 = float(input())
x_2 = float(input())
y_1 = float(input())
y_2 = float(input())

print("The coordinates are:" + "\n" + str(x_1) + "°N " + str(x_2) + "°E\n" + str(y_1) + "°N " + str(y_2) + "°E")

dis = cmath.sqrt((x_1 - x_2)**2 + (y_1 - y_2)**2)
print("The distance between these points is " + str(dis))
```

    Enter four coordinates for two geographical points:
    48.2084
    16.3735
    51.1662
    13.4715
    The coordinates are:
    48.2084°N 16.3735°E
    51.1662°N 13.4715°E
    The distance between these points is (49.33914537261464+0j)

#### 10) Read **two floating point numbers** on a line that represent **coordinates of a cartesian point**. With this, use **conditional structures** to determine if you are at the origin, printing the message **'origin'**; in one of the axes, printing **'x axis'** or **'y axis'**; or in one of the four quadrants, printing **'q1'**, **'q2**', **'q3'** or **'q4'**.

```python
print("Enter two coordinates seperated by space: ")
X, Y = input().split()
X = (float(X))
Y = (float(Y))

print("This point's location is:")
if X > 0 and Y > 0 :
  print("q1")
if X < 0 and Y > 0 :
  print("q2")
if X < 0 and Y < 0 :
  print("q3")
if X > 0 and Y < 0 :
  print("q4")
if X == 0 and Y != 0 :
  print("y-axis")
if X != 0 and Y == 0 :
  print("x-axis")
if X == 0 and Y == 0 :
  print("origin")
```

    Enter two coordinates seperated by space:
    0 0
    This point's location is:
    origin

11) Read an integer that represents a phone code for international dialing.
#### Then, inform to which country the code belongs to, considering the generated table below:
(You just need to consider the first 10 entries)

```python
import pandas as pd
df = pd.read_html('https://en.wikipedia.org/wiki/Telephone_numbers_in_Europe')[1]
df = df.iloc[:,:2]
df.head(20)
```

      Country
      Country calling code

      0
      Austria
      43

      1
      Belgium
      32

      2
      Bulgaria
      359

      3
      Croatia
      385

      4
      Cyprus
      357

      5
      Czech Republic
      420

      6
      Denmark
      45

      7
      Estonia
      372

      8
      Finland
      358

      9
      France
      33

      10
      Germany
      49

      11
      Greece
      30

      12
      Hungary
      36

      13
      Iceland
      354

      14
      Ireland
      353

      15
      Italy
      39

      16
      Latvia
      371

      17
      Liechtenstein
      423

      18
      Lithuania
      370

      19
      Luxembourg
      352

```python
print("Enter a country calling code:")
ccd = str(input())

check = df[df["Country calling code"]==ccd]

print("\nThis country calling code belongs to\n" + str(check["Country"]))
```

    Enter a country calling code:
    43

    This country calling code belongs to
    0    Austria
    Name: Country, dtype: object

#### 12) Write a piece of code that reads 6 numbers in a row. Next, show the number of positive values entered. On the next line, print the average of the values to one decimal place.

```python
print("Enter 6 numbers: ")
p = float(input()); o = float(input()); u = float(input()); t = float(input()); r = float(input()); q = float(input())
nums = [o, p, u, t, r, q]
aver = (sum(nums))/6

start = 0
if o > 0 :
  start = start + 1
if p > 0 :
  start = start + 1
if u > 0 :
  start = start + 1
if t > 0 :
  start = start + 1
if r > 0 :
  start = start + 1
if q > 0 :
  start = start + 1

print("The number of positive values entered is " + (str(start)))
print("The average of the values is " + str(aver))

```

    Enter 6 numbers:
    3
    5
    2
    5
    -323
    4
    The number of positive values entered is 5
    The average of the values is -50.666666666666664

#### 13) Read an integer **N**. Then print the **square of each of the even values**, from 1 to N, including N, if applicable, arranged one per line.

```python
print("Enter an integer:")
N = int(input())

sqr = []
for i in range (1, N):
  if (i % 2) == 0:
    sqr.append(i*i)
if (N % 2) == 0:
   sqr.append(N*N)
print("square output:")
for item in sqr:
  print(item)
```

    Enter an integer:
    21
    square output:
    4
    16
    36
    64
    100
    144
    196
    256
    324
    400

#### 14) Using **input()**, read an integer and print its classification as **'even / odd'** and **'positive / negative'** . The two classes for the number must be printed on the same line separated by a space. In the case of zero, print only **'null'**.

```python
z = input("Enter a number: ")
z = int(z)

if (z % 2) == 0 and z != 0 :
  print("This number is even.")
elif z == 0 :
  print("null")
else :
  print("This number is odd.")

if z > 0 :
  print("This number is positive.")
elif z == 0:
  print("null")
else :
  print("This number is negative.")

```

    Enter a number: 65764356
    This number is even.
    This number is positive.

## Challenge
#### 15) Ordering problems are recurrent in the history of programming. Over time, several algorithms have been developed to fulfill this function. The simplest of these algorithms is the [**Bubble Sort**](https://en.wikipedia.org/wiki/Bubble_sort), which is based on comparisons of elements two by two in a loop of passes through the elements. Your mission, if you decide to accept it, will be to input six whole numbers ramdonly ordered. Then implement the **Bubble Sort** principle to order these six numbers **using only loops and conditionals**.
#### At the end, print the six numbers in ascending order on a single line separated by spaces.

```python
str_inp = input("Enter six numbers seperated by spaces:\n")
nums= [int(x) for x in str_inp.split()]

count = len(nums)

for outer in range(count - 1):
  for i in range(count - outer - 1):
    if nums[i] > nums[i + 1]:
      nums[i],nums[i + 1] = nums[i + 1],nums[i]
res = str(nums)
res = res.replace(",", " ")
print("Numbers sorted in ascending order:\n" + str(res))
```

    Enter six numbers seperated by spaces:
    4 -76 3 0 -333 2
    Numbers sorted in ascending order:
    [-333  -76  0  2  3  4]


# 4兆通りの足し算を同時に行う

量子状態生成の感覚がつかめてきたところで、量子計算をするとはどういうことかに話を移して行きましょう。

```{contents} 目次
---
local: true
---
```

$\newcommand{\ket}[1]{|#1\rangle}$

## 量子計算の特徴

量子計算をするとはどういうことかを一概に決めることはできませんし、それ自体がまだ世界中で盛んに研究されているトピックです。ただ、現在知られている量子コンピュータの実用法に共通する大まかな特徴を挙げるならば、以下の2点になります。

- **並列性を利用する**：Equal superposition状態で見たように、$n$量子ビットがあるとき、比較的簡単な操作で$2^n$個の計算基底が顕に登場する状態が作れます。また、この状態には全ての量子ビットが関与するため、どのビットに対するゲート操作も全ての計算基底に影響を及ぼします。つまり、各ゲート操作が常に$2^n$重の並列演算となります。
- **干渉を利用する**：量子振幅は複素数なので、二つの振幅が足し合わされるとき、それぞれの位相によって和の振幅の値が変わります。特に絶対値が等しく位相が逆である（$\pi$だけ異なる）場合和が0となるので、そのことを利用して回路の量子状態の重ね合わせから特定の計算基底を除くといったことが可能です。

この2つの要素のうち、特に干渉を上手に使ったアルゴリズムを見出すのが難しいため、量子コンピュータの応用可能性にまだ未知数な部分が大きい、というのが現状です。今回の実習でも、干渉を利用する部分もありますが、主に並列性ということに着目します。

## 巨大SIMDマシンとしての量子コンピュータ

SIMD (single instruction multiple data)とは並列計算パラダイムの一つで、プロセッサの命令（instruction）が多数のデータに同時に適用されるケースを指します。私達の身の回りの（古典）コンピュータのプロセッサにもSIMD用のインストラクションセットが搭載されており、例えば（2021年2月現在）最先端の商用CPUでは、16個の単精度浮動小数点数に対し同時に四則演算や平方根の計算を行えます。

量子コンピュータでは、第一回の実習で触れられたように、すべてのゲートがすべての計算基底に作用します。ゲート操作を命令、各計算基底の振幅をデータとして解釈すれば、これは常に$2^n$個のデータに命令を与えながら計算をしていることにあたります。量子コンピュータは巨大SIMDマシンとも考えられるのです。

ただし、これもすでに触れられたことですが、巨大並列計算ができたとしても、そのデータをすべて引き出すことはできません[^and_you_dont_want_to]ので、古典計算機のSIMDとはいろいろな意味で単純に比較できるものではありません。

とはいえ、並列計算を行っているんだということを実感できると、より量子コンピュータを使う感覚が身に付いてくると思われるので、今回は最も単純に「足し算」をたくさん並列に行う回路を書いてみましょう。

[^and_you_dont_want_to]: そもそも、例えば65量子ビットの計算機からすべてのデータを保存しようと思うと、各振幅を128（古典）ビットの浮動小数点複素数で表現したとすれば512EiB (エクサバイト)のストレージが必要です。これはだいたい現在インターネットを行き来する情報二ヶ月分に相当するので、保存するファシリティを作るにはそれなりの投資が必要です。

## 量子フーリエ変換による足し算

これまで量子「計算機」の話をしていながら、単純であるはずの四則演算のやりかたについて触れていませんでした。理由は、実は量子コンピュータでは四則演算がそんなに単純でないから、です。

足し算を行う量子サブルーチンはいくつか知られていますが、その中で量子ビットの数や用いるゲートの種類の面で効率的なのが、フーリエ変換を用いたものです{cite}`quantum_addition`。ただの足し算にフーリエ変換を持ち出すのは奇妙に思えますが、実際に動かしてみるとなかなかスマートな手法であることがわかります。

まずは計算の流れを数式で追ってみましょう。整数$a$と$b$の足し算を考えます。まず、2つのレジスタをそれぞれ状態$\ket{a}$と$\ket{b}$に用意します。以下のように、第3のレジスタ（初期状態$\ket{0}$）の状態が和$a+b$を表すようにすることが目標です。それぞれのレジスタは十分に大きい（レジスタ$i$のビット数を$n_i$として$2^{n_1} > a$, $2^{n_2} > b$, $2^{n_3} > a + b$）とします。

$$
\ket{a}\ket{b}\ket{0} \rightarrow \ket{a}\ket{b}\ket{a+b}
$$

量子フーリエ変換は、ビット数$n$のレジスタの計算基底$\ket{j}$を

$$
\ket{j} \xrightarrow{\text{Q.F.T.}} \frac{1}{\sqrt{2^n}}\sum_{k=0}^{2^n-1} e^{2\pi i jk/2^n} \ket{k}
$$

という状態に変える操作でした。では、その逆を考えると、整数$a+b < 2^n$について

$$
\frac{1}{\sqrt{2^n}}\sum_{k=0}^{2^n-1} e^{2\pi i (a+b)k/2^n} \ket{k} \xrightarrow{\text{Q.F.T.}^{-1}} \ket{a+b}
$$

ができることがわかります。すべての量子ゲートには逆操作が存在するので、すべての量子サブルーチンは逆回しできます。

左辺の状態を作るには、これも量子フーリエ変換のアルゴリズムを参考にします。整数$a, b, k$の二進分解

$$
a = \sum_{m=0}^{n_1-1} 2^m a_m \\
b = \sum_{m=0}^{n_2-1} 2^m b_m \\
k = \sum_{m=0}^{n_3-1} 2^m k_m
$$

を用いて、

$$
\exp\left(2\pi i \frac{(a+b)k}{2^{n_3}}\right) = \left[\prod_{l=0}^{n_1-1}\prod_{m=0}^{n_3-1} \exp\left(2\pi i \frac{2^{l+m} a_l k_m}{2^{n_3}}\right)\right]\left[\prod_{l=0}^{n_2-1}\prod_{m=0}^{n_3-1} \exp\left(2\pi i \frac{2^{l+m} b_l k_m}{2^{n_3}}\right)\right]
$$

と書けることを利用します。つまり、レジスタ1または2の各ビットとレジスタ3の各ビットを一つずつ組み合わせて、両方のビットが1である（$a_l = k_m = 1$または$b_l = k_m = 1$の）ときに対応する分（$2\pi 2^{l + m} / 2^{n_3}$）位相を進めれば、左辺の状態ができあがります。

具体的には、まずレジスタ3をequal superpositionに用意し、レジスタ1の各ビットを制御、レジスタ3の各ビットを標的とした$C[P]$ゲートをかけていきます。

$$
\begin{align}
\ket{a}\ket{b}\ket{0} & \xrightarrow{H^{\otimes n_3}} \ket{a}\ket{b} \frac{1}{\sqrt{2^{n_3}}} \sum_{k=0}^{2^{n_3}-1} \ket{k} \\
& \xrightarrow{C^{1;0}_{3;0}[P(2\pi \cdot 2^0 \cdot 2^0/2^{n_3})]} \ket{a}\ket{b} \frac{1}{\sqrt{2^{n_3}}} \sum_{k=0}^{2^{n_3}-1} \exp \left( 2\pi i \frac{a_0 k_0}{2^{n_3}} \right) \ket{k} \\
& \xrightarrow{C^{1;0}_{3;1}[P(2\pi \cdot 2^0 \cdot 2^1/2^{n_3})]} \ket{a}\ket{b} \frac{1}{\sqrt{2^{n_3}}} \sum_{k=0}^{2^{n_3}-1} \exp \left( 2\pi i \frac{a_0 (k_0 + 2k_1)}{2^{n_3}} \right) \ket{k} \\
\cdots & \\
& \xrightarrow{C^{1;0}_{3;n_3 - 1}[P(2\pi \cdot 2^0 \cdot 2^{n_3 - 1}/2^{n_3})]} \ket{a}\ket{b} \frac{1}{\sqrt{2^{n_3}}} \sum_{k=0}^{2^{n_3}-1} \exp \left( 2\pi i \frac{a_0 k}{2^{n_3}} \right) \ket{k} \\
& \xrightarrow{C^{1;1}_{3;0}[P(2\pi \cdot 2^1 \cdot 2^0/2^{n_3})]} \ket{a}\ket{b} \frac{1}{\sqrt{2^{n_3}}} \sum_{k=0}^{2^{n_3}-1} \exp \left( 2\pi i \frac{a_0 k + 2a_1 k_0}{2^{n_3}} \right) \ket{k} \\
\cdots & \\
& \xrightarrow{C^{1;n_1 - 1}_{3;n_3 - 1}[P(2\pi \cdot 2^{n_1-1} \cdot 2^{n_3 - 1}/2^{n_3})]} \ket{a}\ket{b} \frac{1}{\sqrt{2^{n_3}}} \sum_{k=0}^{2^{n_3}-1} \exp \left( 2\pi i \frac{a k}{2^{n_3}} \right) \ket{k}
\end{align}
$$

続いてレジスタ2のビットを制御として、同様の$C[P]$ゲートをかけていくと、

$$
\begin{align}
& \xrightarrow{C^{2;0}_{3;0}[P(2\pi \cdot 2^0 \cdot 2^0/2^{n_3})]} \ket{a}\ket{b} \frac{1}{\sqrt{2^{n_3}}} \sum_{k=0}^{2^{n_3}-1} \exp \left( 2\pi i \frac{ak + b_0 k_0}{2^{n_3}} \right) \ket{k} \\
& \xrightarrow{C^{2;0}_{3;1}[P(2\pi \cdot 2^0 \cdot 2^1/2^{n_3})]} \ket{a}\ket{b} \frac{1}{\sqrt{2^{n_3}}} \sum_{k=0}^{2^{n_3}-1} \exp \left( 2\pi i \frac{ak + b_0 (k_0 + 2k_1)}{2^{n_3}} \right) \ket{k} \\
\cdots & \\
& \xrightarrow{C^{2;0}_{3;n_3 - 1}[P(2\pi \cdot 2^0 \cdot 2^{n_3 - 1}/2^{n_3})]} \ket{a}\ket{b} \frac{1}{\sqrt{2^{n_3}}} \sum_{k=0}^{2^{n_3}-1} \exp \left( 2\pi i \frac{(a + b_0) k}{2^{n_3}} \right) \ket{k} \\
& \xrightarrow{C^{2;1}_{3;0}[P(2\pi \cdot 2^1 \cdot 2^0/2^{n_3})]} \ket{a}\ket{b} \frac{1}{\sqrt{2^{n_3}}} \sum_{k=0}^{2^{n_3}-1} \exp \left( 2\pi i \frac{(a + b_0) k + 2b_1 k_0}{2^{n_3}} \right) \ket{k} \\
\cdots & \\
& \xrightarrow{C^{2;n_2 - 1}_{3;n_3 - 1}[P(2\pi \cdot 2^{n_2 - 1} \cdot 2^{n_3 - 1}/2^{n_3})]} \ket{a}\ket{b} \frac{1}{\sqrt{2^{n_3}}} \sum_{k=0}^{2^{n_3}-1} \exp \left( 2\pi i \frac{(a + b) k}{2^{n_3}} \right) \ket{k}
\end{align}
$$

となり、めでたく$\ket{a+b}$のフーリエ変換状態が実現されました。

では以上の操作をQiskitで実装してみましょう。レジスタ1と2は4ビットとして、$a=9, b=13$を考えます。後の便利のために、まずは足し算のサブルーチン部分だけを関数化します。

```python
# まずは全てインポート
import numpy as np
import matplotlib.pyplot as plt
from qiskit import ClassicalRegister, QuantumRegister, QuantumCircuit, IBMQ, Aer, transpile
from qiskit.tools.monitor import job_monitor
from qiskit.providers.ibmq import least_busy
from utils.show_state import show_state
from utils.optimized_additions import optimized_additions, get_initial_layout

print('notebook ready')
```

```python
def setup_addition(circuit, reg1, reg2, reg3):
    """Set up an addition subroutine to a circuit with three registers
    """

    # Equal superposition in register 3
    circuit.h(reg3)

    # Smallest unit of phi
    dphi = 2. * np.pi / (2 ** reg3.size)

    # Loop over reg1 and reg2
    for reg_ctrl in [reg1, reg2]:
        # Loop over qubits in the control register (reg1 or reg2)
        for qctrl in reg_ctrl:
            # Loop over qubits in the target register (reg3)
            for qtarg in reg3:
                # C[P(phi)], phi = 2pi * 2^{ictrl} * 2^{itarg} / 2^{n3}
                circuit.cp(dphi * (2 ** (qctrl.index + qtarg.index)), qctrl, qtarg)

    # Insert a barrier for better visualization
    circuit.barrier()

    # Inverse QFT
    for j in range(reg3.size // 2):
        circuit.swap(reg3[j], reg3[-1 - j])

    for itarg in range(reg3.size):
        for ictrl in range(itarg):
            power = ictrl - itarg - 1 + reg3.size
            circuit.cp(-dphi * (2 ** power), reg3[ictrl], reg3[itarg])

        circuit.h(reg3[itarg])

print('Defined function setup_addition')
```

回路を作り、レジスタ1と2を入力9と13を表すように初期化します。

```python
a = 9
b = 13

# Calculate the necessary register sizes
n1 = np.ceil(np.log2(a + 1)).astype(int)
n2 = np.ceil(np.log2(b + 1)).astype(int)
n3 = np.ceil(np.log2(a + b + 1)).astype(int)

print('n1 =', n1, 'n2 =', n2, 'n3 =', n3)

reg1 = QuantumRegister(n1, 'r1')
reg2 = QuantumRegister(n2, 'r2')
reg3 = QuantumRegister(n3, 'r3')

# QuantumCircuit can be instantiated from multiple registers
circuit = QuantumCircuit(reg1, reg2, reg3)

# Set register 1 to state |a>
for j in range(n1):
    if ((a >> j) & 1) == 1:
        circuit.x(reg1[j])

# Set register 2 to state |b>
for j in range(n2):
    if ((b >> j) & 1) == 1:
        circuit.x(reg2[j])

setup_addition(circuit, reg1, reg2, reg3)

circuit.draw('mpl', scale=0.6, fold=100)
```

再び`show_state`関数を使って終状態を確認してみましょう。

```python
show_state(circuit, register_sizes=(n1, n2, n3), draw=False)
```

ここで、`register_sizes`というオプション引数を渡して、13ビットの回路を$n_1 + n_2 + n_3$ビットに分けて解釈するよう指定しました。結果表示された状態は期待通り単一の計算基底$22:13:9$、つまりレジスタ1, 2, 3がそれぞれ$\ket{9}, \ket{13}, \ket{22}$となっている状態です（回路全体でビットを右から書いて状態を表示するため、レジスタも右から左に並びます）。つまり、めでたく状態の変遷

$$
\ket{9}\ket{13}\ket{0} \rightarrow \ket{9}\ket{13}\ket{22}
$$

が実現しました。

## 足し算の並列化

上では小学一年生ができる足し算を一回行うために13個の量子ビットと67個のゲートを利用しました。しかし、出来上がった回路は入力の値（9と13）によらず、4ビットで表現できる2つの整数すべてに対して成り立ちます（一般に2つの$n$ビット数の和は$n+1$ビットに収まるので、レジスタ3の大きさにも不足はありません）。さらに、量子演算は線形（つまり演算$U$について$U(\sum_{k} c_k \ket{k}) = \sum_{k} c_k U\ket{k}$）なので、初期状態としてレジスタ1と2がどんな計算基底の重ね合わせにあっても、それぞれの組み合わせに対してレジスタ3の状態が和を表してくれます。特に、初期状態がequal superpositionであれば、この回路は

$$
\sum_{j=0}^{2^{n_1}-1} \sum_{k=0}^{2^{n_2}-1} \ket{j}\ket{k}\ket{0} \rightarrow \sum_{j=0}^{2^{n_1}-1} \sum_{k=0}^{2^{n_2}-1} \ket{j}\ket{k}\ket{j+k}
$$

を行うので、$\mathcal{O}\left((n_1 + n_2 + n_3) n_3\right)$個のゲートで$2^{n_1+n_2}$通りの足し算を並列に行います。実際にこれを確認してみましょう。

```python
n1 = 4
n2 = 4
n3 = np.ceil(np.log2((2 ** n1) + (2 ** n2) - 1)).astype(int)

reg1 = QuantumRegister(n1, 'r1')
reg2 = QuantumRegister(n2, 'r2')
reg3 = QuantumRegister(n3, 'r3')

# QuantumCircuit can be instantiated from multiple registers
circuit = QuantumCircuit(reg1, reg2, reg3)

# Set register 1 and 2 to equal superpositions
circuit.h(reg1)
circuit.h(reg2)

setup_addition(circuit, reg1, reg2, reg3)

show_state(circuit, register_sizes=(n1, n2, n3), amp_norm=(1. / np.sqrt(2 ** (n1 + n2)), r'\frac{1}{\sqrt{2^{n_1 + n_2}}}'), draw=False)
```

65量子ビットのマシンを最大限利用するならば、$n_1 = n_2 = 21, n_3 = 22$で$2^{42}$通り、つまり約4兆通りの足し算を同時に行うことができます。

もちろん、上で書いたようにここには重要な但し書きがあって、実機でこの計算をして測定から答えを得ようとすると、毎回の測定でどの組み合わせが得られるかをコントロールできないので、これはあまり実用的とは言えない回路です。強いて言えば毎日ランダムに6桁＋6桁の正しい足し算を教えてくれる「日めくり足し算カレンダー」にくらいは使えます。4兆通りなので100億年程度使い続けられます。

## シミュレータでの実行

上の足し算回路の結果がランダムに出る様子をシミュレーションで確認しましょう。課題では実機でも実行します。その際、上の回路実装では非効率的でエラーが出すぎるので、これからは[こちらの効率化した回路](https://github.com/UTokyo-ICEPP/qc-workbook/tree/master/source/utils/optimized_additions.py)を代わりに使用します。

```python
# 元の回路に測定を加える
circuit.measure_all()
circuit_original = circuit

# 効率化した回路（測定付き）
circuit_optimized = optimized_additions(n1, n2)
print('Constructed an optimized addition circuit')
```

回路の効率化とは具体的にどういうことでしょうか。もともとの回路と効率化したものとを比べてみましょう。まずは、単純にオペレーションの数を比較します。ゲート一つ一つで一定の確率でエラーが起こるということは、同じことをする回路ならゲートの数が少ないほうがより正確な計算をしてくれます。

```python
print('Number of operations in the original circuit:', circuit_original.size())
print('Number of operations in the optimized circuit:', circuit_optimized.size())
```

効率化したはずの回路のほうがはるかにゲート数が多いという結果になりました。なぜでしょうか。

### トランスパイルと物理的回路

{doc}`circuit_from_scratch`でも少し触れましたが、回路が実機で実行される前には、トランスパイルという回路の変換が行われ、様々な複合ゲートからなる「論理的」な回路から、実機のハードウェアに実装されている基本ゲートのみで書かれる「物理的」な回路が作られます[^physical]。論理的な回路はどのようにでも書ける（例えば回路全体を一つのゲートと呼んでしまうこともできる）ので、回路の効率の比較はトランスパイル後でなければ意味がありません。

いい機会なので、実機での量子計算について少し詳細に考えてみましょう。トランスパイルがまさに論理的なアルゴリズムの世界と物理的実装の世界のインターフェースとなるので、この過程に注目します。

トランスパイル時には、以下のような回路の変換が起こります。

- 冗長なゲートの削除
- 多重制御ゲートのCNOTと1量子ビットゲートへの分解
- 実機のトポロジーに即した量子ビットのマッピング（詳細は下）
- 物理的に隣接しない量子ビット間の制御ゲートを実行するためのSWAPの挿入
- 1量子ビットゲートの基本ゲートへの分解
- 物理的回路の最適化

実機のトポロジーとは、実際の量子プロセッサチップ上での量子ビット同士の繋がりかたのことを指します。2つの量子ビットが繋がっているとは、その間で基本制御ゲート（IBMQではCNOT）が実行できるということを意味します。これまで考慮してきませんでしたが、実はすべての量子ビットが繋がっているわけではないのです。例えば後で使うibmq_16_melbourneというマシンは以下のようなトポロジーを持っています。

```{image} figs/melbourne_topology.png
:height: 200px
:name: ibmq_16_melbourne
```

図中、数字のついた丸が量子ビットを表し、線が量子ビット同士の繋がりを表します。

直接接続のない量子ビット間で制御ゲートを実行する場合、SWAPを使って2つの量子ビットが隣り合うように状態を遷移させていく必要があります。例えば上のibmq_16_melbourneでビット2と6の間のCNOTが必要なら、（いくつか方法がありますが）2↔3, 3↔4, 4↔5とSWAPを繰り返して、5と6の間でCNOTを行い、ビットの並びを元に戻す必要があれば再度5↔4, 4↔3, 3↔2とSWAPをすることになります。

{ref}`ゲートの解説 <other_single_and_double_qubit_gates>`に出てきたように、SWAPは3つのCNOTに分解されます。つまり、直接接続のない量子ビット同士の制御ゲートが多出するような回路があると、莫大な数のCNOTが使われることになります。**CNOTのエラー率（ゲート操作一回あたりに操作の結果を間違える確率）は1量子ビットゲートのエラー率より一桁ほど高い**ので、これは大きな問題になります。そこで、論理的回路の量子ビットと実機の量子ビットとのマッピング（SWAPが発生すれば対応は変わっていくので、あくまで初期対応）と、回路中にどうSWAPを挿入していくかというルーティングの両方を上手に決めるということが、トランスパイルにおける中心的な課題です。

しかし、実は一般の回路に対して最適なマッピングとルーティングを探すという問題自体がいわゆるNP-hardな問題なので、qiskitのトランスパイル・ルーチンではこの問題の最適解を探してくれません。代わりにstochastic swapという、乱数を用いた手法が標準設定では利用されます。Stochastic swapは多くの回路で比較的効率のいいルーティングを作ることが知られていますが、乱数を利用するため実行のたびに異なるルーティングが出てくるなど、やや扱いにくい面もあります。また、単純な回路で事前に最適なルーティングがわかっている場合は、stochastic swapを使うべきではありません。

### 回路の比較

上を踏まえて、改めて2つの足し算回路を比較してみましょう。

これまで実機で回路を実行するときは`execute`という関数を利用していましたが、実はこの関数にトランスパイル前の回路を渡すと、自動で標準設定のトランスパイルがなされていました。しかし`transpile`という関数を使い、トランスパイルを明示的に行うこともできます。この関数は主にトランスパイルの設定を細かくコントロールしたいときなどに使います。今回の効率化した回路は特定のマッピングのために作られているので、stochastic swapを使用しないよう設定を変更してトランスパイルをします。マッピングはibmq_16_melbourneのトポロジーとエラー率を参考にして事前に決めてあり、`get_initial_layout`という関数の中にハードコードされています。

IBMQのマシンはチップごとに異なるトポロジーを持っているので、`transpile`関数にも引数としてバックエンドを指定し、そのバックエンド専用の物理的回路を作ります。

[^physical]: 「物理的」な回路もまだ実は論理的な存在であり、本当にハードウェアが理解するインストラクションに変換するには、さらに基本ゲートを特定のマイクロ波パルス列に直す必要があります。

```python
IBMQ.enable_account('__paste_your_token_here__')

provider = IBMQ.get_provider(hub='ibm-q', group='open', project='main')

# 4 + 4 + 5 = 13量子ビット以上のマシンで ibm-q/open/main からアクセス可能なのはibmq_16_melbourneのみ（2021年3月現在）
backend = provider.get_backend('ibmq_16_melbourne')

# オリジナルの回路をトランスパイルする。optimization_level=3は自動設定のうち、最も効率のいい回路を作る
print('Transpiling the original circuit with standard settings')
circuit_original_tr = transpile(circuit_original, backend=backend, optimization_level=3)

# 効率化した回路をトランスパイルする。ibmq_16_melbourneのうち、一直線に繋がった量子ビット列にマッピングする
print('Transpiling the optimized circuit with basic routing and custom qubit mapping')
initial_layout = get_initial_layout(backend, n1, n2)
circuit_optimized_tr = transpile(circuit_optimized, backend=backend, routing_method='basic', initial_layout=initial_layout, optimization_level=3)

print('Number of operations in the original circuit:', circuit_original_tr.size())
print('  Breakdown: N(Rz)={rz}, N(SX)={sx}, N(CNOT)={cx}'.format(**circuit_original_tr.count_ops()))
print('Number of operations in the optimized circuit:', circuit_optimized_tr.size())
print('  Breakdown: N(Rz)={rz}, N(SX)={sx}, N(CNOT)={cx}'.format(**circuit_optimized_tr.count_ops()))
```

```{tip}
上のように`optimization_level=3`でトランスパイルをすると、回路の最適化ルーチンに時間がかかりすぎることがたまにあります。セルを実行して1分以上反応がない場合は、一度Jupyterのカーネルを再起動してみてください。
```

上のセルを実行すると、今度は効率化回路のオペレーションの数が元の回路の半数以下という結果になることがわかります。このマシンの基本ゲート$R_z, X$, SX, CNOTに分解した内訳でも、エラー率の大きいCNOTゲートの数が大幅に削減されています（両回路とも$X$ゲートは0個です）。

元の回路と効率化した回路の違いは、後者では「数珠つなぎ」になった量子ビット列というトポロジーを仮定して、制御ゲートの順番を工夫して直接明示的にSWAPを挿入していることです。さらに、可能なところでは$C[P]$ゲートの分解で生じるCNOTとSWAPのCNOTが打ち消し合うことも利用しています。最後の逆フーリエ変換でもゲートの順番が工夫してあります。

それでは、トランスパイルした回路を実行してみます。`transpile`関数を使って「手動で」トランスパイルを行った回路を`execute`に渡すと再び標準設定でトランスパイルされてしまうので、別の手続きを踏みます。

```python
qasm_simulator = Aer.get_backend('qasm_simulator')

# Will call backend.run() instead of execute()
job_original = qasm_simulator.run(circuit_original_tr, shots=100)
counts_original = job_original.result().get_counts()

job_optimized = qasm_simulator.run(circuit_optimized_tr, shots=100)
counts_optimized = job_optimized.result().get_counts()

# Keys of counts are single binaries; need to split them into three parts and interpret as decimals
def interpret_counts(counts, n1, n2):
    heights = []
    labels = []

    for key, value in counts.items():
        heights.append(value)

        x1 = int(key[-n1:], 2)
        x2 = int(key[-n1 - n2:-n1], 2)
        x3 = int(key[:-n1 - n2], 2)
        labels.append('{} + {} = {}'.format(x1, x2, x3))

    return np.linspace(0., len(labels), len(labels), endpoint=False), heights, labels

fig, (ax_original, ax_optimized) = plt.subplots(2, figsize=[16, 10])

x, heights, labels = interpret_counts(counts_original, n1, n2)
ax_original.bar(x, heights, width=0.5)
ax_original.set_xticks(x - 0.5)
ax_original.set_xticklabels(labels, rotation=70)
ax_original.tick_params('x', length=0.)

x, heights, labels = interpret_counts(counts_optimized, n1, n2)
ax_optimized.bar(x, heights, width=0.5)
ax_optimized.set_xticks(x - 0.5)
ax_optimized.set_xticklabels(labels, rotation=70)
ax_optimized.tick_params('x', length=0.)

fig.subplots_adjust(bottom=-0.2)
```

正しい足し算の式がランダムに出現することを確認してください。

## 参考文献

```{bibliography}
:filter: docname in docnames
```


This notebook is part of https://github.com/AudioSceneDescriptionFormat/splines, see also https://splines.readthedocs.io/.

[back to overview](catmull-rom.ipynb)

# Non-Uniform Catmull--Rom Splines

Catmull and Rom (1974)
describes only the [uniform case](catmull-rom-uniform.ipynb),
but it is straightforward to extend the method to non-uniform splines.

The method creates three linear interpolations
(and *extra*polations)
between neighboring pairs of the four relevant control points
and then blends the three resulting points
with a quadratic B-spline basis function.

As we have seen in the
[notebook about uniform Catmull--Rom splines](catmull-rom-uniform.ipynb#Cardinal-Functions)
and as we will again see in the
[notebook about the Barry--Goldman algorithm](catmull-rom-barry-goldman.ipynb#Combining-Both-Algorithms),
the respective degrees can be reversed.
This means that equivalently,
two (overlapping) quadratic Lagrange interpolations can be used,
followed by linearly blending the two resulting points.

Since latter is both easier to implement
and easier to wrap one's head around,
we use it in the following derivations.

We will derive
the [tangent vectors](#Tangent-Vectors) at the segment boundaries
(which will serve as basis for deriving
[non-uniform Kochanek--Bartels splines](kochanek-bartels-non-uniform.ipynb)
later)
and
the [basis matrix](#Basis-Matrix).
See the
[notebook about the Barry--Goldman algorithm](catmull-rom-barry-goldman.ipynb)
for an alternative (but closely related) derivation.

```python
import sympy as sp
sp.init_printing()
```

```python
x3, x4, x5, x6 = sp.symbols('xbm3:7')
```

```python
t, t3, t4, t5, t6 = sp.symbols('t t3:7')
```

We use some tools from [utility.py](utility.py):

```python
from utility import NamedExpression, NamedMatrix
```

As shown in the [notebook about Lagrange interpolation](lagrange.ipynb),
it can be implemented using *Neville's algorithm*:

```python
def lerp(xs, ts, t):
    """Linear interpolation.

    Returns the interpolated value at time *t*,
    given the two values *xs* at times *ts*.

    """
    x_begin, x_end = xs
    t_begin, t_end = ts
    return (x_begin * (t_end - t) + x_end * (t - t_begin)) / (t_end - t_begin)
```

```python
def neville(xs, ts, t):
    """Lagrange interpolation using Neville's algorithm.

    Returns the interpolated value at time *t*,
    given the values *xs* at times *ts*.

    """
    assert len(xs) == len(ts)
    while len(xs) > 1:
        step = len(ts) - len(xs) + 1
        xs = [
            lerp(*args, t)
            for args in zip(zip(xs, xs[1:]), zip(ts, ts[step:]))]
    return xs[0]
```

Alternatively,
[sympy.interpolate()](https://docs.sympy.org/latest/modules/polys/reference.html#sympy.polys.polyfuncs.interpolate) could be used.

We use two overlapping quadratic Lagrange interpolations
followed by linear blending:

```python
p4 = NamedExpression(
    'pbm4',
    lerp([
        neville([x3, x4, x5], [t3, t4, t5], t),
        neville([x4, x5, x6], [t4, t5, t6], t),
    ], [t4, t5], t))
```

Note

Since the two invocations of Neville's algorithm overlap,
some values that are used by both are unnecessarily computed by both.
It would be more efficient to calculate each of these values only once.

The [Barry--Goldman algorithm](catmull-rom-barry-goldman.ipynb)
avoids this repeated computation.

But here, since we are using symbolic expressions,
this doesn't really matter
because the redundant expressions should be simplified away by SymPy.

```python
p4.simplify()
```

The following expressions can be simplified
by introducing a few new symbols $\Delta_i$:

```python
delta3, delta4, delta5 = sp.symbols('Delta3:6')
deltas = {
    t4 - t3: delta3,
    t5 - t4: delta4,
    t6 - t5: delta5,
    t5 - t3: delta3 + delta4,
    t6 - t4: delta4 + delta5,
    t6 - t3: delta3 + delta4 + delta5,
    # A few special cases that SymPy has a hard time resolving:
    t4 + t4 - t3: t4 + delta3,
    t6 + t6 - t3: t6 + delta3 + delta4 + delta5,
}
```

## Tangent Vectors

To get the tangent vectors at the control points,
we just have to take the first derivative ...

```python
pd4 = p4.diff(t)
```

... and evaluate it at $t_4$ and $t_5$:

```python
start_tangent = pd4.evaluated_at(t, t4)
start_tangent.subs(deltas).simplify()
```

```python
end_tangent = pd4.evaluated_at(t, t5)
end_tangent.subs(deltas).simplify()
```

Both results lead to the same general expression
(which is expected,
since the incoming and outgoing tangents are supposed to be equal):

\begin{align*}
\boldsymbol{\dot{x}}_i
&=
\frac{
(t_{i+1} - t_i)^2 (\boldsymbol{x}_i - \boldsymbol{x}_{i-1}) +
(t_i - t_{i-1})^2 (\boldsymbol{x}_{i+1} - \boldsymbol{x}_i)
}{
(t_{i+1} - t_i)(t_i - t_{i-1})(t_{i+1} - t_{i-1})
} \\
&= \frac{
{\Delta_i}^2 (\boldsymbol{x}_i - \boldsymbol{x}_{i-1}) +
{\Delta_{i-1}}^2 (\boldsymbol{x}_{i+1} - \boldsymbol{x}_i)
}{
\Delta_i \Delta_{i-1} (\Delta_i + \Delta_{i-1})
}
\end{align*}

Equivalently, this can be written as:

\begin{align*}
\boldsymbol{\dot{x}}_i
&=
\frac{
(t_{i+1} - t_i) (\boldsymbol{x}_i - \boldsymbol{x}_{i-1})
}{
(t_i - t_{i-1})(t_{i+1} - t_{i-1})
}
+
\frac{
(t_i - t_{i-1}) (\boldsymbol{x}_{i+1} - \boldsymbol{x}_i)
}{
(t_{i+1} - t_i)(t_{i+1} - t_{i-1})
} \\
&=
\frac{
\Delta_i (\boldsymbol{x}_i - \boldsymbol{x}_{i-1})
}{
\Delta_{i-1} (\Delta_i + \Delta_{i-1})
}
+
\frac{
\Delta_{i-1} (\boldsymbol{x}_{i+1} - \boldsymbol{x}_i)
}{
\Delta_i (\Delta_i + \Delta_{i-1})
}
\end{align*}

An alternative (but very similar) way to derive these tangent vectors
is shown in the
[notebook about the Barry--Goldman algorithm](catmull-rom-barry-goldman.ipynb#Tangent-Vectors).

And there is yet another way to calculate the tangents,
without even needing to obtain a *cubic* polynomial and its derivative:
Since we are using a linear blend of two *quadratic* polynomials,
we know that at the beginning ($t = t_4$)
only the first quadratic polynomial has an influence
and at the end ($t = t_5$) only the second quadratic polynomial is relevant.
Therefore, to determine the tangent vector at the beginning of the segment,
it is sufficient to get the derivative of the first quadratic polynomial.

```python
first_quadratic = neville([x3, x4, x5], [t3, t4, t5], t)
```

```python
sp.degree(first_quadratic, t)
```

```python
first_quadratic.diff(t).subs(t, t4)
```

This can be written as
(which is sometimes called the
*standard three-point difference formula*):

\begin{equation*}
\boldsymbol{\dot{x}}_i =
\frac{
\Delta_i \boldsymbol{v}_{i-1} + \Delta_{i-1} \boldsymbol{v}_i
}{
\Delta_{i-1} + \Delta_i
},
\end{equation*}

with $\Delta_i = t_{i+1} - t_i$ and
$\boldsymbol{v}_i = \frac{\boldsymbol{x}_{i+1} - \boldsymbol{x}_i}{\Delta_i}$.

(de Boor 1978)
calls this *piecewise cubic Bessel interpolation*,
and it has also been called
*Bessel tangent method*,
*Overhauser method* and
*Bessel--Overhauser splines*.

Note

Even though this formula
is commonly associated with the name *Overhauser*,
it is *not* describing the tangents of *Overhauser splines*
(as presented in
Overhauser (1968)).

Long story short, it's the same as we had above:

```python
assert sp.simplify(_ - start_tangent.expr) == 0
```

The first derivative of the second quadratic polynomial
can be used to get the tangent vector at the end of the segment.

```python
second_quadratic = neville([x4, x5, x6], [t4, t5, t6], t)
second_quadratic.diff(t).subs(t, t5)
```

```python
assert sp.simplify(_ - end_tangent.expr) == 0
```

You might encounter another way
to write the equation for $\boldsymbol{\dot{x}}_4$
(e.g. at https://stackoverflow.com/a/23980479/) ...

```python
(x4 - x3) / (t4 - t3) - (x5 - x3) / (t5 - t3) + (x5 - x4) / (t5 - t4)
```

... but again, this is equivalent to the equation shown above:

```python
assert sp.simplify(_ - start_tangent.expr) == 0
```

## Using Non-Uniform Bézier Segments

Similar to [the uniform case](catmull-rom-uniform.ipynb#Using-Bézier-Segments),
the above equation for the tangent vectors can be used to construct
non-uniform [Hermite splines](hermite.ipynb) or,
after multiplying them with the appropriate parameter interval
and dividing them by 3,
to obtain the two additional control points for
[non-uniform cubic Bézier spline segments](bezier-non-uniform.ipynb#Control-Points-From-Tangent-Vectors):

\begin{align*}
\boldsymbol{\tilde{x}}_i^{(+)}
&= \boldsymbol{x}_i + \frac{\Delta_i \boldsymbol{\dot{x}}_i}{3}
= \boldsymbol{x}_i + \frac{
{\Delta_i}^2 (\boldsymbol{x}_i - \boldsymbol{x}_{i-1})
}{
3 \Delta_{i-1} (\Delta_i + \Delta_{i-1})
}
+
\frac{
\Delta_{i-1} (\boldsymbol{x}_{i+1} - \boldsymbol{x}_i)
}{
3 (\Delta_i + \Delta_{i-1})
}
\\
\boldsymbol{\tilde{x}}_i^{(-)}
&= \boldsymbol{x}_i - \frac{\Delta_{i-1} \boldsymbol{\dot{x}}_i}{3}
= \boldsymbol{x}_i - \frac{
\Delta_i (\boldsymbol{x}_i - \boldsymbol{x}_{i-1})
}{
(\Delta_i + \Delta_{i-1})
}
-
\frac{
{\Delta_{i-1}}^2 (\boldsymbol{x}_{i+1} - \boldsymbol{x}_i)
}{
\Delta_i (\Delta_i + \Delta_{i-1})
}
\end{align*}

## Animation

To illustrate how two quadratic Lagrange interpolations
followed by linear blending
might look like,
we can generate an animation
by means of the file [catmull_rom.py](catmull_rom.py):

```python
from catmull_rom import animation_2_1, animation_1_2
from IPython.display import HTML
```

```python
vertices = [
    (1, 0),
    (0.5, 1),
    (6, 2),
    (5, 0),
]
```

```python
times = [
    0,
    1,
    6,
    8,
]
```

```python
ani_2_1 = animation_2_1(vertices, times)
```

```python
HTML(ani_2_1.to_jshtml(default_mode='reflect'))
```

In the beginning of this notebook
we claimed that two quadratic interpolations
followed by linear blending are easier to understand.
To prove this, let's have a look at how
three linear interpolations (and *extra*polations)
followed by quadratic B-spline blending would look like:

```python
ani_1_2 = animation_1_2(vertices, times)
```

```python
HTML(ani_1_2.to_jshtml(default_mode='reflect'))
```

Would you agree that this is less straightforward?

If you would rather replace the quadratic B-spline basis function
with a bunch of linear interpolations (using De Boor's algorithm),
take a look at
[the notebook about the Barry--Goldman algorithm](catmull-rom-barry-goldman.ipynb#Animation).