カルマンフィルター

出典: フリー百科事典『ウィキペディア（Wikipedia）』

カルマンフィルターは、誤差のある観測値を用いて、ある動的システムの状態を推定あるいは制御するための、無限インパルス応答フィルターの一種である。

[編集] 実用例

カルマンフィルターは、離散的な誤差のある観測から、時々刻々と時間変化する量（例えばある物体の位置と速度）を推定するために用いられる。レーダーやコンピュータビジョンなど、工学分野で広く用いられる。例えば、カーナビゲーションでは、機器内蔵の加速度計や人工衛星からの誤差のある情報を統合して、時々刻々変化する自動車の位置を推定するのに応用されている。カルマンフィルターは、目標物の時間変化を支配する法則を活用して、目標物の位置を現在（フィルター）、未来（予測）、過去（内挿あるいは平滑化）に推定することができる。

[編集] 歴史

このフィルターはルドルフ・カルマンによって提唱されたが、同様の原理はトルバルド・ティエレとピーター・スワーリングによってすでに開発されていた^[1]。カルマンがアメリカ航空宇宙局のエイムズ研究センターを訪問した際、この理論がロケットの軌道推定に有用なことに気づき、のちのアポロ計画で用いられた。

[編集] 用いられる動的システム

カルマンフィルターは、時間が離散化された線形動的システムを用いる。すなわち、正規分布ノイズ項を含む一次マルコフ過程線形演算子のモデルである。状態空間は、実数のベクトルで、表される。時間ステップが進むたびに、行列で表される線形演算子によって、状態空間が更新される。必要なら、ここに系を制御する変数を加えることもできる。その際、ノイズの影響が加味される。この状態空間は、観測できないが、別の線形演算子とノイズの組によって、観測量と関連づけられている。

以上を式で表す。時刻 k における真の系の状態は、一ステップ前の時刻 (k − 1) の状態をもとに、次のように決定される。

$\textbf{x}_{k} = \textbf{F}_{k} \textbf{x}_{k-1} + \textbf{B}_{k}\textbf{u}_{k} + \textbf{w}_{k}$

ここに、

F_k は、系の時間変化のモデル。
B_k は、制御変数 u_k に対する演算子。
w_k は、時間変化に関するノイズで、共分散行列 Q_k かつ零平均の多変数正規分布に従う。

$\textbf{w}_{k} \sim N(0, \textbf{Q}_k)$

ある時刻 k において、観測量（測定量）z_k は、真の（すなわち観測不可能な）物理量 x_k と、以下のような関係にある。

$\textbf{z}_{k} = \textbf{H}_{k} \textbf{x}_{k} + \textbf{v}_{k}$

ここに、H_k は、観測モデルで、観測誤差 v_k は、共分散行列 R_k かつ零平均の多変数正規分布に従う。

$\textbf{v}_{k} \sim N(0, \textbf{R}_k)$

系の初期条件とノイズ {x₀, w₁, ..., w_k, v₁,..., v_k} は、互いに独立であるとする。

このモデルが厳密に成り立たなくても、ノイズ項があるため近似的に適用できる場合が多い。以下に述べる拡張されたカルマンフィルターはさらに応用範囲が広い。

[編集] カルマンフィルター

カルマンフィルターは無限インパルス応答フィルターであり、系の今の観測量とひとつ前の推定量さえ分かれば、今の推定量が得られる。

系の今の（時間ステップ k ）の状態は、二つの変数で表される。

$\hat{\textbf{x}}_{k|k}$ 系の状態の推定値。
$\textbf{P}_{k|k}$ 誤差の共分散行列（推定値の精度）。

カルマンフィルターは、時間ステップをひとつ進めるために予測と更新の二つの手続きを行う。予測の手続きでは、前の時刻の推定状態から、今の時刻の推定状態を計算する。更新では、今の時刻の観測を用いて、推定値を補正してより正確な状態を推定する。

[編集] 予測

$\hat{\textbf{x}}_{k|k-1} = \textbf{F}_{k}\hat{\textbf{x}}_{k-1|k-1} + \textbf{B}_{k} \textbf{u}_{k}$ （今の時刻の推定値）

