2D Visual-Inertial Extended Kalman Filter

Question

I am trying to implement an Extended Kalman filtering for combining IMU data and visual odometry in a simple 2D case where I have a robot that that can only accelerate in its local forward direction which is dictated by its current heading (theta). I am restricting IMU readings to a single acceleration reading (a) and a single angular velocity reading (omega). Visual odometry will only provide a single angular displacement as well as displacement in the u and v directions (x and y relative to the robot). The equations for the derivation of my state transition matrix are

$$ x_{k+1} = x_k + \dot{x_k}\Delta T + 0.5a \cdot cos(\theta) \Delta T^2 $$ $$ y_{k+1} = y_k + \dot{x_k}\Delta T + 0.5a \cdot sin(\theta) \Delta T^2 $$ $$ \theta_{k+1} = \theta_k + \dot{\theta} \Delta T $$ $$ \dot{x_{k+1}} = \dot{x_{k}} + a \cdot cos(\theta) \Delta $$ $$ \dot{y_{k+1}} = \dot{y_{k}} + a \cdot sin(\theta) \Delta $$ $$ \dot{\theta_{k+1}} = \dot{\theta_{k}} $$ $$ \dot{\dot{x_{k+1}}} = \dot{\dot{x_{k}}}$$ $$ \dot{\dot{y_{k+1}}} = \dot{\dot{y_{k}}}$$

and the equations that I use to obtain the measurements are

$$ \Delta x = \dot{x} \Delta T + 0.5 \dot{\dot{x}} \Delta T^2 $$ $$ \Delta y = \dot{y} \Delta T + 0.5 \dot{\dot{y}} \Delta T^2 $$ $$ \Delta u = \Delta x \cdot cos(\theta) + \Delta y \cdot sin(\theta) $$ $$ \Delta v = -\Delta x \cdot sin(\theta) + \Delta y \cdot cos(\theta) $$ $$ \Delta \theta = \dot{\theta} \cdot \Delta T $$ $$ a = \dot{\dot{x}} \cdot cos(\theta) + \dot{\dot{y}} \cdot sin(\theta) $$ $$ \omega = \dot{\theta} $$

To calculate the Jacobian of the measurement function I used the following MATLAB code

deltaX = xDot*t + 0.5*xDotDot*(t^2);
deltaY = yDot*t + 0.5*yDotDot*(t^2);
deltaU = deltaX * cos(theta) + deltaY * sin(theta);
deltaV = -deltaX * sin(theta) + deltaY * cos(theta);
deltaTheta = thetaDot*t;
accel = xDotDot*cos(theta) + yDotDot*sin(theta);
omega = thetaDot;

jacobian([accel, omega, deltaU, deltaV, deltaTheta], [x, y, theta, xDot, yDot, thetaDot, xDotDot, yDotDot])

To test my implementation I am creating test data from random acceleration and angular velocity values. I am plotting the trajectory calculated from this as well as from the trajectory calculated directly using the odometry values and the IMU values. I am then comparing this with the odometry estimated by my Kalman filter.

The Kalman filter has been implemented without any control values and is combining all the sensor reading into a single measurement vector.

To test if the filter has any hope of working, I first tested it without any added measurement noise but the outcome is fairly crazy as can be seen in

where it can also be seen that using both sensor readings on their own without the filter produces the exact trajectory. This simulation, including my Kalman filter was implemented with the following Python code

import numpy as np
import matplotlib.pyplot as plt
from random import *

# Sampling period
deltaT = 1

# Array to store the true trajectory
xArr = [0]
yArr = [0]
thetaArr = [0]

# Array to store IMU measurement
imuA = []
imuOmega = []

# Current state variables
x = 0
y = 0
theta = 0
x_dot = 0
y_dot = 0

# Arrays to store odometry measurements
odoU = []
odoV = []
odoTheta = []

