はじめに
以下の論文
[1] H. Durrant-Whyte, et. al "Simultaneous Localisation and Mapping (SLAM): Part I The Essential Algorithms"
いくつかのヴァージョンが出回ってるみたいだが、こちら
https://ieeexplore.ieee.org/abstract/document/1638022
はgoogle scholarで被引用が5721と圧倒的な数。
chat GPTで「What are the key papers in SLAM (Simultaneous Localization and Mapping)?」と問い合わせたところ、１つ目に出てきた。
ここでは III. Formulation and Structure of the SLAM problem を中心にまとめ。
概要
- 本論文はSLAMのイントロダクション的な内容である
- SLAMはロボットが周囲環境のマッピングと自己位置の推定を同時に行うしくみである
設定
以下のような状況を考える。
${\bf x}_k$ :k時におけるロボットの位置
${\bf u}_k$ :k-1時からk時にむけてのロボットの遷移ベクトル（control vector）
${\bf m}_i$ :i 番目のランドマークの位置
${\bf z}_{ik}$ :k時におけるロボットが i 番目のランドマークを観測したときの観測ベクトル
これらのセットの表記が以下。
${\bf X}_{0:k} = { {\bf x}_0, {\bf x}_1, \cdots {\bf x}_k }$ : ロボットの位置の系列
${\bf U}_{0:k} = { {\bf u}_0, {\bf u}_1, \cdots {\bf u}_k }$ : control vectorの系列
${\bf m} = { {\bf m}_0, {\bf m}_1, \cdots {\bf m}_n }$ : 全てのランドマーク
${\bf Z}_{0:k} = { {\bf z}_0, {\bf z}_1, \cdots {\bf z}_k }$ : あるランドマークに対する全ての観測ベクトル
SLAMの定式化
0時のロボットの位置（${\bf x}_0$）、control vectorの系列（${\bf u}_k$）、観測の系列（${\bf z}_k$）が与えら得た場合に、現在 k 時のロボットの位置（${\bf x}_k$）と全てのランドマークの３次元位置（${\bf m}$）を推定する。
P \left( {\bf x}_{k}, {\bf m} \mid {\bf Z}_{0:k}, {\bf U}_{0:k}, {\bf x}_{0} \right)
observation model
k時にロボットからランドマークを観測した観測ベクトル（${\bf z}_{k}$）は k 時のロボット位置（${\bf x}_k$）とランドマークの３次元位置（${\bf m}$）から決まるはずなので、
P \left( {\bf z}_{k} \mid {\bf x}_k, {\bf m} \right) \tag{2}
とかける。
motion model
マルコフ性を仮定すると、k時のロボット位置（${\bf x}_k$）は k-1 時のロボット位置（${\bf x}_{k-1}$）と、そこから k 時位置までの遷移ベクトル（control vector：${\bf u}_k$）で決まるはずなので、
P \left( {\bf x}_{k} \mid {\bf x}_{k-1}, {\bf u}_k \right) \tag{3}
とかける。
Time-update
\begin{eqnarray}
&P& \left( {\bf x}_{k}, {\bf m} \mid {\bf Z}_{0:k-1}, {\bf U}_{0:k}, {\bf x}_{0} \right) \\
&=& \int P \left( {\bf x}_{k} \mid {\bf x}_{k-1}, {\bf u}_k \right) \\
&\times& P \left( {\bf x}_{k-1}, {\bf m} \mid {\bf Z}_{0:k-1}, {\bf U}_{0:k-1}, {\bf x}_{0} \right) {\rm d}{\bf x}_{k-1} \tag{4}
\end{eqnarray}
これをSLAM問題（１）式と比較すると、条件に k 時の観測ベクトル ${\bf z}_k$ が入ってない。つまりここまでは IMU・オドメトリなどの情報は用いるが、一方でランドマークの観測を用いずに推定している。
Measurement Update
time-updateで求めた x, m は観測ベクトル ${\bf z}_k$ が入ってないので、事前確率と考える。 ${\bf z}_k$ を入れた事後確率を求めるとすると、ベイズの公式から
\begin{eqnarray}
&P& \left( {\bf x}_{k}, {\bf m} \mid {\bf Z}_{0:k}, {\bf U}_{0:k}, {\bf x}_{0} \right)
= \frac{P \left( {\bf z}_{k} \mid {\bf x}_k, {\bf m} \right) P \left( {\bf x}_{k}, {\bf m} \mid {\bf Z}_{0:k-1}, {\bf U}_{0:k}, {\bf x}_{0} \right)}
{P \left( {\bf z}_{k} \mid {\bf Z}_{0:k-1}, {\bf U}_{0:k} \right)} \tag{5}
\end{eqnarray}
となる。右辺分子第１項目は観測モデルを尤度として用いている。
右辺分母に関しては？
SLAMの例１ EKF-SLAM
motion model
P \left( {\bf x}_{k} \mid {\bf x}_{k-1} , {\bf u}_{k}\right) \leftrightarrow {\bf x}_{k} = {\bf f} ({\bf x}_{k-1}, {\bf m}) + {\bf w}_k \tag{6}
observation model
P \left( {\bf z}_{k} \mid {\bf x}_{k} , {\bf m} \right) \leftrightarrow {\bf z}(k) = {\bf h} ({\bf x}_{k}, {\bf m}) + {\bf v}_k \tag{7}
書きかけ