$\textbf{P}_{k|k-1} = \textbf{F}_{k} \textbf{P}_{k-1|k-1} \textbf{F}_{k}^{T} + \textbf{Q}_{k}$ （今の時刻の誤差行列）

[編集] 更新

$\tilde{\textbf{y}}_{k} = \textbf{z}_{k} - \textbf{H}_{k}\hat{\textbf{x}}_{k|k-1}$ （観測残差、innovation）

$\textbf{S}_{k} = \textbf{H}_{k}\textbf{P}_{k|k-1} \textbf{H}_{k}^{T}+\textbf{R}_{k}$ （残差（あるいは innovation）の共分散）

$\textbf{K}_{k} = \textbf{P}_{k|k-1}\textbf{H}_{k}^{T}\textbf{S}_{k}^{-1}$ （最適カルマンゲイン）

$\hat{\textbf{x}}_{k|k} = \hat{\textbf{x}}_{k|k-1} + \textbf{K}_{k}\tilde{\textbf{y}}_{k}$ （更新された状態の推定値）

$\textbf{P}_{k|k} = (I - \textbf{K}_{k} \textbf{H}_{k}) \textbf{P}_{k|k-1}$ （更新された誤差の共分散）

[編集] 不変量

もし、モデルが正確で初期条件 $\hat{\textbf{x}}_{0|0}$ と $\textbf{P}_{0|0}$ が正確ならば、全ての推定量は期待値零である。

$\textrm{E}[\textbf{x}_k - \hat{\textbf{x}}_{k|k}] = \textrm{E}[\textbf{x}_k - \hat{\textbf{x}}_{k|k-1}] = 0$
$\textrm{E}[\tilde{\textbf{y}}_k] = 0$

ここに、 $E[]$ は、期待値。また、共分散は、推定値の誤差共分散である。

$\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_k - \hat{\textbf{x}}_{k|k})$
$\textbf{P}_{k|k-1} = \textrm{cov}(\textbf{x}_k - \hat{\textbf{x}}_{k|k-1})$
$\textbf{S}_{k} = \textrm{cov}(\tilde{\textbf{y}}_k)$

[編集] 設定例

まっすぐで無限の長さを持つ摩擦の無いレールの上に乗っているトロッコを考えよう。初期条件は、トロッコは位置 0 に静止している。トロッコにはランダムな力（加速度）が与えられる。Δt 秒ごとにトロッコの位置 x を観測する。ただしこの観測には誤差が混入している。トロッコの位置と速度のモデルを考えると、以下の様に設定すると、カルマンフィルターが用い得る。

制御の必要はないから、B_k と u_k は考えない。行列 F、 H、 R、 Q は時間変化しないので添字は付けない。

トロッコの場所と速度は、

$\textbf{x}_{k} = \begin{bmatrix} x \\ \dot{x} \end{bmatrix}$

で、表される。 $\dot{x}$ は位置の時間微分、すなわち速度である。

時刻 k − 1 と時刻 k の間に加速度 a_k がトロッコに与えられる。加速度は平均 0 標準偏差 σ_a の正規分布をしている。運動の第2法則により、

$\textbf{x}_{k} = \textbf{F} \textbf{x}_{k-1} + \textbf{G}a_{k}$

ここに、

$\textbf{F} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}$

かつ

$\textbf{G} = \begin{bmatrix} \frac{\Delta t^{2}}{2} \\ \Delta t \end{bmatrix}$

である。最右辺の共分散は、

$\textbf{Q} = \textrm{cov}(\textbf{G}a) = \textrm{E}[(\textbf{G}a)(\textbf{G}a)^{T}] = \textbf{G} \textrm{E}[a^2] \textbf{G}^{T} = \textbf{G}[\sigma_a^2]\textbf{G}^{T} = \sigma_a^2 \textbf{G}\textbf{G}^{T}$ （σ_a は、スカラーであることを用いた。）

それぞれの時刻に、トロッコの位置を観測する。観測誤差も平均 0 で標準偏差 σ_z の正規分布と仮定する。

$\textbf{z}_{k} = \textbf{H x}_{k} + \textbf{v}_{k}$

ここに、

$\textbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix}$

かつ

$\textbf{R} = \textrm{E}[\textbf{v}_k \textbf{v}_k^{T}] = \begin{bmatrix} \sigma_z^2 \end{bmatrix}$

である。

