Vertical speed estimation by Kalman filter

Vertical speed estimation can be improved by adding an accelerometer to measure vertical acceleration and using Kalman filter to process the data from multiple sensors.

Using an accelerometer requires tracking the device orientation to compute vertical component from three-dimensional accelerometer unit. This problem can be solved by integration of MEMS gyro sensor data [1].

This example simulates data from an accelerometer and altimeter based on barometric sensor at 25 samples per second (Δt = 0.04 s). Simulated noise spectral density for accelerometer Sa = 300 μg/√Hz = 0.002942 (m/s²)/√Hz, listed as maximum noise density for Google Pixel 3 accelerometer [3]. Simulated accelerometer bias is set to 0.03 m/s² to illustrate how sensor bias affects the filter. Altitude noise: σh = 0.1 m.

The state transition matrix describes the following physical model.

Altitude:

\[ \begin{equation} h_{n + 1} = h_n + v_n dt + a_n \frac{\Delta t^2}{2} \end{equation} \]

Vertical speed:

\[ \begin{equation} v_{n + 1} = v_n + a_n \Delta t \end{equation} \]

Vertical acceleration:

\[ \begin{equation} a_{n + 1} = a_n \end{equation} \] \[ \begin{equation} F = \begin{bmatrix} 1 & \Delta t & \frac{\Delta t^2}{2} \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 \\ \end{bmatrix} \end{equation} \]

Accelerometer measurement variance is computed from the noise spectral density. The bandwidth is between zero an half of the sampling rate:

\[ \begin{equation} \sigma_a^2 = S_a^2 \frac{f_s}{2} = S_a^2 \frac{1}{2 \Delta t} \end{equation} \]

The measurement covariance matrix is diagonal, which means the sensors are independent:

\[ \begin{equation} R = \begin{bmatrix} \sigma_h^2 & 0 \\ 0 & \sigma_a^2 \\ \end{bmatrix} \end{equation} \] Linear regression vs. Kalman filter

Processing data from both sensors with Kalman filter gets a good estimation of the climb/descent speed. Using only barometric sensor data gives more noisy result with a few seconds lag.

Similar idea is used in Instantaneous VSI (IVSI), an aircraft instrument that has a vertical accelerometer and has little or no lag comparing to the typical VSI that uses only static port pressure [2].

Simulation program source in Python

The simulation program uses filterpy library by Roger Labbe.

References

  1. Using the Game Rotation Vector Sensor - Android Developers
  2. Federal Aviation Administration, 2016 Pilot's Handbook of Aeronautical Knowledge
  3. Inertial Measurement Unit - Bosch Sensortec

© Ivan Gorinov, 2016