A toy example helps understand OpenCL and particle filter [source code].
Notations
- state vector: \(\mathrm{x} \in \mathfrak{se}(2)\)
- motion noise: \(\mathrm{w}\)
- motion measurement: \(\mathrm{v}\)
- observation: \(\mathrm{y}\)
- motion model: \(\mathrm{x}_k = f(\mathrm{x}_{k-1}, \mathrm{v}_k, \mathrm{w}_k)\), \(k = 1, \dots, K\)
- observation model: \(\mathrm{y}_k = g(\mathrm{x}_k, \mathrm{n}_k)\), \(k = 0, \dots, K\)
- Number of particles: \(N\)
- Weight of particles: \(\omega\)
Estimation
1. Prediction
Unlike steps in the textbook[1], I do prediction before sampling particles.
\[ \hat{\mathrm{x}}_{k} = f(\mathrm{x}_{k-1}, \mathrm{v}_k, \mathrm{w}_{k}) \tag{2-1} \]2. Particle sampling
\[ \left[ \begin{matrix} \check{\mathrm{x}}_{k,m} \\ \mathrm{w}_{k,m} \end{matrix} \right] \leftarrow p(\hat{\mathrm{x}}_{k}) p(\mathrm{w}_{k-1}) \tag{2-2} \]3. Likelihood calculation
Here I use the likelihood between the simulated LiDAR measurement to actual LiDAR sensor data as the particle weight.
\[ \omega_{k,m} \propto p(y_k | \check{\mathrm{x}}_{k,m}) = p(y_k | \check{\mathrm{y}}_{k,m}) \tag{2-3} \]LiDAR measurement model:
\[ p(y_k|\check{\mathrm{y}}_{k,m}) = \begin{cases} \text{PDF}(y_k - \check{\mathrm{y}}_{k,m}) && \text{ if } \ y_k, \check{\mathrm{y}}_{k,m} \le L_{max}), \\ 1 - \text{CDF}(y_k - \check{\mathrm{y}}_{k,m}) && \text{ if } \ y_k > L_{max} \text{ or } \check{\mathrm{y}}_{k,m} > L_{max}, \\ \epsilon && \text{else} \end{cases} \tag{2-4} \]\(L_{max}\) means the maximum measure distance of the LiDAR sensor. \(\text{PDF}\) is the probabilistic distribution function of the sensor model, a gaussian model is used in this project. \(\text{CDF}\) is the cumulative distribution function of the sensor model. \(epsilon\) is a constant small value in code implementation which minimizes the impact of overranged LiDAR beam.
4. Resampling
I use the method in this paper[2] to do resampling. Leetcode 528 is also helpful for understanding.
Parallelization
In this project, I use line segments to represent objects in map without any acceleration structure. For a single line LiDAR with resolution \(H\), working in a map with \(S\) segments, a particle filter with \(N\) particles has to calculate line intersection for with complexity \(O(H \times S \times N)\). While those calculations are totally independent, it is available to do parallelization. The determination of line segment intersection is presented here[4].
After eliminating the LiDAR beams that never intersect with objects, the rest beams' measure distances were calculated. Parallel reduction was used to find the "close hit" among all intersections of a single LiDAR beam.
Besides, the likelihood between simulation and actual measurement are also calculated in GPU. To avoid float point overflow, the calculation was done in logarithmic space. For there is no built-in cumulative gaussian function in OpenCL, it is approximated by error function:
\[ \text{CDF}(x) = \frac{1}{2} \left[ 1 + \text(erf)(\frac{x - \mu}{\sigma \sqrt 2}) \right] \]Experiments
Ice World
Featureless Corridor
Circular Corridor
References
- Barfoot, Timothy D. "State estimation for robotics." Cambridge University Press, 2017.
- Terejanu, Gabriel A. "Tutorial on Monte Carlo Techniques." Department of Computer Science and Engineering. University at Buffalo (2009).
- Atsushi, Sakai. "PythonRobotics."
- Xiaoxing, Chen. "Determine Intersection of Two Planar Line Segments"