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

# Kalman Filter Math

```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>

If you've gotten this far I hope that you are thinking that the Kalman filter's fearsome reputation is somewhat undeserved. Sure, I hand waved some equations away, but I hope implementation has been fairly straightforward for you. The underlying concept is quite straightforward - take two measurements, or a measurement and a prediction, and choose the output to be somewhere between the two. If you believe the measurement more your guess will be closer to the measurement, and if you believe the prediction is more accurate your guess will lie closer it it. That's not rocket science (little joke - it is exactly this math that got Apollo to the moon and back!).

To be honest I have been choosing my problems carefully. For an arbitrary problem designing the Kalman filter matrices can be extremely difficult. I haven't been *too tricky*, though. Equations like Newton's equations of motion can be trivially computed for Kalman filter applications, and they make up the bulk of the kind of problems that we want to solve.

I have illustrated the concepts with code and reasoning, not math. But there are topics that do require more mathematics than I have used so far. This chapter presents the math that you will need for the rest of the book.

## Modeling a Dynamic System

A *dynamic system* is a physical system whose state (position, temperature, etc) evolves over time. Calculus is the math of changing values, so we use differential equations to model dynamic systems. Some systems cannot be modeled with differential equations, but we will not encounter those in this book.

Modeling dynamic systems is properly the topic of several college courses. To an extent there is no substitute for a few semesters of ordinary and partial differential equations followed by a graduate course in control system theory. If you are a hobbyist, or trying to solve one very specific filtering problem at work you probably do not have the time and/or inclination to devote a year or more to that education.

Fortunately, I can present enough of the theory to allow us to create the system equations for many different Kalman filters. My goal is to get you to the stage where you can read a publication and understand it well enough to implement the algorithms. The background math is deep, but in practice we end up using a few simple techniques over and over again.

This is the longest section of pure math in this book. You will need to master everything in this section to understand the Extended Kalman filter (EKF), the workhorse nonlinear filter. I do cover more modern filters that do not require as much of this math. You can choose so skim now, and come back to this if you decide to learn the EKF.

We need to start by understanding the underlying equations and assumptions that the Kalman filter uses. We are trying to model real world phenomena, so what do we have to consider?

Each physical system has a process. For example, a car traveling at a certain velocity goes so far in a fixed amount of time, and its velocity varies as a function of its acceleration. We describe that behavior with the well known Newtonian equations that we learned in high school.

$$
\begin{aligned}
v&=at\\
x &= \frac{1}{2}at^2 + v_0t + x_0
\end{aligned}
$$

Once we learned calculus we saw them in this form:

$$ \mathbf v = \frac{d \mathbf x}{d t},
\quad \mathbf a = \frac{d \mathbf v}{d t} = \frac{d^2 \mathbf x}{d t^2}
$$

A typical automobile tracking problem would have you compute the distance traveled given a constant velocity or acceleration as we did in previous chapters. But, of course we know this is not all that is happening. No car travels on a perfect road. There are bumps that cause the car to slow down, there is wind drag, there are hills that raise and lower the speed. The suspension is a mechanical system with friction and imperfect springs. Gusts of wind alter the car's state.

Acurately modeling a system with linear equations is impossible except for the most trivial problems. So control theory is forced to make a simplification. At any time $t$ we say that the true state (such as the position of our car) is the predicted value from the imperfect model plus some unknown *process noise*:

$$
x(t) = x_{pred}(t) + noise(t)
$$

This is not meant to imply that $noise(t)$ is a function that we can derive analytically. It is merely a statement of fact - we can always describe the true value as the predicted value  plus the process noise. "Noise" does not imply random events. If we are tracking a thrown ball in the atmosphere, and our model assumes the ball is in a vacuum, then the effect of air drag is process noise in this context.

In the next section we will learn techniques to convert a set of differential equations into a set of first-order differential equations.  Assuming that, we can say that our model of the system without noise is:

$$ \dot{\mathbf x} = \mathbf{Ax}$$

$\mathbf A$ is known as the *systems dynamics matrix* as it describes the dynamics of the system. Now we need to model the noise. We will call that $\mathbf w$, and add it to the equation.

$$ \dot{\mathbf x} = \mathbf{Ax} + \mathbf w$$

$\mathbf w$ may strike you as a poor choice for the name, but you will soon see that the Kalman filter assumes *white* noise.

Finally, we need to consider any inputs into the system. We assume an input $\mathbf u$, and that there exists a linear model that defines how that input changes the system. For example, pressing the accelerator in your car makes it accelerate, and gravity causes balls to fall. Both are contol inputs. We will need a matrix $\mathbf B$ to convert $u$ into the effect on the system. We add that into our equation:

$$ \dot{\mathbf x} = \mathbf{Ax} + \mathbf{Bu} + \mathbf{w}$$

And that's it. That is one of the equations that Dr. Kalman set out to solve, and he found an optimal esitmator if we assume certain properties of $\mathbf w$.

## State-Space Representation of Dynamic Systems

In the last section we derived the equation

$$ \dot{\mathbf x} = \mathbf{Ax}+ \mathbf{Bu} + \mathbf{w}$$.

However, for our filters we are not interested in the derivative of $\mathbf x$, but in $\mathbf x$ itself. Ignoring the noise for a moment, we want an equation that recusively finds the value of $\mathbf x$ at time $t_k$ in terms of $\mathbf x$ at time $t_{k-1}$:

$$\mathbf x(t_k) = \mathbf F(\Delta t)\mathbf x(t_{k-1}) + \mathbf B(t_k) + \mathbf u (t_k)$$

Convention allows us to write $\mathbf x(t_k)$ as $\mathbf x_k$, which means the
the value of $\mathbf x$ at the k$^{th}$ value of $t$.

$$\mathbf x_k = \mathbf{Fx}_{k-1} + \mathbf B_k\mathbf u_k$$

$\mathbf F$ is the familiar *state transition matrix*, named due to its ability to transition the state from the previous time step to the current time step. It is very similar to the system dynamics matrix $\mathbf A$. The difference is that the system dynamics matrix $\mathbf A$ models a set of linear differential equations, and is continuous. $\mathbf F$ is discrete, and represents a set of linear equations (not differential equations) which step transition $\mathbf x_{k-1}$ to $\mathbf x_k$ over a discrete time step $\Delta t$.

Normally finding this equation is quite difficult. The equation $\dot x = v$ is the simplest possible differential equation and we trivially integrate it as:

$$ \int\limits_{x_{k-1}}^{x_k}  \mathrm{d}x = \int\limits_{0}^{\Delta t} v\, \mathrm{d}t \\
x_k-x_0 = v \Delta t \\
x_k = v \Delta t + x_0$$

This equation is *recursive*: we compute the value of $x$ at time $t$ based on its value at time $t-1$. This recursive form enables us to represent the system (process model) in the form required by the Kalman filter:

$$\begin{aligned}
\mathbf x_k &= \mathbf{Fx}_{k-1}  \\
&= \begin{bmatrix} 1 & \Delta t \\ 0 & 1\end{bmatrix}
\begin{bmatrix}x_{k-1} \\ \dot x_{k-1}\end{bmatrix}
\end{aligned}$$

We can do that only because $\dot x = v$ is simplest differential equation possible. Almost all other in physical systems result in more complicated differential equation which do not yield to this approach.

*State-space* methods became popular around the time of the Apollo missions, largely due to the work of Dr. Kalman. The idea is simple. Model a system with a set of  $n^{th}$-order differential equations. Convert them into an equivalent set of first-order differential equations. Put them into the vector-matrix form used in the previous section: $\dot{\mathbf x} = \mathbf{Ax} + \mathbf{Bu}$. Once in this form we use of of several techniques to convert these linear differential equations into the recursive equation:

$$ \mathbf x_k = \mathbf{Fx}_{k-1} + \mathbf B_k\mathbf u_k$$

Some books call the state transition matrix the *fundamental matrix*. Many use $\mathbf \Phi$ instead of $\mathbf F$. Sources based heavily on control theory tend to use these forms.

These are called *state-space* methods because we are expressing the solution of the differential equations in terms of the system state.

### Forming First Order Equations from Higher Order Equations

Many models of physical systems require second or higher order differential equations with control input $u$:

$$a_n \frac{d^ny}{dt^n} + a_{n-1} \frac{d^{n-1}y}{dt^{n-1}} +  \dots + a_2 \frac{d^2y}{dt^2} + a_1 \frac{dy}{dt} + a_0 = u$$

State-space methods require first-order equations. Any higher order system of equations can be reduced to first-order by defining extra variables for the derivatives and then solving.

Let's do an example. Given the system $\ddot{x} - 6\dot x + 9x = t$ find the equivalent first order equations. I've used the dot notation for the time derivatives for clarity.

The first step is to isolate the highest order term onto one side of the equation.

$$\ddot{x} = 6\dot x - 9x + t$$

We define two new variables:

$$ x_1(t) = x \\
x_2(t) = \dot x
$$

Now we will substitute these into the original equation and solve. The solution yields a set of first-order equations in terms of these new variables. It is conventional to drop the $(t)$ for notational convenience.

We know that $\dot x_1 = x_2$ and that $\dot x_2 = \ddot{x}$. Therefore

$$\begin{aligned}
\dot x_2 &= \ddot{x} \\
          &= 6\dot x - 9x + t\\
          &= 6x_2-9x_1 + t
\end{aligned}$$

Therefore our first-order system of equations is

$$\begin{aligned}\dot x_1 &= x_2 \\
\dot x_2 &= 6x_2-9x_1 + t\end{aligned}$$

If you practice this a bit you will become adept at it. Isolate the highest term, define a new variable and its derivatives, and then substitute.

### First Order Differential Equations In State-Space Form

Substituting the newly defined variables from the previous section:

$$\frac{dx_1}{dt} = x_2,\,
\frac{dx_2}{dt} = x_3, \, ..., \,
\frac{dx_{n-1}}{dt} = x_n$$

into the first order equations yields:

$$\frac{dx_n}{dt} = \frac{1}{a_n}\sum\limits_{i=0}^{n-1}a_ix_{i+1} + \frac{1}{a_n}u
$$

Using vector-matrix notation we have:

$$\begin{bmatrix}\frac{dx_1}{dt} \\ \frac{dx_2}{dt} \\ \vdots \\ \frac{dx_n}{dt}\end{bmatrix} =
\begin{bmatrix}\dot x_1 \\ \dot x_2 \\ \vdots \\ \dot x_n\end{bmatrix}=
\begin{bmatrix}0 & 1 & 0 &\cdots & 0 \\
0 & 0 & 1 & \cdots & 0 \\
\vdots & \vdots & \vdots & \ddots & \vdots \\
-\frac{a_0}{a_n} & -\frac{a_1}{a_n} & -\frac{a_2}{a_n} & \cdots & -\frac{a_{n-1}}{a_n}\end{bmatrix}
\begin{bmatrix}x_1 \\ x_2 \\ \vdots \\ x_n\end{bmatrix} +
\begin{bmatrix}0 \\ 0 \\ \vdots \\ \frac{1}{a_n}\end{bmatrix}u$$

which we then write as $\dot{\mathbf x} = \mathbf{Ax} + \mathbf{B}u$.

### Finding the Fundamental Matrix for Time Invariant Systems

We express the system equations in state-space form with

$$ \dot{\mathbf x} = \mathbf{Ax}$$

where $\mathbf A$ is the system dynamics matrix, and want to find the *fundamental matrix* $\mathbf F$ that propagates the state $\mathbf x$ over the interval $\Delta t$ with the equation

$$\begin{aligned}
\mathbf x(t_k) = \mathbf F(\Delta t)\mathbf x(t_{k-1})\end{aligned}$$

In other words, $\mathbf A$ is a set of continuous differential equations, and we need $\mathbf F$ to be a set of discrete linear equations that computes the change in $\mathbf A$ over a discrete time step.

It is conventional to drop the $t_k$ and $(\Delta t)$ and use the notation

$$\mathbf x_k = \mathbf {Fx}_{k-1}$$

$\mathbf x_k$ does not mean the k$^{th}$ value of $\mathbf x$, but the value of $\mathbf x$ at the k$^{th}$ value of $t$. In this book I go even further and drop the suffixes entirely in favor of an overline to denote the prediction: $\overline{\mathbf x} = \mathbf{Fx}$.

Broadly speaking there are three common ways to find this matrix for Kalman filters. The technique most often used with Kalman filters is to use a matrix exponential. Linear Time Invariant Theory, also known as LTI System Theory, is a second technique. Finally, there are numerical techniques. You may know of others, but these three are what you will most likely encounter in the Kalman filter literature and praxis.

### The Matrix Exponential

The solution to the equation $\frac{dx}{dt} = kx$ can be found by:

$$\frac{dx}{dt} = kx \\
\frac{dx}{x} = k\, dt \\
\int \frac{1}{x}\, dx = \int k\, dt \\
\log x = kt + c \\
x = e^{kt+c} \\
x = e^ce^{kt} \\
x = c_0e^{kt}$$

Using similar math, the solution to the first-order equation

$$\dot{\mathbf x} = \mathbf{Ax} ,\, \, \, \mathbf x(0) = \mathbf x_0$$

where $\mathbf A$ is a constant matrix, is

$$\mathbf x = e^{\mathbf At}\mathbf x_0$$

Substituting $F = e^{\mathbf At}$, we can write

$$\mathbf x_k = \mathbf F\mathbf x_{k-1}$$

which is the form we are looking for! We have reduced the problem of finding the fundamental matrix to one of finding the value for $e^{\mathbf At}$.

$e^{\mathbf At}$ is known as the matrix exponential. It can be computed with this power series:

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

That series is found by doing a Taylor series expansion of $e^{\mathbf At}$, which I will not cover here.

Let's use this to find the solution to Newton's equations. Using $v$ as an substitution for $\dot x$, and assuming constant velocity we get the linear matrix-vector form

$$\begin{bmatrix}\dot x \\ \dot v\end{bmatrix} =\begin{bmatrix}0&1\\0&0\end{bmatrix} \begin{bmatrix}x \\ v\end{bmatrix}$$

This is a first order differential equation, so we can set $\mathbf{A}=\begin{bmatrix}0&1\\0&0\end{bmatrix}$ and solve the following equation. I have substituted the interval $\Delta t$ for $t$ to emphasize that the fundamental matrix is discrete:

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

If you perform the multiplication you will find that $\mathbf{A}^2=\begin{bmatrix}0&0\\0&0\end{bmatrix}$, which means that all higher powers of $\mathbf{A}$ are also $\mathbf{0}$. Thus we get an exact answer without an infinite number of terms:

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

We plug this into $\mathbf x_k= \mathbf{Fx}_{k-1}$ to get

$$
\begin{aligned}
x_k &=\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}x_{k-1}
\end{aligned}$$

You will recognize this as the matrix we derived analytically for the constant velocity Kalman filter in the **Multivariate Kalman Filter** chapter.

### Time Invariance

If the behavior of the system depends on time we can say that a dynamic system is described by the first-order differential equation

$$ g(t) = \dot x$$

However, if the system is *time invariant* the equation is of the form:

$$ f(x) = \dot x$$

What does *time invariant* mean? Consider a home stereo. If you input a signal $x$ into it at time $t$, it will output some signal $f(x)$. If you instead perform the input at time $t + \Delta t$ the output signal will be the same $f(x)$, shifted in time.

A counter-example is $x(t) = \sin(t)$, with the system  $f(x) = t\,  x(t) = t \sin(t)$. This is not time invariant; the value will be different at different times due to the multiplication by t.  An aircraft is not time invariant. If you make a control input to the aircraft at a later time its behavior will be different because it will have burned fuel and thus lost weight. Lower weight results in different behavior.

We can solve these equations by integrating each side. I demonstrated integrating the time invariants system $v = \dot x$ above. However, integrating the time invariant equation $\dot x = f(x)$ is not so straightforward. Using the *separation of variables* techniques we divide by $f(x)$ and move the $dt$ term to the right so we can integrate each side:

$$
\frac{dx}{dt} = f(x) \\
\int^x_{x_0} \frac{1}{f(x)} dx = \int^t_{t_0} dt\\
$$

If we let $F(x) = \int \frac{1}{f(x)} dx$ we get

$$F(x) - F(x_0) = t-t_0$$

We then solve for x with

$$F(x) = t - t_0 + F(x_0) \\
x = F^{-1}[t-t_0 + F(x_0)]$$

In other words, we need to find the inverse of $F$. This is not trivial, and a significant amount of coursework in a STEM education is devoted to finding tricky, analytic solutions to this problem.

However, they are tricks, and many simple forms of $f(x)$ either have no closed form solution or pose extreme difficulties. Instead, the practicing engineer turns to state-space methods to find approximate solutions.

The advantage of the matrix exponential is that we can use it for any arbitrary set of differential equations which are *time invariant*. However, we often use this technique even when the equations are not time invariant. As an aircraft flies it burns fuel and loses weight. However, the weight loss over one second is negligible, and so the system is nearly linear over that time step. Our answers will still be reasonably accurate so long as the time step is short.

#### Example: Mass-Spring-Damper Model

Suppose we wanted to track the motion of a weight on a spring and connected to a damper, such as an automobile's suspension. The equation for the motion with $m$ being the mass, $k$ the spring constant, and $c$ the damping force, under some input $u$ is

$$m\frac{d^2x}{dt^2} + c\frac{dx}{dt} +kx = u$$

For notational convenience I will write that as

$$m\ddot x + c\dot x + kx = u$$

I can turn this into a system of first order equations by setting $x_1(t)=x(t)$, and then substituting as follows:

$$\begin{aligned}
x_1 &= x \\
x_2 &= \dot x_1 \\
\dot x_2 &= \dot x_1 = \ddot x
\end{aligned}$$

As is common I dropped the $(t)$ for notational convenience. This gives the equation

$$m\dot x_2 + c x_2 +kx_1 = u$$

Solving for $\dot x_2$ we get a first order equation:

$$\dot x_2 = -\frac{c}{m}x_2 - \frac{k}{m}x_1 + \frac{1}{m}u$$

We put this into matrix form:

$$\begin{bmatrix} \dot x_1 \\ \dot x_2 \end{bmatrix} =
\begin{bmatrix}0 & 1 \\ -k/m & -c/m \end{bmatrix}
\begin{bmatrix} x_1 \\ x_2 \end{bmatrix} +
\begin{bmatrix} 0 \\ 1/m \end{bmatrix}u$$

Now we use the matrix exponential to find the state transition matrix:

$$\Phi(t) = e^{\mathbf At} = \mathbf{I} + \mathbf At  + \frac{(\mathbf At)^2}{2!} + \frac{(\mathbf At)^3}{3!} + ... $$

The first two terms give us

$$\mathbf F = \begin{bmatrix}1 & t \\ -(k/m) t & 1-(c/m) t \end{bmatrix}$$

This may or may not give you enough precision. You can easily check this by computing $\frac{(\mathbf At)^2}{2!}$ for your constants and seeing how much this matrix contributes to the results.

### Linear Time Invariant Theory

*Linear Time Invariant Theory*, also known as LTI System Theory, gives us a way to find $\Phi$ using the inverse Laplace transform. You are either nodding your head now, or completely lost. Don't worry, I will not be using the Laplace transform in this book except in this paragraph, as the computation can be quite difficult. LTI system theory tells us that

$$ \Phi(t) = \mathcal{L}^{-1}[(s\mathbf{I} - \mathbf{F})^{-1}]$$

I have no intention of going into this other than to say that the Laplace transform $\mathcal{L}$ converts a signal into a space $s$ that excludes time, but finding a solution to the equation above is non-trivial. If you are interested, the Wikipedia article on LTI system theory provides an introduction [2]. I mention LTI because you will find some literature using it to design the Kalman filter matrices for difficult problems.

### Numerical Solutions

Finally, there are numerous numerical techniques to find $\mathbf F$. As filters get larger finding analytical solutions becomes very tedious (though packages like SymPy make it easier). C. F. van Loan [3] has developed a technique that finds both $\Phi$ and $\mathbf Q$ numerically. Given the continuous model

$$ \dot x = Ax + Gw$$

where $w$ is the unity white noise, van Loan's method computes both $\mathbf F_k$ and $\mathbf Q_k$.

I have implemented van Loan's method in `FilterPy`. You may use it as follows:

```python
from filterpy.common import van_loan_discretization

