# Observables

In the [tutorial](../tutorials/tutorial_params_options_simulate.ipynb) on `params`, `options`, and simulation, we simulated a population of identical individuals: The difference in their behavior was solely due to different random shocks to the reward associated with a choice. In more realistic models, individuals can differ with respect to multiple characteristics, which need to be sampled at the start of the simulation. These characteristics can be:

- **Experience**. Individuals can start with nonzero years of experience for some choice.
- **Lagged choices**. The previous (lagged) choice in the first period can be a subset of all choices in the model.
- **Observables**. An observed characteristic, which does not change over the time-horizon of the model, is not evenly distributed in the population.

Taken together, the assumptions on these characteristics are called the **initial conditions** of a model. An initial condition is also called a seed value and determines the value of a variable in the first period of a dynamic system.

***
In this tutorial we will learn how to enrich our baseline Robinson Crusoe economy with observables: The simulated Robinsons will differ with respect to the conditions they experience on the island, which will enter directly the reward for a choice and therefore potentially determine different conditional choice probabilities.

Similarly, in more realistic models, observables such as demographic characteristics or measures of ability need to be controlled for, as they may influence the agents' behavior.

```python
%matplotlib inline

import pandas as pd
import respy as rp
import matplotlib.pyplot as plt
import seaborn as sns
from statsmodels.graphics.mosaicplot import mosaic

# Plot style
sns.set_style("white")
sns.set_context("notebook", font_scale=1.5)
```

## The model: a simple Robinson Crusoe economy, revisited

We revisit the basic Robinson Crusoe economy. We add one observable characteristic to the baseline model, `"Fishing_Grounds"`: Now Robinson can end up, with a certain probability, on the side of the island which has `"poor"` or `"rich"` fishing grounds. Experiencing rich fishing grounds affects the **non-pecuniary reward** for fishing:

$$\begin{align}
N^f = \alpha^f + \zeta^f \unicode{x1D7D9}_{\{FG = "rich"\}} \\
\end{align}$$

The indicator function $\unicode{x1D7D9}_{\{condition\}}$ takes value 1 when the condition is true and value 0 otherwise: Therefore, if Robinson finds himself in rich fishing grounds, his total non-pecuniary rewards from fishing will be equal to $\alpha^f + \zeta^f$.

    Tutorials

    Find out more about the basic Robinson Crusoe economy in params, options, and simulation.

## Specification: ``params`` and ``options``

To introduce observables we need to modify both `params` and `options`. The observable needs to be identified by the keyword `observable_*_*`, while we can use labels to identify its levels (in this case, `"rich"` and `"poor"`). Everything after the last underscore is considered to be the level's label.

First, we load the specifications of the basic model:

```python
params, options = rp.get_example_model("robinson_crusoe_basic", with_data=False)
```

Then, we add three additional rows to `params`, to specify:

- The probability with which Robinson will find himself in rich and in poor fishing grounds;
- The value of $\zeta^f$, which here is set to be positive and constant.

**respy** allows for complex probability distributions of observables, which may for instance depend on other covariates.
However, throughout this tutorial, we will assume that the observables' probability distributions do not depend on any other information, and we will add them to the model via **probability mass function**: Each Robinson is randomly assigned to a certain side of the island, according to the float specified under `value` in the name-level `probability`.

Note that all probabilities sum to one. If that is not the case, **respy** will emit a warning and normalize probabilities.

```python
params.loc[("observable_fishing_grounds_rich", "probability"), "value"] = 0.5
params.loc[("observable_fishing_grounds_poor", "probability"), "value"] = 0.5
params.loc[("nonpec_fishing", "rich_fishing_grounds"), "value"] = 0.3
```

```python
params
```

      value

      category
      name

      delta
      delta
      0.95

      wage_fishing
      exp_fishing
      0.30

      nonpec_fishing
      constant
      -0.20

      nonpec_hammock
      constant
      2.00

      shocks_sdcorr
      sd_fishing
      0.50

      sd_hammock
      0.50

      corr_hammock_fishing
      0.00

      observable_fishing_grounds_rich
      probability
      0.50

      observable_fishing_grounds_poor
      probability
      0.50

      nonpec_fishing
      rich_fishing_grounds
      0.30

We also need to overwrite the covariates section of `options` to include which level of the observable is associated with a higher nonpecuniary reward for fishing:

```python
options["covariates"] = {
    "constant": "1",
    "rich_fishing_grounds": "fishing_grounds == 'rich'",
}
```

    To how-to guide

    Find out how to specify more complex distributions of
    observables in the how-to guide on Initial
    conditions.

## Simulation

We will now sample and simulate 1000 Robinsons, which will differ with respect to their `"Fishing_Grounds"` value. We will then let the decision rule from the solution of the model guide them for 5 periods, during which their `"Fishing_Grounds"` value assigned at the start of the simulation cannot change.

```python
simulate = rp.get_simulate_func(params, options)
df = simulate(params)
```

Note that the new characteristic is displayed in a column of the resulting dataset:

