Particle Filter Localization

How to calculate covariance matrix from particles

The covariance matrix $\Xi$ from particle information is calculated by the following equation:

$\Xi_{j,k}=\frac{1}{1-\sum^N_{i=1}(w^i)^2}\sum^N_{i=1}w^i(x^i_j-\mu_j)(x^i_k-\mu_k)$

  • $\Xi_{j,k}$ is covariance matrix element at row $i$ and column $k$.

  • $w^i$ is the weight of the $i$ th particle.

  • $x^i_j$ is the $j$ th state of the $i$ th particle.

  • $\mu_j$ is the $j$ th mean state of particles.

Ref: