# Kalman Filter for 2D Motion

This page describes a method to estimate position and velocity in 2D given position and velocity measurements from devices like GNSS and acceleration measurements from accelerometer. Kalman filter is used with constant velocity model. A working Python code is also provided.

## Overview

Given system and measurement equations,

Discrete-time Kalman filter steps are,

 1 Prediction: state 2 Prediction: covariance 3 Innovation 4 Innovation: covariance 5 Kalman gain 6 Update: state 7 Update: state

Accelerometer data are assumed to be in m/s2. Accelerometer data are not treated as observations. Instead, they are used as inputs in the prediction step.

## State, x

State vector is:

The first two parameters are position (m) and the last two parameters are velocity (m/sec).

## State Transition Model (F) and Control Input Model (G)

State transition matrix F is needed to predict the state and covariance. F is a 4x4 matrix given by:

Control input model matrix G is needed to calculate the process noise Q. G is a 4x2 matrix given by:

F and G are derived as follows:

Let

then the equation of motion in state-space form is

where A is the system matrix and B is continuous input matrix. It is a constant velocity model with acceleration as input. A and B can be discretized using the following property:

Matrix exponential can be solved using the following property:

Using these properties, it can be found that

## Process Noise, Q

Discrete constant white noise model (piecewise white noise model) is used. Acceleration is assumed to be constant for the duration of each time period but differs for each time period. Each of these is uncorrelated between time periods. At each time step, there is a discontinuous jump in acceleration.

From the previous section, we have

where

σax and σay are IMU accelerometer noise (1 standard deviation) in units of m/s2. Assuming the noise is the same in both x and y directions, the process noise matrix is:

Reference: Bar-Shalom. “Estimation with Applications To Tracking and Navigation”. John Wiley & Sons, 2001. Page 274.

## Observation, z

There are two types of observations: position and velocity (both scalar).

## Observation Model, H

### Position

The measurement equation for position observation is,

so the observation model is

### Velocity

The measurement equation for velocity observation is,

so the observation model is

## Observation Noise, R

Observation noise is given by,

σpos is 1-standard deviation position error (positional accuracy estimate) in m obtained from GNSS receiver (often called eph).

σvel is 1-standard deviation speed error (speed accuracy estimate) in m/sec obtained from GNSS receiver (often called sAcc).

## Python Implementation

filter.py

```import numpy as np

class KF:
def __init__(self, x0=0, y0=0, vx0=0, vy0=0,
init_pos_acc=0.5, init_vel_acc=0.5, a_acc=0.35):
'''
x0 -- initial X position [m]
y0 -- initial Y position [m]
vx0 -- initial X velocity [m/sec]
vy0 -- initial Y velocity [m/sec]
init_pos_acc -- initial position accuracy (1-standard deviation) [m]
init_vel_acc -- initial velocity accuracy (1-standard deviation) [m/sec]
a_acc -- accelerometer noise (1-standard deviation). [m/sec^2]
'''
self.x = np.array([[x0], [y0], [vx0], [vy0]]) # State
self.P = np.diag([init_pos_acc ** 2, init_pos_acc ** 2,
init_vel_acc ** 2, init_vel_acc ** 2])
self.W = np.diag([a_acc ** 2, a_acc ** 2])

def predict(self, ax, ay, dt):
'''
ax -- X accelerometer measurement [m/sec^2]
ay -- Y accelerometer measurement [m/sec^2]
dt -- time step [sec]
'''
F = np.array([
[1, 0, dt,  0],
[0, 1,  0, dt],
[0, 0,  1,  0],
[0, 0,  0,  1]
])
G = np.array([
[dt*dt/2,       0],
[      0, dt*dt/2],
[     dt,       0],
[      0,      dt]
])
u = np.array([[ax], [ay]])
self.x = F @ self.x + G @ u
self.P = F @ self.P @ F.T + G @ self.W @ G.T

def update(self, z, H, R):
y = z - H @ self.x
S = H @ self.P @ H.T + R
K = (self.P @ H.T) @ np.linalg.inv(S)
self.x = self.x + (K @ y)
self.P = (np.identity(4) - K @ H) @ self.P

def update_pos(self, x, y, pos_acc):
'''
x -- X position measurement [m]
y -- Y position measurement [m]
pos_acc -- position measurement accuracy (1-standard deviation) [m]
'''
z = np.array([[x], [y]])
H = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
R = np.diag([pos_acc ** 2, pos_acc ** 2])
self.update(z, H, R)

def update_vel(self, vx, vy, vel_acc):
'''
vx -- X velocity measurement [m/sec]
vy -- Y velocity measurement [m/sec]
vel_acc -- velocity measurement accuracy (1-standard deviation) [m/sec]
'''
z = np.array([[vx], [vy]])
H = np.array([
[0, 0, 1, 0],
[0, 0, 0, 1]
])
R = np.diag([vel_acc ** 2, vel_acc ** 2])
self.update(z, H, R)
```

