def g500_slam_fn_defs(): # Vehicle state prediction state_markov_predict_fn_handle = None state_markov_predict_fn_parameters = PARAMETERS() state_markov_predict_fn = fn_params(state_markov_predict_fn_handle, state_markov_predict_fn_parameters) # Vehicle state to observation space state_obs_fn_handle = None state_obs_fn_parameters = PARAMETERS() state_obs_fn_parameters.gpsH = np.hstack(( np.eye(2), np.zeros((2,4)) )) state_obs_fn_parameters.dvlH = np.hstack(( np.zeros((3,3)), np.eye(3) )) state_obs_fn = fn_params(state_obs_fn_handle, state_obs_fn_parameters) # Likelihood function for importance sampling state_likelihood_fn_handle = None state_likelihood_fn_parameters = PARAMETERS() state_likelihood_fn = fn_params(state_likelihood_fn_handle, state_likelihood_fn_parameters) # Update function for state state__state_update_fn_handle = None state__state_update_fn_parameters = PARAMETERS() state__state_update_fn = fn_params(state__state_update_fn_handle, state__state_update_fn_parameters) # State estimation from particles state_estimate_fn_handle = None #misctools.sample_mn_cv state_estimate_fn_parameters = PARAMETERS() state_estimate_fn = fn_params(state_estimate_fn_handle, state_estimate_fn_parameters) # Parameters for the filter # Roll, pitch, yaw + velocities is common to all particles - we assume # that the information from the imu is perfect # We only need to estimate x,y,z. The roll, pitch and yaw must be fed # externally state_parameters = {"nparticles":13, "ndims":6, "resample_threshold":-1} # Parameters for the PHD filter feature_parameters = {"max_terms":100, "elim_threshold":1e-4, "merge_threshold":3, "ndims":3} ndims = feature_parameters["ndims"] # Landmark state-prediction feature_markov_predict_fn_handle = gmphdfilter.markov_predict feature_markov_predict_fn_parameters = PARAMETERS() feature_markov_predict_fn_parameters.F = np.eye(3) feature_markov_predict_fn_parameters.Q = np.zeros(ndims) feature_markov_predict_fn = fn_params(feature_markov_predict_fn_handle, feature_markov_predict_fn_parameters) # Landmark state-to-observation function feature_obs_fn_handle = None feature_obs_fn_parameters = PARAMETERS() feature_obs_fn_parameters.H = np.eye(ndims) feature_obs_fn_parameters.R = 0.1*np.eye(ndims) feature_obs_fn_parameters.parent_state_xyz = np.zeros(3) feature_obs_fn_parameters.parent_state_rpy = np.zeros(3) feature_obs_fn = fn_params(feature_obs_fn_handle, feature_obs_fn_parameters) # Likelihood function - not used for the GM PHD filter feature_likelihood_fn = fn_params() # Landmark state update function - not used feature__state_update_fn = fn_params() # Clutter function clutter_fn_handle = None#gmphdfilter.uniform_clutter clutter_fn_parameters = PARAMETERS() clutter_fn_parameters.intensity = 0.01 # Range should be the field of view of the sensor clutter_fn_parameters.range = [[-1, 1], [-1, 1], [-1, 1]] clutter_fn = fn_params(clutter_fn_handle, clutter_fn_parameters) # Birth function birth_fn_handle = camera_birth birth_fn_parameters = PARAMETERS() birth_fn_parameters.intensity = 0.001 birth_fn_parameters.obs2state = lambda x: np.array(x) birth_fn_parameters.parent_state_xyz = np.zeros(3) birth_fn_parameters.parent_state_rpy = np.zeros(3) birth_fn_parameters.R = 0.1*np.eye(3) birth_fn = fn_params(birth_fn_handle, birth_fn_parameters) # Survival/detection probability ps_fn_handle = gmphdfilter.constant_survival ps_fn_parameters = PARAMETERS() ps_fn_parameters.ps = 1 ps_fn = fn_params(ps_fn_handle, ps_fn_parameters) pd_fn_handle = None pd_fn_parameters = PARAMETERS() pd_fn_parameters.width = 2.0 pd_fn_parameters.depth = 3.0 pd_fn_parameters.height = 1.0 pd_fn_parameters.pd = 0.98 pd_fn_parameters.parent_state_xyz = np.zeros(3) pd_fn_parameters.parent_state_rpy = np.zeros(3) pd_fn = fn_params(pd_fn_handle, pd_fn_parameters) # Use default estimator feature_estimate_fn = fn_params() return SLAM_FN_DEFS(state_markov_predict_fn, state_obs_fn, state_likelihood_fn, state__state_update_fn, state_estimate_fn, state_parameters, feature_markov_predict_fn, feature_obs_fn, feature_likelihood_fn, feature__state_update_fn, clutter_fn, birth_fn, ps_fn, pd_fn, feature_estimate_fn, feature_parameters)