Multi-Sensor Calibration Pt 1 (Or, How to Kalman Filter Your Way Out)
Tags: tutorial, vision, computervision, sensors
Published: 2020-12-02 01:06:26

In the world of automated vision, there's only so much one can do with a single sensor. Optimize for one thing, and lose the other; and even with one-size-fits-all attempts, it's hard to paint a full sensory picture of the world at 30fps.

We use sensor combinations to overcome this restriction. This intuitively seems like the right play; more sensors mean more information. The jump from one camera to two, for instance, unlocks binocular vision, or the ability to see behind as well as in front. Better yet, use three cameras to do both at once. Add in a LiDAR unit, and see farther. Add in active depth, and see with more fidelity. Tying together multiple data streams is so valuable this act of **Sensor Fusion** is a whole discipline in itself. 

Yet this boon in information often makes vision-enabled systems harder to build, not easier. Binocular vision relies on stable intrinsic and extrinsic camera properties, which cameras don't have. Depth sensors lose accuracy with distance. A sensor can fail entirely, like LiDAR on a foggy day. 

This means that effective sensor fusion involves constructing vision architecture in a way that *minimizes uncertainty in uncertain conditions*. Sensors aren't perfect, and data can be noisy. It's the job of the engineer to sort this out and derive assurances about what is actually true. This challenge is what makes sensor fusion so difficult: it takes competency in information theory, geometry, optimization, fault tolerance, and a whole mess of other things to get right. 

So how do we start?

# Guessing

...just kidding. Though you would be surprised how many times an educated guess gets thrown in! No, we're talking

# Kalman filters

So let's review our predicament:

- We have a rough idea of our current **state** (e.g. the position of our robot), and we have a **model** of how that state changes through time.
- We have **multiple sensor modalities**, each with their own data streams.
- All of these sensors give **noisy and uncertain data.**

Nonetheless, this is all that we have to work with. This seems troubling; we can't be certain about anything! 

Instead, what we *can* do is *minimize our uncertainty*. Through the beauty of mathematics, we can combine all of this knowledge and actually come out with a *more certain* idea of our state through time than if we used any one sensor or model. 

This 👆 is the magic of Kalman filters.

*Warning: Math.*

# Phase 1: Prediction

## Model Behavior

Let's pretend that we're driving an RC car in a completely flat, very physics-friendly line. 

There are two things that we can easily track about our car's state: its position *p<sub>t</sub>* and velocity *v<sub>t</sub>*. 

We can speed up our robot by punching the throttle, something we do frequently. We do this by exerting a force *f* on the RC car's mass *m*, resulting in an acceleration *a* (see Newton's Second Law of Motion).

With just this information, we can derive a model for how our car will act over a time period {% katex inline %}\Delta t{% endkatex %} using some classical physics:

{% katex %}

\tag{1.1}
p\_{t+1} = p\_{t} + (v\_{t} \Delta t)+ \frac{f \Delta t^2} {2m}

{% endkatex %}

{% katex %}


\tag{1.2}
v\_{t+1} = v\_{t} + \frac{ f \Delta t} {m}

{% endkatex %}

We can simplify this for ourselves using some convenient matrix notation. Let's put the values we can track, position *p<sub>t</sub>* and velocity *v<sub>t</sub>*, into a **state vector:**

{% katex %}

\tag{1.3}
\textbf{x}\_{t} = \begin{bmatrix}
  p_{t} \\\\
  v_{t}
\end{bmatrix}

{% endkatex %}

...and let's put out applied forces into a **control vector** that represents all the outside influences affecting our state:

{% katex %}

\tag{1.4}
\textbf{u}\_{t} = \begin{bmatrix}
  \frac{f}{m}
\end{bmatrix} = \frac{f}{m}

{% endkatex %}

Now, with a little rearranging, we can organize our motion model for position and velocity into something a bit more compact:

{% katex %}

\tag{1.5}
\begin{bmatrix} 
  p\_{t+1} \\\\
  v\_{t+1}
\end{bmatrix} = 
\underbrace{
\begin{bmatrix} 
  1 & \Delta t \\\\
  0 & 1
\end{bmatrix}
}\_{F\_{t}}

\begin{bmatrix} 
  p\_{t} \\\\
  v\_{t}
\end{bmatrix}
+
\underbrace{
\begin{bmatrix} 
  \frac{\Delta t^2}{2} \\\\
  \Delta t
\end{bmatrix}}\_{B\_{t}}
\frac{f}{m} 


{% endkatex %}

{% katex %}

\tag{1.6}
\Rightarrow
\textbf{x}\_{t+1} = 
F\_{t}\textbf{x}\_{t} +
B\_{t}\textbf{u}\_{t}

{% endkatex %}

By rolling up these terms, we get some handy notation that we can use later:

- {% katex inline %}F\_{t}{% endkatex %} is called our **prediction matrix**. It models what our system would do over {% katex inline %}\Delta t{% endkatex %}, given its current state.
- {% katex inline %}B\_{t}{% endkatex %} is called our **control matrix.** This relates the forces in our control vector {% katex inline %}\textbf{u}_t{% endkatex %} to the state prediction over {% katex inline %}\Delta t{% endkatex %}.

