# How to use Kalman filter for altitude prediction based on barometer data?

I have barometer noisy data with known variance.
I studied Kalman filter but I did not find an answer to this problem:
My process model is: altitude is changed because of velocity that is changed because of acceleration that is normally distributed.
s[k+1]=s[k]+v[k]*dt+a[k]dtdt/2
Is my process state just (altitude) or (altitude and velocity) or (altitude, velocity, acceleration)?
When I use velocity and acceleration - how shall I fill the measurement matrix when measuring just altitude? In some examples, I have seen 0 as a measurement of unknown variables, but it makes no sense to me, because velocity and acceleration remain constant, because no correction is applied to them.
My goal is to mainly compute velocity.
Is there some other recommended algorithm to estimate the vertical velocity?

UPDATE: I will try to ask the more specific question about Kalman filter :-)

• I have state X = (s, v, a) - trajectory, velocity, acceleration I have state transition model F = ((1 dt dt*dt/2)(0 1 dt)(0 0 1)) (where dt is interval between last and current measurement)

• I have one-dimensional measurement Z (I measure trajectory only)

• So observation model H is (1 0 0)?

I have seen examples where only acceleration was measured and H was (0 0 1), but when acceleration was corrected, velocity and trajectory was updated also in "a priori state estimate" because velocity is dependent on acceleration. But will the velocity and acceleration be updated when they are not dependent on trajectory? See bold zeros in F matrix.

• You might find this useful. – A_A May 1 '18 at 10:54
• Thanks for answer and comment. To my original question - how to fill measurement vector when I measure just position but I want Kalman filter to adjust velocity also, so I can use this value? – user1853472 May 2 '18 at 12:12
• Are there any known forces acting on the system, which could be the cause of the acceleration. Or if it is unknown do you know if the acceleration changes fast or slowly over time? – fibonatic May 2 '18 at 16:06
• Application for paragliding - vertical speed is limited to cca 20m/s, acceleration is random but changes quite continuously, max acceleration about 10m/s/s down and cca 5m/s/s up. – user1853472 May 2 '18 at 19:50

You might get good results when you consider the acceleration as an input $u$, so the model could then be written as

$$x[k+1] = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} x[k] + \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} u[k] + w[k]$$

$$y[k] = \begin{bmatrix} 1 & 0 \end{bmatrix} x[k] + v[k]$$

where $x[k]$ a vector with the position and velocity and $w[k],v[k]$ zero mean Gaussian white noise. Here $v[k]$ has a covariance $R$ equal to the variance of the barometer and $w[k]$ probably has the following covariance $Q$

$$Q = \sigma_u^2\, \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix}^\top = \sigma_u^2\, \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} \\ \frac{\Delta t^3}{2} & \Delta t^2 \end{bmatrix}$$

where $\sigma_u^2$ the variance of the measured acceleration (this assumes that there are not disturbance forces acting on the system).

In terms of alternative algorithms there is the alpha beta filter https://en.m.wikipedia.org/wiki/Alpha_beta_filter

and Savitzky Golay.

https://en.m.wikipedia.org/wiki/Savitzky%E2%80%93Golay_filter

Alpha Beta is an online algorithm while Savitzky Golay needs data on both sides, before and after.

In terms of a Kalman Filter, if your state observation system is observable and controllable, you don’t have to directly observe your state. The measurement matrix accommodates what you can directly measure and what you can’t.

The alpha beta filter is conceptually simpler and works well for slowly evolving systems. If you have big jumps in your data, it suffers from lag and overshoot. Even Kalman Filters require modification in those circumstances

• Thanks - I tried to study Alpha Beta filter and that is exactly what I need but I would like it to adjust automatically not to give it constant parameters. AlphaBeta is adjusting velocity and I would like to find how to use Kalman to do exactly the same thing - to adjust velocity in my state vector. May I fill the measurement vector with measured altitude and velocity and acceleration computed from this and previous value? – user1853472 May 2 '18 at 12:17
• The short answer is no. While you studied Kalman Filtering, you need to study more. You keep on asking the same wrong question. States are states and measurements are measurements. You can have a measurement that is directly related to a state, but it is corrupted by noise. A necessary condition for a Kalman Filter is measurements are noisy. If that isn’t the case, you need a State Observer, not a Kalman Filter. Try editing your original post with all your equations in matrix notation and then ask for specific advice. – Stanley Pawlukiewicz May 2 '18 at 13:18
• I have noisy measurements of trajectory and i know its variance. When I use AlphaBetaFilter it will predict position and velocity. On wikipedia is written that: A Kalman filter designed to track a moving object using a constant-velocity target dynamics (process) model (i.e., constant velocity between measurement updates) with process noise covariance and measurement covariance held constant will converge to the same structure as an alpha-beta filter. Im trying to write the Kalman filter will converge to AlphaBetaGama filter (with added acceleration). Is it wrong question? – user1853472 May 2 '18 at 20:21