A = np.array([[0., 1.], [-1., 0.]])
G = np.array([[0.], [2.]]) # white noise scaling
F, Q = van_loan_discretization(A, G, dt=0.1)
```

In the section *Numeric Integration of Differential Equations* I present alternative methods which are very commonly used in Kalman filtering.

## Design of the Process Noise Matrix

In general the design of the $\mathbf Q$ matrix is among the most difficult aspects of Kalman filter design. This is due to several factors. First, the math requires a good foundation in signal theory. Second, we are trying to model the noise in something for which we have little information. Consider trying to model the process noise for a thrown baseball. We can model it as a sphere moving through the air, but that leave many unknown factors - the wind, ball rotation and spin decay, the coefficient of drag of a ball with stitches, the effects of wind and air density, and so on. We develop the equations for an exact mathematical solution for a given process model, but since the process model is incomplete the result for $\mathbf Q$ will also be incomplete. This has a lot of ramifications for the behavior of the Kalman filter. If $\mathbf Q$ is too small then the filter will be overconfident in its prediction model and will diverge from the actual solution. If $\mathbf Q$ is too large than the filter will be unduly influenced by the noise in the measurements and perform sub-optimally. In practice we spend a lot of time running simulations and evaluating collected data to try to select an appropriate value for $\mathbf Q$. But let's start by looking at the math.

Let's assume a kinematic system - some system that can be modeled using Newton's equations of motion. We can make a few different assumptions about this process.

We have been using a process model of

$$ \dot{\mathbf x} = \mathbf{Ax} + \mathbf{Bu} + \mathbf{w}$$

where $\mathbf{w}$ is the process noise. Kinematic systems are *continuous* - their inputs and outputs can vary at any arbitrary point in time. However, our Kalman filters are *discrete*. We sample the system at regular intervals. Therefore we must find the discrete representation for the noise term in the equation above. This depends on what assumptions we make about the behavior of the noise. We will consider two different models for the noise.

### Continuous White Noise Model

We model kinematic systems using Newton's equations. We have either used position and velocity, or position, velocity, and acceleration as the models for our systems. There is nothing stopping us from going further - we can model jerk, jounce, snap, and so on. We don't do that normally because adding terms beyond the dynamics of the real system degrades the estimate.

Let's say that we need to model the position, velocity, and acceleration. We can then assume that acceleration is constant for each discrete time step. Of course, there is process noise in the system and so the acceleration is not actually constant. The tracked object will alter the acceleration over time due to external, unmodeled forces. In this section we will assume that the acceleration changes by a continuous time zero-mean white noise $w(t)$. In other words, we are assuming that velocity is acceleration changing by small amounts that over time average to 0 (zero-mean).

Since the noise is changing continuously we will need to integrate to get the discrete noise for the discretization interval that we have chosen. We will not prove it here, but the equation for the discretization of the noise is

$$\mathbf Q = \int_0^{\Delta t} \mathbf F(t)\mathbf{Q_c}\mathbf F^\mathsf{T}(t) dt$$

where $\mathbf{Q_c}$ is the continuous noise. This gives us

$$\Phi = \begin{bmatrix}1 & \Delta t & {\Delta t}^2/2 \\ 0 & 1 & \Delta t\\ 0& 0& 1\end{bmatrix}$$

for the fundamental matrix, and

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

for the continuous process noise matrix, where $\Phi_s$ is the spectral density of the white noise.

We could carry out these computations ourselves, but I prefer using SymPy to solve the equation.

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

```python
import sympy
from sympy import (init_printing, Matrix,MatMul,
                   integrate, symbols)