初期条件は

$\hat{\textbf{x}}_{0|0} = \begin{bmatrix} 0 \\ 0 \end{bmatrix}$

と、正確に

$\textbf{P}_{0|0} = \begin{bmatrix} 0 & 0 \\ 0 & 0 \end{bmatrix}$

分かっている。もし、初期条件に誤差があるならば、

$\textbf{P}_{0|0} = \begin{bmatrix} B & 0 \\ 0 & B \end{bmatrix}$

と、取るべきである。もし B が大きければカルマンフィルターは、初期条件より、それ以降の観測に重みを置くようになる。

[編集] 導出

[編集] 更新後の共分散行列

時間を進めるための予測と更新の手続きのうち、更新が終わったあとの共分散行列 P_k|k をまず求める。上の定義式、

$\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_{k} - \hat{\textbf{x}}_{k|k})$

に、推定値 $\hat{\textbf{x}}_{k|k}$ の定義を代入。

$\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_{k} - (\hat{\textbf{x}}_{k|k-1} + \textbf{K}_k\tilde{\textbf{y}}_{k}))$

続いて、観測残差 $\tilde{\textbf{y}}_k$ を代入。

$\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_{k} - (\hat{\textbf{x}}_{k|k-1} + \textbf{K}_k(\textbf{z}_k - \textbf{H}_k\hat{\textbf{x}}_{k|k-1})))$

そして、観測値 $\textbf{z}_{k}$ と真の値の関係を代入。

$\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_{k} - (\hat{\textbf{x}}_{k|k-1} + \textbf{K}_k(\textbf{H}_k\textbf{x}_k + \textbf{v}_k - \textbf{H}_k\hat{\textbf{x}}_{k|k-1})))$

変形して、

$\textbf{P}_{k|k} = \textrm{cov}((I - \textbf{K}_k \textbf{H}_{k})(\textbf{x}_k - \hat{\textbf{x}}_{k|k-1}) - \textbf{K}_k \textbf{v}_k )$

観測誤差 v_k は、他の項と相関がないから、

$\textbf{P}_{k|k} = \textrm{cov}((I - \textbf{K}_k \textbf{H}_{k})(\textbf{x}_k - \hat{\textbf{x}}_{k|k-1})) + \textrm{cov}(\textbf{K}_k \textbf{v}_k )$

となり、さらに変形

$\textbf{P}_{k|k} = (I - \textbf{K}_k \textbf{H}_{k})\textrm{cov}(\textbf{x}_k - \hat{\textbf{x}}_{k|k-1})(I - \textbf{K}_k \textbf{H}_{k})^{T} + \textbf{K}_k\textrm{cov}(\textbf{v}_k )\textbf{K}_k^{T}$

して、前述の不変量 P_k|k-1 と、観測誤差共分散 R_k を用いて、

$\textbf{P}_{k|k} = (I - \textbf{K}_k \textbf{H}_{k}) \textbf{P}_{k|k-1} (I - \textbf{K}_k \textbf{H}_{k})^T + \textbf{K}_k \textbf{R}_k \textbf{K}_k^T$

を得る。この式は、 K_k がどんな値であっても成立するが、 K_k が、最適カルマンゲインの時は、以下のようにさらに簡略化される。

[編集] カルマンゲインの導出

カルマンフィルターは、最小自乗誤差推定値である。すなわち、更新後の誤差の推定値は

$\textbf{x}_{k} - \hat{\textbf{x}}_{k|k}$

であり、このベクトルの大きさの自乗の期待値 $\textrm{E}[|\textbf{x}_{k} - \hat{\textbf{x}}_{k|k}|^2]$ を最小にするような推定値を与える。これは、更新後の共分散 P_k|k のトレースを最小とすることと同じである。上の式を展開して、

$\textbf{P}_{k\|k}$	$= \textbf{P}_{k\|k-1} - \textbf{K}_k \textbf{H}_k \textbf{P}_{k\|k-1} - \textbf{P}_{k\|k-1} \textbf{H}_k^T \textbf{K}_k^T + \textbf{K}_k (\textbf{H}_k \textbf{P}_{k\|k-1} \textbf{H}_k^T + \textbf{R}_k) \textbf{K}_k^T$
	$= \textbf{P}_{k\|k-1} - \textbf{K}_k \textbf{H}_k \textbf{P}_{k\|k-1} - \textbf{P}_{k\|k-1} \textbf{H}_k^T \textbf{K}_k^T + \textbf{K}_k \textbf{S}_k\textbf{K}_k^T$

