Kalman Filter for 1D Motion with Acceleration and Bias

This page describes a method to estimate position, velocity, and accelerometer bias in 1D 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. Accelerometer measurement and true acceleration is related with the following property:

where b is accelerometer bias.

State, x

State vector is:

x is position (m), v is velocity (m/sec), and b is accelerometer bias (m/sec2).

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

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

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

F and G are derived as follows:

We can write the state equation as

which can be rewritten in matrix form:

Process Noise, Q

The process noise is a combination of two parts. The first part is due to acceleration and the second part is due to bias drift.

For the process noise due to acceleration, we use a discrete constant white noise model (piecewise white noise model). Let,

where

σa is the IMU accelerometer noise (1 standard deviation) in units of m/s2. The process noise matrix for acceleration is then:

For the process noise due to accelerometer bias drift, we let,

where

The process noise matrix for bias drift is:

σb is the bias noise (1 standard deviation) in units of m/s2.

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,

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

σv 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, v0=0, b0=0, x0_acc=0.5, v0_acc=0.5, b0_acc=0.2,
                 a_acc=0.35, b_acc=0.01):
        '''
        x0 -- initial position [m]
        v0 -- initial velocity [m/sec]
        b0 -- initial accelerometer bias [m/sec^2]
        x0_acc -- initial position accuracy (1-standard deviation) [m]
        v0_acc -- initial velocity accuracy (1-standard deviation) [m/sec]
        b0_acc -- initial accelerometer bias accuracy (1-standard deviation) [m/sec^2]
        a_acc -- accelerometer noise (1-standard deviation) [m/sec^2]
        b_acc -- accelerometer bias noise (1-standard deviation) [m/sec^3]
        '''
        self.x = np.array([[x0], [v0], [b0]]) # State
        self.P = np.diag([x0_acc ** 2, v0_acc ** 2, b0_acc ** 2])
        self.W = np.array([[a_acc ** 2]])
        self.Q_b = np.array([
            [0, 0,        0],
            [0, 0,        0],
            [0, 0, b_acc**2]
        ])

    def predict(self, a, dt):
        '''
        a -- accelerometer measurement [m/sec^2]
        dt -- time step [sec]
        '''
        F = np.array([
            [1, dt, -dt*dt/2],
            [0,  1,      -dt],
            [0,  0,        1]
        ])
        G = np.array([
            [dt*dt/2],
            [     dt],
            [      0]
        ])
        u = np.array([[a]])
        self.x = F @ self.x + G @ u
        self.P = F @ self.P @ F.T + G @ self.W @ G.T + self.Q_b
    
    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(3) - K @ H) @ self.P

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

    def update_vel(self, vel, vel_acc):
        '''
        vel -- velocity measurement [m/sec]
        vel_acc -- velocity measurement accuracy (1-standard deviation) [m/sec]
        '''
        z = np.array([[vel]])
        H = np.array([
            [0, 1, 0]
        ])
        R = np.array([[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 position with fixed accelerometer bias of 0.5m/sec2. Acceleration measurements are offset slightly from true acceleration due to accelerometer bias. Position, velocity, and bias error all converge. Bias is estimated correctly after about 1 second.

Test 2: stationary with measurement loss

Target is not moving and there is no measurement between 1 to 4 seconds. There is also fixed accelerometer bias of 0.5m/sec2. Position, velocity, and bias errors increase while there is no measurement.

Test 3: moving at constant velocity

Target moves at a constant speed of 1m/sec. There is no problem tracking.

Test 4: stationary with measurement at low sampling rate

Target is not moving and there is GNSS measurement at 5Hz. There is also fixed accelerometer bias of 0.5m/sec2. It takes longer for the bias to converge to the true value.

Test 5: stationary with only velocity measurements

Target is not moving and there is position measurement loss between 1 to 4 seconds. There is also fixed accelerometer bias of 0.5m/sec2. Since we are tracking with only velocity, position error slowly increases over time. Bias seems unaffected.

Test 6: moving at constant velocity with only position measurements

Target is moving at a constant speed of 1m/sec. There is no velocity measurement. Accelerometer bias is fixed at 0.5m/sec2. Although we have no velocity measurement, velocity is correctly calculated after about 1 second. Bias also converges to the correct value after 2 seconds.

Test 7: moving at constant velocity with bias drift

Target is moving at a constant speed of 1m/sec. Accelerometer bias is increasing at 0.1m/sec2/sec. Some tuning of accelerometer bias noise was required for the bias to be tracked correctly.

Test 8: moving in a sine wave with bias drift

Target is moving in a sine wave at 0.5Hz and 1m amplitude. Accelerometer bias is increasing at 0.1m/sec2/sec. Position, velocity, and bias are correctly tracked.

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
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_v = np.array(estimated['v'])
    estimated_b = np.array(estimated['b'])
    x_std = np.array(estimated['x_std'])
    v_std = np.array(estimated['v_std'])
    b_std = np.array(estimated['b_std'])

    fig, axs = plt.subplots(4, 1)

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

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

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

    axs[3].plot(t, ground_truth['b'], color='red', label='true')
    axs[3].plot(t, estimated_b, color='black', label='estimated')
    axs[3].fill_between(t, 
                        estimated_b-b_std,
                        estimated_b+b_std,
                        color='gray', alpha=0.5)
    axs[3].grid(True)
    axs[3].legend(loc='upper right')
    axs[3].set_ylabel('a_bias (m/s^2)')

    plt.show()


def estimate(t, gnss, a):
    N = len(t)
    result = {
        'x'    : [None] * N,
        'v'    : [None] * N,
        'b'    : [None] * N,
        'x_std': [None] * N,
        'v_std': [None] * N,
        'b_std': [None] * N
    }
    kf = filter.KF()

    result['x'][0] = kf.x[0, 0]
    result['v'][0] = kf.x[1, 0]
    result['b'][0] = kf.x[2, 0]
    result['x_std'][0] = np.sqrt(kf.P[0, 0])
    result['v_std'][0] = np.sqrt(kf.P[1, 1])
    result['b_std'][0] = np.sqrt(kf.P[2, 2])
    for i in range(1, N):
        dt = t[i] - t[i-1]
        kf.predict(a[i], dt)
        if ~np.isnan(gnss['x'][i]):
            kf.update_pos(gnss['x'][i], gnss['x_std'][i])
        if ~np.isnan(gnss['v'][i]):
            kf.update_vel(gnss['v'][i], gnss['v_std'][i])
        result['x'][i] = kf.x[0, 0]
        result['v'][i] = kf.x[1, 0]
        result['b'][i] = kf.x[2, 0]
        result['x_std'][i] = np.sqrt(kf.P[0, 0])
        result['v_std'][i] = np.sqrt(kf.P[1, 1])
        result['b_std'][i] = np.sqrt(kf.P[2, 2])

    return result


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

    # Generate ground truth data
    # Position stationary with constant accelerometer bias
    x0 = 1
    b0 = 0.5
    ground_truth = {}
    ground_truth['x'] = 0 * t + x0
    ground_truth['v'] = np.gradient(ground_truth['x'], t)
    ground_truth['a'] = np.gradient(ground_truth['v'], t)
    ground_truth['b'] = 0 * t + b0

    # Generate GNSS sensor data
    stdev = 0.1
    gnss = {
        'x'    : random_generator.normal(ground_truth['x'], stdev),
        'x_std': [stdev] * len(t),
        'v'    : random_generator.normal(ground_truth['v'], stdev),
        'v_std': [stdev] * len(t)
    }

    # Generate accelerometer data
    a = random_generator.normal(ground_truth['a'] + ground_truth['b'], 0.35)

    return t, ground_truth, gnss, a


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

    # Generate ground truth data
    # Position stationary with constant accelerometer bias
    x0 = 1
    b0 = 0.5
    ground_truth = {}
    ground_truth['x'] = 0 * t + x0
    ground_truth['v'] = np.gradient(ground_truth['x'], t)
    ground_truth['a'] = np.gradient(ground_truth['v'], t)
    ground_truth['b'] = 0 * t + b0

    # Generate GNSS sensor data
    stdev = 0.1
    gnss = {
        'x'    : random_generator.normal(ground_truth['x'], stdev),
        'x_std': [stdev] * len(t),
        'v'    : random_generator.normal(ground_truth['v'], stdev),
        'v_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['x_std'][i] = np.nan, np.nan
            gnss['v'][i], gnss['v_std'][i] = np.nan, np.nan

    # Generate accelerometer data
    a = random_generator.normal(ground_truth['a'] + ground_truth['b'], 0.35)

    return t, ground_truth, gnss, a


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

    # Generate ground truth data
    # Position moving at constant velocity with constant accelerometer bias
    v = 1
    b0 = 0.5
    ground_truth = {}
    ground_truth['x'] = v * t
    ground_truth['v'] = np.gradient(ground_truth['x'], t)
    ground_truth['a'] = np.gradient(ground_truth['v'], t)
    ground_truth['b'] = 0 * t + b0

    # Generate GNSS sensor data
    stdev = 0.1
    gnss = {
        'x'    : random_generator.normal(ground_truth['x'], stdev),
        'x_std': [stdev] * len(t),
        'v'    : random_generator.normal(ground_truth['v'], stdev),
        'v_std': [stdev] * len(t)
    }

    # Generate accelerometer data
    a = random_generator.normal(ground_truth['a'] + ground_truth['b'], 0.35)

    return t, ground_truth, gnss, a


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

    # Generate ground truth data
    # Position stationary with constant accelerometer bias
    x0 = 1
    b0 = 0.5
    ground_truth = {}
    ground_truth['x'] = 0 * t + x0
    ground_truth['v'] = np.gradient(ground_truth['x'], t)
    ground_truth['a'] = np.gradient(ground_truth['v'], t)
    ground_truth['b'] = 0 * t + b0

    # Generate GNSS sensor data
    stdev = 0.1
    gnss = {
        'x'    : random_generator.normal(ground_truth['x'], stdev),
        'x_std': [stdev] * len(t),
        'v'    : random_generator.normal(ground_truth['v'], stdev),
        'v_std': [stdev] * len(t)
    }

    # Make GNSS data sample at 5Hz
    for i in range(len(t)):
        if i % 20 != 0:
            gnss['x'][i], gnss['x_std'][i] = np.nan, np.nan
            gnss['v'][i], gnss['v_std'][i] = np.nan, np.nan

    # Generate accelerometer data
    a = random_generator.normal(ground_truth['a'] + ground_truth['b'], 0.35)

    return t, ground_truth, gnss, a


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

    # Generate ground truth data
    # Position stationary with constant accelerometer bias
    x0 = 1
    b0 = 0.5
    ground_truth = {}
    ground_truth['x'] = 0 * t + x0
    ground_truth['v'] = np.gradient(ground_truth['x'], t)
    ground_truth['a'] = np.gradient(ground_truth['v'], t)
    ground_truth['b'] = 0 * t + b0

    # Generate GNSS sensor data
    stdev = 0.1
    gnss = {
        'x'    : random_generator.normal(ground_truth['x'], stdev),
        'x_std': [stdev] * len(t),
        'v'    : random_generator.normal(ground_truth['v'], stdev),
        'v_std': [stdev] * len(t)
    }

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

    # Generate accelerometer data
    a = random_generator.normal(ground_truth['a'] + ground_truth['b'], 0.35)

    return t, ground_truth, gnss, a


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

    # Generate ground truth data
    # Moving at constant velocity with constant accelerometer bias
    v = 1
    b0 = 0.5
    ground_truth = {}
    ground_truth['x'] = v * t
    ground_truth['v'] = np.gradient(ground_truth['x'], t)
    ground_truth['a'] = np.gradient(ground_truth['v'], t)
    ground_truth['b'] = 0 * t + b0

    # Generate GNSS sensor data
    # No GNSS velocity data
    stdev = 0.1
    gnss = {
        'x'    : random_generator.normal(ground_truth['x'], stdev),
        'x_std': [stdev] * len(t),
        'v'    : [np.nan] * len(t),
        'v_std': [np.nan] * len(t)
    }

    # Generate accelerometer data
    a = random_generator.normal(ground_truth['a'] + ground_truth['b'], 0.35)

    return t, ground_truth, gnss, a


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

    # Generate ground truth data
    # Position stationary with constant accelerometer bias
    v = 1
    bv = 0.2
    ground_truth = {}
    ground_truth['x'] = v * t
    ground_truth['v'] = np.gradient(ground_truth['x'], t)
    ground_truth['a'] = np.gradient(ground_truth['v'], t)
    ground_truth['b'] = bv * t

    # Generate GNSS sensor data
    stdev = 0.1
    gnss = {
        'x'    : random_generator.normal(ground_truth['x'], stdev),
        'x_std': [stdev] * len(t),
        'v'    : random_generator.normal(ground_truth['v'], stdev),
        'v_std': [stdev] * len(t)
    }

    # Generate accelerometer data
    a = random_generator.normal(ground_truth['a'] + ground_truth['b'], 0.35)

    return t, ground_truth, gnss, a


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

    # Generate ground truth data
    # Position moving in sine wave
    f = 0.5
    bv = 0.2
    ground_truth = {}
    ground_truth['x'] = np.sin(2 * np.pi * f * t)
    ground_truth['v'] = np.gradient(ground_truth['x'], t)
    ground_truth['a'] = np.gradient(ground_truth['v'], t)
    ground_truth['b'] = bv * t

    # Generate GNSS sensor data
    stdev = 0.1
    gnss = {
        'x'    : random_generator.normal(ground_truth['x'], stdev),
        'x_std': [stdev] * len(t),
        'v'    : random_generator.normal(ground_truth['v'], stdev),
        'v_std': [stdev] * len(t)
    }

    # Generate accelerometer data
    a = random_generator.normal(ground_truth['a'] + ground_truth['b'], 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()
#t, ground_truth, gnss, a = generate_test4_data()
#t, ground_truth, gnss, a = generate_test5_data()
#t, ground_truth, gnss, a = generate_test6_data()
#t, ground_truth, gnss, a = generate_test7_data()
t, ground_truth, gnss, a = generate_test8_data()

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

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