def _make_sigma_states_(self, slam_worker, mean_state, nparticles):
sc_process_noise = \
slam_worker.trans_matrices(np.zeros(3), 1.0)[1] + \
slam_worker.trans_matrices(np.zeros(3), 0.01)[1]
sigma_states = sigma_pts(mean_state,
sc_process_noise[0:3,0:3].copy(),
_alpha=UKF_ALPHA, _beta=UKF_BETA,
_kappa=UKF_KAPPA)[0]
sigma_states = np.array(
np.hstack((sigma_states, np.zeros((nparticles,3)))),
order='C')
return sigma_states
def resetNavigation(self, req):
print "Resetting navigation..."
rospy.loginfo("%s: Reset Navigation", self.name)
#self.slam_worker.states[:,0:2] = 0
ndims = self.slam_worker.parameters.state_parameters["ndims"]
nparticles = self.slam_worker.parameters.state_parameters["nparticles"]
if nparticles == 2*ndims + 1:
#pose_angle = tf.transformations.euler_from_quaternion(
# self.vehicle.pose_orientation)
sc_process_noise = self.slam_worker.trans_matrices(np.zeros(3), 1.0)[1] + self.slam_worker.trans_matrices(np.zeros(3), 0.01)[1]
self.slam_worker.states = sigma_pts(np.zeros((1, ndims)), sc_process_noise, _alpha=UKF_ALPHA, _beta=UKF_BETA, _kappa=UKF_KAPPA)[0]
else:
self.slam_worker.states[:, :] = 0
return EmptyResponse()
def setNavigation(self, req):
print "Setting new navigation..."
#rospy.loginfo("%s: Set Navigation to: \n%s", self.name, req)
#self.slam_worker.states[:,0] = req.north
#self.slam_worker.states[:,1] = req.east
ndims = self.slam_worker.parameters.state_parameters["ndims"]
nparticles = self.slam_worker.parameters.state_parameters["nparticles"]
if nparticles == 2*ndims + 1:
#pose_angle = tf.transformations.euler_from_quaternion(
# self.vehicle.pose_orientation)
sc_process_noise = self.slam_worker.trans_matrices(np.zeros(3), 1.0)[1] + self.slam_worker.trans_matrices(np.zeros(3), 0.01)[1]
mean_state = np.array([[req.north, req.east, 0, 0, 0, 0]])
self.slam_worker.states = sigma_pts(mean_state, sc_process_noise, _alpha=UKF_ALPHA, _beta=UKF_BETA, _kappa=UKF_KAPPA)[0]
else:
self.slam_worker.states[:,0] = req.north
self.slam_worker.states[:,1] = req.east
self.slam_worker.states[:,2] = self.vehicle.pose_position[2]
ret = SetNEResponse()
ret.success = True
return ret
def init_slam(self):
# Get default parameters
slam_properties = girona500.g500_slam_fn_defs()
# Add new parameters from ros config
# Process noise
slam_properties.state_markov_predict_fn.parameters.process_noise = \
np.eye(3)*self.config.model_covariance
# GPS observation noise
slam_properties.state_likelihood_fn.parameters.gps_obs_noise = \
np.eye(2)*self.config.gps_covariance
# DVL bottom velocity noise
slam_properties.state_likelihood_fn.parameters.dvl_bottom_noise = \
np.eye(3)*self.config.dvl_bottom_covariance
# DVL water velocity noise
slam_properties.state_likelihood_fn.parameters.dvl_water_noise = \
np.eye(3)*self.config.dvl_water_covariance
slam_worker = girona500.G500_PHDSLAM(*slam_properties)
ndims = slam_worker.parameters.state_parameters["ndims"]
nparticles = slam_worker.parameters.state_parameters["nparticles"]
if nparticles == 2*ndims+1:
sc_process_noise = slam_worker.trans_matrices(np.zeros(3), 1.0)[1] + slam_worker.trans_matrices(np.zeros(3), 0.01)[1]
slam_worker.states = sigma_pts(np.zeros((1, ndims)), sc_process_noise, _alpha=UKF_ALPHA, _beta=UKF_BETA, _kappa=UKF_KAPPA)[0]
return slam_worker