init_printing(use_latex='mathjax')
dt, phi = symbols('\Delta{t} \Phi_s')
F_k = Matrix([[1, dt, dt**2/2],
              [0,  1,      dt],
              [0,  0,       1]])
Q_c = Matrix([[0, 0, 0],
              [0, 0, 0],
              [0, 0, 1]])*phi

Q=sympy.integrate(F_k * Q_c * F_k.T, (dt, 0, dt))

# factor phi out of the matrix to make it more readable
Q = Q / phi
sympy.MatMul(Q, phi)
```

$$\left[\begin{matrix}\frac{\Delta{t}^{5}}{20} & \frac{\Delta{t}^{4}}{8} & \frac{\Delta{t}^{3}}{6}\\\frac{\Delta{t}^{4}}{8} & \frac{\Delta{t}^{3}}{3} & \frac{\Delta{t}^{2}}{2}\\\frac{\Delta{t}^{3}}{6} & \frac{\Delta{t}^{2}}{2} & \Delta{t}\end{matrix}\right] \Phi_{s}$$

For completeness, let us compute the equations for the 0th order and 1st order equations.

```python
F_k = sympy.Matrix([[1]])
Q_c = sympy.Matrix([[phi]])

print('0th order discrete process noise')
sympy.integrate(F_k*Q_c*F_k.T,(dt, 0, dt))
```

    0th order discrete process noise

$$\left[\begin{matrix}\Delta{t} \Phi_{s}\end{matrix}\right]$$

```python
F_k = sympy.Matrix([[1, dt],
                    [0, 1]])
