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 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.01 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} \]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 accelerationDevice 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 variationsStandard gravity gn is just an average value. It varies from 9.780 at the equator to 9.832 at the poles. It also decreases with altitude. 
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
© Ivan Gorinov, 2016 |