※ 現在書き途中です．
「SLAM入門」学習の備忘録です．
漸化式化
前回，漸化式に変形した完全SLAM問題
p(x_{0:t}| z_{1:t}, a_{1:t}, c_{1:t})= η p(z_t | x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t})p(x_t| x_{t-1}, a_t)p(x_{0:t-1}| z_{1:t-1}, a_{1:t-1}, c_{1:t-1}) \tag{1}
このうち右辺の1項目$p(z_t | x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t})$も同様に漸化式に変形していきます．
\begin{equation*}
\begin{split}
加法定理（周辺化）でmの積分の形を作ります\\
\ p(z_t | x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t}) =& \int p(z_t, m | x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t}) dm\\
乗法定理で分割します\\
=& \int \frac{p(z_t, m, x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t})}{p(x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t})} dm\\
分子を乗法定理で分割する\\
=& \int \frac{p(z_t| m, x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t})p(m, x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t})}{p(x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t})} dm\\
分子の2項目を乗法定理で分割する\\
=& \int \frac{p(z_t| m, x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t})p(m| x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t})p(x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t})}{p(x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t})} dm\\
分母と分子で同じ項を削除する（これで2行目の式になる）\\
=& \int p(z_t| m, x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t})p(m| x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t}) dm\\
1項目において，軌跡x_{0:t-1}によって求められた地図mと，\\
位置x_tとランドマークと計測値の対応付けc_tがあれば計測値z_tが\\
決まるという過程に基づいて依存関係のない変数を取り除く\\
=& \int p(z_t| m, x_t, c_t)p(m| x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t}) dm\\
2項目において，すべての項を \\
t-1に合わせる（これで3行目になる）\\
=& \int p(z_t| m, x_t, c_t)p(m| x_{0:t-1}, z_{1:t-1}, a_{1:t-1}, c_{1:t-1}) dm\\
\end{split}
\end{equation*}
一様分布による近似
使用するのがレーザスキャナのような精度のよいセンサの場合，$m$の分布は十分にせまいと考えられます．
そこで，与えられた$x_{0:t-1}$のもとで推定した地図$\bar{m_{t-1}}$を考え，$p(m| x_{0:t-1}, z_{1:t-1}, a_{1:t-1}, c_{1:t-1}) $を下図のように$\bar{m}_{t-1}$近傍の微小領域$S$区間で一定値$K$をとり，それ以外は0をとる一様分布で近似します．
一様分布なので$\int_{S}Kdm=1$です．
\begin{equation*}
\begin{split}
\ p(z_t | x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t}) =& \int p(z_t| m, x_t, c_t)p(m| x_{0:t-1}, z_{1:t-1}, a_{1:t-1}, c_{1:t-1}) dm\\
上記の通り右辺を近似します．\\
\approx & \int_{S} p(z_t| \bar{m}_{t-1}, x_t, c_t) K dm\\
上記の通り積分項が一様分布で1となるので\\
取り除きます（これで(4.9)の3行目になります）\\
=& p(z_t| \bar{m}_{t-1}, x_t, c_t)\\
\end{split}
\end{equation*}
式(4.8)を以下のように表すことができます．
\begin{equation*}
\begin{split}
\ p(x_{0:t}| z_{1:t}, a_{1:t}, c_{1:t})=&η p(z_t | x_{0:t}, z_{1:t-1}, a_{1:t}, c_{1:t})p(x_t| x_{t-1}, a_t)p(x_{0:t-1}| z_{1:t-1}, a_{1:t-1}, c_{1:t-1}) \\
右辺2項目を式(4.8)により変換します\\
=&η p(z_t| \bar{m}_{t-1}, x_t, c_t)p(x_t| x_{t-1}, a_t)p(x_{0:t-1}| z_{1:t-1}, a_{1:t-1}, c_{1:t-1})\\
漸化式であるため，初期値を用いた総乗の式に変換します(これで2項目の式になる)\\
=&η p(x_0) \prod_t p(z_t| \bar{m}_{t-1}, x_t, c_t)p(x_t| x_{t-1}, a_t)\\
場所を入れ替えます(これで2項目の式になる)\\
=&η p(x_0) \prod_t p(x_t| x_{t-1}, a_t)p(z_t| x_t, \bar{m}_{t-1}, c_t)\\
\end{split}
\end{equation*}
センサ融合
このとき$p(x_t| x_{t-1}, a_t)$ と $ p(z_t| x_t, \bar m_{t-1}, c_t)$ をセンサ融合したものを$p(x_t| x_{t-1}, a_t)p(z_t| x_t, \bar m_{t-1}, c_t)=A$とあらわすと，
その要素$p(x_t| x_{t-1}, a_t)$ と $p(z_t| x_t, \bar m_{t-1}, c_t)$は正規分布であり，その積$A$も同様に正規分布となります．
※ センサ融合とは異なるセンサ情報を合成して正しい値を求めることをいいます．今回ではオドメトリから求めた位置の確率密度$p(x_t| x_{t-1}, a_t)$と，レーザースキャンのスキャンマッチングにより求めた位置の確率密度$ p(z_t| x_t, \bar m_{t-1}, c_t)$を融合（かけ合わせ）してより正確な値$\bar x_t$（平均値）を求めることをいいます．
正規分布による定式化と相対位置への変換
上式の運動モデル$p(x_t| x_{t-1}, a_t)$は正規分布$N( \tilde x_t, \Sigma_{a_t} )$で表されます．
$\tilde x_t$はオドメトリから求めたロボット位置$x_t$の推定値，$\Sigma_{a_t}$はオドメトリで計測した移動量$a_t$の共分散行列です．
計測モデル$p(z_t| x_t, \bar m_{t-1}, c_t)$は正規分布$N( \hat x_t, \Sigma_{S_t} )$で表されます．
$\hat x_t$はレーザースキャン$z_t$をマッチングして求めたロボット位置$x_t$の推定値，$\Sigma_{S_t}$はスキャンマッチングにおける共分散行列です．
ポーズグラフとして表すために，$A$の正規分布を相対位置を用いたものに変換します．
すなわち$x_t$から$x_{t-1}$を引き「相対位置」にすることで，「地図座標系」から「直前のロボット位置の極所座標系」に変換します．
これは実際には$d_t = \bar x_t \ominus \bar x_{t-1}$と計算できます．
平均$\bar x_t$はロボット位置$x_t$の融合結果ですが，地図座標系で定義されています．
こうして得た相対位置の融合結果を正規分布$N(d_t, \Sigma_t)$で表すと，式(4.8)は
\begin{equation*}
\begin{split}
\ p(x_{0:t}| z_{1:t}, a_{1:t}, c_{1:t})=&η p(x_0) \prod_t p(x_t| x_{t-1}, a_t)p(z_t| x_t, \bar{m}_{t-1}, c_t)\\
正規分布と仮定しているので平均と分散で表示する\\
=& η p(x_0) \prod_t N(d_t, \Sigma_t )\\
正規分布の式に変換する（これで式(4.10)の式になる）\\
=& η p(x_0) \prod_t \frac{1}{\sqrt{2\pi\Sigma_t}} exp \left\{ -\frac{1}{2} \left( r_t - d_t \right)^T \Sigma_t^{-1} \left( r_t - d_t \right) \right\}
\end{split}
\end{equation*}
のようになります．
ここで，$r_t$は隣接するロボと位置間の相対距離であり，$r_t = f(x_{t-1}, x_t)$と表せます．$f$は相対位置を求める関数で$f(x_1, x_2) = x_2 \ominus x_1$です．
$r_t - d_t$は省略を省くと以下の式になります．
\begin{equation*}
\begin{split}
r_t - d_t =& x_t \ominus x_{t-1} - \bar x_t \ominus \bar x_{t-1} \\
=& (ロボットの相対位置） - （ロボット相対位置の融合(計算)結果)
\end{split}
\end{equation*}
ポーズ調整での最小二乗問題によるロボット軌跡の取得
$x_t$は時刻$t$におけるロボットの位置ですが，ポーズ調整の最小二乗問題では求めるべき変数となります．
前回同様にポーズ調整の最小二乗問題を解くには，式(4.10)の負の対数をとり
J = \sum_t ( f(x_{t-1}, x_t) - d_t )^T \Sigma_t^{-1} (f(x_{t-1}, x_t) - d_t )
を最小にする$x_{0:t}$を解く必要があります．ただし，この式ではループが考慮されていません．ループ検出もスキャンマッチングで行うので，その推定位置は正規分布に従うと仮定します．
時刻$t_i$と$t_j$のロボット位置の間でループが検出されたとして，その結果を$N(d_{st}, \Sigma_{st})$と書くと，ポーズ調整で最小化すべき式は
J = \sum_t ( f(x_{t-1}, x_t) - d_t )^T \Sigma_t^{-1} (f(x_{t-1}, x_t) - d_t ) + \sum_{(s,t) \in C} (f(x_s, x_t) - d_{st})^T \Sigma_{st}^{-1} (f(x_s, x_t) - d_{st})
になります．ここで$C$はランドマークと計測データの対応づけの時系列です．
この式$J$を最小化するよう$x_{0:t}$を決定します．
ポーズ調整を行うと，$J$を最小化するロボットの軌跡$x_{0:t}$とその共分散が得られます．
なお，「SLAM入門」ではロボット軌跡だけを用い，共分散は扱いません．共分散を扱わない故に，ロボット軌跡を確率分布ではなく確定値として得ることを意味します．
地図生成
ロボット軌跡が得られたら，各時刻のロボット位置に合わせてスキャンの点群を配置して地図を生成します．
最終的な地図は，ポーズ調整を行ってロボット軌跡が確定した後，式(4.7)で求めることになります．
「SLAM入門」では，ポーズ調整後のロボット軌跡を確定値として扱うので，地図はそのロボット軌跡に対してだけ計算します．すなわち，確定したロボット軌跡を$\bar x_{0:t}$とすると，式(4.7)は，
p(m| \bar x_{0:t}, z_{1:t}, a_{1:t}, c_{1:t}) = \prod_i p(q_i | \bar x_{0:t}, z_{1:t}, a_{1:t}, c_{1:t})
となります．
続きはまた編集し直します。