Q_c = sympy.Matrix([[0, 0],
                    [0, 1]])*phi

Q = sympy.integrate(F_k * Q_c * F_k.T, (dt, 0, dt))

print('1st order discrete process noise')
# factor phi out of the matrix to make it more readable
Q = Q / phi
sympy.MatMul(Q, phi)
```

    1st order discrete process noise

$$\left[\begin{matrix}\frac{\Delta{t}^{3}}{3} & \frac{\Delta{t}^{2}}{2}\\\frac{\Delta{t}^{2}}{2} & \Delta{t}\end{matrix}\right] \Phi_{s}$$

### Piecewise White Noise Model

Another model for the noise assumes that the that highest order term (say, acceleration) is constant for the duration of each time period, but differs for each time period, and each of these is uncorrelated between time periods. In other words there is a discontinuous jump in acceleration at each time step. This is subtly different than the model above, where we assumed that the last term had a continuously varying noisy signal applied to it.

We will model this as

$$f(x)=Fx+\Gamma w$$

where $\Gamma$ is the *noise gain* of the system, and $w$ is the constant piecewise acceleration (or velocity, or jerk, etc).

Lets start by looking at a first order system. In this case we have the state transition function

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

In one time period, the change in velocity will be $w(t)\Delta t$, and the change in position will be $w(t)\Delta t^2/2$, giving us

$$\Gamma = \begin{bmatrix}\frac{1}{2}\Delta t^2 \\ \Delta t\end{bmatrix}$$

The covariance of the process noise is then

$$Q = \mathbb E[\Gamma w(t) w(t) \Gamma^\mathsf{T}] = \Gamma\sigma^2_v\Gamma^\mathsf{T}$$.

We can compute that with SymPy as follows

```python
var=symbols('sigma^2_v')
v = Matrix([[dt**2 / 2], [dt]])

Q = v * var * v.T

# factor variance out of the matrix to make it more readable
Q = Q / var
sympy.MatMul(Q, var)
```

$$\left[\begin{matrix}\frac{\Delta{t}^{4}}{4} & \frac{\Delta{t}^{3}}{2}\\\frac{\Delta{t}^{3}}{2} & \Delta{t}^{2}\end{matrix}\right] \sigma^{2}_{v}$$

The second order system proceeds with the same math.

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

Here we will assume that the white noise is a discrete time Wiener process. This gives us

$$\Gamma = \begin{bmatrix}\frac{1}{2}\Delta t^2 \\ \Delta t\\ 1\end{bmatrix}$$

There is no 'truth' to this model, it is just convenient and provides good results. For example, we could assume that the noise is applied to the jerk at the cost of a more complicated equation.

The covariance of the process noise is then

$$Q = \mathbb E[\Gamma w(t) w(t) \Gamma^\mathsf{T}] = \Gamma\sigma^2_v\Gamma^\mathsf{T}$$.

We can compute that with SymPy as follows

```python
var=symbols('sigma^2_v')
v = Matrix([[dt**2 / 2], [dt], [1]])

Q = v * var * v.T

# factor variance out of the matrix to make it more readable
Q = Q / var
sympy.MatMul(Q, var)
```

$$\left[\begin{matrix}\frac{\Delta{t}^{4}}{4} & \frac{\Delta{t}^{3}}{2} & \frac{\Delta{t}^{2}}{2}\\\frac{\Delta{t}^{3}}{2} & \Delta{t}^{2} & \Delta{t}\\\frac{\Delta{t}^{2}}{2} & \Delta{t} & 1\end{matrix}\right] \sigma^{2}_{v}$$

We cannot say that this model is more or less correct than the continuous model - both are approximations to what is happening to the actual object. Only experience and experiments can guide you to the appropriate model. In practice you will usually find that either model provides reasonable results, but typically one will perform better than the other.

The advantage of the second model is that we can model the noise in terms of $\sigma^2$ which we can describe in terms of the motion and the amount of error we expect. The first model requires us to specify the spectral density, which is not very intuitive, but it handles varying time samples much more easily since the noise is integrated across the time period. However, these are not fixed rules - use whichever model (or a model of your own devising) based on testing how the filter performs and/or your knowledge of the behavior of the physical model.

A good rule of thumb is to set $\sigma$ somewhere from $\frac{1}{2}\Delta a$ to $\Delta a$, where $\Delta a$ is the maximum amount that the acceleration will change between sample periods. In practice we pick a number, run simulations on data, and choose a value that works well.

### Using FilterPy to Compute Q

FilterPy offers several routines to compute the $\mathbf Q$ matrix. The function `Q_continuous_white_noise()` computes $\mathbf Q$ for a given value for $\Delta t$ and the spectral density.

```python
from filterpy.common import Q_continuous_white_noise
from filterpy.common import Q_discrete_white_noise

Q = Q_continuous_white_noise(dim=2, dt=1, spectral_density=1)
print(Q)
```

    [[ 0.333  0.5  ]
     [ 0.5    1.   ]]

```python
Q = Q_continuous_white_noise(dim=3, dt=1, spectral_density=1)
print(Q)
```

    [[ 0.05   0.125  0.167]
     [ 0.125  0.333  0.5  ]
     [ 0.167  0.5    1.   ]]

The function `Q_discrete_white_noise()` computes $\mathbf Q$ assuming a piecewise model for the noise.

```python
Q = Q_discrete_white_noise(2, var=1.)
print(Q)
```

    [[ 0.25  0.5 ]
     [ 0.5   1.  ]]

```python
Q = Q_discrete_white_noise(3, var=1.)
print(Q)
```

    [[ 0.25  0.5   0.5 ]
     [ 0.5   1.    1.  ]
     [ 0.5   1.    1.  ]]

### Simplification of Q

Many treatments use a much simpler form for $\mathbf Q$, setting it to zero except for a noise term in the lower rightmost element. Is this justified? Well, consider the value of $\mathbf Q$ for a small $\Delta t$

```python
import numpy as np

np.set_printoptions(precision=8)
Q = Q_continuous_white_noise(
    dim=3, dt=0.05, spectral_density=1)