## Simulation Test

True vs estimated values are compared using simulated sensor data.

### Test 1: stationary

Target is not moving and is fixed at x=1m y=0.5m position. Position error starts large, decreases, and converges to a value. Same with velocity error.

### Test 2: moving at constant velocity with measurement loss

Target is moving with velocity (1, 0.5) m/sec and there is no GNSS measurement between 1 to 4 seconds. Position error increases while there is no measurement. Error diminishes once measurement is available. Confidence ellipse is a circle.

### Test 3: accelerating

Target moves at a constant speed of 1m/sec in x-direction and accelerating at 0.5m/sec2 in the y-direction. There is no problem tracking thanks to accelerometer input and non-zero process noise. Confidence Ellipse is a circle.

### Note

Since X and Y are independent, they can be separated into two 1D kalman filters. This is convenient because 1D kalman filter is simpler.

### Source code for the above tests

It is assumed that filter.py is placed in the same folder as the below script.

```import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse
import numpy as np
import filter

random_generator = np.random.default_rng(0)

def plot(t, ground_truth, gnss, a, estimated):
estimated_x = np.array(estimated['x'])
estimated_y = np.array(estimated['y'])
estimated_vx = np.array(estimated['vx'])
estimated_vy = np.array(estimated['vy'])
x_std = np.sqrt(np.array(estimated['pos_cov'])[:, 0, 0])
y_std = np.sqrt(np.array(estimated['pos_cov'])[:, 1, 1])
vx_std = np.sqrt(np.array(estimated['vel_cov'])[:, 0, 0])
vy_std = np.sqrt(np.array(estimated['vel_cov'])[:, 1, 1])

fig = plt.figure()

ax1.plot(t, ground_truth['ax'], color='red', label='true')
ax1.plot(t, a['x'], '.', markersize=1, label='measured')
ax1.grid(True)
ax1.legend(loc='upper right')
ax1.set_ylabel('ax (m/s^2)')

ax2.plot(t, ground_truth['ay'], color='red', label='true')
ax2.plot(t, a['y'], '.', markersize=1, label='measured')
ax2.grid(True)
ax2.legend(loc='upper right')
ax2.set_ylabel('ay (m/s^2)')

ax3.plot(t, ground_truth['x'], color='red', label='true')
ax3.plot(t, gnss['x'], '.', markersize=1, label='measured')
ax3.plot(t, estimated_x, color='black', label='estimated')
ax3.fill_between(t,
estimated_x-x_std,
estimated_x+x_std,
color='gray', alpha=0.5)
ax3.grid(True)
ax3.legend(loc='upper right')
ax3.set_ylabel('x (m)')

ax4.plot(t, ground_truth['y'], color='red', label='true')
ax4.plot(t, gnss['y'], '.', markersize=1, label='measured')
ax4.plot(t, estimated_y, color='black', label='estimated')
ax4.fill_between(t,
estimated_y-y_std,
estimated_y+y_std,
color='gray', alpha=0.5)
ax4.grid(True)
ax4.legend(loc='upper right')
ax4.set_ylabel('y (m)')

ax5.plot(t, ground_truth['vx'], color='red', label='true')
ax5.plot(t, gnss['vx'], '.', markersize=1, label='measured')
ax5.plot(t, estimated_vx, color='black', label='estimated')
ax5.fill_between(t,
estimated_vx-vx_std,
estimated_vx+vx_std,
color='gray', alpha=0.5)
ax5.grid(True)
ax5.legend(loc='upper right')
ax5.set_ylabel('vx (m/s)')

ax6.plot(t, ground_truth['vy'], color='red', label='true')
ax6.plot(t, gnss['vy'], '.', markersize=1, label='measured')
ax6.plot(t, estimated_vy, color='black', label='estimated')
ax6.fill_between(t,
estimated_vy-vy_std,
estimated_vy+vy_std,
color='gray', alpha=0.5)
ax6.grid(True)
ax6.legend(loc='upper right')
ax6.set_ylabel('vy (m/s)')

ax7.plot(ground_truth['x'], ground_truth['y'], color='red', label='true')
ax7.plot(gnss['x'], gnss['y'], '.', markersize=1, label='measured')
ax7.plot(estimated_x, estimated_y, color='black', label='estimated')
ax7.grid(True)
ax7.legend(loc='upper right')
ax7.set_xlabel('x (m)')
ax7.set_ylabel('y (m)')
ax7.axis('equal')

# Confidence ellipse
for i in range(len(estimated['x'])):
if i % 50 == 0:
eigenvalues, eigenvectors = np.linalg.eig(estimated['pos_cov'][i])
ellipse = Ellipse(xy=(estimated['x'][i], estimated['y'][i]),
facecolor='none',
edgecolor='orange')

plt.show()

def estimate(t, gnss, a):
N = len(t)
result = {
'x'      : [None] * N,
'y'      : [None] * N,
'vx'     : [None] * N,
'vy'     : [None] * N,
'pos_cov': [None] * N,
'vel_cov': [None] * N
}
kf = filter.KF()

result['x'][0] = kf.x[0, 0]
result['y'][0] = kf.x[1, 0]
result['vx'][0] = kf.x[2, 0]
result['vy'][0] = kf.x[3, 0]
result['pos_cov'][0] = kf.P[0:2, 0:2]
result['vel_cov'][0] = kf.P[2:4, 2:4]
for i in range(1, N):
dt = t[i] - t[i-1]
kf.predict(a['x'][i], a['y'][i], dt)
if ~np.isnan(gnss['x'][i]):
kf.update_pos(gnss['x'][i], gnss['y'][i], gnss['pos_std'][i])
if ~np.isnan(gnss['vx'][i]):
kf.update_vel(gnss['vx'][i], gnss['vy'][i], gnss['vel_std'][i])
result['x'][i] = kf.x[0, 0]
result['y'][i] = kf.x[1, 0]
result['vx'][i] = kf.x[2, 0]
result['vy'][i] = kf.x[3, 0]
result['pos_cov'][i] = kf.P[0:2, 0:2]
result['vel_cov'][i] = kf.P[2:4, 2:4]

return result

def generate_test1_data():
# Generate time
dt = 0.01
t = np.arange(0, 5, dt)

# Generate ground truth data
# Position stationary
x0 = 1
y0 = 0.5
ground_truth = {}
ground_truth['x'] = 0 * t + x0
ground_truth['y'] = 0 * t + y0

# Generate GNSS sensor data
stdev = 0.1
gnss = {
'x'      : random_generator.normal(ground_truth['x'], stdev),
'y'      : random_generator.normal(ground_truth['y'], stdev),
'pos_std': [stdev] * len(t),
'vx'     : random_generator.normal(ground_truth['vx'], stdev),
'vy'     : random_generator.normal(ground_truth['vy'], stdev),
'vel_std': [stdev] * len(t)
}

# Generate accelerometer data
a = {
'x': random_generator.normal(ground_truth['ax'], 0.35),
'y': random_generator.normal(ground_truth['ay'], 0.35)
}

return t, ground_truth, gnss, a

def generate_test2_data():
# Generate time
dt = 0.01
t = np.arange(0, 5, dt)

# Generate ground truth data
# Position stationary
vx = 1
vy = 0.5
ground_truth = {}
ground_truth['x'] = vx * t
ground_truth['y'] = vy * t

# Generate GNSS sensor data
stdev = 0.1
gnss = {
'x'      : random_generator.normal(ground_truth['x'], stdev),
'y'      : random_generator.normal(ground_truth['y'], stdev),
'pos_std': [stdev] * len(t),
'vx'     : random_generator.normal(ground_truth['vx'], stdev),
'vy'     : random_generator.normal(ground_truth['vy'], stdev),
'vel_std': [stdev] * len(t)
}

# Remove GNSS data betweeen 1 to 4 seconds
for i in range(len(t)):
if t[i] > 1 and t[i] < 4:
gnss['x'][i], gnss['y'][i] = np.nan, np.nan
gnss['vx'][i], gnss['vy'][i] = np.nan, np.nan

# Generate accelerometer data
a = {
'x': random_generator.normal(ground_truth['ax'], 0.35),
'y': random_generator.normal(ground_truth['ay'], 0.35)
}

return t, ground_truth, gnss, a

def generate_test3_data():
# Generate time
dt = 0.01
t = np.arange(0, 5, dt)

# Generate ground truth data
# Position stationary
vx = 1
ay = 0.5
ground_truth = {}
ground_truth['x'] = vx * t
ground_truth['y'] = 0.5 * ay * t ** 2

# Generate GNSS sensor data
stdev = 0.1
gnss = {
'x'      : random_generator.normal(ground_truth['x'], stdev),
'y'      : random_generator.normal(ground_truth['y'], stdev),
'pos_std': [stdev] * len(t),
'vx'     : random_generator.normal(ground_truth['vx'], stdev),
'vy'     : random_generator.normal(ground_truth['vy'], stdev),
'vel_std': [stdev] * len(t)
}

# Generate accelerometer data
a = {
'x': random_generator.normal(ground_truth['ax'], 0.35),
'y': random_generator.normal(ground_truth['ay'], 0.35)
}

return t, ground_truth, gnss, a

# Generate test data
#t, ground_truth, gnss, a = generate_test1_data()
t, ground_truth, gnss, a = generate_test2_data()
#t, ground_truth, gnss, a = generate_test3_data()

# Estimate with Kalman filter
estimated = estimate(t, gnss, a)

# Graph result
plot(t, ground_truth, gnss, a, estimated)
```