In this tutorial we will consider filtering of a 1D position track, similar in spirit to what one could have obtained from a GPS device, but limited to 1D for easier visualization. We will use a constant-velocity model, i.e., use a double integrator,

\begin{aligned} x_{k+1} &= \begin{bmatrix} 1 & T_s \\ 0 & 1 \end{bmatrix} x_k + \begin{bmatrix} T_s^2/2 \\ T_s \end{bmatrix} w_k \\ y_k &= \begin{bmatrix} 1 & 0 \end{bmatrix} x_k + v_k \end{aligned}

where $w_k \sim \mathcal{N}(0, σ_w)$ is the process noise, and $v_k \sim \mathcal{N}(0, R_2)$ is the measurement noise, and illustrate how we can make use of an adaptive noise covariance to improve the filter performance.

## Data generation

We start by generating some position data that we want to perform filtering on. The "object" we want to track is initially stationary, and transitions to moving with a constant velocity after a while.

using LowLevelParticleFilters, Plots, Random
Random.seed!(1)

# Create a time series for filtering
x = [zeros(50); 0:100]
T = length(x)
Y = x + randn(T)
plot([Y x], lab=["Measurement" "True state to be tracked"], c=[1 :purple])

## Simple Kalman filtering

We will use a Kalman filter to perform the filtering. The model is a double integrator, i.e., a constant-acceleration model. The state vector is thus $x = [p, v]^T$, where $p$ is the position and $v$ is the velocity. When designing a Kalman filter, we need to specify the noise covariances $R_1$ and $R_2$. While it's often easy to measure the covariance of the measurement noise, $R_2$, it can be quite difficult to know ahead of time what the dynamics noise covariance, $R_1$, should be. In this example, we will use an adaptive filter, where we will increase the dynamics noise covariance if the filter prediction error is too large. However, we first run the filter twice, once with a large $R_1$ and once with a small $R_1$ to illustrate the difference.

y = [[y] for y in Y] # create a vector of vectors for the KF
u = fill([], T) # No inputs in this example :(

# Define the model
Ts = 1
A = [1 Ts; 0 1]
B = zeros(2, 0)
C = [1 0]
D = zeros(0, 0)
R2 = [1;;]

σws = [1e-2, 1e-5] # Dynamics noise standard deviations

fig = plot(Y, lab="Measurement")
for σw in σws
R1 = σw*[Ts^3/3 Ts^2/2; Ts^2/2 Ts] # The dynamics noise covariance matrix is σw*Bw*Bw' where Bw = [Ts^2/2; Ts]
kf = KalmanFilter(A, B, C, D, R1, R2)
yh = []
measure = LowLevelParticleFilters.measurement(kf)
for t = 1:T # Main filter loop
kf(u[t], y[t]) # Performs both prediction and correction
xh = state(kf)
yht = measure(xh, u[t], nothing, t)
push!(yh, yht)
end

Yh = reduce(hcat, yh)

## Disturbance modeling and noise tuning

See this notebook for a blog post about disturbance modeling and noise tuning using LowLevelParticleFilter.jl