```python
df.head(20)
```

      Experience_Fishing
      Fishing_Grounds
      Shock_Reward_Fishing
      Meas_Error_Wage_Fishing
      Shock_Reward_Hammock
      Meas_Error_Wage_Hammock
      Choice
      Wage
      Discount_Rate
      Present_Bias
      Nonpecuniary_Reward_Fishing
      Wage_Fishing
      Flow_Utility_Fishing
      Value_Function_Fishing
      Continuation_Value_Fishing
      Nonpecuniary_Reward_Hammock
      Wage_Hammock
      Flow_Utility_Hammock
      Value_Function_Hammock
      Continuation_Value_Hammock

      Identifier
      Period

      0
      0
      0
      rich
      1.431303
      1
      0.515252
      1
      fishing
      1.431303
      0.95
      1
      0.1
      1.431303
      1.531303
      10.784925
      9.740654
      2
      NaN
      2.515252
      10.132237
      8.017878

      1
      1
      rich
      0.383519
      1
      0.529793
      1
      hammock
      NaN
      0.95
      1
      0.1
      0.517697
      0.617697
      8.723622
      8.532553
      2
      NaN
      2.529793
      8.988758
      6.798911

      2
      1
      rich
      0.950278
      1
      -0.189833
      1
      fishing
      1.282740
      0.95
      1
      0.1
      1.282740
      1.382740
      6.354070
      5.232979
      2
      NaN
      1.810167
      6.025253
      4.436933

      3
      2
      rich
      0.582585
      1
      -0.585088
      1
      fishing
      1.061539
      0.95
      1
      0.1
      1.061539
      1.161539
      4.093131
      3.085887
      2
      NaN
      1.414912
      3.822200
      2.533988

      4
      3
      rich
      1.680125
      1
      -0.108781
      1
      fishing
      4.132441
      0.95
      1
      0.1
      4.132441
      4.232441
      4.232441
      0.000000
      2
      NaN
      1.891219
      1.891219
      0.000000

      1
      0
      0
      rich
      1.419559
      1
      1.121115
      1
      fishing
      1.419559
      0.95
      1
      0.1
      1.419559
      1.519559
      10.773181
      9.740654
      2
      NaN
      3.121115
      10.738100
      8.017878

      1
      1
      rich
      2.408754
      1
      0.133023
      1
      fishing
      3.251478
      0.95
      1
      0.1
      3.251478
      3.351478
      11.457404
      8.532553
      2
      NaN
      2.133023
      8.591988
      6.798911

      2
      2
      rich
      0.655700
      1
      0.650588
      1
      fishing
      1.194763
      0.95
      1
      0.1
      1.194763
      1.294763
      7.633632
      6.672494
      2
      NaN
      2.650588
      7.621918
      5.232979

      3
      3
      rich
      0.464923
      1
      -0.308845
      1
      fishing
      1.143526
      0.95
      1
      0.1
      1.143526
      1.243526
      5.014991
      3.969963
      2
      NaN
      1.691155
      4.622748
      3.085887

      4
      4
      rich
      2.757647
      1
      -0.133189
      1
      fishing
      9.155711
      0.95
      1
      0.1
      9.155711
      9.255711
      9.255711
      0.000000
      2
      NaN
      1.866811
      1.866811
      0.000000

      2
      0
      0
      rich
      1.116904
      1
      -1.094805
      1
      fishing
      1.116904
      0.95
      1
      0.1
      1.116904
      1.216904
      10.470526
      9.740654
      2
      NaN
      0.905195
      8.522180
      8.017878

      1
      1
      rich
      0.896039
      1
      0.452955
      1
      fishing
      1.209527
      0.95
      1
      0.1
      1.209527
      1.309527
      9.415452
      8.532553
      2
      NaN
      2.452955
      8.911920
      6.798911

      2
      2
      rich
      0.461766
      1
      0.762777
      1
      hammock
      NaN
      0.95
      1
      0.1
      0.841392
      0.941392
      7.280262
      6.672494
      2
      NaN
      2.762777
      7.734107
      5.232979

      3
      2
      rich
      1.350840
      1
      0.571080
      1
      fishing
      2.461392
      0.95
      1
      0.1
      2.461392
      2.561392
      5.492984
      3.085887
      2
      NaN
      2.571080
      4.978368
      2.533988

      4
      3
      rich
      0.776213
      1
      0.410387
      1
      hammock
      NaN
      0.95
      1
      0.1
      1.909176
      2.009176
      2.009176
      0.000000
      2
      NaN
      2.410387
      2.410387
      0.000000

      3
      0
      0
      poor
      1.106631
      1
      -0.060911
      1
      fishing
      1.106631
      0.95
      1
      -0.2
      1.106631
      0.906631
      9.296601
      8.831548
      2
      NaN
      1.939089
      9.258196
      7.704322

      1
      1
      poor
      0.383690
      1
      -0.377365
      1
      fishing
      0.517928
      0.95
      1
      -0.2
      0.517928
      0.317928
      7.736349
      7.808865
      2
      NaN
      1.622635
      7.695893
      6.392903

      2
      2
      poor
      1.798205
      1
      -0.600881
      1
      fishing
      3.276543
      0.95
      1
      -0.2
      3.276543
      3.076543
      8.943197
      6.175425
      2
      NaN
      1.399119
      6.045155
      4.890564

      3
      3
      poor
      1.734778
      1
      -0.466337
      1
      fishing
      4.266866
      0.95
      1
      -0.2
      4.266866
      4.066866
      7.597816
      3.716790
      2
      NaN
      1.533663
      4.281438
      2.892395

      4
      4
      poor
      0.861123
      1
      -0.354589
      1
      fishing
      2.859029
      0.95
      1
      -0.2
      2.859029
      2.659029
      2.659029
      0.000000
      2
      NaN
      1.645411
      1.645411
      0.000000

Robinson's behavior is affected by the observable we introduced: The figure below shows that rich fishing grounds lead to higher engagement in fishing.

```python
fig, ax = plt.subplots(1, 2, figsize=(14, 5))

for i, observable in enumerate(["rich", "poor"]):
    df.query("Fishing_Grounds == @observable").groupby("Period").Choice.value_counts(
        normalize=True,
    ).unstack().plot.bar(width=0.4, stacked=True, rot=0, legend=False, ax=ax[i])
    ax[i].set_title("Fishing grounds: " + observable, pad=10)
    ax[i].xaxis.label.set_visible(False)

plt.legend(loc="lower center", bbox_to_anchor=(-0.15, -0.3), ncol=2)
plt.suptitle("Robinson's choices by period", y=1.05)

plt.show()
```

## Multiple observables

On top of `"Fishing_Grounds` we add now a second observable, `"Cicadas"`, which also has two evenly distributed levels: `"many"` or `"few"`. Ending up on a side of the island where many cicadas live affects, this time negatively, the **non-pecuniary reward** for relaxing on the hammock:

$$\begin{align}
N^h = \alpha^h + \zeta^h \unicode{x1D7D9}_{\{C = "many"\}} \\
\end{align}$$

where $\zeta^h < 0$. The intuition is simple: Robinson finds it less pleasant to spend time on his hammock when he is surrounded by many noisy cicadas.

We again modify `params` and `options` to include this new characteristic:

```python
params.loc[("observable_cicadas_few", "probability"), "value"] = 0.5
params.loc[("observable_cicadas_many", "probability"), "value"] = 0.5
params.loc[("nonpec_hammock", "many_cicadas"), "value"] = -0.15
```

```python
options["covariates"] = {
    "constant": "1",
    "rich_fishing_grounds": "fishing_grounds == 'rich'",
    "many_cicadas": "cicadas == 'many'",
}
```

When inspecting a simulated dataset, we can see that the observable `"Cicadas"` has now its column:

```python
simulate = rp.get_simulate_func(params, options)
df_eq = simulate(params)
```

