pomdp_py.algorithms.bsp package

Submodules

pomdp_py.algorithms.bsp.blqr module

Implementation of B-LQR algorithm described in “Belief space planning assuming maximum likelihood observations” [4]

class pomdp_py.algorithms.bsp.blqr.BLQR(func_sysd, func_obs, jac_sysd, jac_obs, jac_sysd_u, noise_obs, noise_sysd, Qlarge, L, Q, R, planning_horizon=15)[source]

Bases: Planner

ekf_update_mlo(bt, ut)[source]

Performs the ekf belief update assuming maximum likelihood observation. This follows equations 12, 13 in the paper. It’s the function \(F\).

Parameters:

  • bt (tuple) – a belief point bt = (mt, Cov_t) representing the belief state. where mt is np.array of shape (d,) and Cov_t is np.array of shape (d,d). The cost function equation needs to turn Cov_t into a long vector consist of the columns of Cov_t stiched together.

  • ut (np.array) – control vector

  • noise_t (pomdp_py.Gaussian) – The Gaussian noise with “possibly state-dependent” covariance matrix Wt.

  • noise_sysd (np.array) – A noise term (e.g. Gaussian noise) added to the system dynamics (\(v\) in Eq.12). This array should have the sam dimensionality as mt.

  • noise_obs_cov (np.array) – The covariance matrix of the Gaussian noise of the observation function. This corresponds to Wt in equation 13.

integrate_belief_segment(b_i, u_i, num_segments)[source]

This is to represent equation 18.

phi(b_i’, u_i’) = F(b_i’, u_i’) + sum F(b_{t+1}, u_i) - F(b_t, u_i)

t in seg

This essentially returns b_{i+1}’

segmented_cost_function(bu_traj, b_des, u_des, num_segments)[source]

The cost function in eq 17.

Parameters:

  • b_des (tuple) – The desired belief (mT, CovT). This is needed to compute the cost function.

  • u_des (list) – A list of desired controls at the beginning of each segment. If this information is not available, pass in an empty list.

create_plan(b_0, b_des, u_init, num_segments=5, control_bounds=None, opt_options={}, opt_callback=None)[source]

Solves the SQP problem using direct transcription, and produce belief points and controls at segments. Reference: https://docs.scipy.org/doc/scipy/reference/tutorial/optimize.html

interpret_sqp_plan(opt_res, num_segments)[source]

Module contents