# 2D Visual-Inertial Extended Kalman Filter

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);
accel = xDotDot*cos(theta) + yDotDot*sin(theta);

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 = 
yArr = 
thetaArr = 

# 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([
, # x
, # y
, # theta
, # x_dot
, # y_dot
, # theta_dot
, # x_dot_dot
  # 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]
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)
accel = xDotDot * np.cos(theta) + yDotDot * np.sin(theta)

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?

• Have you though about using the acceleration/gryo information in the propagation step instead of the measurements step? It's easier that way because you don't have to devise a process model for how the acceleration changes. Jul 4 '20 at 19:04
• @holmeski thanks I actually tried that first but it was more complex and also more erratic which is why I tried redoing it in a simpler way like this. I will look at that again Jul 5 '20 at 15:28

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

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

dstate = 0. * state
dstate[0:2] = dxdy
dstate = dtheta
dstate = dvelx
dstate = 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, x.shape])

for i in range(x.shape):
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, getVehRate, data)

# 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()
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 += np.random.randn(1) * accel_cov**.5
imu_meas += 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()

• Thanks, I realised as much but I was thinking that it having the wrong values would just result in decreased performance and not in it going completely crazy. I will try getting better values Jul 5 '20 at 15:30