```python
try:
    import openmdao.api as om
    import dymos as dm
except ImportError:
    !python -m pip install openmdao[notebooks]
    !python -m pip install dymos[docs]
    import openmdao.api as om
    import dymos as dm
```

# The Van der Pol Oscillator

In dynamics, the Van Der Pol oscillator {cite}`wikivanderpol` is a non-conservative oscillator with non-linear damping. It evolves in time according to the second-order differential equation:

\begin{align}
    \frac{d^2x}{dt^2} - u (1 - x^2) \frac{dx}{dt} + x &= 0
\end{align}

where $x$ is the position coordinate (a function of the time $t$), and $u$ is a scalar parameter
indicating the nonlinearity and the strength of the damping.

To make this an optimal control problem, we want to find the smallest control that will dampen the oscillation
(drive the state variables to zero). We can express this as an objective function $J$ to minimize:

\begin{align}
    J &= \int x^2_0 + x^2_1 + u^2
\end{align}

In other words, we want to find the optimal (smallest) trajectory of the control $u$ such that the oscillation
and the oscillation's rate of change are driven to zero.

## State Variables

There are three _state_ variables are used to define the configuration of the system at any given instant in time.

-   $x_1$: The primary output of the oscillator.
-   $x_0$: The rate of change of the primary output.
-   $J$: The objective function to be minimized.

The objective function is included as a state variable so that Dymos will do the integration.

The $x_1$ and $x_0$ state variables are also inputs to the system, along with the control $u$.

## System Dynamics

The evolution of the state variables is given by the following ordinary differential equations (ODE):

\begin{align}
    \frac{dx_0}{dt} &= (1 - x^2_1) x_0 - x_1 + u \\
    \frac{dx_1}{dt} &= x_0 \\
    \frac{dJ}{dt} &= x^2_0 + x^2_1 + u^2
\end{align}

## Control Variables

This system has a single control variable:

-   $u$: The control input.

The control variable has a constraint: $-0.75 \leq u \leq 1.0$

## The initial and final conditions

The initial conditions are:

\begin{align}
    x_0 &= 1 \\
    x_1 &= 1 \\
      u &= -0.75
\end{align}

The final conditions are:

\begin{align}
    x_0 &= 0 \\
    x_1 &= 0 \\
      u &= 0
\end{align}

## Defining the ODE as an OpenMDAO System

In Dymos, the ODE is an OpenMDAO System (a Component, or a Group of components).
The following _ExplicitComponent_ computes the state rates for the Van der Pol problem.

More detail on the workings of an _ExplicitComponent_ can be found in the OpenMDAO documentation.  In summary:

-   **initialize**:  Called at setup, and used to define options for the component.  **ALL** Dymos ODE components
    should have the property `num_nodes`, which defines the number of points at which the outputs are simultaneously computed.
-   **setup**: Used to add inputs and outputs to the component, and declare which outputs (and indices of outputs)
    are dependent on each of the inputs.
-   **compute**: Used to compute the outputs, given the inputs.
-   **compute_partials**: Used to compute the derivatives of the outputs with respect to each of the inputs analytically.
    This method may be omitted if finite difference or complex-step approximations are used, though analytic is recommended.

```{Note} Things to note about the Van der Pol ODE system
- Only the _vanderpol_ode_ class below is important for defining the basic problem. The other classes are
  used to demonstrate Message Passing Interface (MPI) parallel calculation of the system. They can be ignored.
- $x_1$, $x_0$, and $u$ are inputs.
- $\dot{x_1}$, $\dot{x_0}$, and $\dot{J}$ are outputs.
- **declare_partials** is called for every output with respect to every input.
- For efficiency, partial derrivatives that are constant have values specified in the **setup** method rather than
  the **compute_partials** method. So although 9 partials are declared, only 5 are computed in **compute_partials**.
```

```python
import numpy as np
import openmdao.api as om
import time
from openmdao.utils.array_utils import evenly_distrib_idxs

class VanderpolODE(om.ExplicitComponent):
    """intentionally slow version of vanderpol_ode for effects of demonstrating distributed component calculations

    MPI can run this component in multiple processes, distributing the calculation of derivatives.
    This code has a delay in it to simulate a longer computation. It should run faster with more processes.
    """

    def __init__(self, *args, **kwargs):
        self.progress_prints = False
        super().__init__(*args, **kwargs)

    def initialize(self):
        self.options.declare('num_nodes', types=int)
        self.options.declare('distrib', types=bool, default=False)
        self.options.declare('delay', types=(float,), default=0.0)

    def setup(self):
        nn = self.options['num_nodes']
        comm = self.comm
        rank = comm.rank

        sizes, offsets = evenly_distrib_idxs(comm.size, nn)  # (#cpus, #inputs) -> (size array, offset array)
        self.start_idx = offsets[rank]
        self.io_size = sizes[rank]  # number of inputs and outputs managed by this distributed process
        self.end_idx = self.start_idx + self.io_size

        # inputs: 2 states and a control
        self.add_input('x0', val=np.ones(nn), desc='derivative of Output', units='V/s')
        self.add_input('x1', val=np.ones(nn), desc='Output', units='V')
        self.add_input('u', val=np.ones(nn), desc='control', units=None)

        # outputs: derivative of states
        # the objective function will be treated as a state for computation, so its derivative is an output
        self.add_output('x0dot', val=np.ones(self.io_size), desc='second derivative of Output',
                        units='V/s**2', distributed=self.options['distrib'])
        self.add_output('x1dot', val=np.ones(self.io_size), desc='derivative of Output',
                        units='V/s', distributed=self.options['distrib'])
        self.add_output('Jdot', val=np.ones(self.io_size), desc='derivative of objective',
                        units='1.0/s', distributed=self.options['distrib'])

        # self.declare_coloring(method='cs')
        # # partials
        r = np.arange(self.io_size, dtype=int)
        c = r + self.start_idx

        self.declare_partials(of='x0dot', wrt='x0',  rows=r, cols=c)
        self.declare_partials(of='x0dot', wrt='x1',  rows=r, cols=c)
        self.declare_partials(of='x0dot', wrt='u',   rows=r, cols=c, val=1.0)

        self.declare_partials(of='x1dot', wrt='x0',  rows=r, cols=c, val=1.0)

        self.declare_partials(of='Jdot', wrt='x0',  rows=r, cols=c)
        self.declare_partials(of='Jdot', wrt='x1',  rows=r, cols=c)
        self.declare_partials(of='Jdot', wrt='u',   rows=r, cols=c)

    def compute(self, inputs, outputs):
        # introduce slowness proportional to size of computation
        time.sleep(self.options['delay'] * self.io_size)

        # The inputs contain the entire vector, be each rank will only operate on a portion of it.
        x0 = inputs['x0'][self.start_idx:self.end_idx]
        x1 = inputs['x1'][self.start_idx:self.end_idx]
        u = inputs['u'][self.start_idx:self.end_idx]

        outputs['x0dot'] = (1.0 - x1**2) * x0 - x1 + u
        outputs['x1dot'] = x0
        outputs['Jdot'] = x0**2 + x1**2 + u**2

    def compute_partials(self, inputs, jacobian):
        time.sleep(self.options['delay'] * self.io_size)

        x0 = inputs['x0'][self.start_idx:self.end_idx]
        x1 = inputs['x1'][self.start_idx:self.end_idx]
        u = inputs['u'][self.start_idx:self.end_idx]

        jacobian['x0dot', 'x0'] = 1.0 - x1 * x1
        jacobian['x0dot', 'x1'] = -2.0 * x1 * x0 - 1.0

        jacobian['Jdot', 'x0'] = 2.0 * x0
        jacobian['Jdot', 'x1'] = 2.0 * x1
        jacobian['Jdot', 'u'] = 2.0 * u
```

## Defining the Dymos Problem

Once the ODEs are defined, they are used to create a Dymos _Problem_ object that allows solution.

```{Note} Things to note about the Van der Pol Dymos Problem definition
- The **vanderpol** function creates and returns a Dymos _Problem_ instance that can be used
  for simulation or optimization.
- The **vanderpol** function has optional arguments for specifying options for the
  type of transcription, number of segments, optimizer, etc. These can be ignored
  when first trying to understand the code.
- The _Problem_ object has a _Trajectory_ object, and the trajectory has a single _Phase_.
  Most of the problem setup is performed by calling methods on the phase (**set_time_options**,
  **add_state**, **add_boundary_constraint**, **add_objective**).
- The **add_state** and **add_control** calls include the _target_ parameter for $x_0$, $x_1$, and $u$.
  This is required so that the inputs are correctly calculated.
- Initial (linear) guesses are supplied for the states and control.
```

```python
import openmdao.api as om
import dymos as dm

def vanderpol(transcription='gauss-lobatto', num_segments=40, transcription_order=3,
              compressed=True, optimizer='SLSQP', use_pyoptsparse=False, delay=0.0, distrib=True,
              solve_segments=False):
    """Dymos problem definition for optimal control of a Van der Pol oscillator"""

    # define the OpenMDAO problem
    p = om.Problem(model=om.Group())

    if not use_pyoptsparse:
        p.driver = om.ScipyOptimizeDriver()
    else:
        p.driver = om.pyOptSparseDriver()
    p.driver.options['optimizer'] = optimizer
    if use_pyoptsparse:
        if optimizer == 'SNOPT':
            p.driver.opt_settings['iSumm'] = 6  # show detailed SNOPT output
        elif optimizer == 'IPOPT':
            p.driver.opt_settings['print_level'] = 4
    p.driver.declare_coloring()

    # define a Trajectory object and add to model
    traj = dm.Trajectory()
    p.model.add_subsystem('traj', subsys=traj)

    # define a Transcription
    if transcription == 'gauss-lobatto':
        t = dm.GaussLobatto(num_segments=num_segments,
                            order=transcription_order,
                            compressed=compressed,
                            solve_segments=solve_segments)
    elif transcription == 'radau-ps':
        t = dm.Radau(num_segments=num_segments,
                     order=transcription_order,
                     compressed=compressed,
                     solve_segments=solve_segments)

    # define a Phase as specified above and add to Phase
    phase = dm.Phase(ode_class=VanderpolODE, transcription=t,
                     ode_init_kwargs={'delay': delay, 'distrib': distrib})
    traj.add_phase(name='phase0', phase=phase)

    t_final = 15
    phase.set_time_options(fix_initial=True, fix_duration=True, duration_val=t_final, units='s')

    # set the State time options
    phase.add_state('x0', fix_initial=False, fix_final=False,
                    rate_source='x0dot',
                    units='V/s',
                    targets='x0')  # target required because x0 is an input
    phase.add_state('x1', fix_initial=False, fix_final=False,
                    rate_source='x1dot',
                    units='V',
                    targets='x1')  # target required because x1 is an input
    phase.add_state('J', fix_initial=False, fix_final=False,
                    rate_source='Jdot',
                    units=None)

    # define the control
    phase.add_control(name='u', units=None, lower=-0.75, upper=1.0, continuity=True,
                      rate_continuity=True, targets='u')  # target required because u is an input

    # add constraints
    phase.add_boundary_constraint('x0', loc='initial', equals=1.0)
    phase.add_boundary_constraint('x1', loc='initial', equals=1.0)
    phase.add_boundary_constraint('J', loc='initial', equals=0.0)

    phase.add_boundary_constraint('x0', loc='final', equals=0.0)
    phase.add_boundary_constraint('x1', loc='final', equals=0.0)

    # define objective to minimize
    phase.add_objective('J', loc='final')

    # setup the problem
    p.setup(check=True)

    p['traj.phase0.t_initial'] = 0.0
    p['traj.phase0.t_duration'] = t_final

    # add a linearly interpolated initial guess for the state and control curves
    p['traj.phase0.states:x0'] = phase.interp('x0', [1, 0])
    p['traj.phase0.states:x1'] = phase.interp('x1', [1, 0])
    p['traj.phase0.states:J'] = phase.interp('J', [0, 1])
    p['traj.phase0.controls:u'] = phase.interp('u', [-0.75, -0.75])

    return p

if __name__ == '__main__':
    # just set up the problem, test it elsewhere
    p = vanderpol(transcription='radau-ps', num_segments=30, transcription_order=3,
                  compressed=True, optimizer='SLSQP', delay=0.005, distrib=True, use_pyoptsparse=True)

    dm.run_problem(p, run_driver=True, simulate=False)
```

