Given I am building a grid map, and poses are represented using particles, how can I calculate the uncertainty of a pose In a grid based fastslam 2.0?
1 Answer
$\begingroup$
$\endgroup$
This depends on how you want to quantify the uncertainty. For example, you can compute the sample covariance between all state variables i.e.,
$$ q_{jk} = \frac{1}{N-1} \sum_{i=1}^{N} (x_{ij} - \bar{x}_j) (x_{ik} - \bar{x}_k) $$
where the covariance matrix is $\mathbf{Q}$ with entries $q_{ji}$. This is straightforward; however, different approaches exist. For estimating the uncertainty, you could then compute the trace or determinant of the covariance matrix, but other metrics exist as well.