```python
df_eq.head()
```

      Experience_Fishing
      Cicadas
      Fishing_Grounds
      Shock_Reward_Fishing
      Meas_Error_Wage_Fishing
      Shock_Reward_Hammock
      Meas_Error_Wage_Hammock
      Choice
      Wage
      Discount_Rate
      ...
      Nonpecuniary_Reward_Fishing
      Wage_Fishing
      Flow_Utility_Fishing
      Value_Function_Fishing
      Continuation_Value_Fishing
      Nonpecuniary_Reward_Hammock
      Wage_Hammock
      Flow_Utility_Hammock
      Value_Function_Hammock
      Continuation_Value_Hammock

      Identifier
      Period

      0
      0
      0
      many
      poor
      1.431303
      1
      0.515252
      1
      fishing
      1.431303
      0.95
      ...
      -0.2
      1.431303
      1.231303
      9.504631
      8.708766
      1.85
      NaN
      2.365252
      9.274431
      7.272819

      1
      1
      many
      poor
      0.383519
      1
      0.529793
      1
      hammock
      NaN
      0.95
      ...
      -0.2
      0.517697
      0.317697
      7.664871
      7.733868
      1.85
      NaN
      2.379793
      8.217822
      6.145294

      2
      1
      many
      poor
      0.950278
      1
      -0.189833
      1
      fishing
      1.282740
      0.95
      ...
      -0.2
      1.282740
      1.082740
      5.605257
      4.760544
      1.85
      NaN
      1.660167
      5.500165
      4.042104

      3
      2
      many
      poor
      0.582585
      1
      -0.585088
      1
      fishing
      1.061539
      0.95
      ...
      -0.2
      1.061539
      0.861539
      3.555348
      2.835589
      1.85
      NaN
      1.264912
      3.464384
      2.315234

      4
      3
      many
      poor
      1.680125
      1
      -0.108781
      1
      fishing
      4.132441
      0.95
      ...
      -0.2
      4.132441
      3.932441
      3.932441
      0.000000
      1.85
      NaN
      1.741219
      1.741219
      0.000000

5 rows × 21 columns

Note that `Cicadas` and `Fishing_Grounds` are independent, as we did not specify any additional constraint on their probability distribution.

We can decrease Robinson's probability of experiencing many cicadas to show how the observables' distribution changes.

```python
params.loc[("observable_cicadas_many", "probability"), "value"] = 0.35
params.loc[("observable_cicadas_few", "probability"), "value"] = 0.65
```

```python
simulate = rp.get_simulate_func(params, options)
df_diff = simulate(params)
```

```python
fig, ax = plt.subplots(1, 2, figsize=(14, 5))

colors = ["#ff7f0e", "#70a8d0", "#ffb369", "#428dc1"]
observables = [
    ("poor", "few"),
    ("poor", "many"),
    ("rich", "few"),
    ("rich", "many"),
]
titles = ["Evenly distributed observables", "Many cicadas less likely"]

for i, df in enumerate([df_eq, df_diff]):

    crosstab = pd.crosstab(df["Fishing_Grounds"], df["Cicadas"], normalize="all")

    properties_dict = {}
    for observable, color in zip(observables, colors):
        properties = {observable: [color, "{:.1%}".format(crosstab.loc[observable])]}
        properties_dict.update(properties)

    mosaic(
        df,
        ["Fishing_Grounds", "Cicadas"],
        ax=ax[i],
        properties=lambda key: {"color": properties_dict[key][0],},
        labelizer=lambda key: properties_dict[key][1],
        gap=0.01,
    )

    ax[i].set_title(titles[i], pad=10)

ax[0].set_xlabel("Fishing Grounds", x=1.08)
ax[0].set_ylabel("Cicadas")

plt.suptitle("Distribution of observables", y=1.05)
plt.show()
```

Moreover, we can investigate how the within-sample behavior of Robinson changes according to the fishing grounds and the number of cicadas that he experiences:

```python
fig, ax = plt.subplots(2, 2, figsize=(14, 10))

ax = ax.flatten()

plt.subplots_adjust(hspace=0.25)

for i, observable in enumerate(observables):
    (
        df_eq.query("Fishing_Grounds == @observable[0] and Cicadas == @observable[1]")
        .groupby("Period")
        .Choice.value_counts(normalize=True)
        .unstack()
        .plot.bar(width=0.4, stacked=True, rot=0, ax=ax[i], legend=False)
    )
    ax[i].xaxis.label.set_visible(False)
    ax[i].set_title(
        observable[0] + " fishing grounds, " + observable[1] + " cicadas", pad=10
    )

plt.legend(loc="right", bbox_to_anchor=(0.3, -0.2), ncol=2)
plt.suptitle("Robinson's choices by period")

plt.show()
```

The figure shows that different realizations of observables lead to different incentives for Robinson: His engagement in fishing decreases with poor fishing grounds or few cicadas, while it increases with rich fishing grounds and many cicadas.


# An overview of rigid body dynamics