## Simulating the Problem (without control)

The following script creates an instance of the Dymos vanderpol problem and simulates it.

Since the problem was only simulated and not solved, the solution lines in the plots show only
the initial guesses for $x_0$, $x_1$, and $u$. The simulation lines shown in the plots are the
system response with the control variable $u$ held constant.

The first two plots shows the variables $x_0$ and $x_1$ vs time. The third plots shows $x_0$ vs. $x_1$
(which will be mostly circular in the case of undamped oscillation). The final plot is the (fixed)
control variable $u$ vs time.

```python
from dymos.examples.plotting import plot_results
import matplotlib.pyplot as plt

# Create the Dymos problem instance
p = vanderpol(transcription='gauss-lobatto', num_segments=75)

# Run the problem (simulate only)
p.run_model()

# check validity by using scipy.integrate.solve_ivp to integrate the solution
exp_out = p.model.traj.simulate()

# Display the results
plot_results([('traj.phase0.timeseries.time',
               'traj.phase0.timeseries.states:x1',
               'time (s)',
               'x1 (V)'),
             ('traj.phase0.timeseries.time',
              'traj.phase0.timeseries.states:x0',
              'time (s)',
              'x0 (V/s)'),
              ('traj.phase0.timeseries.states:x0',
               'traj.phase0.timeseries.states:x1',
               'x0 vs x1',
               'x0 vs x1'),
             ('traj.phase0.timeseries.time',
              'traj.phase0.timeseries.controls:u',
              'time (s)',
              'control u'),
              ],
             title='Van Der Pol Simulation',
             p_sol=p, p_sim=exp_out)

plt.show()
```

## Solving the Optimal Control Problem

The next example shows optimization followed by simulation.

With a successful optimization, the resulting plots show a good match between the simulated (with varying control)
and optimized results. The state variables $x_0$ and $x_1$ as well as the control variable $u$ are all driven to zero.

```python
import dymos as dm
from dymos.examples.plotting import plot_results

# Create the Dymos problem instance
p = vanderpol(transcription='gauss-lobatto', num_segments=75,
              transcription_order=3, compressed=True, optimizer='SLSQP')

# Find optimal control solution to stop oscillation
dm.run_problem(p)

# check validity by using scipy.integrate.solve_ivp to integrate the solution
exp_out = p.model.traj.simulate()

# Display the results
plot_results([('traj.phase0.timeseries.time',
               'traj.phase0.timeseries.states:x1',
               'time (s)',
               'x1 (V)'),
             ('traj.phase0.timeseries.time',
              'traj.phase0.timeseries.states:x0',
              'time (s)',
              'x0 (V/s)'),
              ('traj.phase0.timeseries.states:x0',
               'traj.phase0.timeseries.states:x1',
               'x0 vs x1',
               'x0 vs x1'),
             ('traj.phase0.timeseries.time',
              'traj.phase0.timeseries.controls:u',
              'time (s)',
              'control u'),
              ],
             title='Van Der Pol Optimization',
             p_sol=p, p_sim=exp_out)

plt.show()
```

## Solving the Optimal Control Problem with Grid Refinement

Repeating the optimization with grid refinement enabled requires changing only two lines in the code. For the sake
of grid refinement demonstration, the initial number of segments is also reduced by a factor of 5.

Optimization with grid refinement gets results similar to the example without grid refinement, but runs faster
and does not require supplying a good guess for the number segments.

```python
import dymos as dm
from dymos.examples.plotting import plot_results

# Create the Dymos problem instance
p = vanderpol(transcription='gauss-lobatto', num_segments=15,
              transcription_order=3, compressed=True, optimizer='SLSQP')

# Enable grid refinement and find optimal control solution to stop oscillation
p.model.traj.phases.phase0.set_refine_options(refine=True)
dm.run_problem(p, refine_iteration_limit=10)

# check validity by using scipy.integrate.solve_ivp to integrate the solution
exp_out = p.model.traj.simulate()

# Display the results
plot_results([('traj.phase0.timeseries.time',
               'traj.phase0.timeseries.states:x1',
               'time (s)',
               'x1 (V)'),
             ('traj.phase0.timeseries.time',
              'traj.phase0.timeseries.states:x0',
              'time (s)',
              'x0 (V/s)'),
              ('traj.phase0.timeseries.states:x0',
               'traj.phase0.timeseries.states:x1',
               'x0 vs x1',
               'x0 vs x1'),
             ('traj.phase0.timeseries.time',
              'traj.phase0.timeseries.controls:u',
              'time (s)',
              'control u'),
              ],
             title='Van Der Pol Optimization with Grid Refinement',
             p_sol=p, p_sim=exp_out)

plt.show()
```

## References

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


