## Vertical speed estimation by Kalman filterVertical 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 S
The 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 Readings from independent sensors can be processed one input input at a time (sequential updates), enabling different sampling rates. In this case, accelerometer can read up to 1600 samples per second (limited by 400 in Android) while barometers read at much lower rates. ## Vertical acceleration## Device orientationAn orientation sensor in Android provides a unit quaternion that describes rotation of the device frame. It is usually implemented as another Kalman filter (extended) in a sensor hub's firmware. A vector from the acceleration sensor can be projected to the reference frame by two quaternion multiplications (Hamilton products): \[ \begin{equation} a' = q a q^{\ast} \end{equation} \]## Gravity variations
Standard gravity g 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- Using the Game Rotation Vector Sensor - Android Developers
- Federal Aviation Administration, 2016 Pilot's Handbook of Aeronautical Knowledge
- Inertial Measurement Unit - Bosch Sensortec
© Ivan Gorinov, 2016 |