# Altitude sensor fusion

For an autonomous quadrocopter, it is essential to know the height above ground. We have to distinguish between:

1. height above ground (as measured from a downward facing distance sensor)
2. height above starting place
3. height above average sea level from barometer
4. height above average sea level from GPS

If we assume that the ground is even, (1) and (2) are equal. However, this is seldom the case. The height in (3) is subject to weather conditions, temperature deviations and different air layers, while (4) suffers from an overly simple model of the earth’s geoid.

### Sensors to fuse

• Barometer (height above sea level according to some avionic standard model)
• Ultrasonic distance sensor (height above ground)
• Accelerometer (linear “up” acceleration)
• GPS (height above modelled sea level)

Regarding the accuracy of barometer vs GPS, I have found a nice article on xcmag.com. There seem to be huge differences in the altitude readings in both technologies, so we need some alignment mechanism.

### Benefits of this sensor fusion

1. Even if a single sensor fails (might be a bird flying under the quadrocopter, indoor or a storm), the system keeps working
2. Noise filtering: even when used with just the ultrasonic distance sensor, the noise is reduced.
3. We get a vertical speed estimate that we can use for a smoother height control
4. No startup phase is needed: in the very beginning, GPS and barometer might not have reached their full accuracy. This lack of knowledge is part of our model and doesn’t hurt. A less elegant (but computationally cheaper) solution could remove the GPS and barometer bias from the Kalman state and use a startup phase at the ground to find the biases.
5. If the ground level changes, the system adapts. By keeping track of the states, we might even combine them to get a map of the landscape.

Now that the marketing part is done, let’s get our hands dirty :).

### The Kalman filter

To fuse these sensor readings to one unified state, I use a Kalman filter, which is a very common tool in this domain. Here is a great introduction to Kalman Filters that helped me some time ago and Wikipedia has a nice description with a great example.

At its core, the Kalman filter is a very powerful enhancement of a weighted sum between some previous estimation $\hat{x}_{k-1}$ at time $k-1$ and a measurement $\hat{z}_{k}$ (at time $k$):

$\hat{x}_{k} = K_k\cdot z_k + (1-K_k)\cdot \hat{x}_{k-1}$.

For an assumed $K_k = 0.5$ we would get a simple average, but the Kalman filter is much smarter than that and (just as important) multi-dimensional. The power of the Kalman filter lies in the probability distributions it works with, and a linear state prediction model. I will not give an introduction to Kalman Filters in general, so please study the links above if you have problems to follow the next paragraphs.

### Kalman State and Prediction Model

When designing a Kalman Filter, the most essential part is the state vector $x$.

$x=\begin{pmatrix}\text{heightAboveGround}\\\text{verticalSpeed}\\\text{barometerGroundHeight}\\\text{gpsGroundHeight}\end{pmatrix}$

As a state, we mainly want to model the height above ground. To model its change, we also need the quadcopter’s vertical speed. The other two state variables (barometerGroundHeight and gpsGroundHeight) are offsets that help with relating the GPS and barometer readings to the ultrasonic distances.

The Kalman Filter tries to predict the next state based on the current state estimate and user control input (if present):

\begin{aligned} x_k= \underbrace{F_k x_{k-1}}_{\text{prediction}} + \underbrace{B_k u_k}_{\substack{\text{influence of}\\\text{vertical acceleration}}} + \underbrace{\mathcal{N}(0,Q_k)}_{\text{noise}} \end{aligned}

We define $F$ to be:

$F_k = \begin{pmatrix} 1 & \Delta{t} & 0 & 0 \\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{pmatrix}$

As we see, the diagonal is one, so we take all the values from the last state as a basis. Additionally the time delta is multiplied with the vertical speed and added to the height above ground to reflect the current movement. The ground heights in contrast should not move with the quadrocopter.

\begin{aligned} u_k &= \underbrace{\text{vertical acceleration}}_{\text{accelerometer reading in world space}}\quad B_k &= \begin{pmatrix} \frac{\Delta{t}^2}{2}\\ \Delta{t}\\ 0\\ 0 \end{pmatrix} \end{aligned}

Here we apply integration rules: vertical speed is the integral of vertical acceleration in $\Delta{t}$ and the height above ground is the second integral. The offsets of GPS and barometer do not change with quadrocopter acceleration.