# PHY321: Harmonic oscillations: Time-dependent Forces and Fourier Series
**[Morten Hjorth-Jensen](http://mhjgit.github.io/info/doc/web/)**, Department of Physics and Astronomy and Facility for Rare Ion Beams (FRIB), Michigan State University, USA and Department of Physics, University of Oslo, Norway

Date: **Mar 21, 2022**

Copyright 1999-2022, [Morten Hjorth-Jensen](http://mhjgit.github.io/info/doc/web/). Released under CC Attribution-NonCommercial 4.0 license

## Aims and Overarching Motivation

Driven oscillations and resonances with physical examples.

### Monday, March 14

Summary of analytical expressions from February 28-March 4 (see slides at <https://mhjensen.github.io/Physics321/doc/pub/week9/html/week9-reveal.html>. Discussion of resonances and other examples like the mathematical pendulum and other systems.

* [Video of lecture](https://youtu.be/xV3sIH6AXiE)

* [Handwritten notes](https://github.com/mhjensen/Physics321/blob/master/doc/HandWrittenNotes/Spring2022/NotesMarch14.pdf)

**Reading suggestion**: Taylor sections 5.6-5.8.

### Wednesday

Examples of oscillations and Fourier analysis applied to harmonic oscillations.

* [Video of lecture](https://youtu.be/RDzLMV5ymxc)

* [Handwritten notes](https://github.com/mhjensen/Physics321/blob/master/doc/HandWrittenNotes/Spring2022/NotesMarch16.pdf)

* [Video on solving differential equations numerically](https://youtu.be/7nYIfV0z1VM)

* [Video on Fourier aanalysis](https://youtu.be/neXZ4fb-4Rs)

* [Handwritten notes for Fourier analysis](https://github.com/mhjensen/Physics321/blob/master/doc/HandWrittenNotes/Spring2022/NotesFourierAnalysisMarch21.pdf)

**Reading suggestion**: See lecture notes at <https://mhjensen.github.io/Physics321/doc/pub/week10/html/week10-reveal.html> and Taylor sections 5.6-5.8.

### Friday

Work on homework 6 and discussions of harmonic oscillation examples and systems.

## Numerical Studies of Driven Oscillations

Solving the problem of driven oscillations numerically gives us much
more flexibility to study different types of driving forces. We can
reuse our earlier code by simply adding a driving force. If we stay in
the $x$-direction only this can be easily done by adding a term
$F_{\mathrm{ext}}(x,t)$. Note that we have kept it rather general
here, allowing for both a spatial and a temporal dependence.

Before we dive into the code, we need to briefly remind ourselves
about the equations we started with for the case with damping, namely

$$
m\frac{d^2x}{dt^2} + b\frac{dx}{dt}+kx(t) =0,
$$

with no external force applied to the system.

Let us now for simplicty assume that our external force is given by

$$
F_{\mathrm{ext}}(t) = F_0\cos{(\omega t)},
$$

where $F_0$ is a constant (what is its dimension?) and $\omega$ is the frequency of the applied external driving force.
**Small question:** would you expect energy to be conserved now?

Introducing the external force into our lovely differential equation
and dividing by $m$ and introducing $\omega_0^2=\sqrt{k/m}$ we have

$$
\frac{d^2x}{dt^2} + \frac{b}{m}\frac{dx}{dt}+\omega_0^2x(t) =\frac{F_0}{m}\cos{(\omega t)},
$$

Thereafter we introduce a dimensionless time $\tau = t\omega_0$
and a dimensionless frequency $\tilde{\omega}=\omega/\omega_0$. We have then

$$
\frac{d^2x}{d\tau^2} + \frac{b}{m\omega_0}\frac{dx}{d\tau}+x(\tau) =\frac{F_0}{m\omega_0^2}\cos{(\tilde{\omega}\tau)},
$$

Introducing a new amplitude $\tilde{F} =F_0/(m\omega_0^2)$ (check dimensionality again) we have

$$
\frac{d^2x}{d\tau^2} + \frac{b}{m\omega_0}\frac{dx}{d\tau}+x(\tau) =\tilde{F}\cos{(\tilde{\omega}\tau)}.
$$

Our final step, as we did in the case of various types of damping, is
to define $\gamma = b/(2m\omega_0)$ and rewrite our equations as

$$
\frac{d^2x}{d\tau^2} + 2\gamma\frac{dx}{d\tau}+x(\tau) =\tilde{F}\cos{(\tilde{\omega}\tau)}.
$$

This is the equation we will code below using the Euler-Cromer method.

```
%matplotlib inline

# Common imports
import numpy as np
import pandas as pd
from math import *
import matplotlib.pyplot as plt
import os

# Where to save the figures and data files
PROJECT_ROOT_DIR = "Results"
FIGURE_ID = "Results/FigureFiles"
DATA_ID = "DataFiles/"

if not os.path.exists(PROJECT_ROOT_DIR):
    os.mkdir(PROJECT_ROOT_DIR)

if not os.path.exists(FIGURE_ID):
    os.makedirs(FIGURE_ID)

if not os.path.exists(DATA_ID):
    os.makedirs(DATA_ID)

def image_path(fig_id):
    return os.path.join(FIGURE_ID, fig_id)

def data_path(dat_id):
    return os.path.join(DATA_ID, dat_id)

def save_fig(fig_id):
    plt.savefig(image_path(fig_id) + ".png", format='png')

from pylab import plt, mpl
plt.style.use('seaborn')
mpl.rcParams['font.family'] = 'serif'

DeltaT = 0.001
#set up arrays
tfinal = 20 # in dimensionless time
n = ceil(tfinal/DeltaT)
# set up arrays for t, v, and x
t = np.zeros(n)
v = np.zeros(n)
x = np.zeros(n)
# Initial conditions as one-dimensional arrays of time
x0 =  1.0
v0 = 0.0
x[0] = x0
v[0] = v0
gamma = 0.2
Omegatilde = 0.5
Ftilde = 1.0
# Start integrating using Euler-Cromer's method
for i in range(n-1):
    # Set up the acceleration
    # Here you could have defined your own function for this
    a =  -2*gamma*v[i]-x[i]+Ftilde*cos(t[i]*Omegatilde)
    # update velocity, time and position
    v[i+1] = v[i] + DeltaT*a
    x[i+1] = x[i] + DeltaT*v[i+1]
    t[i+1] = t[i] + DeltaT
# Plot position as function of time
fig, ax = plt.subplots()
ax.set_ylabel('x[m]')
ax.set_xlabel('t[s]')
ax.plot(t, x)
fig.tight_layout()
save_fig("ForcedBlockEulerCromer")
plt.show()
```

In the above example we have focused on the Euler-Cromer method. This
method has a local truncation error which is proportional to $\Delta t^2$
and thereby a global error which is proportional to $\Delta t$.
We can improve this by using the Runge-Kutta family of
methods. The widely popular Runge-Kutta to fourth order or just **RK4**
has indeed a much better truncation error. The RK4 method has a global
error which is proportional to $\Delta t$.

Let us revisit this method and see how we can implement it for the above example.

## Differential Equations, Runge-Kutta methods

Runge-Kutta (RK) methods are based on Taylor expansion formulae, but yield
in general better algorithms for solutions of an ordinary differential equation.
The basic philosophy is that it provides an intermediate step in the computation of $y_{i+1}$.

To see this, consider first the following definitions

$$
\begin{equation}
\frac{dy}{dt}=f(t,y),
\label{_auto1} \tag{1}
\end{equation}
$$

and

$$
\begin{equation}
y(t)=\int f(t,y) dt,
\label{_auto2} \tag{2}
\end{equation}
$$

and

$$
\begin{equation}
y_{i+1}=y_i+ \int_{t_i}^{t_{i+1}} f(t,y) dt.
\label{_auto3} \tag{3}
\end{equation}
$$

To demonstrate the philosophy behind RK methods, let us consider
the second-order RK method, RK2.
The first approximation consists in Taylor expanding $f(t,y)$
around the center of the integration interval $t_i$ to $t_{i+1}$,
that is, at $t_i+h/2$, $h$ being the step.
Using the midpoint formula for an integral,
defining $y(t_i+h/2) = y_{i+1/2}$ and
$t_i+h/2 = t_{i+1/2}$, we obtain

$$
\begin{equation}
\int_{t_i}^{t_{i+1}} f(t,y) dt \approx hf(t_{i+1/2},y_{i+1/2}) +O(h^3).
\label{_auto4} \tag{4}
\end{equation}
$$

This means in turn that we have

$$
\begin{equation}
y_{i+1}=y_i + hf(t_{i+1/2},y_{i+1/2}) +O(h^3).
\label{_auto5} \tag{5}
\end{equation}
$$

However, we do not know the value of   $y_{i+1/2}$. Here comes thus the next approximation, namely, we use Euler's
method to approximate $y_{i+1/2}$. We have then

$$
\begin{equation}
y_{(i+1/2)}=y_i + \frac{h}{2}\frac{dy}{dt}=y(t_i) + \frac{h}{2}f(t_i,y_i).
\label{_auto6} \tag{6}
\end{equation}
$$

This means that we can define the following algorithm for
the second-order Runge-Kutta method, RK2.

$$
\begin{equation}
k_1=hf(t_i,y_i),
\label{_auto7} \tag{7}
\end{equation}
$$

and

$$
\begin{equation}
k_2=hf(t_{i+1/2},y_i+k_1/2),
\label{_auto8} \tag{8}
\end{equation}
$$

with the final value

$$
\begin{equation}
y_{i+i}\approx y_i + k_2 +O(h^3).
\label{_auto9} \tag{9}
\end{equation}
$$

The difference between the previous one-step methods
is that we now need an intermediate step in our evaluation,
namely $t_i+h/2 = t_{(i+1/2)}$ where we evaluate the derivative $f$.
This involves more operations, but the gain is a better stability
in the solution.

The fourth-order Runge-Kutta, RK4, has the following algorithm

$$
k_1=hf(t_i,y_i) \hspace{0.5cm}   k_2=hf(t_i+h/2,y_i+k_1/2)
$$

and

$$
k_3=hf(t_i+h/2,y_i+k_2/2)\hspace{0.5cm}   k_4=hf(t_i+h,y_i+k_3)
$$

with the final result

$$
y_{i+1}=y_i +\frac{1}{6}\left( k_1 +2k_2+2k_3+k_4\right).
$$

Thus, the algorithm consists in first calculating $k_1$
with $t_i$, $y_1$ and $f$ as inputs. Thereafter, we increase the step
size by $h/2$ and calculate $k_2$, then $k_3$ and finally $k_4$. The global error goes as $O(h^4)$.

However, at this stage, if we keep adding different methods in our
main program, the code will quickly become messy and ugly. Before we
proceed thus, we will now introduce functions that enbody the various
methods for solving differential equations. This means that we can
separate out these methods in own functions and files (and later as classes and more
generic functions) and simply call them when needed. Similarly, we
could easily encapsulate various forces or other quantities of
interest in terms of functions. To see this, let us bring up the code
we developed above for the simple sliding block, but now only with the simple forward Euler method. We introduce
two functions, one for the simple Euler method and one for the
force.

Note that here the forward Euler method does not know the specific force function to be called.
It receives just an input the name. We can easily change the force by adding another function.

```
def ForwardEuler(v,x,t,n,Force):
    for i in range(n-1):
        v[i+1] = v[i] + DeltaT*Force(v[i],x[i],t[i])
        x[i+1] = x[i] + DeltaT*v[i]
        t[i+1] = t[i] + DeltaT
```

```
def SpringForce(v,x,t):
#   note here that we have divided by mass and we return the acceleration
    return  -2*gamma*v-x+Ftilde*cos(t*Omegatilde)
```

It is easy to add a new method like the Euler-Cromer

```
def ForwardEulerCromer(v,x,t,n,Force):
    for i in range(n-1):
        a = Force(v[i],x[i],t[i])
        v[i+1] = v[i] + DeltaT*a
        x[i+1] = x[i] + DeltaT*v[i+1]
        t[i+1] = t[i] + DeltaT
```

and the Velocity Verlet method (be careful with time-dependence here, it is not an ideal method for non-conservative forces))

```
def VelocityVerlet(v,x,t,n,Force):
    for i in range(n-1):
        a = Force(v[i],x[i],t[i])
        x[i+1] = x[i] + DeltaT*v[i]+0.5*a*DeltaT*DeltaT
        anew = Force(v[i],x[i+1],t[i+1])
        v[i+1] = v[i] + 0.5*DeltaT*(a+anew)
        t[i+1] = t[i] + DeltaT
```

Finally, we can now add the Runge-Kutta2 method via a new function

```
def RK2(v,x,t,n,Force):
    for i in range(n-1):
# Setting up k1
        k1x = DeltaT*v[i]
        k1v = DeltaT*Force(v[i],x[i],t[i])
# Setting up k2
        vv = v[i]+k1v*0.5
        xx = x[i]+k1x*0.5
        k2x = DeltaT*vv
        k2v = DeltaT*Force(vv,xx,t[i]+DeltaT*0.5)
# Final result
        x[i+1] = x[i]+k2x
        v[i+1] = v[i]+k2v
	t[i+1] = t[i]+DeltaT
```

Finally, we can now add the Runge-Kutta2 method via a new function

```
def RK4(v,x,t,n,Force):
    for i in range(n-1):
# Setting up k1
        k1x = DeltaT*v[i]
        k1v = DeltaT*Force(v[i],x[i],t[i])
# Setting up k2
        vv = v[i]+k1v*0.5
        xx = x[i]+k1x*0.5
        k2x = DeltaT*vv
        k2v = DeltaT*Force(vv,xx,t[i]+DeltaT*0.5)
# Setting up k3
        vv = v[i]+k2v*0.5
        xx = x[i]+k2x*0.5
        k3x = DeltaT*vv
        k3v = DeltaT*Force(vv,xx,t[i]+DeltaT*0.5)
# Setting up k4
        vv = v[i]+k3v
        xx = x[i]+k3x
        k4x = DeltaT*vv
        k4v = DeltaT*Force(vv,xx,t[i]+DeltaT)
# Final result
        x[i+1] = x[i]+(k1x+2*k2x+2*k3x+k4x)/6.
        v[i+1] = v[i]+(k1v+2*k2v+2*k3v+k4v)/6.
        t[i+1] = t[i] + DeltaT
```

The Runge-Kutta family of methods are particularly useful when we have a time-dependent acceleration.
If we have forces which depend only the spatial degrees of freedom (no velocity and/or time-dependence), then energy conserving methods like the Velocity Verlet or the Euler-Cromer method are preferred. As soon as we introduce an explicit time-dependence and/or add dissipitave forces like friction or air resistance, then methods like the family of Runge-Kutta methods are well suited for this.
The code below uses the Runge-Kutta4 methods.

```
DeltaT = 0.001
#set up arrays
tfinal = 20 # in dimensionless time
n = ceil(tfinal/DeltaT)
# set up arrays for t, v, and x
t = np.zeros(n)
v = np.zeros(n)
x = np.zeros(n)
# Initial conditions (can change to more than one dim)
x0 =  1.0
v0 = 0.0
x[0] = x0
v[0] = v0
gamma = 0.2
Omegatilde = 0.5
Ftilde = 1.0
# Start integrating using the RK4 method
# Note that we define the force function as a SpringForce
RK4(v,x,t,n,SpringForce)

# Plot position as function of time
fig, ax = plt.subplots()
ax.set_ylabel('x[m]')
ax.set_xlabel('t[s]')
ax.plot(t, x)
fig.tight_layout()
save_fig("ForcedBlockRK4")
plt.show()
```

## Example: The classical pendulum and scaling the equations

Let us end our discussion of oscillations with another classical case, the pendulum. You will find the material here useful for homework 7.

The angular equation of motion of the pendulum is given by
Newton's equation and with no external force it reads

$$
\begin{equation}
  ml\frac{d^2\theta}{dt^2}+mgsin(\theta)=0,
\label{_auto10} \tag{10}
\end{equation}
$$

with an angular velocity and acceleration given by

$$
\begin{equation}
     v=l\frac{d\theta}{dt},
\label{_auto11} \tag{11}
\end{equation}
$$

and

$$
\begin{equation}
     a=l\frac{d^2\theta}{dt^2}.
\label{_auto12} \tag{12}
\end{equation}
$$

We do however expect that the motion will gradually come to an end due
a viscous drag torque acting on the pendulum.  In the presence of the
drag, the above equation becomes

$$
\begin{equation}
   ml\frac{d^2\theta}{dt^2}+\nu\frac{d\theta}{dt}  +mgsin(\theta)=0, \label{eq:pend1} \tag{13}
\end{equation}
$$

where $\nu$ is now a positive constant parameterizing the viscosity
of the medium in question. In order to maintain the motion against
viscosity, it is necessary to add some external driving force.
We choose here a periodic driving force. The last equation becomes then

$$
\begin{equation}
   ml\frac{d^2\theta}{dt^2}+\nu\frac{d\theta}{dt}  +mgsin(\theta)=Asin(\omega t), \label{eq:pend2} \tag{14}
\end{equation}
$$

with $A$ and $\omega$ two constants representing the amplitude and
the angular frequency respectively. The latter is called the driving frequency.

We define

$$
\omega_0=\sqrt{g/l},
$$

the so-called natural frequency and the new dimensionless quantities

$$
\hat{t}=\omega_0t,
$$

with the dimensionless driving frequency

$$
\hat{\omega}=\frac{\omega}{\omega_0},
$$

and introducing the quantity $Q$, called the *quality factor*,

$$
Q=\frac{mg}{\omega_0\nu},
$$

and the dimensionless amplitude

$$
\hat{A}=\frac{A}{mg}
$$

## More on the Pendulum

We have

$$
\frac{d^2\theta}{d\hat{t}^2}+\frac{1}{Q}\frac{d\theta}{d\hat{t}}
     +sin(\theta)=\hat{A}cos(\hat{\omega}\hat{t}).
$$

This equation can in turn be recast in terms of two coupled first-order differential equations as follows

$$
\frac{d\theta}{d\hat{t}}=\hat{v},
$$

and

$$
\frac{d\hat{v}}{d\hat{t}}=-\frac{\hat{v}}{Q}-sin(\theta)+\hat{A}cos(\hat{\omega}\hat{t}).
$$

These are the equations to be solved.  The factor $Q$ represents the
number of oscillations of the undriven system that must occur before
its energy is significantly reduced due to the viscous drag. The
amplitude $\hat{A}$ is measured in units of the maximum possible
gravitational torque while $\hat{\omega}$ is the angular frequency of
the external torque measured in units of the pendulum's natural
frequency.

## The Pendulum code

We need to define a new force, which we simply call the pendulum
force. The only thing which changes from our previous spring-force
problem is the non-linearity introduced by angle $\theta$ due to the
$\sin{\theta}$ term.  Here we have kept a generic variable $x$
instead. This makes our codes very similar.  Feel free to use these examples for homework seven.

```
def PendulumForce(v,x,t):
#   note here that we have divided by mass and we return the acceleration
    return  -gamma*v-sin(x)+Ftilde*cos(t*Omegatilde)
```

## Setting up the various variables and running the code

```
DeltaT = 0.001
#set up arrays
tfinal = 20 # in years
n = ceil(tfinal/DeltaT)
# set up arrays for t, v, and x
t = np.zeros(n)
v = np.zeros(n)
theta = np.zeros(n)
# Initial conditions (can change to more than one dim)
theta0 =  1.0
v0 = 0.0
theta[0] = theta0
v[0] = v0
gamma = 0.2
Omegatilde = 0.5
Ftilde = 1.0
# Start integrating using the RK4 method
# Note that we define the force function as a PendulumForce
RK4(v,theta,t,n,PendulumForce)

# Plot position as function of time
fig, ax = plt.subplots()
ax.set_ylabel('theta[radians]')
ax.set_xlabel('t[s]')
ax.plot(t, theta)
fig.tight_layout()
save_fig("PendulumRK4")
plt.show()
```

## Principle of Superposition and Periodic Forces (Fourier Transforms)

If one has several driving forces, $F(t)=\sum_n F_n(t)$, one can find
the particular solution to each $F_n$, $x_{pn}(t)$, and the particular
solution for the entire driving force is

$$
\begin{equation}
x_p(t)=\sum_nx_{pn}(t).
\label{_auto13} \tag{15}
\end{equation}
$$

This is known as the principal of superposition. It only applies when
the homogenous equation is linear. If there were an anharmonic term
such as $x^3$ in the homogenous equation, then when one summed various
solutions, $x=(\sum_n x_n)^2$, one would get cross
terms. Superposition is especially useful when $F(t)$ can be written
as a sum of sinusoidal terms, because the solutions for each
sinusoidal (sine or cosine)  term is analytic, as we saw above.

Driving forces are often periodic, even when they are not
sinusoidal. Periodicity implies that for some time $\tau$

$$
\begin{eqnarray}
F(t+\tau)=F(t).
\end{eqnarray}
$$

One example of a non-sinusoidal periodic force is a square wave. Many
components in electric circuits are non-linear, e.g. diodes, which
makes many wave forms non-sinusoidal even when the circuits are being
driven by purely sinusoidal sources.

The code here shows a typical example of such a square wave generated using the functionality included in the **scipy** Python package. We have used a period of $\tau=0.2$.

```
import numpy as np
import math
from scipy import signal
import matplotlib.pyplot as plt

# number of points
n = 500
# start and final times
t0 = 0.0
tn = 1.0
# Period
t = np.linspace(t0, tn, n, endpoint=False)
SqrSignal = np.zeros(n)
SqrSignal = 1.0+signal.square(2*np.pi*5*t)
plt.plot(t, SqrSignal)
plt.ylim(-0.5, 2.5)
plt.show()
```

For the sinusoidal example studied in the previous week the
period is $\tau=2\pi/\omega$. However, higher harmonics can also
satisfy the periodicity requirement. In general, any force that
satisfies the periodicity requirement can be expressed as a sum over
harmonics,

$$
\begin{equation}
F(t)=\frac{f_0}{2}+\sum_{n>0} f_n\cos(2n\pi t/\tau)+g_n\sin(2n\pi t/\tau).
\label{_auto14} \tag{16}
\end{equation}
$$

We can write down the answer for
$x_{pn}(t)$, by substituting $f_n/m$ or $g_n/m$ for $F_0/m$. By
writing each factor $2n\pi t/\tau$ as $n\omega t$, with $\omega\equiv
2\pi/\tau$,

$$
\begin{equation}
\label{eq:fourierdef1} \tag{17}
F(t)=\frac{f_0}{2}+\sum_{n>0}f_n\cos(n\omega t)+g_n\sin(n\omega t).
\end{equation}
$$

The solutions for $x(t)$ then come from replacing $\omega$ with
$n\omega$ for each term in the particular solution,

$$
\begin{eqnarray}
x_p(t)&=&\frac{f_0}{2k}+\sum_{n>0} \alpha_n\cos(n\omega t-\delta_n)+\beta_n\sin(n\omega t-\delta_n),\\
\nonumber
\alpha_n&=&\frac{f_n/m}{\sqrt{((n\omega)^2-\omega_0^2)+4\beta^2n^2\omega^2}},\\
\nonumber
\beta_n&=&\frac{g_n/m}{\sqrt{((n\omega)^2-\omega_0^2)+4\beta^2n^2\omega^2}},\\
\nonumber
\delta_n&=&\tan^{-1}\left(\frac{2\beta n\omega}{\omega_0^2-n^2\omega^2}\right).
\end{eqnarray}
$$

Because the forces have been applied for a long time, any non-zero
damping eliminates the homogenous parts of the solution, so one need
only consider the particular solution for each $n$.

The problem will considered solved if one can find expressions for the
coefficients $f_n$ and $g_n$, even though the solutions are expressed
as an infinite sum. The coefficients can be extracted from the
function $F(t)$ by

$$
\begin{eqnarray}
\label{eq:fourierdef2} \tag{18}
f_n&=&\frac{2}{\tau}\int_{-\tau/2}^{\tau/2} dt~F(t)\cos(2n\pi t/\tau),\\
\nonumber
g_n&=&\frac{2}{\tau}\int_{-\tau/2}^{\tau/2} dt~F(t)\sin(2n\pi t/\tau).
\end{eqnarray}
$$

To check the consistency of these expressions and to verify
Eq. ([18](#eq:fourierdef2)), one can insert the expansion of $F(t)$ in
Eq. ([17](#eq:fourierdef1)) into the expression for the coefficients in
Eq. ([18](#eq:fourierdef2)) and see whether

$$
\begin{eqnarray}
f_n&=?&\frac{2}{\tau}\int_{-\tau/2}^{\tau/2} dt~\left\{
\frac{f_0}{2}+\sum_{m>0}f_m\cos(m\omega t)+g_m\sin(m\omega t)
\right\}\cos(n\omega t).
\end{eqnarray}
$$

Immediately, one can throw away all the terms with $g_m$ because they
convolute an even and an odd function. The term with $f_0/2$
disappears because $\cos(n\omega t)$ is equally positive and negative
over the interval and will integrate to zero. For all the terms
$f_m\cos(m\omega t)$ appearing in the sum, one can use angle addition
formulas to see that $\cos(m\omega t)\cos(n\omega
t)=(1/2)(\cos[(m+n)\omega t]+\cos[(m-n)\omega t]$. This will integrate
to zero unless $m=n$. In that case the $m=n$ term gives

$$
\begin{equation}
\int_{-\tau/2}^{\tau/2}dt~\cos^2(m\omega t)=\frac{\tau}{2},
\label{_auto15} \tag{19}
\end{equation}
$$

and

$$
\begin{eqnarray}
f_n&=?&\frac{2}{\tau}\int_{-\tau/2}^{\tau/2} dt~f_n/2\\
\nonumber
&=&f_n~\checkmark.
\end{eqnarray}
$$

The same method can be used to check for the consistency of $g_n$.

Consider the driving force:

$$
\begin{equation}
F(t)=At/\tau,~~-\tau/2<t<\tau/2,~~~F(t+\tau)=F(t).
\label{_auto16} \tag{20}
\end{equation}
$$

Find the Fourier coefficients $f_n$ and $g_n$ for all $n$ using Eq. ([18](#eq:fourierdef2)).

Only the odd coefficients enter by symmetry, i.e. $f_n=0$. One can find $g_n$ integrating by parts,

$$
\begin{eqnarray}
\label{eq:fouriersolution} \tag{21}
g_n&=&\frac{2}{\tau}\int_{-\tau/2}^{\tau/2}dt~\sin(n\omega t) \frac{At}{\tau}\\
\nonumber
u&=&t,~dv=\sin(n\omega t)dt,~v=-\cos(n\omega t)/(n\omega),\\
\nonumber
g_n&=&\frac{-2A}{n\omega \tau^2}\int_{-\tau/2}^{\tau/2}dt~\cos(n\omega t)
+\left.2A\frac{-t\cos(n\omega t)}{n\omega\tau^2}\right|_{-\tau/2}^{\tau/2}.
\end{eqnarray}
$$

The first term is zero because $\cos(n\omega t)$ will be equally
positive and negative over the interval. Using the fact that
$\omega\tau=2\pi$,

$$
\begin{eqnarray}
g_n&=&-\frac{2A}{2n\pi}\cos(n\omega\tau/2)\\
\nonumber
&=&-\frac{A}{n\pi}\cos(n\pi)\\
\nonumber
&=&\frac{A}{n\pi}(-1)^{n+1}.
\end{eqnarray}
$$

## Fourier Series

More text will come here, chpater 5.7-5.8 of Taylor are discussed
during the lectures. The code here uses the Fourier series discussed
in chapter 5.7 for a square wave signal. The equations for the
coefficients are are discussed in Taylor section 5.7, see Example
5.4. The code here visualizes the various approximations given by
Fourier series compared with a square wave with period $T=0.2$, witth
$0.1$ and max value $F=2$. We see that when we increase the number of
components in the Fourier series, the Fourier series approximation gets closes and closes to the square wave signal.

```
import numpy as np
import math
from scipy import signal
import matplotlib.pyplot as plt

# number of points
n = 500
# start and final times
t0 = 0.0
tn = 1.0
# Period
T =0.2
# Max value of square signal
Fmax= 2.0
# Width of signal
Width = 0.1
t = np.linspace(t0, tn, n, endpoint=False)
SqrSignal = np.zeros(n)
FourierSeriesSignal = np.zeros(n)
SqrSignal = 1.0+signal.square(2*np.pi*5*t+np.pi*Width/T)
a0 = Fmax*Width/T
FourierSeriesSignal = a0
Factor = 2.0*Fmax/np.pi
for i in range(1,500):
    FourierSeriesSignal += Factor/(i)*np.sin(np.pi*i*Width/T)*np.cos(i*t*2*np.pi/T)
plt.plot(t, SqrSignal)
plt.plot(t, FourierSeriesSignal)
plt.ylim(-0.5, 2.5)
plt.show()
```

## Response to Transient Force

Consider a particle at rest in the bottom of an underdamped harmonic
oscillator, that then feels a sudden impulse, or change in momentum,
$I=F\Delta t$ at $t=0$. This increases the velocity immediately by an
amount $v_0=I/m$ while not changing the position. One can then solve
the trajectory by solving the equations with initial
conditions $v_0=I/m$ and $x_0=0$. This gives

$$
\begin{equation}
x(t)=\frac{I}{m\omega'}e^{-\beta t}\sin\omega't, ~~t>0.
\label{_auto17} \tag{22}
\end{equation}
$$

Here, $\omega'=\sqrt{\omega_0^2-\beta^2}$. For an impulse $I_i$ that
occurs at time $t_i$ the trajectory would be

$$
\begin{equation}
x(t)=\frac{I_i}{m\omega'}e^{-\beta (t-t_i)}\sin[\omega'(t-t_i)] \Theta(t-t_i),
\label{_auto18} \tag{23}
\end{equation}
$$

where $\Theta(t-t_i)$ is a step function, i.e. $\Theta(x)$ is zero for
$x<0$ and unity for $x>0$. If there were several impulses linear
superposition tells us that we can sum over each contribution,

$$
\begin{equation}
x(t)=\sum_i\frac{I_i}{m\omega'}e^{-\beta(t-t_i)}\sin[\omega'(t-t_i)]\Theta(t-t_i)
\label{_auto19} \tag{24}
\end{equation}
$$

Now one can consider a series of impulses at times separated by
$\Delta t$, where each impulse is given by $F_i\Delta t$. The sum
above now becomes an integral,

$$
\begin{eqnarray}\label{eq:Greeny} \tag{25}
x(t)&=&\int_{-\infty}^\infty dt'~F(t')\frac{e^{-\beta(t-t')}\sin[\omega'(t-t')]}{m\omega'}\Theta(t-t')\\
\nonumber
&=&\int_{-\infty}^\infty dt'~F(t')G(t-t'),\\
\nonumber
G(\Delta t)&=&\frac{e^{-\beta\Delta t}\sin[\omega' \Delta t]}{m\omega'}\Theta(\Delta t)
\end{eqnarray}
$$

The quantity
$e^{-\beta(t-t')}\sin[\omega'(t-t')]/m\omega'\Theta(t-t')$ is called a
Green's function, $G(t-t')$. It describes the response at $t$ due to a
force applied at a time $t'$, and is a function of $t-t'$. The step
function ensures that the response does not occur before the force is
applied. One should remember that the form for $G$ would change if the
oscillator were either critically- or over-damped.

When performing the integral in Eq. ([25](#eq:Greeny)) one can use
angle addition formulas to factor out the part with the $t'$
dependence in the integrand,

$$
\begin{eqnarray}
\label{eq:Greeny2} \tag{26}
x(t)&=&\frac{1}{m\omega'}e^{-\beta t}\left[I_c(t)\sin(\omega't)-I_s(t)\cos(\omega't)\right],\\
\nonumber
I_c(t)&\equiv&\int_{-\infty}^t dt'~F(t')e^{\beta t'}\cos(\omega't'),\\
\nonumber
I_s(t)&\equiv&\int_{-\infty}^t dt'~F(t')e^{\beta t'}\sin(\omega't').
\end{eqnarray}
$$

If the time $t$ is beyond any time at which the force acts,
$F(t'>t)=0$, the coefficients $I_c$ and $I_s$ become independent of
$t$.

Consider an undamped oscillator ($\beta\rightarrow 0$), with
characteristic frequency $\omega_0$ and mass $m$, that is at rest
until it feels a force described by a Gaussian form,

$$
\begin{eqnarray*}
F(t)&=&F_0 \exp\left\{\frac{-t^2}{2\tau^2}\right\}.
\end{eqnarray*}
$$

For large times ($t>>\tau$), where the force has died off, find
$x(t)$.\\ Solve for the coefficients $I_c$ and $I_s$ in
Eq. ([26](#eq:Greeny2)). Because the Gaussian is an even function,
$I_s=0$, and one need only solve for $I_c$,

$$
\begin{eqnarray*}
I_c&=&F_0\int_{-\infty}^\infty dt'~e^{-t^{\prime 2}/(2\tau^2)}\cos(\omega_0 t')\\
&=&\Re F_0 \int_{-\infty}^\infty dt'~e^{-t^{\prime 2}/(2\tau^2)}e^{i\omega_0 t'}\\
&=&\Re F_0 \int_{-\infty}^\infty dt'~e^{-(t'-i\omega_0\tau^2)^2/(2\tau^2)}e^{-\omega_0^2\tau^2/2}\\
&=&F_0\tau \sqrt{2\pi} e^{-\omega_0^2\tau^2/2}.
\end{eqnarray*}
$$

The third step involved completing the square, and the final step used the fact that the integral

$$
\begin{eqnarray*}
\int_{-\infty}^\infty dx~e^{-x^2/2}&=&\sqrt{2\pi}.
\end{eqnarray*}
$$

To see that this integral is true, consider the square of the integral, which you can change to polar coordinates,

$$
\begin{eqnarray*}
I&=&\int_{-\infty}^\infty dx~e^{-x^2/2}\\
I^2&=&\int_{-\infty}^\infty dxdy~e^{-(x^2+y^2)/2}\\
&=&2\pi\int_0^\infty rdr~e^{-r^2/2}\\
&=&2\pi.
\end{eqnarray*}
$$

Finally, the expression for $x$ from Eq. ([26](#eq:Greeny2)) is

$$
\begin{eqnarray*}
x(t>>\tau)&=&\frac{F_0\tau}{m\omega_0} \sqrt{2\pi} e^{-\omega_0^2\tau^2/2}\sin(\omega_0t).
\end{eqnarray*}
$$


# Standalone Fishbone-Moncrief C Code

We start with the NRPy+ expressions generated in the [Tutorial-FishboneMoncriefID](Tutorial-FishboneMoncriefID.ipynb), and output them to the C file "FishboneMoncriefID/FMstandalone.h".

Further, $\Gamma = \alpha u^0$ is given by (as shown [here](Tutorial-u0_smallb_Poynting-Cartesian.ipynb)):
$$
\Gamma = \alpha u^0 = \sqrt{\frac{1}{1 - \gamma_{ij}v^i_{(n)}v^j_{(n)}}}.
$$

```python
import sympy as sp
from outputC import *
import indexedexp as ixp
import finite_difference as fin
import FishboneMoncriefID.FishboneMoncriefID as fmid

# Step 1: Set up the Fishbone-Moncrief initial data. This sets all the ID gridfunctions.
fmid.FishboneMoncriefID("Spherical")

gammaDD = ixp.zerorank2()

DIM = 3
for i in range(DIM):
    for j in range(DIM):
        if i<=j:
            gammaDD[i][j] = fmid.IDgammaDD[i][j]
        else:
            gammaDD[i][j] = fmid.IDgammaDD[j][i]

# gamma_{ij} v^i_{(n)} v^j_{(n)}
Gammacontraction = sp.sympify(0)
for i in range(DIM):
    for j in range(DIM):
        Gammacontraction += gammaDD[i][j] * fmid.IDValencia3velocityU[i] * fmid.IDValencia3velocityU[j]

Gammafactor = sp.sqrt(1 / (1 - Gammacontraction))

# -={ F-M quantities: Generate C code from expressions and output to file }=-
FishboneMoncrief_to_print = [\
                     lhrh(lhs="alpha",rhs=fmid.IDalpha),\
                     lhrh(lhs="betaU0",rhs=fmid.IDbetaU[0]),\
                     lhrh(lhs="betaU1",rhs=fmid.IDbetaU[1]),\
                     lhrh(lhs="betaU2",rhs=fmid.IDbetaU[2]),\
                     lhrh(lhs="Gammafactor",rhs=Gammafactor),\
                     lhrh(lhs="Gamma_times_ValenciavU0",rhs=Gammafactor*fmid.IDValencia3velocityU[0]),\
                     lhrh(lhs="Gamma_times_ValenciavU1",rhs=Gammafactor*fmid.IDValencia3velocityU[1]),\
                     lhrh(lhs="Gamma_times_ValenciavU2",rhs=Gammafactor*fmid.IDValencia3velocityU[2]),\
                     lhrh(lhs="uKS4U1",rhs=fmid.uKS4U[1]),\
                     lhrh(lhs="uKS4U2",rhs=fmid.uKS4U[2]),\
                     lhrh(lhs="uKS4U3",rhs=fmid.uKS4U[3]),\
                     lhrh(lhs="uBL4U1",rhs=fmid.uBL4U[1]),\
                     lhrh(lhs="uBL4U2",rhs=fmid.uBL4U[2]),\
                     lhrh(lhs="uBL4U3",rhs=fmid.uBL4U[3])
                     ]
print(fmid.uKS4U[3])
fin.FD_outputC("FishboneMoncriefID/FM_standalone.h",FishboneMoncrief_to_print,params="outCverbose=False,CSE_enable=False")
```

    -2*M*a*xx0*(-2*M*a*xx0*sqrt((-a**2*(-2*M*xx0 + a**2 + xx0**2)*sin(xx1)**2 + (a**2 + xx0**2)**2)/(a**2*cos(xx1)**2 + xx0**2))*sqrt(sqrt(4*M*(a**2*cos(xx1)**2 + xx0**2)**2*(-2*M*xx0 + a**2 + xx0**2)*(-2*M*a**2*r_at_max_density + a**2*r_at_max_density**2 - a*sqrt(M*r_at_max_density)*(-a**2 + r_at_max_density**2) + r_at_max_density**4)**2/(r_at_max_density**3*(-a**2*(-2*M*xx0 + a**2 + xx0**2)*sin(xx1)**2 + (a**2 + xx0**2)**2)**2*(-3*M*r_at_max_density + 2*a*sqrt(M*r_at_max_density) + r_at_max_density**2)**2*sin(xx1)**2) + 1)/2 - 1/2)*Abs(sin(xx1))/(-a**2*(-2*M*xx0 + a**2 + xx0**2)*sin(xx1)**2 + (a**2 + xx0**2)**2) - sqrt((-2*M*xx0 + a**2 + xx0**2)/(-a**2*(-2*M*xx0 + a**2 + xx0**2)*sin(xx1)**2 + (a**2 + xx0**2)**2))*sqrt(a**2*cos(xx1)**2 + xx0**2)*sqrt(sqrt(4*M*(a**2*cos(xx1)**2 + xx0**2)**2*(-2*M*xx0 + a**2 + xx0**2)*(-2*M*a**2*r_at_max_density + a**2*r_at_max_density**2 - a*sqrt(M*r_at_max_density)*(-a**2 + r_at_max_density**2) + r_at_max_density**4)**2/(r_at_max_density**3*(-a**2*(-2*M*xx0 + a**2 + xx0**2)*sin(xx1)**2 + (a**2 + xx0**2)**2)**2*(-3*M*r_at_max_density + 2*a*sqrt(M*r_at_max_density) + r_at_max_density**2)**2*sin(xx1)**2) + 1)/2 + 1/2))/((a**2*cos(xx1)**2 + xx0**2)*(-2*M*xx0 + a**2 + xx0**2)) + sqrt((-a**2*(-2*M*xx0 + a**2 + xx0**2)*sin(xx1)**2 + (a**2 + xx0**2)**2)/(a**2*cos(xx1)**2 + xx0**2))*(-4*M**2*a**2*xx0**2/((a**2*cos(xx1)**2 + xx0**2)*(-a**2*(-2*M*xx0 + a**2 + xx0**2)*sin(xx1)**2 + (a**2 + xx0**2)**2)*(-2*M*xx0 + a**2 + xx0**2)) + (a**2*cos(xx1)**2 + xx0**2)/((-a**2*(-2*M*xx0 + a**2 + xx0**2)*sin(xx1)**2 + (a**2 + xx0**2)**2)*sin(xx1)**2))*sqrt(sqrt(4*M*(a**2*cos(xx1)**2 + xx0**2)**2*(-2*M*xx0 + a**2 + xx0**2)*(-2*M*a**2*r_at_max_density + a**2*r_at_max_density**2 - a*sqrt(M*r_at_max_density)*(-a**2 + r_at_max_density**2) + r_at_max_density**4)**2/(r_at_max_density**3*(-a**2*(-2*M*xx0 + a**2 + xx0**2)*sin(xx1)**2 + (a**2 + xx0**2)**2)**2*(-3*M*r_at_max_density + 2*a*sqrt(M*r_at_max_density) + r_at_max_density**2)**2*sin(xx1)**2) + 1)/2 - 1/2)*Abs(sin(xx1))
    Wrote to file "FishboneMoncriefID/FM_standalone.h"

```python
%%writefile FishboneMoncriefID/FM_standalone.c

#include "stdio.h"
#include "stdlib.h"
#include "math.h"

const double a = 0.9375;
const double M = 1.0;
const double r_at_max_density = 12.0;
const double r_in = 6.0;

int main(int argc, const char *argv[]) {

    // Step 0a: Read command-line input, error out if nonconformant
    double xx0,xx1,xx2;
/*
    if(argc != 4) {
        printf("Error: Expected three command-line arguments: ./FM_standalone r theta phi\n");
        exit(1);
    }
    xx0 = strtod(argv[1],NULL);
    xx1 = strtod(argv[2],NULL);
    xx2 = strtod(argv[3],NULL);
*/

//    printf("# Output: r,th,ph, alpha, betaU0, betaU1, betaU2, Gamma, Gamma*vValenciaU0, Gamma*vValenciaU1, Gamma*vValenciaU2\n");
    for(double xx0=1.6;xx0<50.0;xx0+=0.2) {
        xx1 = 1.56463634120e0; //M_PI/2.0;
        xx2 = 0.0;
        double alpha,betaU0,betaU1,betaU2,Gammafactor,Gamma_times_ValenciavU0,Gamma_times_ValenciavU1,Gamma_times_ValenciavU2;
        double uKS4U1,uKS4U2,uKS4U3,uBL4U1,uBL4U2,uBL4U3;
#include "FM_standalone.h"
        if(xx0 < r_in) {
            Gammafactor = 1.0;
            Gamma_times_ValenciavU0 = Gamma_times_ValenciavU1 = Gamma_times_ValenciavU2 = 0.0;
            uKS4U1 = uKS4U2 = uKS4U3 = 0.0;
            uBL4U1 = uBL4U2 = uBL4U3 = 0.0;
        }
        printf("%e %e %e %.15e %.15e %.15e %.15e %.15e %.15e %.15e %.15e %.15e %.15e %.15e %.15e %.15e %.15e\n",
               xx0,xx1,xx2,
               alpha,betaU0,betaU1,betaU2,
               Gammafactor,
               Gamma_times_ValenciavU0, // util1(1) in FMtorus.f90; util(1,i,j,k) near the write statement
               Gamma_times_ValenciavU1, // util1(3) in FMtorus.f90.
               Gamma_times_ValenciavU2, // util1(2) in FMtorus.f90.
               uKS4U1,uKS4U2,uKS4U3,
               uBL4U1,uBL4U2,uBL4U3);
    }
    return 0;
}
```

    Writing FishboneMoncriefID/FM_standalone.c

```python
!gcc -O2 FishboneMoncriefID/FM_standalone.c -o FM_standalone -lm
```

```python
!./FM_standalone > out.txt
```

```python
%matplotlib inline
import sympy as sp
import matplotlib.pyplot as plt
import mpmath as mp
import csv

# Download torus_cuts.csv:
URL     = "http://astro.phys.wvu.edu/zetienne/torus_cuts.csv"
outfile = "torus_cuts.csv"
try:
    with open(outfile,"w") as file:
        file.write(urllib.request.urlopen(URL).read().decode("utf-8"))
except:
    try:
        with open(outfile,"w") as file:
            file.write(urllib.urlopen(URL).read().decode("utf-8"))
    except:
        # If all else fails, hope wget does the job
        !wget -O $outfile $URL

def file_reader(filename,list_of_cols,delim=" "):
    with open(filename) as file:
        reader = csv.reader(file, delimiter=delim)
        data  = list(zip(*reader))
#         print(data)
        # data is a tuple of strings. Tuples are immutable, and we need to perform math on
        #   the data, so here we convert tuple to lists of floats:
#        data_output = [[sp.sympify(0) for i in range(len(list_of_cols))] for j in range(len(data[0]))]
        data_output = [[sp.sympify(0) for i in range(len(data[0]))] for j in range(len(list_of_cols))]
        for i in range(len(data[0])):
            for j in range(len(list_of_cols)):
#                 print(i,j,data[list_of_cols[j]][i])
                data_output[j][i] = float(data[list_of_cols[j]][i])
        return data_output

NRPy_data_output = file_reader('out.txt',       [0,7,8,9,10])
std_data_output  = file_reader('torus_cuts.csv',[0,4,1,3,2])

ylabels = ['Lorentz Gamma_{KS}=G','G*v^r_{KS,Val.}','G*v^{\\theta}_{KS,Val.}','G*v^{\phi}_{KS,Val.}']

for i in range(len(ylabels)):
    # https://matplotlib.org/gallery/text_labels_and_annotations/legend.html#sphx-glr-gallery-text-labels-and-annotations-legend-py
    fig, ax = plt.subplots()
    plt.title("NRPy's FM solve with FMtorus.f90: "+ylabels[i])
    plt.xlabel("r/M")
    plt.ylabel(ylabels[i])
    ax.plot(NRPy_data_output[0], NRPy_data_output[i+1], 'k--', label='NRPyFMSolve')
    ax.plot(std_data_output[0],   std_data_output[i+1], 'k-',  label='FMtorus.f90')
    legend = ax.legend(loc='upper right', shadow=True, fontsize='x-large')
    legend.get_frame().set_facecolor('C1')
    plt.show()
```


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

# Designing Nonlinear Kalman Filters

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

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

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

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

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

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

    code{
        font-size: 6pt;

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

## Introduction

** Author's note: I was initially planning to have a design nonlinear chapter that compares various approaches. This may or may not happen, but for now this chapter has no useful content and I suggest not reading it. **

We see that the Kalman filter reasonably tracks the ball. However, as already explained, this is a silly example; we can predict trajectories in a vacuum with arbitrary precision; using a Kalman filter in this example is a needless complication.

### Kalman Filter with Air Drag

I will dispense with the step 1, step 2, type approach and proceed in a more natural style that you would use in a non-toy engineering problem. We have already developed a Kalman filter that does excellently at tracking a ball in a vacuum, but that does not incorporate the effects of air drag into the model. We know that the process model is implemented with $\textbf{F}$, so we will turn our attention to that immediately.

Notionally, the computation that $\textbf{F}$ computes is

$$x' = Fx$$

With no air drag, we had

$$
\mathbf{F} = \begin{bmatrix}
1 & \Delta t & 0 & 0 & 0 \\
0 & 1 & 0 & 0 & 0 \\
0 & 0 & 1 & \Delta t & \frac{1}{2}{\Delta t}^2 \\
0 & 0 & 0 & 1 & \Delta t \\
0 & 0 & 0 & 0 & 1
\end{bmatrix}
$$

which corresponds to the equations

$$
\begin{aligned}
x &= x + v_x \Delta t \\
v_x &= v_x \\
\\
y &= y + v_y \Delta t + \frac{a_y}{2} {\Delta t}^2 \\
v_y &= v_y + a_y \Delta t \\
a_y &= a_y
\end{aligned}
$$

From the section above we know that our new Euler equations must be

$$
\begin{aligned}
x &= x + v_x \Delta t \\
v_x &= v_x \\
\\
y &= y + v_y \Delta t + \frac{a_y}{2} {\Delta t}^2 \\
v_y &= v_y + a_y \Delta t \\
a_y &= a_y
\end{aligned}
$$

## Realistic 2D Position Sensors

The position sensor in the last example are not very realistic. In general there is no 'raw' sensor that provides (x,y) coordinates. We have GPS, but GPS already uses a Kalman filter to create a filtered output; we should not be able to improve the signal by passing it through another Kalman filter unless we incorporate additional sensors to provide additional information. We will tackle that problem later.

Consider the following set up. In an open field we put two transmitters at a known location, each transmitting a signal that we can detect. We process the signal and determine  how far we are from that signal, with some noise. First, let's look at a visual depiction of that.

```python
import matplotlib.pyplot as plt

circle1=plt.Circle((-4, 0), 5, color='#004080',
                   fill=False, linewidth=20, alpha=.7)
circle2=plt.Circle((4, 0), 5, color='#E24A33',
                   fill=False, linewidth=5, alpha=.7)

fig = plt.gcf()
ax = fig.gca()

plt.axis('equal')
plt.xlim((-10, 10))
plt.ylim((-10, 10))

plt.plot ([-4, 0], [0, 3], c='#004080')
plt.plot ([4, 0], [0, 3], c='#E24A33')
plt.text(-4, -.5, "A", fontsize=16, horizontalalignment='center')
plt.text(4, -.5, "B", fontsize=16, horizontalalignment='center')

ax.add_artist(circle1)
ax.add_artist(circle2)
plt.show()
```

Here I have attempted to show transmitter A, drawn in red, at (-4,0) and a second one B, drawn in blue, at (4,0). The red and blue circles show the range from the transmitters to the robot, with the width illustrating the effect of the $1\sigma$ angular error for each transmitter. Here I have given the blue transmitter more error than the red one. The most probable position for the robot is where the two circles intersect, which I have depicted with the red and blue lines. You will object that we have two intersections, not one, but we will see how we deal with that when we design the measurement function.

This is a very common sensor set up. Aircraft still use this system to navigate, where it is called DME (Distance Measuring Equipment). Today GPS is a much more common navigation system, but I have worked on an aircraft where we integrated sensors like this into our filter along with the GPS, INS, altimeters, etc. We will tackle what is called *multi-sensor fusion* later; for now we will just address this simple configuration.

The first step is to design our state variables. We will assume that the robot is traveling in a straight direction with constant velocity. This is unlikely to be true for a long period of time, but is acceptable for short periods of time. This does not differ from the previous problem - we will want to track the values for the robot's position and velocity. Hence,

$$\mathbf{x} =
\begin{bmatrix}x\\v_x\\y\\v_y\end{bmatrix}$$

The next step is to design the state transition function. This also will be the same as the previous problem, so without further ado,

$$
\mathbf{x}' = \begin{bmatrix}1& \Delta t& 0& 0\\0& 1& 0& 0\\0& 0& 1& \Delta t\\ 0& 0& 0& 1\end{bmatrix}\mathbf{x}$$

The next step is to design the control inputs. We have none, so we set ${\mathbf{B}}=0$.

The next step is to design the measurement function $\mathbf{z} = \mathbf{Hx}$. We can model the measurement using the Pythagorean theorem.

$$
z_a = \sqrt{(x-x_A)^2 + (y-y_A)^2} + v_a\\[1em]
z_b = \sqrt{(x-x_B])^2 + (y-y_B)^2} + v_b
$$

where $v_a$ and $v_b$ are white noise.

We see an immediate problem. The Kalman filter is designed for linear equations, and this is obviously nonlinear. In the next chapters we will look at several ways to handle nonlinear problems in a robust way, but for now we will do something simpler. If we know the approximate position of the robot than we can linearize these equations around that point. I could develop the generalized mathematics for this technique now, but instead let me just present the worked example to give context to that development.

Instead of computing $\mathbf{H}$ we will compute the partial derivative of $\mathbf{H}$ with respect to the robot's position $\mathbf{x}$. You are probably familiar with the concept of partial derivative, but if not, it just means how $\mathbf{H}$ changes with respect to the robot's position. It is computed as the partial derivative of $\mathbf{H}$ as follows:

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

Let's work the first partial derivative. We want to find

$$\frac{\partial }{\partial x} \sqrt{(x-x_A)^2 + (y-y_A)^2}
$$

Which we compute as

$$
\begin{aligned}
\frac{\partial h_1}{\partial x} &= ((x-x_A)^2 + (y-y_A)^2))^\frac{1}{2} \\
&= \frac{1}{2}\times 2(x-x_a)\times ((x-x_A)^2 + (y-y_A)^2))^{-\frac{1}{2}} \\
&= \frac{x_r - x_A}{\sqrt{(x_r-x_A)^2 + (y_r-y_A)^2}}
\end{aligned}
$$

We continue this computation for the partial derivatives of the two distance equations with respect to $x$, $y$, $dx$ and $dy$, yielding

$$\frac{\partial\mathbf{h}}{\partial\mathbf{x}}=
\begin{bmatrix}
\frac{x_r - x_A}{\sqrt{(x_r-x_A)^2 + (y_r-y_A)^2}} & 0 &
\frac{y_r - y_A}{\sqrt{(x_r-x_A)^2 + (y_r-y_A)^2}} & 0 \\
\frac{x_r - x_B}{\sqrt{(x_r-x_B)^2 + (y_r-y_B)^2}} & 0 &
\frac{y_r - y_B}{\sqrt{(x_r-x_B)^2 + (y_r-y_B)^2}} & 0 \\
\end{bmatrix}
$$

That is pretty painful, and these are very simple equations. Computing the Jacobian can be extremely difficult or even impossible for more complicated systems. However, there is an easy way to get Python to do the work for you by using the SymPy module [1]. SymPy is a Python library for symbolic mathematics. The full scope of its abilities are beyond this book, but it can perform algebra, integrate and differentiate equations, find solutions to differential equations, and much more. We will use it to compute our Jacobian!

First, a simple example. We will import SymPy, initialize its pretty print functionality (which will print equations using LaTeX). We will then declare a symbol for NumPy to use.

```python
import sympy
from sympy import init_printing
init_printing(use_latex='png')

phi, x = sympy.symbols('\phi, x')
phi
```

Notice how we use a latex expression for the symbol `phi`. This is not necessary, but if you do it will render as LaTeX when output. Now let's do some math. What is the derivative of $\sqrt{\phi}$?

```python
sympy.diff('sqrt(phi)')
```

We can factor equations.

```python
sympy.factor('phi**3 -phi**2 + phi - 1')
```

SymPy has a remarkable list of features, and as much as I enjoy exercising its features we cannot cover them all here. Instead, let's compute our Jacobian.

```python
from sympy import symbols, Matrix
phi = symbols('\phi')
phi

x, y, xa, xb, ya, yb, dx, dy = symbols('x y x_a x_b y_a y_b dx dy')

H = Matrix([[sympy.sqrt((x-xa)**2 + (y-ya)**2)],
            [sympy.sqrt((x-xb)**2 + (y-yb)**2)]])

state = Matrix([x, dx, y, dy])
H.jacobian(state)
```

In a nutshell, the entry (0,0) contains the difference between the x coordinate of the robot and transmitter A's x coordinate divided by the distance between the robot and A. (2,0) contains the same, except for the y coordinates of the robot and transmitters. The bottom row contains the same computations, except for transmitter B. The 0 entries account for the velocity components of the state variables; naturally the range does not provide us with velocity.

The values in this matrix change as the robot's position changes, so this is no longer a constant; we will have to recompute it for every time step of the filter.

If you look at this you may realize that this is just a computation of x/dist and y/dist, so we can switch this to a trigonometic form with no loss of generality:

$$\frac{\partial\mathbf{h}}{\partial\mathbf{x}}=
\begin{bmatrix}
-\cos{\theta_A} & 0 & -\sin{\theta_A} & 0 \\
-\cos{\theta_B} & 0 & -\sin{\theta_B} & 0
\end{bmatrix}
$$

However, this raises a huge problem. We are no longer computing $\mathbf{H}$, but $\Delta\mathbf{H}$, the change of $\mathbf{H}$. If we passed this into our Kalman filter without altering the rest of the design the output would be nonsense. Recall, for example, that we multiply $\mathbf{Hx}$ to generate the measurements that would result from the given estimate of $\mathbf{x}$ But now that $\mathbf{H}$ is linearized around our position it contains the *change* in the measurement function.

We are forced, therefore, to use the *change* in $\mathbf{x}$ for our state variables. So we have to go back and redesign our state variables.

>Please note this is a completely normal occurrence in designing Kalman filters. The textbooks present examples like this as *fait accompli*, as if it is trivially obvious that the state variables needed to be velocities, not positions. Perhaps once you do enough of these problems it would be trivially obvious, but at that point why are you reading a textbook? I find myself reading through a presentation multiple times, trying to figure out why they made a choice, finally to realize that it is because of the consequences of something on the next page. My presentation is longer, but it reflects what actually happens when you design a filter. You make what seem reasonable design choices, and as you move forward you discover properties that require you to recast your earlier steps. As a result, I am going to somewhat abandon my **step 1**, **step 2**, etc.,  approach, since so many real problems are not quite that straightforward.

If our state variables contain the velocities of the robot and not the position then how do we track where the robot is? We can't. Kalman filters that are linearized in this fashion use what is called a *nominal trajectory* - i.e. you assume a position and track direction, and then apply the changes in velocity and acceleration to compute the changes in that trajectory. How could it be otherwise? Recall the graphic showing the intersection of the two range circles - there are two areas of intersection. Think of what this would look like if the two transmitters were very close to each other - the intersections would be two very long crescent shapes. This Kalman filter, as designed, has no way of knowing your true position from only distance measurements to the transmitters. Perhaps your mind is already leaping to ways of working around this problem. If so, stay engaged, as later sections and chapters will provide you with these techniques. Presenting the full solution all at once leads to more confusion than insight, in my opinion.

So let's redesign our *state transition function*. We are assuming constant velocity and no acceleration, giving state equations of
$$
\dot{x}' = \dot{x} \\
\ddot{x}' = 0 \\
\dot{y}' = \dot{y} \\
\dot{y}' = 0$$

This gives us the the *state transition function* of

$$
\mathbf{F} = \begin{bmatrix}0 &1 & 0& 0\\0& 0& 0& 0\\0& 0& 0& 1\\ 0& 0& 0& 0\end{bmatrix}$$

A final complication comes from the measurements that we pass in. $\mathbf{Hx}$ is now computing the *change* in the measurement from our nominal position, so the measurement that we pass in needs to be not the range to A and B, but the *change* in range from our measured range to our nominal position.

There is a lot here to take in, so let's work through the code bit by bit. First we will define a function to compute $\frac{\partial\mathbf{h}}{\partial\mathbf{x}}$ for each time step.

```python
from math import sin, cos, atan2

def H_of(pos, pos_A, pos_B):
    """ Given the position of our object at 'pos' in 2D, and two
    transmitters A and B at positions 'pos_A' and 'pos_B', return
    the partial derivative of H
    """

    theta_a = atan2(pos_a[1] - pos[1], pos_a[0] - pos[0])
    theta_b = atan2(pos_b[1] - pos[1], pos_b[0] - pos[0])

    return np.array([[0, -cos(theta_a), 0, -sin(theta_a)],
                     [0, -cos(theta_b), 0, -sin(theta_b)]])
```

Now we need to create our simulated sensor.

```python
from numpy.random import randn

class DMESensor(object):
    def __init__(self, pos_a, pos_b, noise_factor=1.0):
        self.A = pos_a
        self.B = pos_b
        self.noise_factor = noise_factor

    def range_of(self, pos):
        """ returns tuple containing noisy range data to A and B
        given a position 'pos'
        """

        ra = math.sqrt((self.A[0] - pos[0])**2 + (self.A[1] - pos[1])**2)
        rb = math.sqrt((self.B[0] - pos[0])**2 + (self.B[1] - pos[1])**2)

        return (ra + randn()*self.noise_factor,
                rb + randn()*self.noise_factor)
```

Finally, we are ready for the Kalman filter code. I will position the transmitters at x=-100 and 100, both with y=-20. This gives me enough space to get good triangulation from both as the robot moves. I will start the robot at (0,0) and move by (1,1) each time step.

```python
import math
from filterpy.kalman import KalmanFilter
import numpy as np
import book_plots as bp

pos_a = (100, -20)
pos_b = (-100, -20)

f1 = KalmanFilter(dim_x=4, dim_z=2)

f1.F = np.array ([[0, 1, 0, 0],
                  [0, 0, 0, 0],
                  [0, 0, 0, 1],
                  [0, 0, 0, 0]], dtype=float)

f1.R *= 1.
f1.Q *= .1

f1.x = np.array([[1, 0, 1, 0]], dtype=float).T
f1.P = np.eye(4) * 5.

# initialize storage and other variables for the run
count = 30
xs, ys = [], []
pxs, pys = [], []

# create the simulated sensor
d = DMESensor(pos_a, pos_b, noise_factor=3.)

# pos will contain our nominal position since the filter does not
# maintain position.
pos = [0, 0]

for i in range(count):
    # move (1,1) each step, so just use i
    pos = [i, i]

    # compute the difference in range between the nominal track
    # and measured ranges
    ra,rb = d.range_of(pos)
    rx,ry = d.range_of((pos[0] + f1.x[0, 0], pos[1] + f1.x[2, 0]))
    z = np.array([[ra - rx], [rb - ry]])

    # compute linearized H for this time step
    f1.H = H_of (pos, pos_a, pos_b)

    # store stuff so we can plot it later
    xs.append(f1.x[0, 0]+i)
    ys.append(f1.x[2, 0]+i)
    pxs.append(pos[0])
    pys.append(pos[1])

    # perform the Kalman filter steps
    f1.predict()
    f1.update(z)

bp.plot_filter(xs, ys)
bp.plot_track(pxs, pys)
plt.legend(loc=2)
plt.show()
```

## Linearizing the Kalman Filter

Now that we have seen an example of linearizing the Kalman filter we are in a position to better understand the math.

We start by assuming some function $\mathbf f$

## Example: A falling Ball

**author's note: ignore this section for now. **

In the **Designing Kalman Filters** chapter I first considered tracking a ball in a vacuum, and then in the atmosphere. The Kalman filter performed very well for vacuum, but diverged from the ball's path in the atmosphere. Let us look at the output; to avoid littering this chapter with code from that chapter I have placed it all in the file `ekf_internal.py'.

```python
import ekf_internal
ekf_internal.plot_ball()
```

We can artificially force the Kalman filter to track the ball by making $Q$ large. That would cause the filter to mistrust its prediction, and scale the kalman gain $K$ to strongly favor the measurments. However, this is not a valid approach. If the Kalman filter is correctly predicting the process we should not 'lie' to the filter by telling it there are process errors that do not exist. We may get away with that for some problems, in some conditions, but in general the Kalman filter's performance will be substandard.

Recall from the **Designing Kalman Filters** chapter that the acceleration is

$$a_x = (0.0039 + \frac{0.0058}{1+\exp{[(v-35)/5]}})*v*v_x \\
a_y = (0.0039 + \frac{0.0058}{1+\exp{[(v-35)/5]}})*v*v_y- g
$$

These equations will be *very* unpleasant to work with while we develop this subject, so for now I will retreat to a simpler one dimensional problem using this simplified equation for acceleration that does not take the nonlinearity of the drag coefficient into account:

$$\begin{aligned}
\ddot{y} &= \frac{0.0034ge^{-y/20000}\dot{y}^2}{2\beta} - g \\
\ddot{x} &= \frac{0.0034ge^{-x/20000}\dot{x}^2}{2\beta}
\end{aligned}$$

Here $\beta$ is the ballistic coefficient, where a high number indicates a low drag.

This is still nonlinear, so we need to linearize this equation at the current state point. If our state is position and velocity, we need an equation for some arbitrarily small change in $\mathbf{x}$, like so:

$$ \begin{bmatrix}\Delta \dot{x} \\ \Delta \ddot{x} \\ \Delta \dot{y} \\ \Delta \ddot{y}\end{bmatrix} =
\large\begin{bmatrix}
\frac{\partial \dot{x}}{\partial x} &
\frac{\partial \dot{x}}{\partial \dot{x}} &
\frac{\partial \dot{x}}{\partial y} &
\frac{\partial \dot{x}}{\partial \dot{y}} \\
\frac{\partial \ddot{x}}{\partial x} &
\frac{\partial \ddot{x}}{\partial \dot{x}}&
\frac{\partial \ddot{x}}{\partial y}&
\frac{\partial \dot{x}}{\partial \dot{y}}\\
\frac{\partial \dot{y}}{\partial x} &
\frac{\partial \dot{y}}{\partial \dot{x}} &
\frac{\partial \dot{y}}{\partial y} &
\frac{\partial \dot{y}}{\partial \dot{y}} \\
\frac{\partial \ddot{y}}{\partial x} &
\frac{\partial \ddot{y}}{\partial \dot{x}}&
\frac{\partial \ddot{y}}{\partial y}&
\frac{\partial \dot{y}}{\partial \dot{y}}
\end{bmatrix}\normalsize
\begin{bmatrix}\Delta x \\ \Delta \dot{x} \\ \Delta \dot{y} \\ \Delta \ddot{y}\end{bmatrix}$$

The equations do not contain both an x and a y, so any partial derivative with both in it must be equal to zero. We also know that $\large\frac{\partial \dot{x}}{\partial x}\normalsize = 0$ and that  $\large\frac{\partial \dot{x}}{\partial \dot{x}}\normalsize = 1$, so our matrix ends up being

$$\mathbf{F} = \begin{bmatrix}0&1&0&0 \\
\frac{0.0034e^{-x/22000}\dot{x}^2g}{44000\beta}&0&0&0
\end{bmatrix}$$

$$\begin{aligned}\ddot{x} &= -\frac{1}{2}C_d\rho A \dot{x}\\
\ddot{y} &= -\frac{1}{2}C_d\rho A \dot{y}-g\end{aligned}$$

```python
from sympy.abc import *
from sympy import *

init_printing(pretty_print=True, use_latex='mathjax')

x1 = (0.0034*g*exp(-x/22000)*((x)**2))/(2*b) - g

x2 = (a*g*exp(-x/c)*(Derivative(x)**2))/(2*b) - g

#pprint(x1)
#pprint(Derivative(x)*Derivative(x,n=2))
#pprint(diff(x2, x))
```

** orphan text
This approach has many issues. First, of course, is the fact that the linearization does not produce an exact answer. More importantly, we are not linearizing the actual path, but our filter's estimation of the path. We linearize the estimation because it is statistically likely to be correct; but of course it is not required to be. So if the filter's output is bad that will cause us to linearize an incorrect estimate, which will almost certainly lead to an even worse estimate. In these cases the filter will quickly diverge. This is where the 'black art' of Kalman filter comes in. We are trying to linearize an estimate, and there is no guarantee that the filter will be stable. A vast amount of the literature on Kalman filters is devoted to this problem. Another issue is that we need to linearize the system using analytic methods. It may be difficult or impossible to find an analytic solution to some problems. In other cases we may be able to find the linearization, but the computation is very expensive. **

## References

[1] http://sympy.org