print(Q)
np.set_printoptions(precision=3)
```

    [[ 0.00000002  0.00000078  0.00002083]
     [ 0.00000078  0.00004167  0.00125   ]
     [ 0.00002083  0.00125     0.05      ]]

We can see that most of the terms are very small. Recall that the only Kalman filter using this matrix is

$$ \mathbf P=\mathbf{FPF}^\mathsf{T} + \mathbf Q$$

If the values for $\mathbf Q$ are small relative to $\mathbf P$
than it will be contributing almost nothing to the computation of $\mathbf P$. Setting $\mathbf Q$ to the zero matrix except for the lower right term

$$\mathbf Q=\begin{bmatrix}0&0&0\\0&0&0\\0&0&\sigma^2\end{bmatrix}$$

while not correct, is often a useful approximation. If you do this you will have to perform quite a few studies to guarantee that your filter works in a variety of situations.

If you do this, 'lower right term' means the most rapidly changing term for each variable. If the state is $x=\begin{bmatrix}x & \dot x & \ddot{x} & y & \dot{y} & \ddot{y}\end{bmatrix}^\mathsf{T}$ Then Q will be 6x6; the elements for both $\ddot{x}$ and $\ddot{y}$ will have to be set to non-zero in $\mathbf Q$.

## Numeric Integration of Differential Equations

We've been exposed to several numerical techniques to solve linear differential equations. These include state-space methods, the Laplace transform, and van Loan's method.

These work well for linear ordinary differential equations (ODEs), but do not work well for nonlinear equations. For example, consider trying to predict the position of a rapidly turning car. Cars maneuver by turning the front wheels. This makes them pivot around their rear axle as it moves forward. Therefore the path will be continuously varying and a linear prediction will necessarily produce an incorrect value. If the change in the system is small enough relative to $\Delta t$ this can often produce adequate results, but that will rarely be the case with the nonlinear Kalman filters we will be studying in subsequent chapters.

For these reasons we need to know how to numerically integrate ODEs. This can be a vast topic that requires several books. If you need to explore this topic in depth *Computational Physics in Python* by Dr. Eric Ayars is excellent, and available for free here:

http://phys.csuchico.edu/ayars/312/Handouts/comp-phys-python.pdf

However, I will cover a few simple techniques which will work for a majority of the problems you encounter.

### Euler's Method

Let's say we have the initial condition problem of

$$ y' = y, \\ y(0) = 1$$

We happen to know the exact answer is $y=e^t$ because we solved it earlier, but for an arbitrary ODE we will not know the exact solution. In general all we know is the derivative of the equation, which is equal to the slope. We also know the initial value: at $t=0$, $y=1$. If we know these two pieces of information we can predict the value at $y(t=1)$ using the slope at $t=0$ and the value of $y(0)$. I've plotted this below.

```python
import matplotlib.pyplot as plt
t = np.linspace(-1, 1, 10)
plt.plot(t, np.exp(t))
t = np.linspace(-1, 1, 2)
plt.plot(t,t+1, ls='--', c='k');
```

You can see that the slope is very close to the curve at $t=0.1$, but far from it
at $t=1$. But let's continue with a step size of 1 for a moment. We can see that at $t=1$ the estimated value of $y$ is 2. Now we can compute the value at $t=2$ by taking the slope of the curve at $t=1$ and adding it to our initial estimate. The slope is computed with $y'=y$, so the slope is 2.

```python
import book_plots
t = np.linspace(-1, 2, 20)
plt.plot(t, np.exp(t))
t = np.linspace(0, 1, 2)
plt.plot([1, 2, 4], ls='--', c='k')
book_plots.set_labels(x='x', y='y');
```

Here we see the next estimate for y is 4. The errors are getting large quickly, and you might be unimpressed. But 1 is a very large step size. Let's put this algorithm in code, and verify that it works by using a small step size.

```python
def euler(t, tmax, y, dx, step=1.):
    ys = []
    while t < tmax:
        y = y + step*dx(t, y)
        ys.append(y)
        t +=step
    return ys
```

```python
def dx(t, y): return y

print(euler(0, 1, 1, dx, step=1.)[-1])
print(euler(0, 2, 1, dx, step=1.)[-1])
```

    2.0
    4.0

This looks correct. So now lets plot the result of a much smaller step size.

```python
ys = euler(0, 4, 1, dx, step=0.00001)
plt.subplot(1,2,1)
plt.title('Computed')
plt.plot(np.linspace(0, 4, len(ys)),ys)
plt.subplot(1,2,2)
t = np.linspace(0, 4, 20)
plt.title('Exact')
plt.plot(t, np.exp(t));
```

```python
print('exact answer=', np.exp(4))
print('euler answer=', ys[-1])
print('difference =', np.exp(4) - ys[-1])
print('iterations =', len(ys))
```

    exact answer= 54.5981500331
    euler answer= 54.59705808834125
    difference = 0.00109194480299
    iterations = 400000

Here we see that the error is reasonably small, but it took a very large number of iterations to get three digits of precision. In practice Euler's method is too slow for most problems, and we use more sophisticated methods.

Before we go on, let's formally derive Euler's method, as it is the basis for the more advanced Runge Kutta methods used in the next section. In fact, Euler's method is the simplest form of Runge Kutta.

Here are the first 3 terms of the Euler expansion of $y$. An infinite expansion would give an exact answer, so $O(h^4)$ denotes the error due to the finite expansion.

$$y(t_0 + h) = y(t_0) + h y'(t_0) + \frac{1}{2!}h^2 y''(t_0) + \frac{1}{3!}h^3 y'''(t_0) +  O(h^4)$$

Here we can see that Euler's method is using the first two terms of the Taylor expansion. Each subsequent term is smaller than the previous terms, so we are assured that the estimate will not be too far off from the correct value.

### Runge Kutta Methods

Runge Kutta integration is the workhorse of numerical integration. There are a vast number of methods in the literature. In practice, using the Runge Kutta algorithm that I present here will solve most any problem you will face. It offers a very good balance of speed, precision, and stability, and it is the 'go to' numerical integration method unless you have a very good reason to choose something different.

Let's dive in. We start with some differential equation

$$\ddot{y} = \frac{d}{dt}\dot{y}$$.

We can substitute the derivative of y with a function f, like so

$$\ddot{y} = \frac{d}{dt}f(y,t)$$.

Deriving these equations is outside the scope of this book, but the Runge Kutta RK4 method is defined with these equations.

$$y(t+\Delta t) = y(t) + \frac{1}{6}(k_1 + 2k_2 + 2k_3 + k_4) + O(\Delta t^4)$$

$$\begin{aligned}
k_1 &= f(y,t)\Delta t \\
k_2 &= f(y+\frac{1}{2}k_1, t+\frac{1}{2}\Delta t)\Delta t \\
k_3 &= f(y+\frac{1}{2}k_2, t+\frac{1}{2}\Delta t)\Delta t \\
k_4 &= f(y+k_3, t+\Delta t)\Delta t
\end{aligned}
$$

Here is the corresponding code:

```python
def runge_kutta4(y, x, dx, f):
    """computes 4th order Runge-Kutta for dy/dx.
    y is the initial value for y
    x is the initial value for x
    dx is the difference in x (e.g. the time step)
    f is a callable function (y, x) that you supply
    to compute dy/dx for the specified values.
    """

    k1 = dx * f(y, x)
    k2 = dx * f(y + 0.5*k1, x + 0.5*dx)
    k3 = dx * f(y + 0.5*k2, x + 0.5*dx)
    k4 = dx * f(y + k3, x + dx)

    return y + (k1 + 2*k2 + 2*k3 + k4) / 6.
```

Let's use this for a simple example. Let

$$\dot{y} = t\sqrt{y(t)}$$

with the initial values

$$\begin{aligned}t_0 &= 0\\y_0 &= y(t_0) = 1\end{aligned}$$

```python
import math
import numpy as np
t = 0.
y = 1.
dt = .1

ys, ts = [], []

def func(y,t):
    return t*math.sqrt(y)

while t <= 10:
    y = runge_kutta4(y, t, dt, func)
    t += dt
    ys.append(y)
    ts.append(t)

exact = [(t**2 + 4)**2 / 16. for t in ts]
plt.plot(ts, ys)
plt.plot(ts, exact)

