![[PukiWiki]](image/pukiwiki.png "[PukiWiki]")
概要 †
参考 †
手順 †
用語 †状態量: 推定する目的の確率変数のベクトル。\( x \)で表す。 平均値: この文書の文脈では、状態量の平均。\( \mu \)で表す。 分散共分散行列: このページの文脈では、状態量の分散共分散行列。\( \Sigma \)で表す。 制御量: 状態量の速度の観測に関する、確率変数のこと。\( u \)で表す。 状態遷移関数: ある制御量が新しく入力された時、以前の平均値とその制御量によって、どの平均値に移るかを推定するベクトル関数。\( \mu_t = g(\mu_{t-1}, u) \)で表す。 予測段: カルマンフィルタは二つのステップに完全に独立に分かれている。その内、制御量を用いて、平均値と分散共分散を更新するステップ。 観測量: センサによる観測に関する、確率変数のこと。\( z \)で表す。 観測関数: 平均値と何らかモデルを入力にして、理想的な観測を計算するための関数。\( h(\mu) \)で表す。モデルを明示する場合は、\( h(\mu, m) \)とすることもある。 観測段: カルマンフィルタは二つのステップに完全に独立に分かれている。その内、観測量を用いて、平均値と分散共分散行列を更新するステップ。 イメージ †状態量: ロボットの位置(x, y, theta)の確率変数 平均値: ロボットの自己位置の中で最も尤もらしい場所。これがいわゆるオドメトリとして扱われる。 分散共分散行列: ロボットの自己位置の不確かさ。例えば、ロボットが「自分は大体x方向に関して標準偏差5cmくらいはずれてるかも…」とか思ってることを表現している。 制御量: エンコーダの情報。確かにこれらはx, y, thetaの速度を司る。 状態遷移関数: オドメトリ更新式。これは、「一つ前のオドメトリとエンコーダ」を入力に、「直後のオドメトリ」を得るベクトル関数である。 予測段: エンコーダによる更新。平均値と分散共分散行列の更新の2本の式に依るが、今までの更新式はまさに平均値の更新にあたる。 観測量: 測距センサによる観測や、カメラや測域センサによるランドマーク検出などによる距離情報や角度情報。 観測関数: オドメトリと測距センサがついている位置から、センサの出力の理想的な出力を計算するための関数。 観測段: 測距、カメラ、測域などによる情報を組みこむ更新。 予測段 †予測段では、状態量の速度に関する情報を更新に組みます。 時間の添え字について= †更新前の状態量: \( \mu_{t-1} \) 更新後の状態量: \( \mu_t \) 更新に使う制御量: \( u_t \) 時刻tで得られた制御量によって、時刻t-1の状態量が、時刻tの状態量に更新される、というイメージです。 式 †式は以下のとおりです。 G: \( \mu_{t-1} \)に関するヤコビ行列。\( G = \frac{\partial \mu_t}{\partial \mu_{t-1}} \) R: \( \mu_t \)の分散共分散行列。その1回の予測により、どれくらい状態量の不確かさが増加するのか、を表す。 \( \mu_t = g(\mu_{t-1}, u) \) \( \Sigma_t = G \Sigma_{t-1} G^t + R \) …(1) 例: 二輪ロボットのオドメトリ †状態量と制御量を以下のように設定します。 \( \mu_t = \begin{pmatrix} x_t \\ y_t \\ \theta_t \end{pmatrix} \) \( u_t = \begin{pmatrix} v_t \\ \omega_t \end{pmatrix} \) gを資料から読み解きます。 \( \begin{pmatrix} x_t \\ y_t \\ \theta_t \end{pmatrix} = \begin{pmatrix} x_{t-1} + v_t \Delta t \cos(\omega_t \Delta t / 2 + \theta_{t-1}) \\ y_{t-1} + v_t \Delta t \sin(\omega_t \Delta t / 2 + \theta_{t-1}) \\ \theta_{t-1} + \omega \Delta t \end{pmatrix} \) Gを、gを\( x_{t-1} \)で偏微分することで求めます。 \( G = \begin{pmatrix} 1 & 0 & - v_t \Delta t \sin(\omega_t \Delta t / 2 + \theta_{t-1}) \\ 0 & 1 & v_t \Delta t \cos(\omega_t \Delta t / 2 + \theta_{t-1}) \\ 0 & 0 & 1 \end{pmatrix} \) Rに相当する行列を求めます。 ロボットの場合、一回の予測ステップの不確かさを生みだすのは、制御量に他ならないので、制御量の分散共分散行列Mを仮定し、それをヤコビ行列によって状態量の分散共分散行列に射影します。 制御量による状態量の分散共分散行列は、状態量の制御量に関するヤコビ行列\( V = \frac{\partial \mu_t}{\partial u_t} \)を用いて、\( V M V^t \)と表されます。…(2) \( M = \begin{pmatrix} \alpha_1 v_t^2 + \alpha_2 \omega_t^2 & 0 \\ 0 & \alpha_3 v_t^2 + \alpha_4 \omega_t^2 \end{pmatrix} \) \( V = \begin{pmatrix} \Delta t cos(\omega_t \Delta t / 2 + \theta_{t-1}) & - v_t \Delta t ^ 2 / 2 sin(\omega_t \Delta t / 2 + \theta_{t-1}) \\ \Delta t sin(\omega_t \Delta t / 2 + \theta_{t-1}) & v_t \Delta t ^ 2 / 2 cos(\omega_t \Delta t / 2 + \theta_{t-1}) \\ 0 & 1 \end{pmatrix} \) \( R = V M V^t \) 以上から、予測段は以下のように立式されます。 \( \begin{pmatrix} x_t \\ y_t \\ \theta_t \end{pmatrix} = \begin{pmatrix} x_{t-1} + v_t \Delta t \cos(\omega_t \Delta t / 2 + \theta_{t-1}) \\ y_{t-1} + v_t \Delta t \sin(\omega_t \Delta t / 2 + \theta_{t-1}) \\ \theta_{t-1} + \omega_t \Delta t \end{pmatrix} \) \( \Sigma_t = G \Sigma_t G^t + V M V^t \)…(3) Tips †分散の線形射影 †(1), (2)では以下の事実を利用しています。 線形空間Xのある正規分布が、平均\( \mu_X \)分散共分散行列を\( \Sigma_X \) 線形空間Yが存在する。 XからYへの線形変換行列Aが存在する。 Xの正規分布をAによって射影した時の、Yの正規分布は、平均\( \mu_Y \)分散共分散行列を\( \Sigma_Y \)とする。 この時、 \( \mu_Y = A \mu_X \) \( \Sigma_Y = A \Sigma_X A^t \) である。 分散の和 †(3)は以下のような事実から本当は明らかです。 関数gが射影しうる分散は、(x, y, theta, v, omega)から来るはずです。 「Tips/分散の線形射影」を用いれば、本当ならばAは5*5の行列になるはずです。 これは「ひとつ前の状態量の分散共分散行列と、今の制御量の分散共分散行列は独立である。」という仮定により分解されています。 つまり、状態の分散共分散と、状態の速度の分散共分散に関係があるわけがない、という信念により、カルマンフィルタのもとの式でもRは別扱いされているのです。 予測段は簡単 †予測段は僕でも導出できるくらいに簡単なものです。 カルマンフィルタの真骨頂は観測段にあります。 観測段 †観測段では、「状態量にとって意味のある観測」を更新に組みます。 状態量にとって意味のある観測= †まず、「状態量にとって意味のある観測」とは何か、ということについて書きます。 「状態量にとって意味のある観測」とは、「状態量とモデルから、理想的な観測量が計算できる」という意味です。…(4) 例えば、 四角い枠に囲まれたフィールドにロボットがいます。 ある壁に対して、測距センサで距離を測ったとします。 この場合、ロボットのオドメトリと測距センサが付いている位置によって、当然理想的な観測量は計算可能できるはずです。…(5) 時間の添え字について †更新前の状態量: \( \mu_{t-1} \) 更新後の状態量: \( \mu_t \) 更新に使う観測量: \( z_t \) 時刻tで得られた観測量によって、時刻t-1の状態量が、時刻tの状態量に更新される、というイメージです。 式= †以上のことを踏まえて、式を記述します。 H: hの\( \mu_{t-1} \)に関する偏微分。\( \frac{\partial h}{\partial \mu_{t-1}} \) Q: \( z_t - h(\mu_{t-1}) \)の分散共分散行列 \( K=\Sigma_{t-1} H^t (H \Sigma_{t-1} H^t + Q)^{-1} \) \( \mu_t = \mu_{t-1} + K (z - h(\mu_{t-1})) \) \( \Sigma_t = (I - K H) \Sigma_{t-1} \) Kは一般に「カルマンゲイン」と呼ばれています。 式を見れば、これは「状態量からの理想的な観測量と、実際の観測量の差分」を反映させた、フィードバックゲインとなっていることがわかります。 例: 点ランドマークベースの更新= †状態量と実観測量と理想観測量を以下のように設定します。 \( \mu = \begin{pmatrix} x \\ y \\ \theta \end{pmatrix} \) $ m_x, m_y$: フィールド座標での点ランドマークの実際のx, y。 \( z = \begin{pmatrix} r \\ \phi \end{pmatrix} \): 観測された、マシン中心から見た点ランドマークとの距離と角度。 \( \hat{z} = h(\mu_{t-1}) \): 状態量から算出された理想的な、マシン中心から見た点ランドマークとの距離と角度。 hを立式します。 \( q = (m_x - x_{t-1})^2 + (m_y - y_{t-1})^2 \) \( \hat{z} = h(\mu_{t-1}) = \begin{pmatrix} \sqrt{q} \\ norm(atan2(m_y - y_{t-1}, m_x - x_{t-1}) - \theta_{t-1}) \end{pmatrix} \) ただしnormは、角度を[-pi, pi]に丸める関数。 Hをhを\( \mu_{t-1} \)で偏微分して求めます。 \( H = \frac{\partial h}{\partial \mu_{t-1}} = \begin{pmatrix} -(m_x - x_{t-1}) / \sqrt{q} & -(m_y - y_{t-1}) / \sqrt{q} & 0 \\ (m_y - y_{t-1}) / q & -(m_x - x_{t-1}) / q & 1 \end{pmatrix} \) Qを設定します。\( r, \phi \)は互いに独立だと仮定して、ランドマークの観測と理想位置とのズレの分散共分散行列は、 \( Q = \begin{pmatrix} Q_r & 0 \\ 0 & Q_\theta \end{pmatrix} \) 以上より、観測段の更新式は、 \( K=\Sigma_{t-1} H^t (H \Sigma_{t-1} H^t + Q)^{-1} \) \( \mu_t = \mu_{t-1} + K (z - h(\mu_{t-1})) \) \( \Sigma_t = (I - K H) \Sigma_{t-1} \) Tips †状態量にとって意味のある観測の数学的な記述 †(4)について、直感的な説明だけにとどめたかったので厳密さに欠くことを書きましたが、実際には、観測量=f(状態量, モデル)、ただし状態量について一階偏微分可能を良く近似するfが存在することが条件となります。 状態量にとって意味のない観測の例 †(5)について、意味のない観測の例は下らないのでTipsに。 四角い枠に囲まれたフィールドにロボットがいます。 この時、工学部二号館8階の自動ドアの測距センサが、床もしくは通行人との距離を測ったとします。 この場合、ロボットのオドメトリと自動ドアの測距センサが付いている位置によって、当然自動ドアの理想的な計算量を推定することはできません。 点ランドマークが同時に複数見つかった場合 †上記の場合では、点ランドマークが一つしか見つかっていない状況しか想定していません。 もしLRFなどで複数のランドマークが同時に見つかった場合は、「それぞれ別々に」上記の更新を行えばよいです。 つまり、3つランドマークが見つかったのならば、3回カルマンゲインを更新して、3回更新をかけることになります。 Tips †ヤコビ行列= †ベクトル関数のベクトルによる偏微分係数をヤコビ行列と呼びます。 ベクトル関数のベクトルによる偏微分係数の定義を知らない人のために、以下に定義を書きます。 x: n次元実数ベクトル変数 f: n次元実数ベクトルからm次元実数ベクトルへのベクトル関数 \( \frac{\partial f}{\partial x} = \begin{pmatrix} \frac{\partial f_1}{\partial x_1} & \cdots & \frac{\partial f_1}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial f_m}{\partial x_1} & \cdots & \frac{\partial f_m}{\partial x_n} \end{pmatrix} \) 観測偏差の分散共分散行列のチューニング方法 †基本 †
方法 †
初期状態の分散共分散行列 †基本 †
方法 †
初期状態を大きくしたら暴走する場合 †
|
