Pixhawk (PX4) Gyroscope Filtering and Sensor Temperature Compensation

First of all, make a small advertisement. I have always liked drones. Recently, I have time to study the Pixhawk (PX4) flight controller, and write the summary and experience in the official account “SantyNotebook” to share with you. Interested friends can come together Let’s talk about how to write drone flight control from scratch~

This article is simultaneously published in the official account push “Gyroscope Filtering and Sensor Temperature Compensation”

We know that the IMU is easily affected by the vibration of the aircraft body. Physically, a shock absorber is usually installed at the installation position of the flight control; and the measurement results of the IMU and the barometer will drift due to temperature changes. Physically, a constant temperature device is usually set to reduce this. kind of influence
How to do it at the software level? Let’s take a look at the principles of frequency domain filtering and temperature compensation and their implementation on PX4

A gyroscope filter

Filtering is used to filter out high-frequency noise and retain low-frequency signals. The defect of filtering processing is that it will cause signal lag. There are two methods of designing filters in the industry, time domain and frequency domain. Filters designed in the time domain, independent of specific models. Frequency domain filtering is more targeted, but computationally expensive

1 principle

1.1 Time Domain Filtering

The filter is mainly to perform convolution calculation on the discrete signal, and the design of the filter in the time domain depends on the empirical formula or the observation of the output waveform to adjust the parameters

The finite impulse response digital filter (Finite Impulse Response Digital Filter, FIR) infinite impulse response digital filter (Infinite Impulse Response Digital Filter, IIR) is expressed as follows

FIR

$y\left( n \right) = \sum\limits_{k = 0}^N {a\left( k \right)x\left( {n - k} \right)}$

IIR

$y\left( n \right) = \sum\limits_{k = 0}^N {a\left( k \right)x\left( {n - k} \right)} + \sum \limits_{j = 0}^P {b\left( j \right)y\left( {n - j} \right)}$

It can be seen that FIR is the smoothing process of the signal looking forward at several time points, and IIR is not only the historical signal, but also combined with the output feedback for smoothing

1.2 Frequency Domain Filtering

Filter parameters can also be designed in the frequency domain

The Fourier principle shows that the sequence or signal of continuous measurement can be expressed as an infinite superposition of sine wave signals of different frequencies. Then, the Fourier transform of the continuous-time signal can be used to analyze the frequency spectrum of the signal

$\begin{array}{l} F\left( {j\omega } \right) = \int_{ - \infty }^\infty {f\left( t \right){e^{ - j\omega t}}dt} \ f\left( t \right) = \frac{1}{?{2\pi }}\int_{ - \infty }^\infty {F\left( {j\ omega } \right){e^{j\omega t}}d\omega } \end{array}$

Its expression can be generalized to discrete signals in form. In contrast, the Discrete Fourier Transform (DFT) spectrum analysis method is more widely used in engineering, such as image processing, because it allows the computer to process the collected discrete signals

$\begin{array}{l} F\left( {?{e^{j\theta }}} \right) = \sum\limits_{k = - \infty }^\infty {f\ left( k \right){e^{ - j\theta k}}} \ f\left( k \right) = \frac{1}{?{2\pi }}\int_{2\pi } { F\left( {?{e^{j\theta }}} \right){e^{j\theta k}}d\theta } \end{array}$

And FFT (Fast Fourier Transform Fast Fourier Transform) is a fast implementation algorithm of DFT, which reduces the computational complexity from $O\left( {?{n^2}} \right) \to O\left( {n\log n} \right)$

From the point of view of the FFT derivation process, its essence is to convert the polynomial coefficient representation into a point value representation within a lower time complexity, and the inverse Fourier transform iFFT converts the point value representation back to the coefficient representation

Suppose the sampling frequency is Fs, and the number of sampling points is N. After doing FFT, the frequency represented by a certain point

${F_n} = \left( {n - 1} \right)*{F_s}/N$