error = np.array(exact) - np.array(ys)
print("max error {}".format(max(error)))
```

## Bayesian Filtering

Starting in the Discrete Bayes chapter I used a Bayesian formulation for filtering. Suppose we are tracking an object. We define its *state* at a specific time as its position, velocity, and so on. For example, we might write the state at time $t$ as $\mathbf x_t = \begin{bmatrix}x_t &\dot x_t \end{bmatrix}^\mathsf T$.

When we take a measurement of the object we are measuring the state. Sensors are noisy, so the measurement is corrupted with noise. Clearly though, the measurement is determined by the state. That is, a change in state may change the measurement, but a change in measurement will not change the state.

In filtering our goal is to compute an optimal estimate for a set of states $\mathbf x_{0:t}$ from time 0 to time $t$. If we knew $\mathbf x_{0:t}$ then it would be trivial to compute a set of measurements $\mathbf z_{0:t}$ corresponding to those states. However, we receive a set of measurements $\mathbf z_{0:t}$, and want to compute the corresponding states $\mathbf x_{0:t}$. This is called *statistical inversion* because we are trying to compute the input from the output.

Inversion is a difficult problem because there is typically no unique solution. For a given set of states $\mathbf x_{0:t}$ there is only one possible set of measurements (plus noise), but for a given set of measurements there are many different sets of states that could have led to those measurements.

Recall Bayes Theorem:

$$P(x \mid z) = \frac{P(z \mid x)P(x)}{P(z)}$$

where $P(z \mid x)$ is the *likelihood* of the measurement $z$, $P(x)$ is the *prior* based on our process model, and $P(z)$ is a normalization constant. $P(x \mid z)$ is the *posterior*, or the distribution after incorporating the measurement $z$, also called the *evidence*.

This is a *statistical inversion* as it goes from $P(z \mid x)$ to $P(x \mid z)$. The solution to our filtering problem can be expressed as:

$$P(\mathbf x_{0:t} \mid \mathbf z_{0:t}) = \frac{P(\mathbf z_{0:t} \mid \mathbf x_{0:t})P(\mathbf x_{0:t})}{P(\mathbf z_{0:t})}$$

That is all well and good until the next measurement $\mathbf z_{t+1}$ comes in, at which point we need to recompute the entire expression for the range $0:t+1$.

In practice this is intractable because we are trying to compute the posterior distribution $P(\mathbf x_{0:t} \mid \mathbf z_{0:t})$ for the state over the full range of time steps. But do we really care about the probability distribution at the third step (say) when we just received the tenth measurement? Not usually. So we relax our requirements and only compute the distributions for the current time step.

The first simplification is we describe our process (e.g., the motion model for a moving object) as a *Markov chain*. That is, we say that the current state is solely dependent on the previous state and a transition probability $P(\mathbf x_k \mid \mathbf x_{k-1})$, which is just the probability of going from the last state to the current one. We write:

$$\mathbf x_k \sim P(\mathbf x_k \mid \mathbf x_{k-1})$$

The next simplification we make is do define the *measurement model* as depending on the current state $\mathbf x_k$ with the conditional probability of the measurement given the current state: $P(\mathbf z_t \mid \mathbf x_x)$. We write:

$$\mathbf z_k \sim P(\mathbf z_t \mid \mathbf x_x)$$

We have a recurrance now, so we need an initial condition to terminate it. Therefore we say that the initial distribution is the probablity of the state $\mathbf x_0$:

$$\mathbf x_0 \sim P(\mathbf x_0)$$

These terms are plugged into Bayes equation. If we have the state $\mathbf x_0$ and the first measurement we can estimate $P(\mathbf x_1 | \mathbf z_1)$. The motion model creates the prior $P(\mathbf x_2 \mid \mathbf x_1)$. We feed this back into Bayes theorem to compute $P(\mathbf x_2 | \mathbf z_2)$. We continue this predictor-corrector algorithm, recursively computing the state and distribution at time $t$ based solely on the state and distribution at time $t-1$ and the measurement at time $t$.

The details of the mathematics for this computation varies based on the problem. The **Discrete Bayes** and **Univariate Kalman Filter** chapters gave two different formulations which you should have been able to reason through. The univariate Kalman filter assumes that for a scalar state both the noise and process are linear model are affected by zero-mean, uncorrelated Gaussian noise.

The Multivariate Kalman filter make the same assumption but for states and measurements that are vectors, not scalars. Dr. Kalman was able to prove that if these assumptions hold true then the Kalman filter is *optimal*. Colloquially this means there is no way to derive more information from the noise. In the remainder of the book I will present filters that relax the constraints on linearity and Gaussian noise.

Before I go on, a few more words about statistical inversion. As Calvetti and Somersalo write in *Introduction to Bayesian Scientific Computing*, "we adopt the Bayesian point of view: *randomness simply means lack of information*."[4] Our state parametize physical phenomena that we could in principle measure or compute: velocity, air drag, and so on. We lack enough information to compute or measure their value, so we opt to consider them as random variables. Strictly speaking they are not random, thus this is a subjective position.

They devote a full chapter to this topic. I can spare a paragraph. Bayesian filters are possible because we ascribe statistical properties to unknown parameters. In the case of the Kalman filter we have closed-form solutions to find an optimal estimate. Other filters, such as the discrete Bayes filter or the particle filter which we cover in a later chapter, model the probability in a more ad-hoc, non-optimal manner. The power of our technique comes from treating lack of information as a random variable, describing that random variable as a probability distribution, and then using Bayes Theorem to solve the statistical inference problem.

## Converting the Multivariate Equations to the Univariate Case

The multivariate Kalman filter equations do not resemble the equations for the univariate filter. However, if we use one dimensional states and measurements the equations do reduce to the univariate equations. This section will provide you with a strong intuition into what the Kalman filter equations are actually doing. While reading this section is not required to understand the rest of the book, I recommend reading this section carefully as it should make the rest of the material easier to understand.

Here are the multivariate equations for the prediction.

$$
\begin{aligned}
\mathbf{\bar{x}} &= \mathbf{F x} + \mathbf{B u} \\
\mathbf{\bar{P}} &= \mathbf{FPF}^\mathsf{T} + \mathbf Q
\end{aligned}
$$

For a univariate problem the state $\mathbf x$ only has one variable, so it is a $1\times 1$ matrix. Our motion $\mathbf{u}$ is also a $1\times 1$ matrix. Therefore, $\mathbf{F}$ and $\mathbf B$ must also be $1\times 1$ matrices. That means that they are all scalars, and we can write

$$\bar{x} = Fx + Bu$$

Here the variables are not bold, denoting that they are not matrices or vectors.

Our state transition is simple - the next state is the same as this state, so $F=1$. The same holds for the motion transition, so, $B=1$. Thus we have

$$x = x + u$$

which is equivalent to the Gaussian equation from the last chapter

$$ \mu = \mu_1+\mu_2$$

Hopefully the general process is clear, so now I will go a bit faster on the rest. We have

$$\mathbf{\bar{P}} = \mathbf{FPF}^\mathsf{T} + \mathbf Q$$

Again, since our state only has one variable $\mathbf P$ and $\mathbf Q$ must also be $1\times 1$ matrix, which we can treat as scalars, yielding

$$\bar{P} = FPF^\mathsf{T} + Q$$

We already know $F=1$. The transpose of a scalar is the scalar, so $F^\mathsf{T} = 1$. This yields

$$\bar{P} = P + Q$$

which is equivalent to the Gaussian equation of

$$\sigma^2 = \sigma_1^2 + \sigma_2^2$$

This proves that the multivariate prediction equations are performing the same math as the univariate equations for the case of the dimension being 1.

These are the equations for the update step:

$$
\begin{aligned}
\mathbf{K}&= \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} \\
\textbf{y} &= \mathbf z - \mathbf{H \bar{x}}\\
\mathbf x&=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} \\
\mathbf P&= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}}
\end{aligned}
$$

As above, all of the matrices become scalars. $H$ defines how we convert from a position to a measurement. Both are positions, so there is no conversion, and thus $H=1$. Let's substitute in our known values and convert to scalar in one step. The inverse of a 1x1 matrix is the reciprocal of the value so we will convert the matrix inversion to division.

$$
\begin{aligned}
K &=\frac{\bar{P}}{\bar{P} + R} \\
y &= z - \bar{x}\\
x &=\bar{x}+Ky \\
P &= (1-K)\bar{P}
\end{aligned}
$$

Before we continue with the proof, I want you to look at those equations to recognize what a simple concept these equations implement. The residual $y$ is nothing more than the measurement minus the prediction. The gain $K$ is scaled based on how certain we are about the last prediction vs how certain we are about the measurement. We choose a new state $x$ based on the old value of $x$ plus the scaled value of the residual. Finally, we update the uncertainty based on how certain we are about the measurement. Algorithmically this should sound exactly like what we did in the last chapter.

Let's finish off the algebra to prove this. Recall that the univariate equations for the update step are:

$$
\begin{aligned}
\mu &=\frac{\sigma_1^2 \mu_2 + \sigma_2^2 \mu_1} {\sigma_1^2 + \sigma_2^2}, \\
\sigma^2 &= \frac{1}{\frac{1}{\sigma_1^2} + \frac{1}{\sigma_2^2}}
\end{aligned}
$$

Here we will say that $\mu_1$ is the state $x$, and $\mu_2$ is the measurement $z$. Thus it follows that that $\sigma_1^2$ is the state uncertainty $P$, and $\sigma_2^2$ is the measurement noise $R$. Let's substitute those in.

$$ \mu = \frac{Pz + Rx}{P+R} \\
\sigma^2 = \frac{1}{\frac{1}{P} + \frac{1}{R}}
$$

I will handle $\mu$ first. The corresponding equation in the multivariate case is

$$
\begin{aligned}
x &= x + Ky \\
&= x + \frac{P}{P+R}(z-x) \\
&= \frac{P+R}{P+R}x + \frac{Pz - Px}{P+R} \\
&= \frac{Px + Rx + Pz - Px}{P+R} \\
&= \frac{Pz + Rx}{P+R}
\end{aligned}
$$

Now let's look at $\sigma^2$. The corresponding equation in the multivariate case is

$$
\begin{aligned}
P &= (1-K)P \\
&= (1-\frac{P}{P+R})P \\
&= (\frac{P+R}{P+R}-\frac{P}{P+R})P \\
&= (\frac{P+R-P}{P+R})P \\
&= \frac{RP}{P+R}\\
&= \frac{1}{\frac{P+R}{RP}}\\
&= \frac{1}{\frac{R}{RP} + \frac{P}{RP}} \\
&= \frac{1}{\frac{1}{P} + \frac{1}{R}}
\quad\blacksquare
\end{aligned}
$$

We have proven that the multivariate equations are equivalent to the univariate equations when we only have one state variable. I'll close this section by recognizing one quibble - I hand waved my assertion that $H=1$ and $F=1$. In general we know this is not true. For example, a digital thermometer may provide measurement in volts, and we need to convert that to temperature, and we use $H$ to do that conversion. I left that issue out to keep the explanation as simple and streamlined as possible. It is very straightforward to add that generalization to the equations above, redo the algebra, and still have the same results.

## Converting Kalman Filter to a g-h Filter

I've stated that the Kalman filter is a form of the g-h filter. It just takes some algebra to prove it. It's more straightforward to do with the one dimensional case, so I will do that. Recall

$$
\mu_{x}=\frac{\sigma_1^2 \mu_2 + \sigma_2^2 \mu_1} {\sigma_1^2 + \sigma_2^2}
$$

which I will make more friendly for our eyes as:

$$
\mu_{x}=\frac{ya + xb} {a+b}
$$

We can easily put this into the g-h form with the following algebra

$$
\begin{aligned}
\mu_{x}&=(x-x) + \frac{ya + xb} {a+b} \\
\mu_{x}&=x-\frac{a+b}{a+b}x  + \frac{ya + xb} {a+b} \\
\mu_{x}&=x +\frac{-x(a+b) + xb+ya}{a+b} \\
\mu_{x}&=x+ \frac{-xa+ya}{a+b}  \\
\mu_{x}&=x+ \frac{a}{a+b}(y-x)\\
\end{aligned}
$$

We are almost done, but recall that the variance of estimate is given by

$${\sigma_{x}^2} = \frac{1}{ \frac{1}{\sigma_1^2} +  \frac{1}{\sigma_2^2}}\\
= \frac{1}{ \frac{1}{a} +  \frac{1}{b}}
$$

We can incorporate that term into our equation above by observing that

$$
\begin{aligned}
\frac{a}{a+b} &= \frac{a/a}{(a+b)/a} = \frac{1}{(a+b)/a}  \\
 &= \frac{1}{1 + \frac{b}{a}} = \frac{1}{\frac{b}{b} + \frac{b}{a}}  \\
 &= \frac{1}{b}\frac{1}{\frac{1}{b} + \frac{1}{a}} \\
 &= \frac{\sigma^2_{x'}}{b}
 \end{aligned}
$$

We can tie all of this together with

$$
\begin{aligned}
\mu_{x}&=x+ \frac{a}{a+b}(y-x) \\
&= x + \frac{\sigma^2_{x'}}{b}(y-x) \\
&= x + g_n(y-x)
\end{aligned}
$$

where

$$g_n = \frac{\sigma^2_{x}}{\sigma^2_{y}}$$

The end result is multiplying the residual of the two measurements by a constant and adding to our previous value, which is the $g$ equation for the g-h filter. $g$ is the variance of the new estimate divided by the variance of the measurement. Of course in this case $g$ is not a constant as it varies with each time step as the variance changes. We can also derive the formula for $h$ in the same way. It is not a particularly illuminating derivation and I will skip it. The end result is

$$h_n = \frac{COV (x,\dot x)}{\sigma^2_{y}}$$

The takeaway point is that $g$ and $h$ are specified fully by the variance and covariances of the measurement and predictions at time $n$. In other words, we are picking a point between the measurement and prediction by a scale factor determined by the quality of each of those two inputs.

## References

 * [1] *Matrix Exponential* http://en.wikipedia.org/wiki/Matrix_exponential

 * [2] *LTI System Theory* http://en.wikipedia.org/wiki/LTI_system_theory

 * [3] C.F. van Loan, "Computing Integrals Involving the Matrix Exponential," IEEE Transactions Automatic Control, June 1978.

 * [4] Calvetti, D and Somersalo E, "Introduction to Bayesian Scientific Computing: Ten Lectures on Subjective Computing,", Springer, 2007.


---
Author: Anastasios Tzanidakis

In this notebook we report the analysis of PLasTiCC light curves (RRL and EB's) to determine the optimal configuration for Rubuin light curves. Specifically, here are the questions we're trying to understand and optimize:
- What version of LSP should we use: `MultiBand` or `SingleBand`
    - What LSP algorithm should we use: `Fast` or `Floating Mean/generalized`
    - What fourier/base component maximizes the correctly identified periods?

- Should we use a pre-determined frequency grid, automatic frequency grid, or zooming-peak approach
    - Pre-determined frequency bin
    - Automatic frequency grid based on light curve duration, oversampling, and nyquist correction
    - Two stage frequency search grid: [highlighted here](https://jakevdp.github.io/blog/2015/06/13/lomb-scargle-in-python/#:~:text=a%20built%2Din%20two%2Dstage%20grid%2Dsearch%20that%20accurately%20determines%20the%20best%20period%20in%20a%20specified%20range)

- How long do these algorithms take in each configuration
    - Timing in bins of number of total observations
    - Timing in bins of number of base components/Fourier terms
    - Timing in bins frequency grid approach

**Extra Analysis**/future TODO:
- What is the **minimum** number of observatiosn we need per light curve in order to recover the correct period?
- After $\mathcal{\tau}$ years of the survey (PlasTiCC maximum is 3 years; supposed Rubin DR3) - how **many** correctly identified periods would we find?
- Should the light curve moment also capture more information from the power spectrum (i.e TOP N peaks?). How many more correctly identified periodicities would be enough?
- Using the Rubin `Opsims` cadence, can there be an optimized pre-determined frequency window that maximizes period finding while minimizing computation?

## Section 1 - Metrics & Setp-up

### PlasTiCC Light Curves & Sample
---

For this analysis, we will make a few assumptions about the light curves under investigation:
- Consider a sample of only RR Lyrae (RRL) and Eclipsing Binaries (EB)
- Investigate light curves with a duration (T) less than 365 days
- To mimic the effects of forced-photometry, we will include in our analysis both `detection==0, 1` (both detections & non-detections) in the PlasTiCC light curves
- We set a minimum number of observations per light curve under investigation to be $N_{min}$=10

### Period Metric
---
We would like to define some success metric for the correctly identified periods within some margin of error. For my metric, I will estimate a simple percentage error:
`
$$\begin{equation} Err = \frac{|p_{inj}-p_{rec}|}{p_{inj}}\end{equation}$$

