Particle filter localization
This is a sensor fusion localization with Particle Filter(PF).
The blue line is true trajectory, the black line is dead reckoning trajectory,
and the red line is estimated trajectory with PF.
It is assumed that the robot can measure a distance from landmarks (RFID).
This measurements are used for PF 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.