Dividing the modulus value of a + bi at this point by N/2 is the amplitude of the signal corresponding to the frequency (for DC signals, it is divided by N >); the phase of this point is the phase of the signal corresponding to this frequency. Phase Available Expressions

$\varphi =arctan2\left( {b,a} \right)$

come and get

During project implementation, it is usually necessary to truncate periodic time-domain signals. If the truncation time length is not an integer multiple of the period, the signal will leak after truncation. At this time, windowing is needed to make the time-domain signal better meet the periodicity requirements of FFT processing and reduce leakage. Multiplying the original truncated signal with this window function is essentially to unequally weight the time domain signal

The Hanning window is a commonly used window function, and its time domain expression is

$w\left( t \right) = \frac{1}{2}\left( {1 - \cos \frac{?{2\pi t}}{T}} \right)$

With frequency domain characteristics, signals of specific frequencies and amplitudes can be suppressed to achieve noise reduction

If the noise spectrum distribution is narrow, a notch filter can be used, and if it is wide, a band-stop filter can be used. Generally, a notch filter is used in combination with a low-pass filter. Compared with ordinary high-pass and low-pass filters, the notch filter precisely suppresses the frequency band, making the signal attenuate rapidly at certain frequency points

PX4 adopts Notch notch filter and filter series noise reduction

Transfer function of the second-order notch filter for continuous systems

$G\left( s \right) = \frac{?{?{s^2} + \omega _c^2}}{?{?{s^2} + {\omega _{bw} }s + \omega _c^2}}$

Where ${\omega _{bw}}$ is Notch width, unit rad/s; ${\omega _c}$ is the notch central frequency, unit rad/s

It can be discretized by bilinear transformation or pole-zero matching method, and PX4 adopts the latter. For the convenience of readers to understand the code, only the pole-zero matching method is derived here

The zero poles are expressed as

$G\left( s \right) = {K_s}\frac{?{\left( {s - j{\omega _c}} \right)\left( {s + j\omega } \right )}}{?{\left( {s - {p_1}} \right)\left( {s - {p_2}} \right)}}$

where ${K_s} = 1$ ; ${ p_{1,2}} = \alpha \pm \beta$ is the pole of the notch filter, with

$\alpha = - \frac{?{?{\omega _{bw}}}}{2},\beta = \frac{?{\sqrt {4\omega _c^2 - \omega _ {bw}^2} }}{2}$

Zero-pole matching means that the zero-pole in the s domain corresponds to the z-domain, and the i-th zero-pole correspondence is

${z_i} = {e^{?{s_i}{T_s}}}$

So, the z-plane transfer function is expressed as

$G\left( z \right) = {K_z}\frac{?{?{z^2} - 2\cos \left( {?{\omega _c}{T_s}} \right)z + 1}}{?{?{z^2} - 2r\cos \left( {\beta {T_s}} \right)z + {r^2}}}$

where $r = {e^{\alpha {T_s}}},{T_s}$ is the discretization sampling time, unit s

When the s field is 0, the z field is 1, that is

${\left. {G\left( s \right)} \right|_{s = 0}} = {\left. {G\left( z \right)} \right|_{z = 1}}$

substitute available

${K_z} = \frac{?{1 - 2r\cos \left( {\beta {T_s}} \right) + {r^2}}}{?{2 - 2r\cos \left ( {?{\omega _c}{T_s}} \right)}}$

So there is

$G\left( z \right) = \frac{?{?{b_0}{z^2} - {b_1}z + {b_2}}}{?{?{z^2} + { a_1}z + {a_2}}}$

The difference equation in the time domain is

${y_n} = {b_0}{x_n} + {b_1}{x_{n - 1}} + {b_2}{x_{n - 2}} - {a_1}{y_{n - 1} } - {a_2}{y_{n - 2}}$

can be used to iterate over new samples

For another $\alpha$ low-pass filter, the parameter design needs to be combined with the cut-off Frequency, etc., due to space limitations, the content is omitted

2 PX4 filter implementation

2.1STM32 support for DSP