where $p_{inj}$ is the injected true period of the light curve, while the $p_{rec}$ is the recovered period from the Lomb-Scargle Periodogram. We note that for this analysis we will assume that the **correct period corresponds to the maximum power**, without considering other harmonics that might be caused by the survey cadence.

```python
import numpy as np
from astropy.io import ascii
import os
from tqdm import tqdm
from astropy.time import Time
import pandas as pd
import time
import random
from gatspy import periodic, datasets
import matplotlib.pyplot as plt
import matplotlib
from astropy.table import Table
from gatspy import datasets, periodic
import gatspy
import scipy.stats as sci_stat
import sys

%matplotlib inline
%config InlineBackend.figure_format = "retina"
from matplotlib import rcParams
rcParams['savefig.dpi'] = 250
rcParams['font.size'] = 20

global data_path
data_path = '../data/plasticc/data/'

# Caution, be careful when ignoring warnings!
import sys
import warnings

if not sys.warnoptions:
    """Not reccomended, but for the moment supress warnings!"""
    warnings.simplefilter("ignore")
```

```python
# TODO: Add entire PlasTiCC stream?!
data = pd.read_csv(data_path + "plasticc_test_lightcurves_11.csv.gz",
                  compression='gzip',
                  error_bad_lines=False)

meta_info = ascii.read(data_path + "plasticc_train_metadata.csv") # ascii meta since it's smaller
meta_theta_EB = ascii.read(data_path + 'plasticc_modelpar/' + 'plasticc_modelpar_016_EB.csv')
meta_theta_RRL = ascii.read(data_path + 'plasticc_modelpar/' + 'plasticc_modelpar_092_RRL.csv')
```

