In [1]:
%pylab --no-import-all
from __future__ import division, print_function, absolute_import
import numpy as np
import kalman as k
In [2]:
%matplotlib inline
In [ ]:
# %load kalman.py
import numpy as np
import matplotlib.pyplot as plt
def kalman_predict( A, # transition matrix
r, # measurement error matrix
H, # transformation matrix from state vector to measurement
p, # initial variance on prediction
xkal, # estimated state vector
xpredict, # predicted state vector
xmeas): # measurements
for i in range(1, xkal.shape[1]): # for each measurement do
# prediction: recursive formula
xpredict[:, i] = np.dot(A, xkal[:, i - 1])
# predict covariance
p = A*p*A.T
# construct kalman gain matrix according to prediction equations
# higher gain leads to higher influence of measurement,
# lower gain to higher influence of predicion
K = np.dot(p*H.T, np.linalg.inv(H*p*H.T + r))
# construct estimate from prediction and gain
xkal[:, i] = xpredict[:, i] + K*(xmeas[:, i] - H*xpredict[:, i])
# update covariance with gain
p = (np.identity(K.shape[0]) - K * H) * p
return xkal, xpredict
def plot_results(xkal, xpredict, xmeas, xtrue):
fig1 = plt.figure()
ax1 = plt.axes()
plt.plot(xtrue, 'b-', label = 'True')
plt.plot(xmeas[0].T, 'rx', label = 'Measurement')
plt.plot(xpredict[0].T, 'g.', label = 'Prediction')
plt.plot(xkal[0].T, 'ko', label = 'Kalman')
plt.xlabel('Iteration')
plt.ylabel('X')
fig2 = plt.figure()
ax2 = plt.axes()
#plt.axhline(v)
plt.axhline(np.mean(xmeas[1]))
plt.plot(xpredict[1].T, 'g.', label = 'Prediction')
plt.plot(xmeas[1].T, 'rx', label = 'Measurement')
plt.plot(xkal[1].T, 'ko', label = 'Kalman')
plt.xlabel('Iteration')
plt.ylabel('Velocity')
return [[fig1, fig2], [ax1, ax2]]
In [4]:
dt = 0.2
# final time for track
T = 2* np.pi
# number of measurements
N = int(T / dt)
# initial position
x0 = 100
# position, velocity and acceleration
state_vec_dim = 2
# parabola with some curvature g
# initial velocity / slope
# errors
sigma_pos = 1e-1
sigma_vel = 2
sigma_acc = 2
_t = np.linspace(0, T, N)
xtrue5 = np.matrix([np.cos(_t), np.sin(_t)])
In [5]:
fig = plt.figure(figsize=(6,6))
ax = plt.axes()
plt.plot(xtrue5[0].T, xtrue5[1].T, marker='o')
plt.title('true track')
plt.xlabel('x')
plt.ylabel('y')
Out[5]:
In [6]:
# add noise
measurement_noise = np.random.normal(loc=0, scale=sigma_pos, size=xtrue5.shape)
print(measurement_noise)
xmeas4 = xtrue5 + measurement_noise
fig = plt.figure(figsize=(6,6))
ax = plt.axes()
plt.plot(xmeas4[0].T, xmeas4[1].T, marker='o')
plt.title('noisy measurements of track')
plt.xlabel('x')
plt.ylabel('y')
Out[6]:
In [9]:
xpredict4 = np.matrix (np.linspace(0,T,N*state_vec_dim).reshape((state_vec_dim, N)))
xkal4 = np.matrix (np.linspace(0,T,N*state_vec_dim).reshape((state_vec_dim, N)))
# initial position -- deliberately slightly off the real values radius=1, x-offset=0
xpredict4[:,0] = xkal4[:,0] = np.array([[1.1], [0.1]]) # np.array ( [[xmeas4[0,0] ], [xmeas4[1,0]] ] )
# initial variance on prediction
p4 = 0.5 * np.matrix ( [[2, 0],
[0, 2]] )
# measurement error
r4 = 1e-1 * np.matrix([[sigma_pos, 0],
[0, sigma_pos]])
# prediction matrix
# global track parameters do not change in this example
A5 = np.matrix ( [[1, 0],
[0, 1]] )
# map state vector to measurements at surface k
H4_k = [np.matrix ([[np.cos(t), 1],
[np.sin(t), 0]] ) for t in _t]
for i in range(1,N):
H4 = H4_k[i]
# prediction: recursive formula
xpredict4[:,i] = np.dot(A5, xkal4[:,i-1] )
p4 = A5*p4*A5.T
K4 = p4*H4.T * np.linalg.inv(H4*p4*H4.T+r4)
_resid = (np.matrix(xmeas4[:,i]) - H4 * (xpredict4[:,i]))
xkal4[:,i] = xpredict4[:,i] + K4 * _resid
p4 = (np.identity(state_vec_dim)-K4 * H4) * p4
print(xkal4)
fig1 = plt.figure()
ax1 = plt.axes()
plt.plot(xpredict4[0].T, 'g.', label = 'Prediction')
plt.plot(xkal4[0].T, 'ko', label = 'Kalman')
plt.xlabel('Iteration')
plt.ylabel('Radius')
fig2 = plt.figure()
ax2 = plt.axes()
plt.plot(xmeas4[1].T, 'rx', label = 'Measurement')
plt.plot(xkal4[1].T, 'ko', label = 'Kalman')
plt.xlabel('Iteration')
plt.ylabel('X-axis offset')
_plot_data = np.array([H4_k[i] * (xpredict4[:, i]) for i in range(N)])
print(_plot_data.shape)
plt.figure()
ax = plt.axes
plt.plot(xmeas4[0].T, xmeas4[1].T, 'rx', label='Measurement')
for i in range(N):
_plot_data =H4_k[i] * (xpredict4[:, i])
plt.plot(_plot_data[0], _plot_data[1], 'ko')