Discrete signal processing on STM32 requires CortexM4 hardware support. Cortex-M4 supports advanced SIMD, MAC instructions. Additionally, Cortex-M4F devices have an FPU (Floating Point Unit) to handle floating point calculations

At the software level, the open source Arm Compiler 5 implements a set of DSP function libraries for discrete signal processing
source code, manual

2.2 Implementation process

PX4 borrows this part of the code to complete the FFT operation, the process is as follows

(1) Convert floating-point numbers to fixed-point format during initialization

for (int n = 0; n < _imu_gyro_fft_len; n ++ ) {
    const float hanning_value = 0.5f * (1.f - cosf(2.f * M_PI_F * n / (_imu_gyro_fft_len - 1)));
    arm_float_to_q15( &hanning_value, &_hanning_window[n], 1);
}

(2) When the gyroscope data is updated, execute the filtering logic

void GyroFFT::Update(const hrt_abstime & amp;timestamp_sample, int16_t *input[], uint8_t N) {
...
}

(3) Window function, multiplication

arm_mult_q15(gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, _imu_gyro_fft_len);

(4) Perform RFFT inverse fast Fourier transform

arm_rfft_q15( &_rfft_q15, _fft_input_buffer, _fft_outupt_buffer);

(5) Determine the noise point according to the peak value, and use the Quiin method to interpolate to estimate the noise frequency

void GyroFFT::FindPeaks(const hrt_abstime & amp;timestamp_sample, int axis, q15_t *fft_outupt_buffer) {
...
}

(6) Combining frequency domain characteristics, design filters, and perform filtering iterations

// Apply dynamic notch filter from FFT
if (_dynamic_notch_fft_available) {
for (int peak = MAX_NUM_FFT_PEAKS - 1; peak >= 0; peak--) {
if (_dynamic_notch_filter_fft[axis][peak].getNotchFreq() > 0.f) {
_dynamic_notch_filter_fft[axis][peak].applyArray(data, N);
}
}
}

In this way, the output signal can be used in the upper layer application

Two temperature compensation

1 principle

Temperature has a significant impact on the measurement results of the IMU and barometer modules. The sensor has a preset reference temperature when it works. When the actual temperature deviates, the measurement result will be offset. Usually, a polynomial is used to describe this offset relationship.

$\Delta b = \sum\limits_{i = 0}^N {?{a_i}\Delta T}$

To implement polynomial operations in computers, the Horner algorithm (also known as Qin Jiushao algorithm) is often used to simplify the execution steps, as follows

$\begin{array}{l} \Delta b = \sum\limits_{i = 0}^N {?{a_i}\Delta {T^i}} = {a_0} + {a_1}\ Delta T + {a_2}\Delta {T^2} + ...\ = {a_0} + {a_1}\left( {\Delta T + {a_2}\left( {\Delta T + {a_3}\ left( {...} \right)} \right)} \right) \end{array}$

2 PX4 implementation

2.1 Calibration

Temperature rise, polynomial fitting calibration coefficient

if (_accel) {
calibrators[num_calibrators] = new TemperatureCalibrationAccel(min_temp_rise, min_start_temp, max_start_temp);
if (calibrators[num_calibrators]) {
+ + num_calibrators;
} else {
PX4_ERR("alloc failed");
}
}

2.2 update

Combining the temperature measurement results to calculate the offset (taking addition as an example)

// calculate the offsets
float delta_temp_2 = delta_temp * delta_temp;
float delta_temp_3 = delta_temp_2 * delta_temp;

for (uint8_t i = 0; i < 3; i ++ ) {
offset[i] = coef.x0[i] + coef.x1[i] * delta_temp + coef.x2[i] * delta_temp_2 + coef.x3[i] * delta_temp_3;
}

From the perspective of implementation, the process of designing filters, sensor calibration, and temperature compensation coefficient calibration are all raw data without any processing. The actual execution process is that the raw sensor data is first filtered, then corrected and temperature compensated, and finally used for upper-layer applications