# Uncertainty through PDFs

However, we’re not *exactly* sure whether or not our state values are true to life; there’s uncertainty! Let’s make some assumptions about what this uncertainty might look like in our system:

* Any error we might get in an observation is inherently random; that is, there isn’t a bias towards one result.
* Errors are independent of one another.

These two assumptions mean that our uncertainty follows the [Central Limit Theorem](https://en.wikipedia.org/wiki/Central_limit_theorem)! We can therefore assume that our error follows a [Normal Distribution](https://en.wikipedia.org/wiki/Normal_distribution), aka a [Gaussian curve](https://en.wikipedia.org/wiki/Gaussian_function).

We will use our understanding of Gaussian curves later to great effect, so take note!

We’re going to give this uncertainty model a special name: a **probability density function (PDF)**. This represents how probable it is that certain states are the *true* state. Peaks in our function correspond to the states that have the highest probability of occurrence.
![PDF Image](https://miro.medium.com/max/700/1*132qfD36wW_sMRr8GL_S9w.png)
<figcaption>Fig. 1. Our first PDF</figcaption>

Our state vector {% katex inline %}\text{x}\_{t}{% endkatex %} represents the mean {% katex inline %}μ{% endkatex %} of this PDF. To derive the rest of the function, we can model our state uncertainty using a **[covariance matrix](https://en.wikipedia.org/wiki/Covariance_matrix)** {% katex inline %}P\_{t}{% endkatex %}:
![Covariance Matrix](https://miro.medium.com/max/700/1*uEwiT8HYTpbK6wfd5ZWp6g.png)

There are some interesting properties here in {% katex inline %}P\_{t}{% endkatex %}. The diagonal elements ({% katex inline %}Σ\_{pp}{% endkatex %}, {% katex inline %}Σ\_{vv}{% endkatex %}) represent how much these variables deviate from their own mean. We call this **variance**.

The off-diagonal elements of {% katex inline %}P\_{t}{% endkatex %} express **covariance** between state elements. If {% katex inline %}Σ\_{pv}{% endkatex %} is zero, for instance, then we know that an error in velocity won’t influence an error in position. If it’s any other value, we can safely say that one affects the other in some way. PDFs without covariance terms look like Figure 1 above, with major and minor axes aligned with our world axes. PDFs *with* covariance are skewed off-axis depending on how extreme the covariance is:
![Fig 2](https://miro.medium.com/max/700/1*5aciC4T3mIVqKE-lrOBg2w.png)<figcaption>Fig. 2. A PDF with non-zero covariance. Notice the ‘tilt’ in the major and minor axes of the ellipsis.
</figcaption>

Variance, covariance, and the related **correlation** of variables are valuable, as they make our PDF more information-dense.

We know how to predict {% katex inline %}\text{x}\_{t+1}{% endkatex %}, but we also need the predicted covariance {% katex inline %}P\_{t+1}{% endkatex %} if we’re going to describe our state fully. We can derive it from {% katex inline %}\text{x}\_{t+1}{% endkatex %} using some (drastically simplified) linear algebra:
![Fig 2.2](https://miro.medium.com/max/700/1*9CKzaK9VwfMpFpZyNWUUnw.png)

Notice that {% katex inline %}B\_{t}u\_{t}{% endkatex %} got tossed out! Control has no uncertainty that we can directly observe, so we can’t use the same math that we did on {% katex inline %}F\_{t}\text{x}\_{t}{% endkatex %}.

However, we can factor in the effects of noisy control inputs another way: by adding a **process noise covariance matrix** {% katex inline %}Q\_{t}{% endkatex %}:
![Fig 2.3](https://miro.medium.com/max/700/1*-5AWVGOBpaaGndrJDn0k_g.png)

Yes, we are literally adding noise.

# Prediction step, solved?

We have now derived the full **prediction step**:
![Fig 2.4 2.5](https://miro.medium.com/max/700/1*Fj2cWmIW-ZvbizmR8GfNNA.png)

Our results are… ok. We got a good guess at our new state out of this process, sure, but we’re a lot more uncertain than we used to be!

![Fig 3](https://miro.medium.com/max/700/1*M0sW5v8HMo8GQaJNiUd57A.png)<figcaption>Fig. 3. Starting state PDF in red, predicted state PDF in blue. Notice how the distribution is more spread out. Our state is less certain than before.
</figcaption>

There’s a good reason for that: everything up to this point has been a sort of “best guess”. We have our state, and we have a model of how the world works; all that we’re doing is using both to predict what might happen over time. We still need something to **support these predictions** outside of our model.

Something like sensor measurements, for instance.

<center>.     .    .</center>

We’re getting there! So far, this post has covered:

* The motivation for using a Kalman filter in the first place
* A toy use case, in this case our physics-friendly RC car
* The Kalman filter prediction step: our best guess at where we’ll be, given our state

We’ll keep it going in Part II (posted later this week) by bringing in our sensor measurements (finally). We will use these measurements, along with our PDFs, to uncover the true magic of Kalman filters!

Spoiler: it’s not magic. It’s just more math.

*Note: this excellent post was written by Tangram Vision CEO Brandon Minor* {% user bminortx%}