User Configurable Localization
Despite recent advancements in AV localization, it is still arguable that versatile navigation has been achieved. In view of this matter, there is a need to develop a unified framework for localization that can handle various cases both for mobile robotics and AV.
So, what we have now is a robust Particle Filter engine with multiple selectable and/or combinable observation model. Please note that observation model is tightly related to map representation. The basic idea is each sensor input must be projected into the map so that a particle which is being evaluated can be weighted based on some assessment value it gets from the map. They are listed below:
- Lane marker based, Type: 2.5D, AV Tech, partly implemented - uses intensity from lidar returns, heavily
- Vertical features based, Type: 2.5D, AV Tech, partly implemented
- Combination (in weight calculation) of lane marker and vertical features, partly implemented
- Raw point-cloud based, Type: 3D, Robotics Tech, partly implemented - uses kd-tree search structure to get the nearest point in the map and sample gaussian noise from there
- Octomap based, Type: 3D, Robotics Tech, partly implemented - uses octomap structure to represent the map point-cloud. More efficient compared to raw methods, as it is also using kind of voxelized structure and additionally uses log likelihood strategy when updating or computing sensor error distribution
- NDT map based, Type: 3D, Robotics Tech, partly implemented - probably state of the art as not only it is voxelized, more efficient, but also more accurate model with the normal distribution transformation to represent points.
- A combination of NDT map and Lane marker, Joint Tech (Robotics+AV?), not implemented - this is may be the next best method that can achieve better robustness in term of combining structure and texture features. We could also add vision to the equation
Methods
Global Nearest Neighbor DA (not related to PF)
The component of the matrix is defined \(D_{ij}^{ }=\left(d_{ij}\right)^2\)
Particle Filter Engine
Model the problem of localization by treating it as a dynamic system:
- It evolves by input and noise, in this case, a robot is moving through space:
- Observability equation, that is modeling what sensor sees: \(\mathbf{z}_k=h_k(\mathbf{x}_k,\mathbf{u}_k,\mathbf{n}_k)\)