### FastSLAM 2.0

In FastSLAM 1.0, we have a 2-step estimation process for the SLAM posterior,

$p(s^t, \Theta|z^t, u^t)=p(s^t|z^t, u^t)\prod^n{p(\theta_n|s^t, z^t, u_t)}$

First, we use a particle filter to estimate the robot path posterior,

$p(s^t|z^{t-1}, u^t) = p(s_t|s^{t-1}, u_t)p(s^{t-1}|z^{t-1}, u^{t-1})$

then based on the above proposal distribution, observations are incorporated to update the landmark filters (EKFs).

However, in the real application where motion noise is larger than measurement noise, FastSLAM 1.0 tends to behave poorly. One could view the first step of applying the motion command to particles as a way to generate possible robot paths, a proposal distribution. The second step we use evidence (observations) to get rid of some of the improbable paths. When the motion noise is quite significant, the proposal distribution will greatly differ from the robot path posterior distribution. Even if we have the pruning process, we still suffer from sampling from a poor distribution.

The simulation uses 200 particles and the robot's motion noise is far greater than measurement noise.

Looking at the first two equations, we can see the SLAM posterior requires $p(s^t|z^t, u^t)$, but we only use $p(s^t|z^{t-1}, u^t)$ to approximate. In FastSLAM 2.0, the latest observation is incorporate into the first step to get a better proposal distribution.

Since from the last iteration we have a particle set which approximate the $t-1$ step posterior distribution, $p(s^{t-1}|z^{t-1}, u^{t-1})$, we need to estimate $p(s_t|s^{t-1}, z^t, u^t)$.

Using Bayes' theorem and Markov property,

$p(s_t|s^{t-1}, z^t, u^t)=\eta\ p(z_t|s_t, s^{t-1}, z^{t-1},u^t)p(s_t|s^{t-1},u_t)$

Conditioned on the landmarks we have,

$p(s_t|s^{t-1}, z^t, u^t)=\eta\ \int{p(z_t|\theta_n, s_t)p(\theta_n|s^{t-1}, z^{t-1}, u^{t-1})}d\theta_n\ p(s_t|s^{t-1},u_t)$

In the above equation, the first term is the measurement model, the second term is the landmark prior distribution and the last term is the motion model. The resulting distribution is a product of a convolution of two Gaussian distribution with another Gaussian distribution. In order to get a closed form, it is required that all the Gaussian distributions are linear. Since the observation model is not linear, we need apply Tayler expansion to linearize it.

$\hat{s_t}=h(s^{t-1}, u_t)$

$\hat{z_t}=g(\hat{s_t}, \mu_{n, t-1})$

$G_{\theta_n} = \nabla_{\theta_n} g(s_t, \theta_n)| _{s_t=\hat{s_t}, \theta_n=\mu_{n, t-1}}$

$G_{s} = \nabla_{s} g(s_t, \theta_n)| _{s_t=\hat{s_t}, \theta_n=\mu_{n, t-1}}$

We can get the linear approximation of the integral,

$z_t \approx \hat{z_t} + G_{\theta_n}(\theta_n - \mu_{n, t-1}) +G_{s}(s_t - \hat{s_t})$

By multiplying with the right most term, we can obtain the proposal distribution,

$\Sigma_{s^t} = [G_s^TZ_t^{-1}G_s + P_t^{t-1}]^{-1}$

$\mu_{s_t} = \Sigma_{s^t}G_s^TZ_t^{-1}(z_t - \hat{z_t}) + \hat{s_t}$

where $Z_t = G_{\theta_n}^T\Sigma_{\theta_n}G_{n, t-1} + R_t$, and $P_t$ is the motion noise added to $h(s^{t-1}, u_t)$

So now, we draw a new particle set based on the above distribution to form the proposal distribution. Incorporation of the observations is the same as that of FastSLAM 1.0 except we have a more accurate proposal distribution.

For the importance weight, we have a slightly change.

The weight is defined as the ratio of target distribution over proposal distribution, which can be simplified as $p(z_t|s^{t-1}, z^{t-1}, u^t)$

Conditioned on $s_t$ and $\theta$, we can simplify the expression and get the distribution,

$w_t \sim \mathcal{N}(z_t; \hat{z_t}, G_{\theta_n}^T\Sigma_{n, t-1}G_{\theta_n} +G_{s}^TP_tG_s + R_t))$

The rest of the steps are the same as those in the FastSLAM 1.0. Thus the only difference between the two versions are the estimation of proposal distribution (particle poses after the motion) and formula for the importance weight. One can adjust $P_t$ to reflect how noisy the motion can be.

The video below is the simulation with FastSLAM 2.0. Each iteration takes longer than 1.0 but more robust.

The python code can be found here.