# Exercise - Sensorfusion and Localization (1D-Kalman Filter)

[TODO]

## Requirements

### Knowledge

To complete this exercise notebook, you should possess knowledge about the following topics:

Kalman Filter 1D

• Notes by Christian Herta [HER18]

### Python Modules

import scipy.stats
import numpy as np
from matplotlib import pyplot as plt
import hashlib

%matplotlib inline
def round_and_hash(value, precision=4, dtype=np.float32):
"""
Function to round and hash a scalar or numpy array of scalars.
Used to compare results with true solutions without spoiling the solution.
"""
rounded = np.array([value], dtype=dtype).round(decimals=precision)
hashed = hashlib.md5(rounded).hexdigest()
return hashed

## Exercises

### Exercise - Sensorfusion

We have three sensors which measure a distance.

We get the following values from the measurements:

• Sensor 1:$50.1$ cm
• Sensor 2:$49.3$ cm
• Sensor 3:$49.7$ cm

From the calibration of the sensors we have the following estimate of the standard deviations for the sensors (in the range 30-90 cm):

• Standard deviation of sensor 1:$0.8$ cm
• Standard deviation of sensor 2:$1.2$ cm
• Standard deviation of sensor 3:$0.9$ cm

Assume that the sensors are prefectly calibrated such we have no systematic error (bias).

Remark: If you are interesed how to estimate the error of an sensor, then read for an easy explaination https://amloceanographic.com/blog/sensor-accuracy/

• What is the distance if we combine of all three measurements?

• Also give an error estimate of the result (the true value should be in at least 0.95 of the Gaussian area), e.g. as$88\pm 4$m (How is this related to the standard deviation?)

Solve the exercise

1. with pen & paper
2. implement it with numpy.
3. plot the differnet Gaussians. For the plot you can utilize the following code snippet:
h_space = np.arange(45.,55.,.1)
pdf1 = scipy.stats.norm(loc=mu1, scale=sigma1).pdf(h_space)
plt.plot(h_space, pdf1)


It should look similar to the following:

Hint:

To combine two measurements, we take the product of the gaussians with:

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

and

$\sigma_{new} = \sqrt{\frac{\sigma_1^2 \sigma_2^2}{\sigma_1^2 + \sigma_2^2}}$

Take care for the significant digits you use.

def measurement(mu, sigma, mu1, sigma1):
# product of two gaussians
raise NotImplementedError()
return mu_, sigma_
mu_combined_measure = 42 ### assign your solution
sigma_combined_measure = 42 ### assign your solution
### Test your solution

assert round_and_hash(mu_combined_measure) == "3ff4d58510b259756d7c9dedf137344f"
assert round_and_hash(sigma_combined_measure) == "f3a60f45efcdfd685302d28d27e4abf3"

### Exercise - 1D Kalman Filter

A robot moves with a velocity of about$3$ m/s. We assume that if the robot moves in a time$\Delta t=2$s we have an standard deviation of the moved distance of$0.8$m.

Each 2 seconds we measure the position with an standard deviation$\sigma$ of$1.2$ m.

We get the following measurements:

-$z(t=0 SINGLESINGLE ) = -2$m (inital$\sigma=0$) -$z(t=2 SINGLESINGLE ) = 3.4$ m -$z(t=4 SINGLESINGLE )$ = no measurement -$z(t=6 SINGLESINGLE )= 16.3$ m

What is the predicted position before the measurement at$t=8$s.

In the state space we use just the position of the robot (1D). Solve the exercise

1. with pen & paper
2. implement it with numpy

Hint:

Again, combining two measurements, use the product of the two guassians, like the exercise above.

For prediction use the formulas for convolution of two gaussians:

$\mu_{new} = \mu_1 + \mu_2$

and

$\sigma_{new} = \sqrt{\sigma_1^2 + \sigma_2^2}$

def predict(mu, sigma, mu1, sigma1):
# convolution of two gaussians
raise NotImplementedError()
return mu_, sigma_
mu_final = 42 ### assign your solution
sigma_final = 42 ### assign your solution
### Test your solution

assert round_and_hash(mu_final) == "bd09b7c10823bb776c3869b823dfcab2"
assert round_and_hash(sigma_final) == "90ec900f88662277f26f003ff2018566"

## Literature

The following license applies to the complete notebook, including code cells. It does however not apply to any referenced external media (e.g., images).

Exercises - Sensorfusion and Localization (1D-Kalman Filter)
by Christian Herta