In [1]:
%matplotlib inline
%load_ext autoreload
%autoreload 2
import numpy as np
from matplotlib import pylab as plt
from mpl_toolkits import mplot3d
from canonical_gaussian import CanonicalGaussian as CG
from gaussian_mixture import GaussianMixtureModel as GMM
from calc_traj import calc_traj
from range_doppler import *
from util import *
np.set_printoptions(precision=2)
In [2]:
names, p, v, w = load_clubs('clubs.csv')
cpi = 40e-3
T = 12
t_sim = np.arange(0, T, cpi)
t1, p1, v1 = calc_traj(p[0, :], v[0, :], w[0, :], t_sim)
t2, p2, v2 = calc_traj(p[-1, :], v[-1, :], w[-1, :], t_sim)
In [3]:
sensor_locations = np.array([[-10, 28.5, 1], [-15, 30.3, 3],
[200, 30, 1.5], [220, -31, 2],
[-30, 0, 0.5], [150, 10, 0.6]])
rd_1 = range_doppler(sensor_locations, p1, v1)
pm_1 = multilateration(sensor_locations, rd_1[:, :, 1])
vm_1 = determine_velocity(t1, pm_1, rd_1[:, :, 0])
rd_2 = range_doppler(sensor_locations, p2, v2)
pm_2 = multilateration(sensor_locations, rd_2[:, :, 1])
vm_2 = determine_velocity(t2, pm_2, rd_2[:, :, 0])
In [4]:
N = 6
if pm_1.shape < pm_2.shape:
M, _ = pm_1.shape
pm_2 = pm_2[:M]
vm_2 = pm_2[:M]
else:
M, _ = pm_2.shape
pm_1 = pm_1[:M]
vm_1 = vm_2[:M]
print(M)
dt = cpi
g = 9.81
sigma_r = 2.5
sigma_q = 0.5
prior_var = 1
In [5]:
A = np.identity(N)
A[0, 3] = A[1, 4] = A[2, 5] = dt
B = np.zeros((N, N))
B[2, 2] = B[5, 5] = 1
R = np.identity(N)*sigma_r
C = np.identity(N)
Q = np.identity(N)*sigma_q
u = np.zeros((6, 1))
u[2] = -0.5*g*(dt**2)
u[5] = -g*dt
In [6]:
#Object 1
mu0_1 = np.zeros((N, 1))
mu0_1[:3, :] = p1[0, :].reshape(3, 1)
mu0_1[3:, :] = v[0, :].reshape(3, 1)
prec0_1 = np.linalg.inv(prior_var*np.identity(N))
h0_1 = (prec0_1)@(mu0_1)
g0_1 = -0.5*(mu0_1.T)@(prec0_1)@(mu0_1) -3*np.log(2*np.pi)
#Object 2
mu0_2 = np.zeros((N, 1))
mu0_2[:3, :] = p2[0, :].reshape(3, 1)
mu0_2[3:, :] = v2[0, :].reshape(3, 1)
prec0_2 = np.linalg.inv(prior_var*np.identity(N))
h0_2 = (prec0_2)@(mu0_2)
g0_2 = -0.5*(mu0_2.T)@(prec0_2)@(mu0_2) -3*np.log(2*np.pi)
In [7]:
print(h0_1)
In [8]:
z_t = np.empty((M, N))
z_t[:, :3] = pm_1
z_t[:, 3:] = vm_1
In [9]:
R_in = np.linalg.inv(R)
P_pred = np.bmat([[R_in, -(R_in)@(A)], [-(A.T)@(R_in), (A.T)@(R_in)@(A)]])
M_pred = np.zeros((2*N, 1))
M_pred[:N, :] = (B)@(u)
h_pred = (P_pred)@(M_pred)
g_pred = -0.5*(M_pred.T)@(P_pred)@(M_pred).flatten() -0.5*np.log( np.linalg.det(2*np.pi*R))
In [10]:
Q_in = np.linalg.inv(Q)
P_meas = np.bmat([[(C.T)@(Q_in)@(C), -(C.T)@(Q_in)], [-(Q_in)@(C), Q_in]])
h_meas = np.zeros((2*N, 1))
g_meas = -0.5*np.log( np.linalg.det(2*np.pi*Q))
In [11]:
L, _ = z_t.shape
X = np.arange(0, L)
Z = np.arange(L-1, 2*L-1)
In [12]:
C_X = [CG([X[0]], [N], h0_1, prec0_1, g0_1)]
C_Z = [CG([X[0]], [N], h0_1, prec0_1, g0_1)]
for i in np.arange(1, L):
C_X.append(CG([X[i], X[i-1]], [N, N], h_pred, P_pred, g_pred))
C_Z.append(CG([X[i], Z[i]], [N, N], h_meas, P_meas, g_meas))
In [13]:
message_out = [C_X[0]]
prediction = [C_X[0]]
mean = np.zeros((N, L))
for i in np.arange(1, L):
#Kalman Filter Algorithm
C_Z[i].introduce_evidence([Z[i]], z_t[i, :])
marg = (message_out[i-1]*C_X[i]).marginalize([X[i-1]])
message_out.append(marg*C_Z[i])
mean[:, i] = (np.linalg.inv(message_out[i]._prec)@(message_out[i]._info)).reshape((N, ))
#For plotting only
prediction.append(marg)
In [14]:
p_e = mean[:3, :]
In [15]:
fig = plt.figure(figsize=(25, 25))
ax = plt.axes(projection='3d')
ax.plot(p1[:, 0], p1[:, 1], p1[:, 2])
ax.plot(p_e[0, :], p_e[1, :], p_e[2, :], 'or')
ax.set_xlabel('x (m)', fontsize = '20')
ax.set_ylabel('y (m)', fontsize = '20')
ax.set_zlabel('z (m)', fontsize = '20')
ax.set_title('Kalman Filtering', fontsize = '20')
ax.set_ylim([-1, 1])
ax.legend(['Actual Trajectory', 'Estimated trajectory'])
plt.show()
In [16]:
D = 100
t = np.linspace(0, 2*np.pi, D)
xz = np.array([[np.cos(t)], [np.sin(t)]]).reshape((2, D))
In [17]:
gaussians = message_out + prediction + C_Z
ellipses = []
for g in gaussians:
g._vars = [1, 2, 3, 4]
g._dims = [1, 1, 1, 3]
c = g.marginalize([2, 4])
cov = np.linalg.inv(c._prec)
mu = (cov)@(c._info)
U, S, _ = np.linalg.svd(cov)
L = np.diag(np.sqrt(S))
ellipses.append(np.dot((U)@(L), xz) + mu)
In [18]:
for i in np.arange(0, M):
plt.figure(figsize= (15, 15))
message_out = ellipses[i]
prediction = ellipses[i+M]
measurement = ellipses[i+2*M]
plt.plot(p1[:, 0], p1[:, 2], 'k--', label='Trajectory')
plt.plot(message_out[0, :], message_out[1, :], 'r', label='After measurement update')
plt.plot(prediction[0, :], prediction[1, :], 'b', label = 'Recursive prediction')
plt.plot(measurement[0, :], measurement[1, :], 'g', label='Measurement')
plt.xlim([-3.5, 250])
plt.ylim([-3.5, 35])
plt.grid(True)
plt.xlabel('x (m)')
plt.ylabel('z (m)')
plt.legend(loc='upper left')
plt.title('x-z position for t = %d'%i)
plt.savefig('images/kalman/%d.png'%i, format = 'png')
plt.close()
In [19]:
fig = plt.figure(figsize=(25, 25))
ax = plt.axes(projection='3d')
ax.plot(p1[:, 0], p1[:, 1], p1[:, 2])
ax.plot(p2[:, 0], p2[:, 1], p2[:, 2], 'or')
ax.set_xlabel('x (m)', fontsize = '20')
ax.set_ylabel('y (m)', fontsize = '20')
ax.set_zlabel('z (m)', fontsize = '20')
ax.set_title('', fontsize = '20')
ax.set_ylim([-20, 20])
ax.legend(['Target 1', 'Target 2'])
plt.show()
In [20]:
L = 10
X_1 = np.arange(0, L).tolist()
X_2 = np.arange(L, 2*L).tolist()
Z_1 = np.arange(2*L, 3*L).tolist()
Z_2 = np.arange(3*L, 4*L).tolist()
z_1 = np.empty((M, N))
z_1[:, :3] = pm_1
z_1[:, 3:] = vm_1
z_2 = np.empty((M, N))
z_2[:, :3] = pm_2
z_2[:, 3:] = vm_2
In [21]:
C_X = [CG([X_1[0]], [N], h0_1, prec0_1, g0_1)*CG([X_2[0]], [N], h0_2, prec0_2, g0_2)]
for i in np.arange(1, L):
C_X.append(CG([X_1[i], X_1[i-1]], [N, N], h_pred, P_pred, g_pred)
*CG([X_2[i], X_2[i-1]], [N, N], h_pred, P_pred, g_pred))
In [22]:
C_Z = [None]
Z_11 = CG([X_1[1], Z_1[1]], [N, N], h_meas, P_meas, g_meas)
Z_11.introduce_evidence([Z_1[1]], z_1[1, :])
Z_22 = CG([X_2[1], Z_2[1]], [N, N], h_meas, P_meas, g_meas)
Z_22.introduce_evidence([Z_2[1]], z_2[1, :])
C_Z.append(Z_11*Z_22)
In [23]:
for i in np.arange(2, L):
Z_11 = CG([X_1[i], Z_1[i]], [N, N], h_meas, P_meas, g_meas)
Z_11.introduce_evidence([Z_1[i]], z_1[i, :])
Z_22 = CG([X_2[i], Z_2[i]], [N, N], h_meas, P_meas, g_meas)
Z_22.introduce_evidence([Z_2[i]], z_2[i, :])
Z_12 = CG([X_1[i], Z_2[i]], [N, N], h_meas, P_meas, g_meas)
Z_12.introduce_evidence([Z_2[i]] ,z_2[i, :])
Z_21 = CG([X_2[i], Z_1[i]], [N, N], h_meas, P_meas, g_meas)
Z_21.introduce_evidence([Z_1[i]], z_1[i, :])
C_Z.append(GMM([0.5*(Z_11*Z_22), 0.5*(Z_12*Z_21)]))
In [24]:
predict = [C_X[0]]
for i in np.arange(1, L):
marg = (C_X[i]*predict[i-1]).marginalize([X_1[i-1], X_2[i-1]])
predict.append(C_Z[i]*marg)
In [25]:
D = 100
t = np.linspace(0, 2*np.pi, D)
xz = np.array([[np.cos(t)], [np.sin(t)]]).reshape((2, D))
ellipses = []
norms = []
In [26]:
i = 0
for p in predict:
if isinstance(p, GMM):
mix = p._mix
else:
mix = [p]
time_step = []
for m in mix:
m._vars = [1, 2, 3, 4]
m._dims = [1, 1, 1, 9]
c = m.marginalize([2, 4])
cov = np.linalg.inv(c._prec)
mu = (cov)@(c._info)
if i == 0:
print(cov)
i = 1
U, S, _ = np.linalg.svd(cov)
lambda_ = np.diag(np.sqrt(S))
norms.append(c._norm)
time_step.append(np.dot((U)@(lambda_), xz) + mu)
ellipses.append(time_step)
In [27]:
for i in np.arange(0, L):
plt.figure(figsize= (15, 15))
plt.plot(p1[1:, 0], p1[1:, 2], 'or', label='Trajectory 1')
plt.plot(p2[1:, 0], p2[1:, 2], 'og', label='Trajectory 2')
for e in ellipses[i]:
plt.plot(e[0, :], e[1, :], 'b')
plt.xlim([-3.5, 25])
plt.ylim([-3.5, 15])
plt.grid(True)
plt.legend(loc='upper left')
plt.xlabel('x (m)')
plt.ylabel('z (m)')
plt.title('x-z position for t = %d'%(i))
plt.savefig('images/two_objects/%d.png'%i, format = 'png')
plt.close()
In [ ]: