Introduction

============

This document describes the principles of altitude data fusion implemented in the QuadCopter version of MatrixPilot.

Please excuse any lack of clarity due to my English.

Altitude data fusion

====================

The goal of altitude data fusion is to evaluate the best altitude estimation, at each instant, based on the incoming data from the four sensors: IMU, Barometer, Lidar or sonar and GPS.

Each sensor has its specificity while none of them can give a precise value on its own:

• IMU has a high rate (1 kHz) but drifts ;

• Barometer is slow, unprecise (standard deviation~ 1 m or 3 ft) and fluctuating

• Lidar has a high rate (100 Hz), is precise (~ 0.3m or 1 ft) but only functions at low range

• Sonar has a medium rate (10 Hz), is precise (~ 0.3m or 1 ft) but only functions at very low range

• GPS has low rate (5 Hz), unprecise (10 m) but high range (∞).

Altitude data fusion will mix all these data for the best outcome value.

In this first implementation, all data is supposed to be captured at the same moment, without taking into account any delays between measurements.

If necessary, a more complex implementation can re-time the data by extrapolation of each of them using the estimated speed to synchronize all of them. This could be usefull in case of important variation of the altitude and only in this case.

In this document, altitude is often used instead of height, but the correct objective is well to have the height because the copter flight at low altitude.

The fusion data rests on the mean measurements, weighted by a certain confidence factor, depending of each sensor’s specifics and trust.

On this figure, the oblique arrow represents an amplifier with a variable gain.

Each sensor’s output is filtered by an alpha/beta filter whose depth is a function of a quality factor.

Then a validity coefficient is computed by multiplying this quality factor by a confidence factor. This validity coefficient is used for the weighting.

The quality coefficient is incremented from a minimum value to a maximum value each time the measurement is coherent with current fusion output.

	Threshold (m)	Quality min	Quality Max
IMU	1	2	4
Lidar	0.5	2	8
Barometer	1	10	60
GPS	10	10	120

The alpha/beta coefficients are computed in accordance with Quality Q as follow:
Alpha = 2(Q-1)/Q/(Q+1) and Beta = 6/Q/(Q+1)

See a filter description in the annex.

Finally, the validity is computed by the product of quality time confidence. Validity is zeroed after a last test of confidence.

	Confidence	Altitude min (m)	Gap max (m) to fusion	Harmo coefficient
IMU	5	-1	4	2
Lidar	10	0	6	1
Barometer	2	-4	10	1
GPS	1	-10	20	0.5

Conclusion

This algorithm is used in my QuadCopter version of MatrixPilot under my own (Jeffem) responsability, without no approval from the team. This description gives a solution suitable for the SK450. It was tested at only very low altitude (less than 5 m) and without GPS. The parameters and logics must be adapted for each use case.

Alpha/beta filter

An alpha beta filter (also called alpha-beta filter, f-g filter or g-h filter) is a simplified form of observer for estimation, data smoothing and control applications. It is closely related to Kalman filters and to linear state observers used in control theory. Its principal advantage is that it does not require a detailed system model.

It is very easy to implement:

For example, suppose that Z is the variable to aproximate and Vz its derive, if the measurement is Zi every dt instant then

Vz=Vz+Beta*(Z-Zi)/dt
Z=Z+Alpha*(Z-Zi)+Vz*dt

Depending upon the values of Alpha et Beta the filter has different characteristics.

In this altitude data fusion, these coefficient Alpha and Beta are relative to the estimated quality Q of the measurement by the relations Alpha = 2(Q-1)/Q/(Q+1) and Beta = 6/Q/(Q+1).

Q represents the depth of the filter, the number of samples taking into account. Minimum value of Q is 2.