トレースを最小にすべく、行列微分をゼロと置いて、

$\frac{\partial \; \textrm{tr}(\textbf{P}_{k|k})}{\partial \;\textbf{K}_k} = -2 (\textbf{H}_k \textbf{P}_{k|k-1})^T + 2 \textbf{K}_k \textbf{S}_k = 0$

ここからカルマンゲイン K_k を求める。

$\textbf{K}_k \textbf{S}_k = (\textbf{H}_k \textbf{P}_{k|k-1})^T = \textbf{P}_{k|k-1} \textbf{H}_k^T$

$\textbf{K}_{k} = \textbf{P}_{k|k-1} \textbf{H}_k^T \textbf{S}_k^{-1}$

[編集] 更新後の誤差共分散行列

カルマンゲインが上述の値を取るとき、更新後の誤差共分散行列は以下のように簡単になる。カルマンゲインの式の両辺の右から S_kK_k^T をかけて、

$\textbf{K}_k \textbf{S}_k \textbf{K}_k^T = \textbf{P}_{k|k-1} \textbf{H}_k^T \textbf{K}_k^T$

更新後の誤差共分散行列を展開して、

$\textbf{P}_{k|k} = \textbf{P}_{k|k-1} - \textbf{K}_k \textbf{H}_k \textbf{P}_{k|k-1} - \textbf{P}_{k|k-1} \textbf{H}_k^T \textbf{K}_k^T + \textbf{K}_k \textbf{S}_k \textbf{K}_k^T$

右の二項は相殺するから、

$\textbf{P}_{k|k} = \textbf{P}_{k|k-1} - \textbf{K}_k \textbf{H}_k \textbf{P}_{k|k-1} = (I - \textbf{K}_{k} \textbf{H}_{k}) \textbf{P}_{k|k-1}$ .

計算量が少ないため、ほとんどの場合この式が用いられるが、カルマンゲインが上記の最適解の時にしか適用できないことに注意。計算上の桁落ちなどで解の安定性が悪いときやなんらかの理由で敢えて最適でない解を用いるときは使えない。

[編集] 再帰ベイズ推定との関係

観測不可能な真の状態がマルコフ過程のとき、観測されたものは隠れマルコフ過程で扱われる。真の状態は一次のマルコフ過程であり、ひとつ前の時刻の状態にのみ依存して、それより前の状態とは独立である。

$p(\textbf{x}_k|\textbf{x}_0,\dots,\textbf{x}_{k-1}) = p(\textbf{x}_k|\textbf{x}_{k-1})$

同様に、時刻 k での観測値は現在の状態にだけ依存して、過去には依存しないものとする。

$p(\textbf{z}_k|\textbf{x}_0,\dots,\textbf{x}_{k}) = p(\textbf{z}_k|\textbf{x}_{k} )$

これらの仮定を用いると、隠れマルコフモデルの観測が z₁, z₂, $\ldots$ z_k と得られる確率は、

$p(\textbf{x}_0,\dots,\textbf{x}_k,\textbf{z}_1,\dots,\textbf{z}_k) = p(\textbf{x}_0)\prod_{i=1}^k p(\textbf{z}_i|\textbf{x}_i)p(\textbf{x}_i|\textbf{x}_{i-1})$

で、表される。

一方、カルマンフィルターで状態 x を求めるには現在の系の状態とそれまでの観測だけを用いる。

カルマンフィルターの予測と更新の手続きを、確率を使って表してみる。予測後の状態の確率分布は、時刻 k − 1 から時刻 k への変化に関する確率と、時刻 (k − 1) の状態の積になるから、

$p(\textbf{x}_k|\textbf{Z}_{k-1}) = \int p(\textbf{x}_k | \textbf{x}_{k-1}) p(\textbf{x}_{k-1} | \textbf{Z}_{k-1} ) \, d\textbf{x}_{k-1}$

時刻 t までの観測は

$\textbf{Z}_{t} = \left \{ \textbf{z}_{1},\dots,\textbf{z}_{t} \right \}$

