<SYSTEM_TASK:>
Given the following text description, write Python code to implement the functionality described below step by step
<END_TASK>
<USER_TASK:>
Description:
 Two Object Tracking
Summary of notebook
<b> Kalman filter
Step1:  Target information
Step2:  The Kalman Filter Model
Step3:  Motion and measurement models
Step4:  Priors
Step5:  Linear Kalman Filtering
Creating the model
Step6:  The Kalman Filter algorithm
Step7:  <img src="images/kalman/kalman_hl.gif">
Two object tracking
<ASSISTANT_TASK:>
Python Code:
%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)
Explanation: Two Object Tracking
Summary of notebook
<b> Kalman filter: PGM implementation </b>
Nearly identical to standard implementation.
This section is just a basis for comparison.
<b> Simulation of the two object tracking </b>
It tracks the objects, but the likelihoods seem incorrect.
End of explanation
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)
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])  
Explanation: Target information
End of explanation
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
Explanation: The Kalman Filter Model
End of explanation
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
Explanation: Motion and measurement models
End of explanation
#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)
print(h0_1)
Explanation: Priors
End of explanation
z_t = np.empty((M, N))
z_t[:, :3] = pm_1
z_t[:, 3:] = vm_1
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))
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))
L, _ = z_t.shape 
X = np.arange(0, L)
Z = np.arange(L-1, 2*L-1)
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))
Explanation: Linear Kalman Filtering
Creating the model
End of explanation
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)
p_e = mean[:3, :]
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()
D = 100
t = np.linspace(0, 2*np.pi, D)
xz = np.array([[np.cos(t)], [np.sin(t)]]).reshape((2, D))
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)
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()
Explanation: The Kalman Filter algorithm: Gaussian belief propagation
End of explanation
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()
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
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))
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)
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)]))
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)
D = 100
t = np.linspace(0, 2*np.pi, D)
xz = np.array([[np.cos(t)], [np.sin(t)]]).reshape((2, D))
ellipses = []
norms = []
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)
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()
Explanation: <img src="images/kalman/kalman_hl.gif">
Two object tracking
End of explanation
<END_TASK>