```python
# Define some helper functions for the analysis

def generate_lc(obj_id, band='all', data_table=data, det='all', dt_cut=365):
    """Unpack and return PlastiCC data in numpy array format.

    Paremters
    ---------
    obj_id: Object ID
    band: Photometric bandpass filter. 'all' includes ugrizy, or 'ugrizy'
    data_table: Pandas data table containing the light curves
    det: Detection from the image subtraction algorithm. ==1 detection, ==0 not detection (i.e upper limit) or 'all': uses both 0 & 1
    dt_cut: Light curve duration cut in days (default 365 days)
    Returns
    -------
    mjd, magnitude, magnitude_error, filter (if band=='all')
    """

    if det==0 or det==1:
        data_table_mod = data_table[data_table['detected_bool']==det]
    elif det=='all':
        data_table_mod = data_table # select both

    # Select light curve based on the ID
    lc = data_table_mod[data_table_mod['object_id']==obj_id]

    lsst_bands = list('ugrizy') # lsst photomeric bands

    lc_array = lc.to_numpy()

    # Capture empty light curve
    assert len(lc_array[:,1])>0, ("Sorry, it seems like your obj_id query was wrong!")

    mjd, flux, flux_err = lc_array[:,1], lc_array[:,3], lc_array[:,4]
    flt = lc_array[:,2].astype(int).astype(str)

    for j in range(6):
        flt[flt==str(j)] = lsst_bands[j]

    # Baseline cut
    baseline = mjd-mjd[0] # calculate baseline
    mjd, flux, flux_err, flt = mjd[baseline<=dt_cut], flux[baseline<=dt_cut], flux_err[baseline<=dt_cut], flt[baseline<=dt_cut]

    if band=='all':
        return mjd, flux, flux_err, flt
    else:
        return mjd[flt==band], flux[flt==band], flux_err[flt==band], flt[flt==band]

def generate_toi_table(data, meta_info, meta_theta_EB, meta_theta_RRL):
    """
    Generate table that contains the light curve ID and transient type given a number of observations cut.

    Input
    -----
    data: Head data table that contains photometry
    meta_info: Table that contains the meta-data (i.e classification name)
    meta_theta_<TYPE>: Table that contains metadata information (i.e Period)
    nthresh (int): Minimum number of observations in each band must contain
    """
    id_av_rrl, id_av_eb = [], []

    for uid in tqdm(np.unique(data['object_id'])):
        ww = np.where(meta_theta_EB['object_id'] == uid)
        if np.shape(ww)[-1]==1:
            id_av_eb.append(uid)

    for uid in tqdm(np.unique(data['object_id'])):
        ww = np.where(meta_theta_RRL['object_id'] == uid)
        if np.shape(ww)[-1]==1:
            id_av_rrl.append(uid)

    id_av_rrl, id_av_eb = np.array(id_av_rrl), np.array(id_av_eb)

    _id1 = np.array(['rrl' for _ in range(len(id_av_rrl))])
    _id2 = np.array(['eb' for _ in range(len(id_av_eb))])

    # All ID's and & ID tags
    all_id = np.concatenate([id_av_rrl, id_av_eb])
    _id_all = np.concatenate([_id1, _id2])

    # Final TOI table
    toi_table = Table([all_id, _id_all], names=('obj_id', 'type'))

    return toi_table
```

```python
# Fetch the Object_id of all the targets of interest (toi)
toi_table = generate_toi_table(data, meta_info, meta_theta_EB, meta_theta_RRL)
```

    100%|██████████| 345996/345996 [00:14<00:00, 23570.68it/s]
    100%|██████████| 345996/345996 [00:24<00:00, 13974.97it/s]

```python
# Define some additional helper functions for metadata handiling and processing
def fetch_meta_info(lc_id, lc_type):
    """Fetch metadata for transient type.

    Input
    -----
    lc_id: Light curve ID
    lc_type: classification type (i.e rrl, eb)

    Output
    ------
    meta_<type>_table: Table that contains metadata (i.e period and other physical properties)
    """
    if lc_type=='rrl':
        # crossmatch to approprirate table
        xm_ = np.where(meta_theta_RRL['object_id']==lc_id)
        return meta_theta_RRL[xm_]
    elif lc_type=='eb':
        # crossmatch to approprirate table
        xm_ = np.where(meta_theta_EB['object_id']==lc_id)
        return meta_theta_EB[xm_]

# Write a function that will generate N random from each class (equal)
def draw_rand_trans(table, N, class_type='rrl'):
    """Draw N random object_id's from the specific class (with no repeats)
       Note: It will not draw the same transiennt
    """
    # isolate each unique class
    req_tab = table[table['type']==class_type]

    # Random number generator w/o repeat
    rng = np.random.default_rng()
    rn = rng.choice(len(req_tab), size=N, replace=False)

    return req_tab[rn]

def percent_error(true, obs):
    """Calculate the absolute percent error."""
    return abs(true-obs)/true
```

## Section 2.1 - Adressing the Single Band LSP
We begin our analysis on single band photometric LSP analysis. Here we will be adressing a few questions:

- For each photometric filter, what configuration minimizes the percent error?
    - Which method is better, `Fast` or `Floating Mean`
    - What frequency grid to choose?

- Timing analysis:
    - Timing per number of total observations (likely in bins=10)
    - Timing per number of components (only applies to Floating Mean)
    - Timing per grid search

**Internal Notes/Figures**:
- Start with with `Fast`:
    - What frequency grid and configuration

```python
# Generate a sample of RRL & EB's with 10 total observations per photometric band
rrl_table = draw_rand_trans(toi_table, 5_000, 'rrl')
eb_table = draw_rand_trans(toi_table, 5_000, 'eb')

def band_cut(dat_table, band='r', nthresh=10, dt_cut=365):
    """Return object_id to sources on a specified photometric filter given some threshold detection cut."""

    idi = []
    for _id in tqdm(dat_table['obj_id']):
        # Fetch light curve info
        lc = generate_lc(_id, band=band, det='all', dt_cut=dt_cut)
        if len(lc[0])>=nthresh:
            idi.append(_id)
    return np.array(idi)

rrl_u, eb_u = band_cut(rrl_table, band='u', nthresh=10), band_cut(eb_table, band='u', nthresh=10)
```

    100%|██████████| 5000/5000 [01:09<00:00, 72.15it/s]
    100%|██████████| 5000/5000 [01:10<00:00, 70.43it/s]

```python
len(rrl_u), len(eb_u)
```

    100%|██████████| 2000/2000 [00:27<00:00, 71.51it/s]

```python
len(ll)
```

    67

```python

```

```python

```

```python

```

```python

```

```python

```

```python

```

```python

```

```python

```

```python

```

```python

```

```python

```

```python

```