Template Function sdsl::fusion_2d

• Defined in File fusion_2d.hpp

Function Documentation

template<typename FT>
std::vector<FT> sdsl::fusion_2d(std::vector<Voxel<3, FT>> Xt_, std::vector<FT> Bel_Xt_, std::vector<Voxel<3, FT>> Xt, Configuration<3, FT> Ut, FT eps)

Fusion of two belief distributions over SE(2) configurations.

Parameters:

• Xt_ – Previous set of voxels (at time t-1).

• Bel_Xt_ – Previous belief values for each voxel in Xt_.

• Xt – Current set of voxels (at time t).

• Ut – Odometry reading (control input) from t-1 to t, in the form of (dx, dy, dtheta).

• eps – Variance parameter for the Gaussian noise model of the odometry. Higher eps means more uncertainty.

Returns:

Updated belief values for each voxel in Xt after fusing with the odometry reading Ut.