示例#1
0
 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
示例#2
0
 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()
示例#3
0
 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
示例#4
0
 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