# Setup simulated data
for i in range(100):
    # Calculate a random forward (u-axis) acceleration
    a = uniform(-10, 10)
    imuA.append(a)

    # Calculate the change in global coordinates
    deltaX = (x_dot * deltaT) + (0.5 * a * np.cos(theta) * deltaT**2)
    deltaY = (y_dot * deltaT) + (0.5 * a * np.sin(theta) * deltaT**2)

    # Update the velocity at the end of the time step
    x_dot += a * np.cos(theta) * deltaT
    y_dot += a * np.sin(theta) * deltaT

    # Update the current coordinates
    x += deltaX
    y += deltaY

    # Store the coordinates for plotting
    xArr.append(x)
    yArr.append(y)

    # Calculate local coordinate odometry
    odoU.append(deltaX * np.cos(theta) + deltaY * np.sin(theta))
    odoV.append(-deltaX * np.sin(theta) + deltaY * np.cos(theta))

    # Calculate a random new angular velocity
    theta_dot = uniform(-0.2, 0.2)
    imuOmega.append(theta_dot)

    # Calculate the change in angular displacement
    deltaTheta = theta_dot * deltaT
    odoTheta.append(deltaTheta)

    # Update the angular displacement
    theta += theta_dot * deltaT
    thetaArr.append(theta)

# Calculate the trajectory from just the odometery
xArr2 = []
yArr2 = []

x = 0
y = 0
theta = 0

for i in range(100):
    deltaU = odoU[i]
    deltaV = odoV[i]
    deltaTheta = odoTheta[i]

    x += deltaU * np.cos(theta) - deltaV * np.sin(theta)
    y += deltaU * np.sin(theta) + deltaV * np.cos(theta)
    theta += deltaTheta

    xArr2.append(x)
    yArr2.append(y)

# Calculate the trajectory from just the IMU readings
xArr3 = []
yArr3 = []

x = 0
y = 0
theta = 0
x_dot = 0
y_dot = 0
theta_dot = 0

for i in range(100):
    # Calculate the change in global coordinates
    a = imuA[i]
    deltaX = (x_dot * deltaT) + (0.5 * a * np.cos(theta) * deltaT**2)
    deltaY = (y_dot * deltaT) + (0.5 * a * np.sin(theta) * deltaT**2)

    # Update the velocity at the end of the time step
    x_dot += a * np.cos(theta) * deltaT
    y_dot += a * np.sin(theta) * deltaT

    # Update the current coordinates
    x += deltaX
    y += deltaY

    # Store the coordinates for plotting
    xArr3.append(x)
    yArr3.append(y)

    # Calculate the change in angular displacement
    theta_dot = imuOmega[i]
    theta += theta_dot * deltaT

# Estimate the true trajectory with a Kalman filter

# State matrix
X_k_min = np.array([
    [0], # x
    [0], # y
    [0], # theta
    [0], # x_dot
    [0], # y_dot
    [0], # theta_dot
    [0], # x_dot_dot
    [0]  # y_dot_dot
])

# State covariance matrix
P_k_min = np.zeros((8, 8))

# State transition matrix
A = np.array([
    [1, 0, 0, deltaT, 0, 0, 0.5*deltaT**2, 0],
    [0, 1, 0, 0, deltaT, 0, 0, 0.5*deltaT**2],
    [0, 0, 1, 0, 0, deltaT, 0, 0],
    [0, 0, 0, 1, 0, 0, deltaT, 0],
    [0, 0, 0, 0, 1, 0, 0, deltaT],
    [0, 0, 0, 0, 0, 1, 0, 0],
    [0, 0, 0, 0, 0, 0, 1, 0],
    [0, 0, 0, 0, 0, 0, 0, 1]
])

# Process covariance matrix
Q = np.eye(8)

# Measurement vector
## 0: a (forward acceleration)
## 1: omega (angular velocity)
## 2: deltaU (local x displacement)
## 3: deltaV (local y displacement)
## 4: deltaTheta (local angular displacement)

# Measurement covariance matrix
R = np.eye(5)

