def predict(self, ctrl_input, predict_to_time):
if self.last_time.predict == 0:
self.last_time.predict = predict_to_time
return
delta_t = predict_to_time - self.last_time.predict
self.last_time.predict = predict_to_time
if delta_t < 0:
return
# Predict states
trans_mat, sc_process_noise = self.trans_matrices(ctrl_input, delta_t)
#pred_states = blas.dgemv(trans_mat, self.vehicle.states)
pred_states = np.array(
np.dot(trans_mat[0], self.vehicle.states.T).T, order='C')
self.vehicle.states = pred_states
# Predict covariance
#code.interact(local=locals())
self.vehicle.covs = kf_predict_cov(self.vehicle.covs, trans_mat,
sc_process_noise)
# Copy the particle state to the PHD parent state
parent_ned = np.array(pred_states[:, 0:3])
#Calculate the rotation matrix to store for the map update
rot_mat = featuredetector.tf.rotation_matrix(ctrl_input)
# Copy the predicted states to the "parent state" attribute and
# perform a prediction for the map
for i in xrange(self.vars.nparticles):
self.vehicle.maps[i].parent_ned = parent_ned[i]
self.vehicle.maps[i].parent_rpy = ctrl_input
self.vehicle.maps[i].vars.H = rot_mat
def predict(self, ctrl_input, predict_to_time):
USE_KF = True
if self.last_odo_predict_time==0:
self.last_odo_predict_time = predict_to_time
return
delta_t = predict_to_time - self.last_odo_predict_time
self.last_odo_predict_time = predict_to_time
if delta_t < 0:
#print "negative delta_t, ignoring"
return
trans_mat, sc_process_noise = self.trans_matrices(ctrl_input, delta_t)
pred_states = blas.dgemv(trans_mat, self.states)
#pred_states2 = np.dot(trans_mat[0], self.states.T).T
nparticles = self.parameters.state_parameters["nparticles"]
ndims = self.parameters.state_parameters["ndims"]
if not USE_KF:
pred_states += np.random.multivariate_normal(mean=np.zeros(ndims, dtype=float),
cov=sc_process_noise,
size=(nparticles))
self.states = pred_states
self.covariances = kf_predict_cov(self.covariances, trans_mat,
sc_process_noise)
states_xyz = np.array(pred_states[:,0:3])
#Calculate the rotation matrix to store for the map update
rot_mat = featuredetector.tf.rotation_matrix(ctrl_input)
# Copy the predicted states to the "parent state" attribute and
# perform a prediction for the map
for i in range(self.parameters.state_parameters["nparticles"]):
#self.maps[i].parameters.obs_fn.H = self.trans_matrices(-ctrl_input, 1.0)[0]
setattr(self.maps[i].parameters.obs_fn.parameters,
"parent_state_xyz", states_xyz[i])
setattr(self.maps[i].parameters.obs_fn.parameters,
"parent_state_rpy", ctrl_input)
self.maps[i].parameters.obs_fn.H = rot_mat
setattr(self.maps[i].parameters.pd_fn.parameters,
"parent_state_xyz", states_xyz[i])
setattr(self.maps[i].parameters.pd_fn.parameters,
"parent_state_rpy", ctrl_input)
setattr(self.maps[i].parameters.birth_fn.parameters,
"parent_state_xyz", states_xyz[i])
setattr(self.maps[i].parameters.birth_fn.parameters,
"parent_state_rpy", ctrl_input)
