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

Question

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.

Answer 1

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).

Answer 2

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

Answer 3

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()