Although this is not really a predict step if we use the accelerometer data from this point in time, the trick seems to work for attitude sensor fusion in The Design and Implementation of a Robust AHRS for Integration into a Quadrotor Platform by Matthew Watson (2013). With this trick, we don’t need acceleration as another state dimension.

### Kalman Measurement Model

The matrix $H$ maps the current state to a measurement vector $z_k$:

$z_k= H_k \cdot x_k + \underbrace{ \mathcal{N}(0,R_k)}_{\text{noise}}$,

where $x_k$ is the current state estimation. The lines of $H_k$ refer to the individual sensors, so we want the upper equation to be:

\begin{aligned} z_k = &\begin{pmatrix}\text{Barometer measurement}\\ \text{Ultrasonic sensor measurement}\\ \text{GPS height measurement} \end{pmatrix} \\ = & \begin{pmatrix} \text{heightAboveGround} + \text{barometerGroundHeight}\\ \text{heightAboveGround}\\ \text{heightAboveGround} + \text{gpsGroundHeight} \end{pmatrix} + noise \\ = & \underbrace{\begin{pmatrix} 1 & 0 & 1 & 0 \\ 1 & 0 & 0 & 0\\ 1 & 0 & 0 & 1\end{pmatrix}}_{H_k} \cdot \underbrace{\begin{pmatrix}\text{heightAboveGround}\\\text{verticalSpeed}\\\text{barometerGroundHeight}\\\text{gpsGroundHeight}\end{pmatrix}}_{x_k} + \underbrace{noise}_{\mathcal{N}(0,R_k)} \end{aligned}

As we see, the vertical speed has no mapping in $H$ (the second column is zero). This is because we cannot directly observe it. The Kalman Filter will however deduce a probability distribution for it from the movement model.

The height above ground is observed by three sensors, which should give good precision and noise insensitivity.

Because the Kalman filter always calculates with gaussian probability distributions, the probabilities and covariances are as important as the expected values.

$Q_k = \begin{pmatrix} 0.3 & 0 & 0 & 0 \\ 0 & 0.5 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{pmatrix} \qquad \text{Reliability of state prediction}$

The state prediction only affects height above ground and vertical speed, so we don’t have to introduce a lack of knowledge in the other two dimensions. Since the influence of the noisy accelerometer reading is stronger on the vertical speed, the noise is stronger there.

\begin{aligned} & R_k &= & \begin{pmatrix} 1 & 0 & 0 \\ 0 & \sigma^{\text{ultrasonic}}_k & 0 \\ 0 & 0 & \sigma^{\text{gps}}_k \end{pmatrix} \qquad \text{Reliability of height sensor data} \\ & \sigma^{\text{ultrasonic}}_k &= & \begin{cases} 10000 & \mbox{if no echo} \\ 0.25 &\mbox{otherwise} \end{cases} \\ & \sigma^{\text{gps}}_k &= & \begin{cases} 10000 & \mbox{if too few satellites} \\ 1 + (\text{num satellites})^{-\frac{1}{2}} &\mbox{otherwise} \end{cases} \end{aligned}

The matrix $R_k$ expresses no noise covariance between the different height sensors because only the values should be correllated, not the noise. It changes as soon as we get above the maximum distance of the ultrasonic sensor, as then we don’t get a result from it. Then the corresponding entries should be very high in $R_k$. Luckily, the Kalman filter supports changes to the matrices in every step.

Similarly, we need to adapt the entries for GPS with the number of satellites available. With fewer than three satellites, the GPS has no value for us and the values in $R_k$ need to be extraordinary high. On the other hand, with more satellites, the accuracy is better and noise should be reduced.

### Initial values

In the beginning, we assume that we are on the ground with zero speed, but we have no idea where we are on earth and what weather conditions we face, so the GPS and barometer offsets are unknown.

$x_0 = \begin{pmatrix} 0 \\ 0 \\ 100 \\ 100 \end{pmatrix}$

One very nice aspect of Kalman filtering is that the initial values can be very inacurate, if the initial probability distribution reflects this lack of knowledge. So we set the initial probability distribution to be quite confident in the first two state dimensions and very unsure in the third and fourth:

$P_0 = \begin{pmatrix} 0.1 & 0 & 0 & 0 \\ 0 & 0.1 & 0 & 0 \\ 0 & 0 & 10000 & 0 \\ 0 & 0 & 0 & 10000 \end{pmatrix}$

All the values in this post should be treated as just a first idea because I did not yet implement this. As soon as the implementation is done, I will post some results and tuned parameters.