である。

更新後の確率は観測の起こりやすさ（尤度）と予測された状態の積に比例するから

$p(\textbf{x}_k|\textbf{Z}_{k}) = \frac{p(\textbf{z}_k|\textbf{x}_k) p(\textbf{x}_k|\textbf{Z}_{k-1})}{p(\textbf{z}_k|\textbf{Z}_{k-1})}$

となる。分母の

$p(\textbf{z}_k|\textbf{Z}_{k-1}) = \int p(\textbf{z}_k|\textbf{x}_k) p(\textbf{x}_k|\textbf{Z}_{k-1}) d\textbf{x}_k$

は、全確率を 1 にするための因子であまり重要ではない。

他の確率分布関数も

$p(\textbf{x}_k | \textbf{x}_{k-1}) = N(\textbf{F}_k\textbf{x}_{k-1}, \textbf{Q}_k)$

$p(\textbf{z}_k|\textbf{x}_k) = N(\textbf{H}_{k}\textbf{x}_k, \textbf{R}_k)$

$p(\textbf{x}_{k-1}|\textbf{Z}_{k-1}) = N(\hat{\textbf{x}}_{k-1},\textbf{P}_{k-1} )$

と書ける。

[編集] 非線型カルマンフィルター

ここまでは線形の仮定が成り立つ系をとりあつかってきたが、実際の系の多くは非線型である。時間発展モデルも観測モデルもどちらも非線型になりうる。

[編集] 拡張カルマンフィルター

ここでは時間発展モデル

$\textbf{x}_{k} = f(\textbf{x}_{k-1}, \textbf{u}_{k}, \textbf{w}_{k})$

と、観測モデル

$\textbf{z}_{k} = h(\textbf{x}_{k}, \textbf{v}_{k})$

を考える。どちらも微分可能であれば線形である必要はない。関数 f は前の状態から推定値を与え、関数 h は観測値を与える。どちらの関数も直接共分散を求めることはできず、偏微分行列（ヤコビアン）を用いる必要がある。

原理としては、非線型モデルを現在の推定値の回りで線形化する。そのためにそれぞれの時刻で、ヤコビアンを計算する。すなわち、

予測

$\hat{\textbf{x}}_{k|k-1} = f(\hat{\textbf{x}}_{k-1|k-1}, \textbf{u}_{k}, 0)$

$\textbf{P}_{k|k-1} = \textbf{F}_{k} \textbf{P}_{k-1|k-1} \textbf{F}_{k}^{T} + \textbf{Q}_{k}$

更新

$\tilde{\textbf{y}}_{k} = \textbf{z}_{k} - h(\hat{\textbf{x}}_{k|k-1}, 0)$

$\textbf{S}_{k} = \textbf{H}_{k}\textbf{P}_{k|k-1}\textbf{H}_{k}^{T} + \textbf{R}_{k}$

$\textbf{K}_{k} = \textbf{P}_{k|k-1}\textbf{H}_{k}^{T}\textbf{S}_{k}^{-1}$

$\hat{\textbf{x}}_{k|k} = \hat{\textbf{x}}_{k|k-1} + \textbf{K}_{k}\tilde{\textbf{y}}_{k}$

$\textbf{P}_{k|k} = (I - \textbf{K}_{k} \textbf{H}_{k}) \textbf{P}_{k|k-1}$

出てくる行列は次のヤコビアンで定義される。

$\textbf{F}_{k} = \left . \frac{\partial f}{\partial \textbf{x} } \right \vert _{\hat{\textbf{x}}_{k-1|k-1},\textbf{u}_{k}}$

$\textbf{H}_{k} = \left . \frac{\partial h}{\partial \textbf{x} } \right \vert _{\hat{\textbf{x}}_{k|k-1}}$

[編集] Unscented カルマンフィルター

非線型性の強いとき、拡張カルマンフィルターの性能は悪い。理由は平均値だけが非線型性に反映されるからである。unscented カルマンフィルターは、シグマ点とよばれる代表点を平均値の回りで用いて、推定値の共分散を計算する。こうすることにより、真の平均と共分散により近い値が得られることがモンテカルロ法や、テイラー展開によって示される。しかも解析的にヤコビアンを計算する必要がなくなるという利点がある。これは複雑なモデルでは有利である。

