Sparcification of SLAM with SEIF (Sparse Extended Information Filter)

Question

I am studying SLAM (Simultaneous Localization and Mapping) with textbook "probabilistic robotics".

I cannot understand well about Sparsification step, so I found original paper for that; https://web.mit.edu/2.166/www/handouts/thrun_et_al_ijrr2004_seifs.pdf

According to this paper, information matrix for the distribution of all variables without passive features can be written as equation (38), this is ok for me, but how equation(40) can be lead ??

It says that using the matrix inversion lemma like annotated (39), but what is "B" matrix here and why has it gone from resulting equation (40) & (41) ...?

Anyone help ??

Answer 1

I still couldn't derive it, but maybe I could prove it thanks to willSapgreen.

Information matrix of distribution $p(x_t, Y^+, Y^0|Y^-=0)$ is $H_t'$; $$H_t'=S_{x_t,Y^+,Y^0}S^T_{x_t,Y^+,Y^0}H_tS^T_{x_t,Y^+,Y^0}S_{x_t,Y^+,Y^0}$$

This matrix can be written as $$H_t' = \left(\begin{array}{cc} H_{x_t,Y^+,x_t,Y^+}&H_{x_t,Y^+,Y^0}\\H_{Y^0,x_t,Y^+}&H_{Y^0,Y^0} \end{array}\right)\tag{1}$$

Information matrix of marginal distribution $p(x_t, Y^+|Y^-=0)$ is as follows. Note that dimension of $\tilde{H}_{x_t,Y^+,x_t,Y^+}$ is $3n\times3n$ where $n$ is the number of active features that we are going to inactivate.

$$\tilde{H}_{x_t,Y^+,x_t,Y^+}=H_{x_t,Y^+,x_t,Y^+}-H_{x_t,Y^+,Y^0}H^{-1}_{Y^0,Y^0}H_{Y^0,x_t,Y^+}$$

What we need as $H_t^1$ should be as follows. Note that dimension of $H_t^1$ is $3(N+1)\times3(N+1)$ where dimension of $x_t$ is $1\times3$ and $N$ is the number of all features($Y^+,Y^0,Y^-$), and $"0"$ here means zero matrix that has appropriate dimension. $$H_t^1 = \left(\begin{array}{cc} \tilde{H}_{x_t,Y^+,x_t,Y^+}&0\\0&0 \end{array}\right)\tag{2}$$

If we calculate equation(38) with using elements of (1), then $$\begin{align} H_t'&-H_t'S_{Y^0}(S_{Y^0}^TH_t'S_{Y^0})^{-1}S_{Y^0}^TH_t' \\ &= \left(\begin{array}{cc} H_{x_t,Y^+,x_t,Y^+}&H_{x_t,Y^+,Y^0}\\H_{Y^0,x_t,Y^+}&H_{Y^0,Y^0} \end{array}\right)- \left(\begin{array}{c}H_{x_t,Y^+,Y^0}\\H_{Y^0,Y^0}\end{array}\right) H_{Y^0,Y^0}^{-1} \left(\begin{array}{cc}H_{x_t,Y^+,x_t,Y^+}&H_{x_t,Y^+,Y^0}\end{array}\right) \\ &= \left(\begin{array}{cc} H_{x_t,Y^+,x_t,Y^+}&H_{x_t,Y^+,Y^0}\\H_{Y^0,x_t,Y^+}&H_{Y^0,Y^0} \end{array}\right)- \left(\begin{array}{cc} H_{x_t,Y^+,x_t,Y^+}H_{Y^0,Y^0}^{-1}H_{Y^0,x_t,Y^+}&H_{x_t,Y^+,Y^0}\\H_{Y^0,x_t,Y^+}&H_{Y^0,Y^0} \end{array}\right)\\ &= \left(\begin{array}{cc}\tilde{H}_{x_t,Y^+,x_t,Y^+}&0\\0&0\end{array}\right)\\ &=H_t^1 \end{align}$$

Equation (38) can be equalled with (2).

This is not exactly the answer to what I questioned, but this is ok for me.

Answer 2

Open discussion

I have the same question and use the marginalization method (instead of matrix inversion lemma) to get the following derivation.

Note: $z_t$ and $u_t$ are ignored to make the equations shorter in the follwoing derivation.

The equation (38) is the information matrix

for the distribution $p(x_t,Y^0,Y^+|Y^-=0)$

If we separate $Y^0$ and non-$Y^0$ ($x_t$ and $Y^+$) parts in $H'_t$ as

$$ \begin{matrix} H_{x_tY^+x_tY^+} & H_{x_tY^+Y^0} & \\ H_{Y^0x_tY^+} & H_{Y^0Y^0} & \end{matrix} $$ , where

$H_{Y^0Y^0}$ is $S_{Y^0}H'_tS_{Y^0}^T$

$H_{x_tY^+x_tY^+}$ is $S_{x_tY^+}H'_tS_{x_tY^+}^T$

$H_{Y^0x_tY^+}$ is $S_{Y^0}H'_tS_{x_tY^+}^T$

$H_{x_tY^+Y^0}$ is $S_{x_tY^+}^TH'_tS_{Y^0}$

And based on the marginalizayion rule:

"from PROBABILISTIC ROBOTICS"

So the informaiton matrix for $p(x_t,Y^+|Y^-=0)$ is

$$ H_{x_tY^+x_tY^+} - H_{x_tY^+Y^0}(H_{Y^0Y^0})^{-1}H_{Y^0x_tY^+} $$

equal to

$$ S_{x_tY^+}(H'_t - H'_tS_{Y^0}^T(S_{Y^0}H'_tS_{Y^0}^T)^{-1}S_{Y^0}H'_t)S_{x_tY^+}^T $$

I am still working on elimiting $S_{x_tY^+}$ ...

I understand this derivation is not even close to an "answer" but think creating another thread is kind of wasting because we already have this thread.