# Function to calculate the measurement function Jacobian
def CalculateH_k(X, t):
    theta = X[2, 0]
    xDot = X[3, 0]
    yDot = X[4, 0]
    xDotDot = X[6, 0]
    yDotDot = X[7, 0]

    return np.array([
        [0, 0, yDotDot * np.cos(theta) - xDotDot * np.sin(theta), 0, 0, 0, np.cos(theta), np.sin(theta)],
        [0, 0, 0, 0, 0, 1, 0, 0],
        [0, 0, np.cos(theta) * ((yDotDot * t**2) / 2 + yDot * t) - np.sin(theta) * (
                    (xDotDot * t**2) / 2 + xDot * t), t * np.cos(theta), t * np.sin(theta), 0, (t**2 * np.cos(theta)) / 2, (
                     t**2 * np.sin(theta)) / 2],
        [0, 0, - np.cos(theta) * ((xDotDot * t**2) / 2 + xDot * t) - np.sin(theta) * (
                    (yDotDot * t**2) / 2 + yDot * t), -t * np.sin(theta), t * np.cos(theta), 0, -(t**2 * np.sin(theta)) / 2, (
                     t**2 * np.cos(theta)) / 2],
        [0, 0, 0, 0, 0, t, 0, 0]
    ])

# Measurement function
def Measure(X):
    theta = X[2, 0]
    xDot = X[3, 0]
    yDot = X[4, 0]
    thetaDot = X[5, 0]
    xDotDot = X[6, 0]
    yDotDot = X[7, 0]

    deltaX = xDot * deltaT + 0.5 * xDotDot * (deltaT**2)
    deltaY = yDot * deltaT + 0.5 * yDotDot * (deltaT**2)
    deltaU = deltaX * np.cos(theta) + deltaY * np.sin(theta)
    deltaV = -deltaX * np.sin(theta) + deltaY * np.cos(theta)
    deltaTheta = thetaDot * deltaT
    accel = xDotDot * np.cos(theta) + yDotDot * np.sin(theta)
    omega = thetaDot

    return np.array([
        [accel],
        [omega],
        [deltaU],
        [deltaV],
        [deltaTheta]
    ])

xArr4 = []
yArr4 = []

for i in range(100):
    a = imuA[i]
    omega = imuOmega[i]

    # Setup the observation matrix
    Z_k = np.array([
        [imuA[i]],
        [imuOmega[i]],
        [odoU[i]],
        [odoV[i]],
        [odoTheta[i]]
    ])

    # Calculate the estimated new state
    X_k = A.dot(X_k_min)

    # Calculate the estimated new state covariance matrix
    P_k = A.dot(P_k_min).dot(np.transpose(A)) + Q

    # Find the measurement Jacobian at the current time step
    H_k = CalculateH_k(X_k_min, deltaT)

    # Calculate the Kalman gain
    G_k = P_k.dot(np.transpose(H_k)).dot(np.linalg.inv(H_k.dot(P_k).dot(np.transpose(H_k)) + R))

    # Calculate the improved current state estimate
    X_k = X_k + G_k.dot(Z_k - Measure(X_k_min))

    # Calculate the improved current state covariance
    P_k = (np.eye(8) - G_k.dot(H_k)).dot(P_k)

    xArr4.append(X_k[0, 0])
    yArr4.append(X_k[1, 0])

    # Make the current state the previous
    X_k_min = X_k
    P_k_min = P_k

plt.plot(xArr, yArr, linewidth=3)
plt.plot(xArr2, yArr2)
plt.plot(xArr3, yArr3)
plt.plot(xArr4, yArr4)
plt.legend(['Ground truth', 'VO', 'IMU', 'Filtered'])
plt.grid()
plt.show()

I have double checked everything and just can't figure out what I am doing wrong even though it must be something obvious. Any ideas?

Answer 1

Your noise term for the KF needs to reflect how you expect the true propagation of the state will differ from your model of the propagation. For example, the acceleration uncertainty is 1 while your true uncertainty is drawn from a uniform distribution of [-10,10].

---

I altered your code so the KF is now using the IMU information within the propagation step. It still needs to incorporate the IMU uncertainty correctly within the process noise. I also simplified the measurements to be the position and orientation of the state. You should probably rewrite the numerical Jacobians that I've used in favor of analytic Jacobians.

import numpy as np
import matplotlib.pyplot as plt
from random import *

# The state vector is
#  pos_x, pos_y, theta, vel_x, vel_y

