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 20 samples pre second (dt = 0.05). The simulated noise distributions are typical for MEMS sensors at this rate (σh = 0.42 m, σa = 0.075 m/s2).

The state transition matrix describes the following physical model:

\[ \begin{equation} h_{n + 1} = h_n + v_n dt + a_n \frac{dt^2}{2} \end{equation} \] \[ \begin{equation} v_{n + 1} = v_n + a_n dt \end{equation} \] \[ \begin{equation} a_{n + 1} = a_n \end{equation} \] \[ \begin{equation} F = \begin{bmatrix} 1 & dt & \frac{dt^2}{2} \\ 0 & 1 & dt \\ 0 & 0 & 1 \\ \end{bmatrix} \end{equation} \]

The measurement covariance matrix:

\[ \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.


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

© Ivan Gorinov, 2016