To get the most out of Kalman filter, both altitude and vertical acceleration should be measured:

$$H = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix}$$

Vertical acceleration can be computed by rotating 3d accelerometer output using quarternion from orientation sensor (which is usually another extended Kalman filter) and subtracting the gravity.

With only altitude sensor, the filter must have significant lag to process noisy data. With only acceleration sensor, it will accumulate integration errors and drift out of bounds.

The following example compares Kalman filter (2 sensors) with linear regression (altitude only) on simulated data. It is based on the filterpy library by Roger R. Labbe Jr.

import numpy as np
import matplotlib.pyplot as plt
import random

from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise

random.seed(65537)

# Standard deviation of simulated sensor data
std_a = 0.075
std_h = 0.42

# Earth gravity
g_n = 9.80665

# Least Squares Linear Regression

class LinearRegression:
def __init__(self, N):
self.N = N
self.c = 0
self.x = np.zeros(N)
self.y = np.zeros(N)

def update(self, x, y):
self.x[1:] = self.x[:-1]
self.y[1:] = self.y[:-1]

self.x[0] = x
self.y[0] = y
if (self.c < self.N):
self.c += 1

def slope(self):

if (self.c < self.N):
return 0

sum_x = np.sum(self.x)
sum_y = np.sum(self.y)
sum_xx = np.sum(self.x * self.x)
sum_xy = np.sum(self.x * self.y)
sum_yy = np.sum(self.y * self.y)

a = (self.N * sum_xy - sum_x * sum_y) / (self.N * sum_xx - sum_x * sum_x)

return a

# Sensor sample period, s
dt = 0.05

# Linear regression size
M = 96
lr = LinearRegression(M)

kf = KalmanFilter(dim_x=3, dim_z=2)
kf.H = np.array([[1, 0, 0], [0, 0, 1]])
kf.F = np.array([[1, dt, dt * dt * 0.5], [0, 1, dt], [0, 0, 1]])

# initial process covariance
kf.P = np.array([[std_h * std_h, 0, 0], [0, 1, 0], [0, 0, std_a * std_a]])

# Process noise matrix
std = 0.004
var = std * std
kf.Q = Q_discrete_white_noise(dim=3, dt=dt, var=var)

# Measurement covariance
kf.R *= np.array([[std_h * std_h, 0], [0, std_a * std_a]])

n = 300
r_n = 1.0 / n

t = np.zeros(n)
h_sim = np.zeros(n)
v_sim = np.zeros(n)
a_sim = np.zeros(n)
measured_h = np.zeros(n)
measured_a = np.zeros(n)
v_est_lr = np.zeros(n)
v_est_kf = np.zeros(n)

v0 = 0
v = v0
h = 0

for i in range(n):
t[i] = (i * dt)
a = 1.0 / 32 * g_n * np.sin(4 * np.pi * i * r_n)
a_sim[i] = (a)
v += a * dt
v_sim[i] = v
h += v * dt + (a * dt * dt) / 2
h_sim[i] = h
measured_a[i] = a + random.gauss(0, std_a)
measured_h[i] = h + random.gauss(0, std_h)

# Compute the speed estimations

for i in range(n):
v_est_kf[i] = kf.x[1]
kf.predict()
kf.update(np.array([[measured_h[i]], [measured_a[i]]]))

lr.update(i * dt, measured_h[i])
v_est_lr[i] = lr.slope()

# Plot the results

plt.figure(1, figsize=(8, 12), dpi=80)

plt.subplot(311)
plt.axis([0, n * dt, -0.75, 0.75])
plt.plot(t, measured_a, 'y+')
plt.plot(t, a_sim, 'r')
plt.title('Vertical acceleration - gravity, m/s^2')
plt.legend(('Measured', 'True (simulation)'), loc='best')

plt.subplot(312)
plt.axis([0, n * dt, -1, 7])
plt.plot(t, measured_h, 'c+')
plt.plot(t, h_sim, 'b')
plt.title('Altitude, m')
plt.legend(('Measured', 'True (simulation)'), loc='best')

plt.subplot(313)
plt.axis([0, n * dt, -0.5, 1.0])
plt.plot(t, v_sim, 'b')
plt.plot(t, v_est_kf, 'g')
plt.plot(t[M:], v_est_lr[M:], 'r')
plt.title('Vertical speed, m/s')
plt.legend(('True (simulation)', 'Kalman filter', 'Linear Regression'), loc='best')
plt.show()