def predict_existing(self, nav_status, sss_path):
# Prediction for existing targets
if len(self.gm) == 0:
return []
means = np.asarray(np.array([ comp._mean for comp in self.gm]))
means = np.squeeze(means)
means = np.squeeze(means)
means = np.reshape(means, (means.size / 2, 2))
if len(means) > 0:
gmm_mask = utils.inside_polygon(means, sss_path)
gmm_fov = list(itertools.compress(self.gm, gmm_mask))
predicted = []
for idx, comp in enumerate(self.gm):
if gmm_mask[idx]:
predicted.append(GmphdComponent(self.survival * comp._weight,
np.dot(self.f, comp._mean), self.q + np.dot(np.dot(self.f, comp._cov), self.f.T)))
else:
predicted.append(self.gm[idx])
return predicted
def predict_existing(self, nav_status, sss_path):
# Prediction for existing targets
if len(self.gm) == 0:
return []
means = np.asarray(np.array([comp._mean for comp in self.gm]))
means = np.squeeze(means)
means = np.squeeze(means)
means = np.reshape(means, (means.size / 2, 2))
if len(means) > 0:
gmm_mask = utils.inside_polygon(means, sss_path)
gmm_fov = list(itertools.compress(self.gm, gmm_mask))
predicted = []
for idx, comp in enumerate(self.gm):
if gmm_mask[idx]:
predicted.append(
GmphdComponent(
self.survival * comp._weight,
np.dot(self.f, comp._mean),
self.q + np.dot(np.dot(self.f, comp._cov), self.f.T),
)
)
else:
predicted.append(self.gm[idx])
return predicted
def auv_update(self, measures, nav_status, sss_path):
'''
Construction of the update components
s = R + H * _Cov * H.T
eta = H * _Mean
K = _Cov * H.T * ( H * _Cov * H.T + R) ^ -1
Pkk = ( I - K * H ) * _Cov
'''
if len(self.gm) == 0:
return
eta = [np.dot(self.h, comp._mean) for comp in self.gm]
s = [self.r + np.dot(np.dot(self.h, comp._cov), self.h.T) for comp in self.gm]
k = []
for index, comp in enumerate(self.gm):
k.append(np.dot(np.dot(comp._cov, self.h.T), np.linalg.inv(s[index])))
pkk = []
for index, comp in enumerate(self.gm):
pkk.append(np.dot(np.eye(np.shape(k[index])[0]) - np.dot(k[index], self.h), comp._cov))
'''
The predicted components are kept with a decay (1 - Pd) iff they are inside the FOV, contained into pr_gm
'''
# Extraction of the coordinates of the components inside the RFS
means = np.asarray(np.array([ comp._mean for comp in self.gm]))
means = np.squeeze(means)
means = np.reshape(means, (means.size / 2, 2))
# Temp copy of the RFS after the prediction step
pr_gm = copy.deepcopy(self.gm)
# Mask of the components inside the FOV
gmm_mask = utils.inside_polygon(means, sss_path)
# print(gmm_mask.shape, pr_gm)
# gmm_fov = list(itertools.compress(self.gmm, gmm_mask))
for idx, comp in enumerate(pr_gm):
if gmm_mask[idx]:
pr_gm[idx]._weight *= (1 - self.detection)
for i in np.ndindex(measures.shape[1]):
z = measures[:, i]
temp_gm = []
for j, comp in enumerate(self.gm):
# Computation of q_k
mvn = multivariate_normal(eta[j].squeeze(), s[j])
mvn_result = mvn.pdf(z.squeeze())
temp_gm.append(GmphdComponent(
self.detection * comp._weight * mvn_result,
comp._mean + np.dot(k[j], z - eta[j]),
comp._cov))
# The Kappa thing (clutter and reweight)
weight_sum = np.sum(comp._weight for comp in temp_gm)
if weight_sum >= 1e-9:
weight_factor = 1.0 / (self.clutter + weight_sum)
for comp in temp_gm:
comp._weight *= weight_factor
pr_gm.extend(temp_gm)
self.gm = pr_gm
#!/usr/bin/python
import itertools
import utils
import numpy as np
import gmphd
if __name__ == "__main__":
width = 50.0
length = 5.0
nav_status = np.array([10.0, 5.0, 1.57])
print(utils.evaluate_sss_path(nav_status, width, length))
# Test of the selection functions for features inside the fov of the sss
gmphd_components = [
gmphd.GmphdComponent(1, [10, 5], [1, 0, 0, 1]),
gmphd.GmphdComponent(1, [100, 1], [1, 0, 0, 1])
]
means = np.squeeze(np.asarray([comp._mean for comp in gmphd_components]))
print(means, means.shape)
sss_path = utils.evaluate_sss_path(nav_status, width, length)
gmm_mask = utils.inside_polygon(means, sss_path)
gmm_masked = list(itertools.compress(gmphd_components, gmm_mask))
print gmm_masked
def auv_update(self, measures, nav_status, sss_path):
"""
Construction of the update components
s = R + H * _Cov * H.T
eta = H * _Mean
K = _Cov * H.T * ( H * _Cov * H.T + R) ^ -1
Pkk = ( I - K * H ) * _Cov
"""
if len(self.gm) == 0:
return
eta = [np.dot(self.h, comp._mean) for comp in self.gm]
s = [self.r + np.dot(np.dot(self.h, comp._cov), self.h.T) for comp in self.gm]
k = []
for index, comp in enumerate(self.gm):
k.append(np.dot(np.dot(comp._cov, self.h.T), np.linalg.inv(s[index])))
pkk = []
for index, comp in enumerate(self.gm):
pkk.append(np.dot(np.eye(np.shape(k[index])[0]) - np.dot(k[index], self.h), comp._cov))
"""
The predicted components are kept with a decay (1 - Pd) iff they are inside the FOV, contained into pr_gm
"""
# Extraction of the coordinates of the components inside the RFS
means = np.asarray(np.array([comp._mean for comp in self.gm]))
means = np.squeeze(means)
means = np.reshape(means, (means.size / 2, 2))
# Temp copy of the RFS after the prediction step
pr_gm = copy.deepcopy(self.gm)
# Mask of the components inside the FOV
gmm_mask = utils.inside_polygon(means, sss_path)
# print(gmm_mask.shape, pr_gm)
# gmm_fov = list(itertools.compress(self.gmm, gmm_mask))
for idx, comp in enumerate(pr_gm):
if gmm_mask[idx]:
pr_gm[idx]._weight *= 1 - self.detection
for i in np.ndindex(measures.shape[1]):
z = measures[:, i]
temp_gm = []
for j, comp in enumerate(self.gm):
# Computation of q_k
mvn = multivariate_normal(eta[j].squeeze(), s[j])
mvn_result = mvn.pdf(z.squeeze())
temp_gm.append(
GmphdComponent(
self.detection * comp._weight * mvn_result, comp._mean + np.dot(k[j], z - eta[j]), comp._cov
)
)
# The Kappa thing (clutter and reweight)
weight_sum = np.sum(comp._weight for comp in temp_gm)
if weight_sum >= 1e-9:
weight_factor = 1.0 / (self.clutter + weight_sum)
for comp in temp_gm:
comp._weight *= weight_factor
pr_gm.extend(temp_gm)
self.gm = pr_gm
#!/usr/bin/python
import itertools
import utils
import numpy as np
import gmphd
if __name__ == "__main__":
width = 50.0
length = 5.0
nav_status = np.array([10.0, 5.0, 1.57])
print(utils.evaluate_sss_path(nav_status, width, length))
# Test of the selection functions for features inside the fov of the sss
gmphd_components = [gmphd.GmphdComponent(1, [10,5],[1,0,0,1]),gmphd.GmphdComponent(1, [100,1],[1,0,0,1])]
means = np.squeeze(np.asarray([comp._mean for comp in gmphd_components]))
print(means,means.shape)
sss_path = utils.evaluate_sss_path(nav_status, width, length)
gmm_mask = utils.inside_polygon(means, sss_path)
gmm_masked = list(itertools.compress(gmphd_components, gmm_mask))
print gmm_masked