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)