def getVehRate(state, imu):
    # position rate is equal to velocity
    dxdy = state[3:5]

    # theta rate is euqal to the gyro measurement
    dtheta = imu[1]

    # velocity rate is equal to the accel broken into the xy basis
    dvelx = imu[0] * np.cos(state[2])
    dvely = imu[0] * np.sin(state[2])

    dstate = 0. * state
    dstate[0:2] = dxdy
    dstate[2] = dtheta
    dstate[3] = dvelx
    dstate[4] = dvely
    return dstate


def rk4(state, imu, func, dt):
    # runs a rk4 numerical integration
    k1 = dt * func(state, imu)
    k2 = dt * func(state + .5*k1, imu)
    k3 = dt * func(state + .5*k2, imu)
    k4 = dt * func(state + k3, imu)

    return state + (1./6.)*(k1 + 2.*k2 + 2.*k3 + k4)


def numericalDifference(x, func, data, ep = .001):
    # calculates the numeical jacobian

    y = func(x, data)

    A = np.zeros([y.shape[0], x.shape[0]])

    for i in range(x.shape[0]):
        x[i] += ep
        y_i = func(x, data)
        A[i] = (y_i - y)/ep
        x[i] -= ep

    return A

def numericalJacobianOfStatePropagationInterface(state, data):
    # data contains both the imu and dt, it needs to be broken up for the rk4
    return rk4(state, data[0], getVehRate, data[1])



# Sampling period
dt = .1
t_steps = 500
state = np.zeros(5)

state_hist = np.zeros([t_steps, 5])
imu_hist = np.zeros([t_steps, 2])


# Setup simulated data
for i in range(t_steps):
    # generate a rate to propagate states with
    accel = uniform(-10, 10)
    theta_dot = uniform(-0.2, 0.2)
    imu = np.array([accel, theta_dot])

    # propagating the state with the IMU measurement
    state = rk4(state, imu, getVehRate, dt)

    # saving off the current state
    state_hist[i] = state *1.
    imu_hist[i] = imu*1.


# kf stuff
state = np.zeros([5])
cov = np.eye(5) * .001

kf_state_hist = np.zeros([t_steps, 5])
kf_cov_hist = np.zeros([t_steps, 5,5])
kf_meas_hist = np.zeros([t_steps, 3])
kf_imu_hist = np.zeros([t_steps, 2])

# imu accel and gyro noise
accel_cov = .0001
gyro_cov  = .0001
Q_imu = np.array([[.1, 0],[0, .01]])

r_meas = .001

#  running the data through the KF with noised measurements
for i in range(t_steps):

    # propagating the state
    imu_meas = imu_hist[i]
    imu_meas[0] += np.random.randn(1) * accel_cov**.5
    imu_meas[1] += np.random.randn(1) * gyro_cov**.5

    A = numericalDifference(state, numericalJacobianOfStatePropagationInterface, [imu_meas, dt])
    cov = A.dot(cov.dot(A.T))

    ###
    # TODO : calculate how the accel and gyro noise turn into the process noise for the system
    ###
    # A_state_wrt_imu = jacobianOfPropagationWrtIMU
    # Q = A_state_wrt_imu * Q_imu * A_state_wrt_imu.T
    # cov += Q
    # sloppy placeholder
    cov += np.eye(5) * .1

    state = rk4(state, imu_meas, getVehRate, dt)

    # measurement update
    zt = state[:3]  + np.random.randn(1) *r_meas**.5
    zt_hat = state[:3]

    H = np.zeros([3,5])
    H[:3,:3] = np.eye(3)

    S = np.linalg.inv(H.dot(cov.dot(H.T)) + r_meas * np.eye(3))
    K = cov.dot(H.T).dot( S )

    #state = state + K.dot(zt - zt_hat)
    cov = (np.eye(5) - K.dot(H)).dot(cov)

    kf_state_hist[i] = state
    kf_cov_hist[i] = cov
    kf_meas_hist[i] = zt_hat
    kf_imu_hist[i] = imu_meas



plt.plot(state_hist[:,0], state_hist[:,1], linewidth=3)
plt.plot(kf_state_hist[:,0], kf_state_hist[:,1], linewidth=3)
plt.legend(['Ground truth', 'kf est'])
plt.grid()
plt.show()