0
$\begingroup$

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 ??

enter image description here

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) ...?

enter image description here

Anyone help ??

$\endgroup$
1
  • $\begingroup$ I can't seem to derive it myself. But I am pretty sure I know what they are trying to do. Another way of using the matrix inversion lemma is something called the schur complement(en.wikipedia.org/wiki/Schur_complement). This is often used in SLAM to marginalize out variables that you no longer need. In this case they are trying to marginalize out $Y_0$ in eq 39 and $Y_0,x$ in eq 40. $\endgroup$ – edwinem Jan 23 at 0:06
1
$\begingroup$

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.

$\endgroup$
0
$\begingroup$

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

enter image description here

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:

enter image description here

"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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.