$$
% vector
\newcommand{\v}[1]{\mathbf{\vec{#1}}}
% unit vector
\newcommand{\u}[1]{\mathbf{\hat{#1}}}
% dot product
\newcommand{\dp}[2]{#1 \cdot #2}
% cross product
\newcommand{\cp}[2]{#1 \times #2}
% rotation matrix
\newcommand{\R}[2]{{}^{#1} R ^{#2}}
% vector derivative
\newcommand{\d}[2]{\frac{{}^#2d#1}{dt}}
% angular velocity
\newcommand{\av}[2]{{}^{#2} \v{\omega} {}^{#1}}
% angular acceleration
\newcommand{\aa}[2]{{}^{#2} \v{\alpha} {}^{#1}}
% position
\newcommand{\pos}[2]{\v{r} {}^{#2/#1}}
% velocity
\newcommand{\vel}[2]{{}^#2 \v{v} {}^{#1}}
% acceleration
\newcommand{\acc}[2]{{}^#2 \v{a} {}^{#1}}
$$

Rigid body dynamics is concerned with describing the motion of systems composed of solid bodies; such as vehicles, skeletons, robots [1-4]:

This document borrows heavily from [5, 6].

# Newton's Second Law

For all these systems, our goal is to determine how each body's position changes in time. Newton told us that the acceleration $\v{a}$ of a system is proportional to the force $\v{F}$ applied to it:

$$\v{F} = m\v{a}$$

Newton gave us the bodies' acceleration, but we would really prefer to obtain their position. Thus, Newton gave us a second order ordinary differential equation for the quantity we seek, i.e. $\v{a}=\frac{d^2}{dt^2}\v{x}$. This equation is actually far too simple for the systems we want to study, but it reveals that there are three topics to consider in describing a rigid body system: its kinematics ($\v{a}$), its mass properties ($m$), and the external forces applied to the system ($\v{F}$). In this notebook, we present the tools necessary to mathematically describe a rigid body system. Once equipped with a mathematical description of a system, we can generate equations that describe its motion. Regardless, we always end up with second-order ordinary differential equations in time.

# Vectors

Newton's second law is a vector equation. A vector is a quantity that has a **magnitude** and a **direction**. For example, "5 miles East" is a vector quantity with magnitude 5 miles and direction East. We draw them as arrows:

We represent the magnitude of a vector $\v{v}$ as $|\v{v}|$. We represent the direction of a vector $\v{v}$ using a unit vector $\u{u}$ (magnitude of 1) that has the same direction as $\v{v}$:

$$\u{u} = \frac{\v{v}}{|\v{v}|}$$

We will work with the following vector quantities: positions, velocities, accelerations, forces, angular velocities, and torques/moments. Don't think about these vectors as linear algebra vectors. Our vectors always have a physical interpretation (and thus are always 2 or 3 dimensinonal), while linear algebra vectors are often more abstract.

## Addition

When we add vector $\v{b}$ to vector $\v{a}$, the result is a vector that starts at the tail of $\v{a}$ and ends at the tip of $\v{b}$:

```python
from __future__ import print_function, division
from sympy import init_printing
init_printing(use_latex='mathjax', pretty_print=False)
```

Physics vectors in SymPy are created by first specifying a reference frame and using the associated unit vectors to construct vectors of arbritrary magnitude and direction. Reference frames will be discussed later on and for now it is only important that `N.x`, `N.y`, and `N.z` are three mutally orthoganl vectors of unit length.

```python
from sympy.physics.vector import ReferenceFrame
N = ReferenceFrame('N')
```

Simple scalar variables can be import from SymPy with:

```python
from sympy.abc import c, d, e, f, g, h
```

Finally, the unit vectors and scalars can be combined to create vectors.

```python
a = c * N.x + d * N.y + e * N.z
a
```

```python
a.to_matrix(N)
```

```python
b = f * N.x + g * N.y + h * N.z
b
```

The magnitude of a vector can be found with::

```python
a.magnitude()
```

```python
a + b
```

## Scaling

Multiplying a vector by a scalar changes its magnitude, but not its direction. Scaling by a negative number changes a vector's magnitude and reverses its sense (rotates it by 180 degrees).

```python
b = 2 * a
b
```

```python
c = -a
c
```

### Exercise

Create three vectors that lie in the $XY$ plane where each vector is:

1. of length $l$ that is at an angle of $\frac{\pi}{4}$ degrees from the $X$ axis
2. of length $10$ and is in the $-Y$ direction
3. of length $l$ and is $\theta$ radians from the $Y$ axis

Finally, add vectors 1 and 2 and substract $5$ times the third vector.

*Hint: SymPy has variables and trigonometic functions, for example `from sympy import tan, pi`*

```python

```

```python
%load exercise_solutions/n01_vector_addition_scaling.py
```

## Dot product (scalar product)

The dot product, which yields a scalar quantity, is defined as:

$$\v{a} \cdot \v{b} = |\v{a}||\v{b}| \cos{\theta}$$

where $\theta$ is the angle between the two vectors. It is used to determine:

* the angle between two vectors: $\theta = \cos^{-1}[\v{a} \cdot \v{b} / (|\v{a}||\v{b}|)]$
* a vector's magnitude: $ |\v{v}| = \sqrt{\v{v} \cdot \v{v}} $
* the length of a vector along a direction/unit vector $\u{u}$ (called the projection): $ \mbox{proj}_{\u{u}}\v{v} = \v{v} \cdot \u{u}$
* if two vectors are perpendicular: $ \v{a} \cdot \v{b} = 0 \mbox{ if } \v{a} \perp \v{b} $
* compute power: $ P = \dp{\v{F}}{\v{v}}$

Also, dot products are used to convert a vector equation into a scalar equation (by "dotting" an entire equation with a vector).

```python
from sympy.abc import c, d, e, f, g, h
from sympy.physics.vector import ReferenceFrame, dot
```

```python
N = ReferenceFrame('N')
a = c * N.x + d * N.y + e * N.z
b = f * N.x + g * N.y + h * N.z
```

```python
dot(a, b)
```

### Exercise

Given the vectors $\v{v}_1 = a \hat{\mathbf{n}}_x + b\hat{\mathbf{n}}_y + a \hat{\mathbf{n}}_z$ and $\v{v}_2=b \hat{\mathbf{n}}_x + a\hat{\mathbf{n}}_y + b \hat{\mathbf{n}}_z$ find the angle between the two vectors using the dot product.

```python

```

```python
%load exercise_solutions/n01_vector_dot_product.py
```

## Cross product (vector product)

The cross product, which yields a vector quantity, is defined as:

$$ \cp{\v{a}}{\v{b}} = |\v{a}||\v{b}| \sin\theta \u{u}$$

where $\theta$ is the angle between the two vectors, and $\u{u}$ is the unit vector perpendicular to both $\v{a}$ and $\v{b}$ whose sense is given by the right-hand rule. It is used to:

* obtain a vector/direction perpendicular to two other vectors
* determine if two vectors are parallel: $\cp{\v{a}}{\v{b}} = \v{0} \mbox{ if } \v{a} \parallel \v{b}$
* compute moments: $ \cp{\v{r}}{\v{F}}$
* compute the area of a triangle

```python
from sympy.abc import c, d, e, f, g, h
from sympy.physics.vector import ReferenceFrame, cross
```

```python
N = ReferenceFrame('N')
a = c * N.x + d * N.y + e * N.z
b = f * N.x + g * N.y + h * N.z
```

```python
cross(a, b)
```

### Exercise

Given three points located in reference frame $N$ by:

$$
\v{p}_1 = 23 \u{n}_x - 12 \u{n}_y \\
\v{p}_2 = 16 \u{n}_x + 2 \u{n}_y - 4 \u{n}_z \\
\v{p}_3 = \u{n}_x + 14 \u{n}_z
$$

Find the area of the triangle bounded by these three points using the cross product.

*Hint: Search online for the relationship of the cross product to triangle area.*

```python

```

```python
%load exercise_solutions/n01_vector_cross_product.py
```

## Some vector properties

* The order in which you add them does not matter: $\v{a} + \v{b} = \v{b} + \v{a}$
* You can distrubute a scalar among vectors: $ s (\v{a} + \v{b}) = s\v{a} + s\v{b} $

**Dot product**

* You can pull out scalars: $ c \v{a} \cdot d \v{b} = cd (\v{a} \cdot \v{b})$
* Order does not matter: $\dp{\v{a}}{\v{b}} = \dp{\v{b}}{\v{a}}$
* You can distribute: $\dp{\v{a}}{(\v{b} + \v{c})} = \dp{\v{a}}{\v{b}} + \dp{\v{a}}{\v{c}}$

**Cross product**

* Crossing a vector with itself "cancels" it: $\cp{\v{a}}{\v{b}} = \vec{0}$
* You can pull out scalars: $ c \v{a} \times d \v{b} = cd (\v{a} \times \v{b})$
* Order DOES matter (because of the right-hand rule): $\cp{\v{a}}{\v{b}} = -\cp{\v{b}}{\v{a}}$
* You can distribute: $\cp{\v{a}}{(\v{b} + \v{c})} = \cp{\v{a}}{\v{b}} + \cp{\v{a}}{\v{c}}$
* They are NOT associative: $\cp{\v{a}}{(\cp{\v{b}}{\v{c}})} \neq \cp{(\cp{\v{a}}{\v{b}})}{\v{c}}$

# Reference frames

A reference frame (or simply, frame) is a rigid 3D object. We always attach a reference frame to rigid bodies in order to describe their motion. We may also use "empty" reference frames to make a system easier to model.

A reference frame has some *location* in space, but it does *not* have a position. Reference frames contain points, and those *points* have positions.

A reference frame also has an *orientation* in space. To specify its orientation, we choose a  vector basis whose orientation is fixed with respect to the reference frame (but there are infinitely many vector bases we *could* label on the frame). In general, we are only interested in the vector bases we attach to reference frames; from here on, we will instead refer to reference frames in the places where we referred vector bases. That is, we express vectors in a reference frame instead of in a vector basis.

A reference frame's location and orientation vary in time. Two important attributes of a reference frame are its **angular velocity** $\v{\omega}$ and its **angular acceleration** $\v{\alpha}$; we'll describe these shortly.

A **Newtonian reference frame** is one in which Newton's second law holds.

## Expressing vectors with a vector basis

We have shown you what a vector $\v{v}$ looks like, but have yet to express an actual vector mathematically. To do so, we first choose three unit vectors $\u{a}_x$, $\u{a}_y$, and $\u{a}_z$ whose directions we accept as given. Consider the human jumper from above; we choose:

* $\u{a}_x$ to point forward,
* $\u{a}_y$ to point upwards,
* $\u{a}_z$ to point out of the plane (to the subject's right).

These three unit vectors are mutually perpendicular. For pratical reasons, we will always make sure that's the case. If so, the three vectors define a vector basis. We can express the position of the subject's hand from its toes in terms of these three vectors:

$$ \v{r} = d_x \u{a}_x + d_y \u{a}_y + 0 \u{a}_z$$

We call $d_x$ the **measure** of $\v{r}$ along $\u{a}_x$, and it is equal to $\v{r} \cdot \u{a}_x$. Note that a vector basis does not have an origin.

We could have chosen a different vector basis, such as $\u{b}_x$, $\u{b}_y$, $\u{b}_z$. Then, we would express $\v{r}$ as:

$$ \v{r} = f_x \u{b}_x + f_y \u{b}_y + 0 \u{b}_z$$

Using this alternative vector basis does not change the fact that $\v{r}$ is the position of the hand from the toes; it simply changes how we *express* this quantity. It is possible to express a single vector in infinitely many ways, since we can choose to use any valid vector basis. In the next section, we will learn how to relate different vector bases to each other.

#### Operating on vectors expressed in a basis
Once we express a vector in a vector basis, it is easy to perform operations on it with vectors expressed in the same basis. Consider the two vectors:

* $\v{a} = a_x \u{n}_x + a_y \u{n}_y + a_z \u{n}_z$
* $\v{b} = b_x \u{n}_x + b_y \u{n}_y + b_z \u{n}_z$

Here are the addition, dot, and cross operations between these two vectors:

$$
\v{a} + \v{b} = (a_x + b_x) \u{n}_x + (a_y + b_y) \u{n}_y + (a_z + b_z) \u{n}_z \\
\dp{\v{a}}{\v{b}} = a_x b_x + a_y b_y + a_z b_z\\
\cp{\v{a}}{\v{b}} = \det{
    \begin{bmatrix} \u{n}_x & \u{n}_y & \u{n}_z \\
                    a_x & a_y & a_z \\
                    b_x & b_y & b_z
    \end{bmatrix}}
$$

#### We must specify a vector basis
When a vector is expressed in typical linear algebra notation, information is lost. For example, we don't know the basis in which the following vector is expressed:

$$
\v{v} = \begin{bmatrix} v_x \\ v_y \\ v_z \end{bmatrix}
$$

If we don't know the basis in which $v_x$, $v_y$, and $v_z$ are its measures, we cannot add $\v{v}$ to another vector, etc. To express a vector in matrix form, we must carry along the basis in which it is expressed. One option for doing so is the following:

$$
[\v{v}]_{n} = \begin{bmatrix} v_x \\ v_y \\ v_z \end{bmatrix}_{n} $$

The notation $[\v{v}]_{n}$ specifies that $\v{v}$ is expressed in the vector basis $\u{n}_x$, $\u{n}_y$, $\u{n}_z$.

```python
from sympy.abc import c, d, e, f, g, h, theta
from sympy.physics.vector import ReferenceFrame, dot, cross
```

```python
A = ReferenceFrame('A')
```

```python
B = A.orientnew('B', 'Axis', (theta, A.z))
```

```python
a = c * A.x + d * A.y + e * A.z
b = f * B.x + g * B.y + h * B.z
```

```python
a + b
```

```python
dot(a, b)
```

```python
cross(a, b)
```

```python
(a+b).express(A)
```

# Rotation matrices (direction cosine matrices)

In almost every problem, we make use of multiple vector bases. The reason is that there is usually a particular basis in which a vector is most conveniently expressed. And, that convenient basis is usually not the same for all vectors we'll deal with. A side effect is that we will often want to change the basis in which a vector is expressed. To do so, we use a rotation matrix (also called a direction cosine matrix). The rotation matrix ${}^a R^b$ allows us to take a vector $\v{v}$ expressed in $\u{b}_x$, $\u{b}_y$, $\u{b}_z$ and re-express it in $\u{a}_x$, $\u{a}_y$, $\u{a}_z$:

$$
[\v{v}]_{a} = {}^a R^b ~ [\v{v}]_{b}
$$

The rotation matrix is given by dot products across the two the vector bases:

$$
\R{a}{b} =
    \begin{bmatrix}
        \dp{\u{a}_x}{\u{b}_x} & \dp{\u{a}_x}{\u{b}_y} & \dp{\u{a}_x}{\u{b}_z} \\
        \dp{\u{a}_y}{\u{b}_x} & \dp{\u{a}_y}{\u{b}_y} & \dp{\u{a}_y}{\u{b}_z} \\
        \dp{\u{a}_z}{\u{b}_x} & \dp{\u{a}_z}{\u{b}_y} & \dp{\u{a}_z}{\u{b}_z} \\
    \end{bmatrix}
$$

Because of the nature of vector bases, this matrix is symmetric and orthogonal. If we instead have a vector in basis $a$ and want to express it in $b$, we can simply use the inverse of $\R{a}{b}$. Since the matrix is orthogonal, its inverse is the same as its transpose.

$$
\R{b}{a} = (\R{a}{b})^{-1} = (\R{a}{b})^T \\
[\v{v}]_{b} = {}^b R^a ~ [\v{v}]_{a} \\
[\v{v}]_{b} = ({}^a R^b)^T ~ [\v{v}]_{a}
$$

The columns of $\R{a}{b}$ are the unit vectors $\u{b}_x$, $\u{b}_y$, $\u{b}_z$ expressed in $a$:

$$
\R{a}{b} = \begin{bmatrix} [\u{b}_x]_a & [\u{b}_y]_a & [\u{b}_z]_a \end{bmatrix}
$$

#### Successive rotations

We'll usually need to re-express a vector multiple times. Luckily, we can do so by multiplying rotation matrices together:

$$
\R{d}{a} = (\R{d}{c} )(\R{c}{b}) (\R{b}{a}) \\
[\v{v}]_{d} = \R{d}{a} [\v{v}]_{a} \\
[\v{v}]_{d} = (\R{d}{c} )(\R{c}{b}) (\R{b}{a})[\v{v}]_{a}
$$

#### A point of confusion: rotating vs. re-expressing

Sometimes, rotation matrices are used to rotate vectors; that is, cause the vector to point somewhere different. That is NOT how we are using rotation matrices here. Rotating a vector  changes the vector itself, while we are only changing how the *same* vector is expressed.

```python
B.dcm(A)
```

### Exercise

Create two reference frames, the first should be attached to your laptop keyboard surface. For the first frame, the $Z$ axis should be directed from the Q key to the P key. The $Y$ unit vector should be directed from the shift key to the tab key. Now on the screen, attach a reference frame where the $Z$ axis is directed from the right side of the screen to the left and lies in the plane of the screen. The $Y$ axis should be directed from the top of the screen to the hinge.

The angle between the laptop and screen is $\theta$ such that $\theta=0$ corresponds to the laptop being closed and $0 < \theta < \pi$ is the laptop being open. With this create a vector that starts at the bottom left hand corner of the wrist rests and ends at the top right corner of the screen. Use $w$ for the width and $l$ for the length of the laptop.

Print the vector expressed in the keyboard frame.

*Hint: You may need to create more than two frames and a simple sketch will help.*

```python

```

```python
%load exercise_solutions/n01_vector_rotation.py
```

# Derivatives of vectors

Consider the vector $\u{a}_x$ in the figure above. To an observer sitting on $A$, $\u{a}_x$ never changes; it is fixed rigidly to $A$. Therefore, the observer would say the time derivative of $\u{a}_x$ is $\v{0}$. However, an observer on $N$ would indeed observe that $\u{a}_x$ changes in time. For this reason, when we take the time derivative of a vector, we must specify the frame in which we take the derivative. The derivative of a generic vector $\v{p}$ in frame $N$ is denoted as:

$$\d{\v{p}}{N}$$

Consider a vector $\v{p}$ expressed in $A$:

$$\v{p} = p_x \u{a}_x + p_y \u{a}_y + p_z \u{a}_z$$

Its time derivative in frame $A$ is:

$$\d{\v{p}}{A} = \dot{p}_x \u{a}_x + \dot{p}_y \u{a}_y + \dot{p}_z \u{a}_z$$

Here, we have benefitted from the fact that $\u{a}_x$, $\u{a}_y$, and $\u{a}_z$ are constant in $A$. We are not so fortunate when taking the derivative in $N$, since these basis vectors are not constant in $N$:

$$\d{\v{p}}{N} = \dot{p}_x \u{a}_x + p_x \d{\u{a}_x}{N} + \dot{p}_y \u{a}_y + p_y \d{\u{a}_y}{A} + \dot{p}_z \u{a}_z + p_z \d{\u{a}_z}{N}$$

This formula for the derivative in $N$ of a vector expressed in $A$ is not so great to use.  Once we introduce angular velocity, we will have a much better way to compute such quantities.

```python
a
```

```python
a.diff(c, A)
```

# Angular velocity and angular acceleration

A reference frame's angular velocity describes the rate of change of the frame's orientation. Consider frame $A$. Since angular velocity is a vector quantity, we must specify the frame from which we observe the change in $A$'s orientation.

 $\av{A}{N}$: the angular velocity of frame $A$ as observed from frame $N$

There are some complicated formulas for $\av{A}{N}$, but you usually don't need them. Typically, you know $\av{A}{N}$ by inspection. Take the linkage below:

In this linkage, the only way that frame/body $B$ can move with respect to $A$ is by rotating about $B_o$ by the angle $q_1$. Thus, by inspection:

$$\av{B}{A} = \dot{q}_1 \u{b}_z$$

$C$ is attached to $B$ similarly:

$$\av{C}{B} = \dot{q}_2 \u{c}_z$$

#### Angular velocity addition theorem

We can add angular velocities together, similar to how we multiplied reference frames:

$$\av{C}{A} = \av{B}{A} + \av{C}{B}$$

#### Derivative theorem

For any vector $\v{p}$, the following equation relates the derivative of $\v{p}$ in two different reference frames via the angular velocity between these two frames:

$$\d{\v{p}}{A} = \d{\v{p}}{B} + \av{B}{A} \times \v{p}$$

Again, this works for *any* vector, not just position vectors.
This theorem is really important, and is the primary way that we compute derivatives of vectors in other frames.

#### Angular acceleration

The equations of rigid body dynamics will also require angular accelerations $\aa{B}{A}$ of the rigid bodies in the system, but this can usually be computed automatically from  $\av{B}{A}$.

 $\aa{A}{N}$: the angular acceleration of frame $A$ as observed from frame $N$

```python
B.ang_vel_in(A)
```

```python
from sympy import Function
from sympy.abc import t
theta = Function('theta')(t)
theta
```

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

```python
B = A.orientnew('B', 'Axis', (theta, A.z))
B.ang_vel_in(A)
```

# Position, velocity, and acceleration

#### Position

Position vectors have the special property that two points must be specified. For example, if I want to obtain the position of point $P$ in the figure above, I must specify the point from which I want that position.

 $\pos{Q}{P}$: the position of point $Q$ with respect to point $P$.

In modeling, we often must write down various position vectors via inspection.

#### Velocity

The velocity of a point is the derivative of its position, and must have associated with it the frame in which the derivative is taken.

$\vel{Q}{N}$: the velocity of point $Q$ in frame $N$

Previously, we used the symbol $\v{v}$ to denote a generic vector. Henceforth, $\v{v}$ refers to a velocity. If $N_o$ is a point fixed in $N$, then:

$$\vel{Q}{N}=\d{\pos{Q}{N_o}}{N}$$

When using PyDy, we rarely need to use inspection to determine the velocity of points of interest. Instead, we are usually in the situation that we want the velocity (in $N$) of point $Q$ fixed on body $B$, and we already know the velocity of another point $P$ fixed on $B$. In this case, we use the following formula to obtain $\vel{Q}{N}$ (`v2pt_theory` in PyDy):

$$\vel{Q}{N} = \vel{P}{N} + \av{B}{N} \times \pos{Q}{P}$$

#### Acceleration

An acceleration of a point is the derivative of its velocity, and must have associated with it the frame in which the derivative is taken.

$\acc{Q}{N}$: the acceleration of point $Q$ in frame $N$

Henceforth, $\v{a}$ refers to an acceleration. If the velocity of $Q$ is given, then the acceleration is:

$$\acc{Q}{N}=\d{\vel{Q}{N}}{N}$$

Similarly to velocity, we rarely need to use inspection to determine the acceleration of points of interest. Instead, we are usually in the situation that we want the acceleration (in $N$) of point $Q$ fixed on body $B$, and we already know the velocity of another point $P$ fixed on $B$. In this case, we use the following formula to obtain $\acc{Q}{N}$ (`a2pt_theory` in PyDy):

$$\acc{Q}{N} = \acc{P}{N} + \aa{B}{N} \times \pos{Q}{P} + \av{B}{N} \times (\av{B}{N} \times \pos{Q}{P})$$

# Inertial properties

Each particle or rigid body has interial properties. We will assume that these properties are constant with respect to time. Each particle in a system has a scalar mass and each rigid body has a scalar mass located at it's center of mass and an inertia dyadic (or tensor) that represents how that mass is distributed in space, which is typically defined with respect to the center of mass.

Just as we do with vectors above, we will use a basis dependent expression of tensors. The inertia of a 3D rigid body is typically expressed as a tensor (symmetric 3 x 3 matrix).

$$
I = \begin{bmatrix}
      I_{xx} & I_{xy} & I_{xz} \\
      I_{xy} & I_{yy} & I_{yz} \\
      I_{xz} & I_{yz} & I_{zz}
    \end{bmatrix}_N
$$

The three terms on the diagnol are the moments of inertia and represent the resistance to angular acceleration about the respective axis in the subscript. The off diagonal terms are the products of inertia and represent the coupled resistance to angular acceleration from one axis to another. The $N$ subscript denotes that this tensor is expressed in the $N$ reference frame.

We can write this tensor as a dyadic to allow for easy combinations of inertia tensors expressed in different frames, just like we combine vectors expressed in different frames above. This basis dependent tensor takes the form:

$$I =
   I_{xx} \begin{bmatrix}
            1 & 0 & 0 \\
            0 & 0 & 0 \\
            0 & 0 & 0
          \end{bmatrix}_N +
   I_{xy} \begin{bmatrix}
            0 & 1 & 0 \\
            0 & 0 & 0 \\
            0 & 0 & 0
          \end{bmatrix}_N +
   I_{xz} \begin{bmatrix}
            0 & 0 & 1 \\
            0 & 0 & 0 \\
            0 & 0 & 0
          \end{bmatrix}_N +
   I_{yx} \begin{bmatrix}
            0 & 0 & 0 \\
            1 & 0 & 0 \\
            0 & 0 & 0
          \end{bmatrix}_N +
   I_{yy} \begin{bmatrix}
            0 & 0 & 0 \\
            0 & 1 & 0 \\
            0 & 0 & 0
          \end{bmatrix}_N +
   I_{yz} \begin{bmatrix}
            0 & 0 & 0 \\
            0 & 0 & 1 \\
            0 & 0 & 0
          \end{bmatrix}_N + \\
   I_{zx} \begin{bmatrix}
            0 & 0 & 0 \\
            0 & 0 & 0 \\
            1 & 0 & 0
          \end{bmatrix}_N +
   I_{zy} \begin{bmatrix}
            0 & 0 & 0 \\
            0 & 0 & 0 \\
            0 & 1 & 0
          \end{bmatrix}_N +
   I_{zz} \begin{bmatrix}
            0 & 0 & 0 \\
            0 & 0 & 0 \\
            0 & 0 & 1
          \end{bmatrix}_N
$$

These "unit" tensors are simply the outer product of the associated unit vectors and can be written as such:

$$ I = I_{xx} \u{n}_x \otimes \u{n}_x +
I_{xy} \u{n}_x \otimes \u{n}_y +
I_{xz} \u{n}_x \otimes \u{n}_z +
I_{yx} \u{n}_y \otimes \u{n}_x +
I_{yy} \u{n}_y \otimes \u{n}_y +
I_{yz} \u{n}_y \otimes \u{n}_z +
I_{zx} \u{n}_z \otimes \u{n}_x +
I_{zy} \u{n}_z \otimes \u{n}_y +
I_{zz} \u{n}_z \otimes \u{n}_z $$

Inertia dyadics and tensors can be created in the following way:

```python
from sympy import symbols
from sympy.physics.mechanics import ReferenceFrame, inertia
```

```python
ixx, iyy, izz, ixy, iyz, ixz = symbols('I_xx I_yy I_zz I_xy I_yz I_xz')
```

```python
N = ReferenceFrame('N')
```

```python
I = inertia(N, ixx, iyy, izz, ixy, iyz, ixz)
I
```

```python
I.to_matrix(N)
```

# Forces and moments/torques

Forces are vectors which are applied to specific points (bound vectors) and moments/torques are vectors than describe rotational load applied to a body. Both can simply be described as vectors but either a point or reference frame must be associated with each, respectively.

**Equal and Opposite**

Don't forget Newton's third law of motion. If there is a force or torque, there is always an equal and opposit force or torque acting on the opposing point or reference frame.

```python
from sympy.abc import a, b, c
from sympy.physics.vector import ReferenceFrame, Point
```

```python
A = ReferenceFrame('A')
P = Point('P')
f = a * A.x + b * A.y + b * A.z
# We will typically denote a force as tuple of a vector and a point.
force = (f, P)
f
```

# Equations of motion

Once all of the important forces acting on a system, the accelerations of all particles and bodies, and the inertial properties of the system are found, the equations of motion can be formed. For planar dyanmics the equations take on this form:

$$\sum \v{F} = m \v{a} \\ \sum \v{T} = I \v{\alpha}$$

The force equation (Newton's second law) and the torque equation (Euler's equation) make up a set of second ordinary differential equations in time. We typically want these equations in first order form:

$$\dot{x} = f(x, u, t)$$

And to do that kinematical differential equations are introduced, which simply define the relationships between the positional and angular states and their derivatives. For example, we could introduce $\omega$ as the time derivative of an angle $\theta$:

$$\omega = \dot{\theta}$$

The states $x$ are typically positions, angles, and their rates.

In general, the equations of motion are non-linear ordinary differential equations and anayltical solutions do not exist. To find the resulting state trajectories we turn to numerical integration methods.

$$x = \int_{t_0}^{t_f} f(x, u, t) dt$$

# References

[1] Moore, J. K. (2012). Human Control of a Bicycle. University of California, Davis.

[2] Ashby, B. M., & Delp, S. L. (2006). Optimal control simulations reveal mechanisms by which arm movement improves standing long jump performance. Journal of biomechanics, 39(9), 1726–34. doi:10.1016/j.jbiomech.2005.04.017

[3] Gong, X., Bai, Y., Hou, Z., Zhao, C., Tian, Y., & Sun, Q. (2012). Backstepping sliding mode tracking control of quad-rotor under input saturation. International Journal of Intelligent Computing and Cybernetics, 5(4), 515–532. doi:10.1108/17563781211282268

[4] Wu, J. Z., An, K.-N., Cutlip, R. G., Krajnak, K., Welcome, D., & Dong, R. G. (2008). Analysis of musculoskeletal loading in an index finger during tapping. Journal of biomechanics, 41(3), 668–76. doi:10.1016/j.jbiomech.2007.09.025

[5] Mitiguy, P. Advanced Dynamics & Motion Simulation. 2013.

[6] http://docs.sympy.org/latest/modules/physics/mechanics/index.html


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

# Crossover Range
***

For an escort type jammer, the crossover range may be expressed as (Equation 11.18)

\begin{equation}\label{eq:crossover_escort}
    r_c = \left[ \frac{P_t\, G_t^2 \,\sigma\, r_j^2 \,B_j}{{ERP}\, \, G_r \,4 \,\pi \,B_r \,L_r}\right]^{1/4} \hspace{0.5in} \text{(m)},
\end{equation}

and for a self-screening type jammer (Equation 11.19)

\begin{equation}\label{eq:crossover_self}
    r_c = \sqrt{\frac{P_t\, G_t^2 \,\sigma\, B_j}{{ERP}\, \, G_r \,4 \,\pi \,B_r \,L_r}} \hspace{0.5in} \text{(m)}
\end{equation}
***

Begin by getting the library path

```python
import lib_path
```

Set up the jammer effective radiated power (dBW)

```python
jammer_erp = [10, 40]
```

Import the `linspace` routine from `scipy` to create the jammer ERP array

```python
from numpy import linspace

jammer_erp_vector = linspace(float(jammer_erp[0]), float(jammer_erp[1]), 1000)
```

Set the peak power (W), antenna gain (dB), target RCS (dBsm), jammer bandwidth (Hz), jammer ERP (dBW), radar bandwidth (Hz), and losses (dB) for a self-screening type jammer

```python
peak_power = 50e3

antenna_gain = 40

target_rcs = 3.0

jammer_bandwidth = 20e6

radar_bandwidth = 15e6

losses = 9.0
```

Set up the keyword args

```python
kwargs = {'peak_power': peak_power,

          'antenna_gain': 10 ** (antenna_gain / 10.0),

          'target_rcs': 10 ** (target_rcs / 10.0),

          'jammer_bandwidth': jammer_bandwidth,

          'effective_radiated_power': 10 ** (jammer_erp_vector / 10.0),

          'radar_bandwidth': radar_bandwidth,

          'losses': 10 ** (losses / 10.0)}
```

Import the `crossover_range_selfscreen` routine from countermeasures

```python
from Libs.ecm.countermeasures import crossover_range_selfscreen
```

Calculate the crossover range for a self-screening type jammer (m)

```python
crossover_range = crossover_range_selfscreen(**kwargs)
```

Import the `matplotlib` routines for plotting the crossover range for a self-screening type jammer

```python
from matplotlib import pyplot as plt

# Set the figure size

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

# Display the results

plt.plot(jammer_erp_vector, crossover_range, '')

# Set the plot title and labels

plt.title('Crossover Range', size=14)

plt.xlabel('Jammer ERP (dBW)', size=12)

plt.ylabel('Crossover Range (m)', size=12)

# Turn on the grid

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

# Set the tick label size

plt.tick_params(labelsize=12)
```

For an escort type jammer, set the jammer range (m) and the antenna gain in the direction of the jammer (dB)

```python
jammer_range = 10e3

antenna_gain_jammer_direction = 5.0
```

Set up the keyword args

```python
kwargs = {'peak_power': peak_power,

          'antenna_gain': 10 ** (antenna_gain / 10.0),

          'target_rcs': 10 ** (target_rcs / 10.0),

          'jammer_range': jammer_range,

          'jammer_bandwidth': jammer_bandwidth,

          'effective_radiated_power': 10 ** (jammer_erp_vector / 10.0),

          'radar_bandwidth': radar_bandwidth,

          'losses': 10 ** (losses / 10.0),

          'antenna_gain_jammer_direction': 10 ** (antenna_gain_jammer_direction / 10.0)}
```

Import the `crossover_range_escort` routine from countermeasures

```python
from Libs.ecm.countermeasures import crossover_range_escort
```

Calculate the crossover range for an escort type jammer (m)

```python
crossover_range = crossover_range_escort(**kwargs)
```

```python
# Set the figure size

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

# Display the results

plt.plot(jammer_erp_vector, crossover_range, '')

# Set the plot title and labels

plt.title('Crossover Range', size=14)

plt.xlabel('Jammer ERP (dBW)', size=12)

plt.ylabel('Crossover Range (m)', size=12)

# Turn on the grid

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

# Set the tick label size

plt.tick_params(labelsize=12)
```


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

.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';
        line-height: 125%;
        font-size: 100%;
        text-align: justify;
        text-justify:inter-word;
    }
    div.text_cell code {
        background: transparent;
        color: #000000;
        font-weight: 400;
        font-size: 11pt;
        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: visible;
        max-height: 5000000px;
    }
    div.output_subarea.output_stream.output_stdout.output_text {
        overflow-x: auto;
        overflow-y: visible;
        max-height: 5000000px;
    }
    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 kf_book.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 F$ 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 discrete state transition matrix and measurement model matrix:

$$
\begin{aligned}
\mathbf F
&= {\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}
$$

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 F = {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_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}
\epsilon &= \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 matrices. We model 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 = 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 f_1}{\partial x} &
\frac{\partial f_1}{\partial y} &
\frac{\partial f_1}{\partial \theta}\\
\frac{\partial f_2}{\partial x} &
\frac{\partial f_2}{\partial y} &
\frac{\partial f_2}{\partial \theta} \\
\frac{\partial f_3}{\partial x} &
\frac{\partial f_3}{\partial y} &
\frac{\partial f_3}{\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 f_1}{\partial v} & \frac{\partial f_1}{\partial \alpha} \\
\frac{\partial f_2}{\partial v} & \frac{\partial f_2}{\partial \alpha} \\
\frac{\partial f_3}{\partial v} & \frac{\partial f_3}{\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 kf_book.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.