予測

拡張カルマンフィルタと同様、 unscented カルマンフィルターの予測手続きは更新手続きと別であり、更新手続きに線形カルマンフィルターや拡張カルマンフィルターを用いたり、その逆を行うことも可能である。推定値と共分散には、予測ノイズの平均と共分散項が加えられる。

$\textbf{x}_{k-1|k-1}^{a} = [ \hat{\textbf{x}}_{k-1|k-1}^{T} \quad E[\textbf{w}_{k}^{T}] \ ]^{T}$

$\textbf{P}_{k-1|k-1}^{a} = \begin{bmatrix} & \textbf{P}_{k-1|k-1} & & 0 & \\ & 0 & &\textbf{Q}_{k} & \end{bmatrix}$

シグマ点 2L+1 個は、付け加えた項から計算される。ここに L は付け加えた状態項の次元である。

$\chi_{k-1\|k-1}^{0}$	$= \textbf{x}_{k-1\|k-1}^{a}$
$\chi_{k-1\|k-1}^{i}$	$=\textbf{x}_{k-1\|k-1}^{a} + \left ( \sqrt{ (L + \lambda) \textbf{P}_{k-1\|k-1}^{a} } \right )_{i}$	$i = 1,\ldots L \,\!$
$\chi_{k-1\|k-1}^{i}$	$= \textbf{x}_{k-1\|k-1}^{a} - \left ( \sqrt{ (L + \lambda) \textbf{P}_{k-1\|k-1}^{a} } \right )_{i-L}$	$i = L+1,\dots{}2L \,\!$

シグマ点は関数 f で時間発展する。

$\chi_{k|k-1}^{i} = f(\chi_{k-1|k-1}^{i}) \quad i = 0..2L$

予測値と共分散は重み付き平均で求められる。

$\hat{\textbf{x}}_{k|k-1} = \sum_{i=0}^{2L} W_{s}^{i} \chi_{k|k-1}^{i}$

$\textbf{P}_{k|k-1} = \sum_{i=0}^{2L} W_{c}^{i}\ [\chi_{k|k-1}^{i} - \hat{\textbf{x}}_{k|k-1}] [\chi_{k|k-1}^{i} - \hat{\textbf{x}}_{k|k-1}]^{T}$

重みは以下のように与えられる。

$W_{s}^{0} = \frac{\lambda}{L+\lambda}$

$W_{c}^{0} = \frac{\lambda}{L+\lambda} + (1 - \alpha^2 + \beta)$

$W_{s}^{i} = W_{c}^{i} = \frac{1}{2(L+\lambda)}$

$\lambda = \alpha^2 (L+\kappa) - L \,\!$

α = 10^-3、β = 2 、κ = 0 といった値がよく用いられる。

更新

予測値と共分散には、上と同様に観測値のノイズの平均と共分散項が加えられる。

$\textbf{x}_{k|k-1}^{a} = [ \hat{\textbf{x}}_{k|k-1}^{T} \quad E[\textbf{v}_{k}^{T}] \ ]^{T}$

$\textbf{P}_{k|k-1}^{a} = \begin{bmatrix} & \textbf{P}_{k|k-1} & & 0 & \\ & 0 & &\textbf{R}_{k} & \end{bmatrix}$

シグマ点 2L+1 個は、付け加えた項から計算される。ここに L は付け加えた状態項の次元である。

$\chi_{k\|k-1}^{0}$	$= \textbf{x}_{k\|k-1}^{a}$
$\chi_{k\|k-1}^{i}$	$=\textbf{x}_{k\|k-1}^{a} + \left ( \sqrt{ (L + \lambda) \textbf{P}_{k\|k-1}^{a} } \right )_{i}$	$i = 1..L \,\!$
$\chi_{k\|k-1}^{i}$	$= \textbf{x}_{k\|k-1}^{a} - \left ( \sqrt{ (L + \lambda) \textbf{P}_{k\|k-1}^{a} } \right )_{i-L}$	$i = L+1,\dots{}2L \,\!$

もし、予測手続きも unscented カルマンフィルターで行われていたならば、以下のような変形も可能である。

