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.
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 vector is:
The first two parameters are position (m) and the last two parameters are velocity (m/sec).
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
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.
There are two types of observations: position and velocity (both scalar).
The measurement equation for position observation is,
so the observation model is
The measurement equation for velocity observation is,
so the observation model is
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).
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)
True vs estimated values are compared using simulated sensor data.
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.
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.
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.
Since X and Y are independent, they can be separated into two 1D kalman filters. This is convenient because 1D kalman filter is simpler.
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() gs = fig.add_gridspec(6, 2) ax1 = fig.add_subplot(gs[0, 0]) ax2 = fig.add_subplot(gs[1, 0]) ax3 = fig.add_subplot(gs[2, 0]) ax4 = fig.add_subplot(gs[3, 0]) ax5 = fig.add_subplot(gs[4, 0]) ax6 = fig.add_subplot(gs[5, 0]) ax7 = fig.add_subplot(gs[:, 1]) 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]) radii = np.sqrt(eigenvalues) ellipse = Ellipse(xy=(estimated['x'][i], estimated['y'][i]), width=radii[0]*2, height=radii[1]*2, angle=np.rad2deg(np.arccos(eigenvectors[0, 0])), facecolor='none', edgecolor='orange') ax7.add_patch(ellipse) 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 ground_truth['vx'] = np.gradient(ground_truth['x'], t) ground_truth['vy'] = np.gradient(ground_truth['y'], t) ground_truth['ax'] = np.gradient(ground_truth['vx'], t) ground_truth['ay'] = np.gradient(ground_truth['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) } # 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 ground_truth['vx'] = np.gradient(ground_truth['x'], t) ground_truth['vy'] = np.gradient(ground_truth['y'], t) ground_truth['ax'] = np.gradient(ground_truth['vx'], t) ground_truth['ay'] = np.gradient(ground_truth['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 ground_truth['vx'] = np.gradient(ground_truth['x'], t) ground_truth['vy'] = np.gradient(ground_truth['y'], t) ground_truth['ax'] = np.gradient(ground_truth['vx'], t) ground_truth['ay'] = np.gradient(ground_truth['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) } # 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)