$\chi_{k|k-1} := [ \chi_{k|k-1} \quad E[\textbf{v}_{k}^{T}] \ ]^{T} \pm \sqrt{ (L + \lambda) \textbf{R}_{k}^{a} }$

ここに、

$\textbf{R}_{k}^{a} = \begin{bmatrix} & 0 & & 0 & \\ & 0 & &\textbf{R}_{k} & \end{bmatrix}$

である。シグマ点は関数 h で観測値に変換される。

$\gamma_{k}^{i} = h(\chi_{k|k-1}^{i}) \quad i = 0..2L$

重み付き平均で、観測値とその共分散を推定する。

$\hat{\textbf{z}}_{k} = \sum_{i=0}^{2L} W_{s}^{i} \gamma_{k}^{i}$

$\textbf{P}_{z_{k}z_{k}} = \sum_{i=0}^{2L} W_{c}^{i}\ [\gamma_{k}^{i} - \hat{\textbf{z}}_{k}] [\gamma_{k}^{i} - \hat{\textbf{z}}_{k}]^{T}$

推定値と観測値の相関行列

$\textbf{P}_{x_{k}z_{k}} = \sum_{i=0}^{2L} W_{c}^{i}\ [\chi_{k|k-1}^{i} - \hat{\textbf{x}}_{k|k-1}] [\gamma_{k}^{i} - \hat{\textbf{z}}_{k}]^{T}$

を用いて unscented カルマンゲイン

$K_{k} = \textbf{P}_{x_{k}z_{k}} \textbf{P}_{z_{k}z_{k}}^{-1}$

を計算する。以下は線形の場合と同様である。

$\hat{\textbf{x}}_{k|k} = \hat{\textbf{x}}_{k|k-1} + K_{k}( \textbf{z}_{k} - \hat{\textbf{z}}_{k} )$

$\textbf{P}_{k|k} = \textbf{P}_{k|k-1} - K_{k} \textbf{P}_{z_{k}z_{k}} K_{k}^{T}$

[編集] 応用例

"http://ja.wikipedia.org../../../%E3%82%AB/%E3%83%AB/%E3%83%9E/%E3%82%AB%E3%83%AB%E3%83%9E%E3%83%B3%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%E3%83%BC.html" より作成

カテゴリ: 制御理論 | 信号処理

$\chi_{k-1\|k-1}^{0}$	$= \textbf{x}_{k-1\|k-1}^{a}$
$\chi_{k-1\|k-1}^{i}$	$=\textbf{x}_{k-1\|k-1}^{a} + \left ( \sqrt{ (L + \lambda) \textbf{P}_{k-1\|k-1}^{a} } \right )_{i}$	$i = 1,\ldots L \,\!$
$\chi_{k-1\|k-1}^{i}$	$= \textbf{x}_{k-1\|k-1}^{a} - \left ( \sqrt{ (L + \lambda) \textbf{P}_{k-1\|k-1}^{a} } \right )_{i-L}$	$i = L+1,\dots{}2L \,\!$

$\chi_{k\|k-1}^{0}$	$= \textbf{x}_{k\|k-1}^{a}$
$\chi_{k\|k-1}^{i}$	$=\textbf{x}_{k\|k-1}^{a} + \left ( \sqrt{ (L + \lambda) \textbf{P}_{k\|k-1}^{a} } \right )_{i}$	$i = 1..L \,\!$
$\chi_{k\|k-1}^{i}$	$= \textbf{x}_{k\|k-1}^{a} - \left ( \sqrt{ (L + \lambda) \textbf{P}_{k\|k-1}^{a} } \right )_{i-L}$	$i = L+1,\dots{}2L \,\!$

カルマンフィルター

出典: フリー百科事典『ウィキペディア（Wikipedia）』

目次

[編集] 実用例

[編集] 歴史

[編集] 用いられる動的システム

[編集] カルマンフィルター

[編集] 予測

[編集] 更新

[編集] 不変量

[編集] 設定例

[編集] 導出

[編集] 更新後の共分散行列

[編集] カルマンゲインの導出

[編集] 更新後の誤差共分散行列

[編集] 再帰ベイズ推定との関係

[編集] 非線型カルマンフィルター

[編集] 拡張カルマンフィルター

[編集] Unscented カルマンフィルター

[編集] 応用例

Views

ナビゲーション

ヘルプ

検索

他の言語