カルマンフィルター

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

実用例

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

歴史

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

用いられる動的システム

カルマンフィルターは時間領域において、連続時間線形動的システム、もしくは離散化された離散時間線型動的システムに基づいて駆動する。以降に導入される解説は、後者の立場のものである。それらはガウス白色雑音によって励振をうける線形演算子からなるマルコフ連鎖モデルにより表現される。より端的にいえば、システムは状態空間モデル (state space model) で表現されるということである。

対象のシステムに定義された「状態 (state)」は、そのシステムの過去の動特性の遷移を保持する役割を果たし、動特性の遷移を保持する線形空間が状態空間として定義される。この空間は実数空間であるため、システムの状態は一般に、任意の次元の状態空間に含まれる実数ベクトルとして与えられる。状態の変化は、現在の状態と、それに付加する雑音の影響と、場合によってはシステムの状態の制御に関与する既知の制御入力の線形結合によって記述される。したがって、状態はシステムの因果性に寄与する存在である。上記の理念は、以下に記述する状態方程式 (state equation) によって表現される。状態が直接観測できない場合には、システムの出力は一般に状態と観測雑音の線形結合として観測が可能なものとして与えられる。この理念は観測方程式 (observation equation) として、以下に示すような線形モデルで表現される。カルマンフィルターは、システムの状態が直接観測できない問題に対する状態推定法であるから、一般的に観測方程式を伴う問題に適用される。

カルマンフィルターは隠れマルコフモデル (hidden Markov model) の類似であると考えることができる。両者の主な差異は隠れマルコフモデルにおける状態変数が、連続であるか離散であるかである。また、隠れマルコフモデルでは状態変数の未来への変化を任意の分布に従う形式で統計的に与えることができる一方で、カルマンフィルターでは、ガウス分布に従う雑音によって未来の状態変数が統計的に記述される点が異なっている。したがって、カルマンフィルターと隠れマルコフモデルの間には強固な双対性が存在する。ちなみに、カルマンフィルターの導出過程においては、「システムに付随する雑音の性質はガウス分布に従う」という仮定の下に行われるのが一般的であるが、雑音の性質がガウス分布に従わない場合であっても、カルマンフィルターは線形なクラスにおける最適な推定値、すなわち線形最小分散推定値を導くことができる点で、汎用性に富んでいるといえる。

観測が可能な唯一の、雑音の影響を受けた出力過程に基づいて（制御問題であれば、入力も観測可能な過程となる）、システムの状態をカルマンフィルターを用いて推定するためには、対象とするシステムに対して、カルマンフィルターの理念と合致するような状態の遷移（すなわち状態過程）に関するモデルを与えなければならない。これは、時変 (time-variant) な行列 $F_{k}$ , $G_{k}$ , $H_{k}$ , $Q_{k}$ , $R_{k}$ によって特徴付けられる線形方程式として、以下で与えられる。これが状態方程式である。

時刻 $k$ における真のシステムの状態は、１ステップ前の時刻 $(k-1)$ の状態をもとに、次のように表現される^[2]。

${\boldsymbol {x}}_{k}=F_{k}{\boldsymbol {x}}_{k-1}+{\boldsymbol {u}}_{k}+G_{k}{\boldsymbol {w}}_{k}$

ここで、

$F_{k}$ は、システムの時間遷移に関する線形モデル。
${\boldsymbol {u}}_{k}$ は制御入力。
$G_{k}$ は時間遷移に関する雑音 (process noise) モデルの行列で、 ${\boldsymbol {w}}_{k}$ はその雑音で、共分散行列 $Q_{k}$ かつ零平均の多変数正規分布に従う。

${\boldsymbol {w}}_{k}\sim N(0,Q_{k})$

これがシステムの状態の遷移を記述する状態方程式である。

ある時刻 $k$ において、観測量（測定量） ${\boldsymbol {z}}_{k}$ は、真の（すなわち観測不可能な）状態 ${\boldsymbol {x}}_{k}$ と、以下のような関係にある。

${\boldsymbol {z}}_{k}=H_{k}{\boldsymbol {x}}_{k}+{\boldsymbol {v}}_{k}$

ここで、 $H_{k}$ は状態空間を観測空間に線形写像する役割を担う観測モデルであり、 ${\boldsymbol {v}}_{k}$ は、その共分散行列が $R_{k}$ で平均が零の多変数正規（ガウス）分布に従う雑音である（観測雑音 (observation noise) ）。これが観測方程式である。

${\boldsymbol {v}}_{k}\sim N(0,R_{k})$

システムの初期条件と雑音 $\{{\boldsymbol {x}}_{0},{\boldsymbol {w}}_{1},...,{\boldsymbol {w}}_{k},{\boldsymbol {v}}_{1},...,{\boldsymbol {v}}_{k}\}$ は、互いに統計的に独立であると仮定する。

状態方程式と観測方程式を合わせて、状態空間モデルという。上記の状態空間モデルは時変システムを表現しているが、特別な場合として添字が $k$ の行列を定数であるとすれば、時不変システム (time-invariant) も表現できる。

実際の多くの動的システムでは上記の状態空間モデルは厳密には適合しないが、カルマンフィルターは雑音の影響を考慮して設計されているため、上記のモデルは対象システムに近似的に適合するものと考えられ、そのことを理由としてカルマンフィルターの有用性は十分に認められている。カルマンフィルターは洗練された様々な拡張がなされており、それは以降に述べられる。

カルマンフィルター

カルマンフィルターは、システム（系）の現在の観測量と 1 ステップ前の状態推定値のみから（モデルが制御入力を受ける場合には現在の入力値も用いて）、現在の状態推定値（ろ波推定値）と 1 ステップ先の状態予測値（ 1 段予測値）を与える、反復推定器（反復推定型フィルター）である。例えばローパスフィルターなどの多くのフィルターが周波数領域で設計され、時間領域へ変換されて実演される中で、カルマンフィルターは純粋に時間領域でのみ設計されるフィルターで、その意味で特異な存在であるといえる。カルマンフィルターは基本的に線形なクラスのフィルターであり、システム（系）が無限の過去から駆動し続けていると仮定すると、状態の推定値は、それまでにシステム（系）から観測された観測値の全て（システム（系）が制御入力を受ける場合は入力値の全ても含めて）を用いた線形結合の形で表現される。その意味で、カルマンフィルターは無限インパルス応答フィルターであると解釈できる。反復推定との対応関係は、1 ステップ前の状態推定値が 1 ステップ前までの全ての観測値（入力値も含め）の情報を線形結合の形で保有しているという事実により与えられる。

以降、 ${\hat {\boldsymbol {x}}}_{n|m}$ は時刻 m 時点での時刻 n の状態推定値を示すものとする。

フィルターの現在(時刻 k )の状態(様子)は、以下の２つの変数で特徴付けられる。

${\hat {\boldsymbol {x}}}_{k|k}$ システム(系)の状態推定値。
$P_{k|k}$ 誤差の共分散行列（推定値の精度）。

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

予測

${\hat {\boldsymbol {x}}}_{k|k-1}=F_{k}{\hat {\boldsymbol {x}}}_{k-1|k-1}+{\boldsymbol {u}}_{k}$ （今の時刻の予測推定値）
$P_{k|k-1}=F_{k}P_{k-1|k-1}F_{k}^{\textrm {T}}+G_{k}Q_{k}G_{k}^{\textrm {T}}$ （今の時刻の予測誤差行列）

更新

${\boldsymbol {e}}_{k}={\boldsymbol {z}}_{k}-H_{k}{\hat {\boldsymbol {x}}}_{k|k-1}$ （観測残差、innovation）
$S_{k}=R_{k}+H_{k}P_{k|k-1}H_{k}^{\textrm {T}}$ （観測残差の共分散）
$K_{k}=P_{k|k-1}H_{k}^{\textrm {T}}S_{k}^{-1}$ （最適カルマンゲイン）
${\hat {\boldsymbol {x}}}_{k|k}={\hat {\boldsymbol {x}}}_{k|k-1}+K_{k}{\boldsymbol {e}}_{k}$ （更新された状態の推定値）
$P_{k|k}=(\mathrm {I} -K_{k}H_{k})P_{k|k-1}$ （更新された誤差の共分散）

不偏量

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

$\mathrm {E} ({\boldsymbol {x}}_{k}-{\hat {\boldsymbol {x}}}_{k|k})=\mathrm {E} ({\boldsymbol {x}}_{k}-{\hat {\boldsymbol {x}}}_{k|k-1})=0$
$\mathrm {E} ({\boldsymbol {e}}_{k})=0$

ここに、 $\mathrm {E}$ は、期待値。また、共分散は、推定値の誤差共分散である。

$P_{k|k}=\mathrm {cov} ({\boldsymbol {x}}_{k}-{\hat {\boldsymbol {x}}}_{k|k})$
$P_{k|k-1}=\mathrm {cov} ({\boldsymbol {x}}_{k}-{\hat {\boldsymbol {x}}}_{k|k-1})$
$S_{k}=\mathrm {cov} ({\boldsymbol {e}}_{k})$

設定例

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

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

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

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

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

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

${\boldsymbol {x}}_{k}=F{\boldsymbol {x}}_{k-1}+G{\boldsymbol {w}}_{k}$

ここに、

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

かつ

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

${\boldsymbol {w}}_{k}={\begin{bmatrix}a_{k}\end{bmatrix}}$

である。 $G{\boldsymbol {w}}_{k}$ の共分散は、 $\sigma _{a}$ がスカラーであることを用いて、

$\mathrm {cov} (G{\boldsymbol {w}}_{k})=\sigma _{a}^{2}\times GG^{\textrm {T}}=\sigma _{a}^{2}\times {\begin{bmatrix}{\frac {\Delta t^{4}}{4}}&{\frac {\Delta t^{3}}{2}}\\{\frac {\Delta t^{3}}{2}}&\Delta t^{2}\end{bmatrix}}$

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

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

ここに、

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

かつ

$R=\mathrm {E} ({\boldsymbol {v}}_{k}{\boldsymbol {v}}_{k}^{\textrm {T}})={\begin{bmatrix}\sigma _{z}^{2}\end{bmatrix}}$

である。

初期条件は正確に分かっているので、

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

$P_{0|0}={\begin{bmatrix}0&0\\0&0\end{bmatrix}}$

もしも、初期条件に誤差があるならば、誤差の大きさに応じて B を設定し、

$P_{0|0}={\begin{bmatrix}B&0\\0&B\end{bmatrix}}$

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

導出

更新後の共分散行列

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

$P_{k|k}=\mathrm {cov} ({\boldsymbol {x}}_{k}-{\hat {\boldsymbol {x}}}_{k|k})$

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

$P_{k|k}=\mathrm {cov} ({\boldsymbol {x}}_{k}-({\hat {\boldsymbol {x}}}_{k|k-1}+K_{k}{\boldsymbol {e}}_{k}))$

続いて、観測残差 ${\boldsymbol {e}}_{k}$ を代入。

$P_{k|k}=\mathrm {cov} ({\boldsymbol {x}}_{k}-({\hat {\boldsymbol {x}}}_{k|k-1}+K_{k}({\boldsymbol {z}}_{k}-H_{k}{\hat {\boldsymbol {x}}}_{k|k-1})))$

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

$P_{k|k}=\mathrm {cov} ({\boldsymbol {x}}_{k}-({\hat {\boldsymbol {x}}}_{k|k-1}+K_{k}(H_{k}{\boldsymbol {x}}_{k}+{\boldsymbol {v}}_{k}-H_{k}{\hat {\boldsymbol {x}}}_{k|k-1})))$

変形して、

$P_{k|k}=\mathrm {cov} ((\mathrm {I} -K_{k}H_{k})({\boldsymbol {x}}_{k}-{\hat {\boldsymbol {x}}}_{k|k-1})-K_{k}{\boldsymbol {v}}_{k})$

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

$P_{k|k}=\mathrm {cov} ((\mathrm {I} -K_{k}H_{k})({\boldsymbol {x}}_{k}-{\hat {\boldsymbol {x}}}_{k|k-1}))+\mathrm {cov} (K_{k}{\boldsymbol {v}}_{k})$

となり、さらに変形

$P_{k|k}=(\mathrm {I} -K_{k}H_{k})\mathrm {cov} ({\boldsymbol {x}}_{k}-{\hat {\boldsymbol {x}}}_{k|k-1})(\mathrm {I} -K_{k}H_{k})^{\textrm {T}}+K_{k}\mathrm {cov} ({\boldsymbol {v}}_{k})K_{k}^{\textrm {T}}$

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

$P_{k|k}=(\mathrm {I} -K_{k}H_{k})P_{k|k-1}(\mathrm {I} -K_{k}H_{k})^{\textrm {T}}+K_{k}R_{k}K_{k}^{\textrm {T}}$

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

カルマンゲインの導出

カルマンフィルターは最小平均二乗誤差(minimum mean-square error: MMSE)推定値を与える。すなわち、更新後の誤差の推定値は

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

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

$P_{k\|k}$	$=P_{k\|k-1}-K_{k}H_{k}P_{k\|k-1}-P_{k\|k-1}H_{k}^{\textrm {T}}K_{k}^{\textrm {T}}+K_{k}(H_{k}P_{k\|k-1}H_{k}^{\textrm {T}}+R_{k})K_{k}^{\textrm {T}}$
	$=P_{k\|k-1}-K_{k}H_{k}P_{k\|k-1}-P_{k\|k-1}H_{k}^{\textrm {T}}K_{k}^{\textrm {T}}+K_{k}S_{k}K_{k}^{\textrm {T}}$

MMSE を導くゲインは、P_k|kのトレースを最小にするから、必要条件として、K_kによる行列微分は下記が成立しなければならない。

${\frac {\partial \;\mathrm {tr} (P_{k|k})}{\partial \;K_{k}}}=-2(H_{k}P_{k|k-1})^{\textrm {T}}+2K_{k}S_{k}=0$

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

$K_{k}S_{k}=(H_{k}P_{k|k-1})^{\textrm {T}}=P_{k|k-1}H_{k}^{\textrm {T}}$

$K_{k}=P_{k|k-1}H_{k}^{\textrm {T}}S_{k}^{-1}$

このゲインは、最適カルマンゲインと呼ばれる。

更新後の誤差共分散行列

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

$K_{k}S_{k}K_{k}^{\textrm {T}}=P_{k|k-1}H_{k}^{\textrm {T}}K_{k}^{\textrm {T}}$

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

$P_{k|k}=P_{k|k-1}-K_{k}H_{k}P_{k|k-1}-P_{k|k-1}H_{k}^{\textrm {T}}K_{k}^{\textrm {T}}+K_{k}S_{k}K_{k}^{\textrm {T}}$

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

$P_{k|k}=P_{k|k-1}-K_{k}H_{k}P_{k|k-1}=(\mathrm {I} -K_{k}H_{k})P_{k|k-1}$ .

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

再帰ベイズ推定との関係

真の状態は一次マルコフ過程であると仮定され、観測値は隠れマルコフモデルからの観測された状態である。^[3] 仮定より、ひとつ前の時刻の状態にのみ依存して

$p({\boldsymbol {x}}_{k}|{\boldsymbol {x}}_{0},\dots ,{\boldsymbol {x}}_{k-1})=p({\boldsymbol {x}}_{k}|{\boldsymbol {x}}_{k-1}).$

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

$p({\boldsymbol {z}}_{k}|{\boldsymbol {x}}_{0},\dots ,{\boldsymbol {x}}_{k})=p({\boldsymbol {z}}_{k}|{\boldsymbol {x}}_{k})$

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

$p({\boldsymbol {x}}_{0},\dots ,{\boldsymbol {x}}_{k},{\boldsymbol {z}}_{1},\dots ,{\boldsymbol {z}}_{k})=p({\boldsymbol {x}}_{0})\prod _{i=1}^{k}p({\boldsymbol {z}}_{i}|{\boldsymbol {x}}_{i})p({\boldsymbol {x}}_{i}|{\boldsymbol {x}}_{i-1})$

で、表される。

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

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

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

時刻 t までの観測は

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

である。

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

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

となる。分母の

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

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

他の確率分布関数も

$p({\boldsymbol {x}}_{k}|{\boldsymbol {x}}_{k-1})=N(F_{k}{\boldsymbol {x}}_{k-1},G_{k}Q_{k}G_{k}^{\textrm {T}})$

$p({\boldsymbol {z}}_{k}|{\boldsymbol {x}}_{k})=N(H_{k}{\boldsymbol {x}}_{k},R_{k})$

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

と書ける。

情報フィルター

情報フィルターもしくは逆共分散フィルターにおいては、カルマンフィルターにおける推定された共分散と状態が、各々フィッシャー情報行列と情報ベクトルに置き換わる。

$Y_{k|k}\triangleq P_{k|k}^{-1}$
${\hat {\boldsymbol {y}}}_{k|k}\triangleq P_{k|k}^{-1}{\hat {\boldsymbol {x}}}_{k|k}$

同様に、予測された共分散と状態は情報形式と等価になり、以下と定義する。

$Y_{k|k-1}\triangleq P_{k|k-1}^{-1}$
${\hat {\boldsymbol {y}}}_{k|k-1}\triangleq P_{k|k-1}^{-1}{\hat {\boldsymbol {x}}}_{k|k-1}$

観測共分散と観測ベクトルがあるとして、以下で定義する。

$I_{k}\triangleq H_{k}^{\textrm {T}}R_{k}^{-1}H_{k}$
${\boldsymbol {i}}_{k}\triangleq H_{k}^{\textrm {T}}R_{k}^{-1}{\boldsymbol {z}}_{k}$

このとき、情報更新は簡便な和算となる。

$Y_{k|k}=Y_{k|k-1}+I_{k}$
${\hat {\boldsymbol {y}}}_{k|k}={\hat {\boldsymbol {y}}}_{k|k-1}+{\boldsymbol {i}}_{k}$

情報フィルターの主たる優位性は、以下に示すように、N 個の観測値は各時間毎に、観測値の情報行列と情報ベクトルの和算でシンプルにフィルター処理される点である。

$Y_{k|k}=Y_{k|k-1}+\sum _{j=1}^{N}I_{k,j}$
${\hat {\boldsymbol {y}}}_{k|k}={\hat {\boldsymbol {y}}}_{k|k-1}+\sum _{j=1}^{N}{\boldsymbol {i}}_{k,j}$

情報フィルターを予測するために、情報空間予測を用いることができる^[4]。

${\tilde {Y}}_{k|k-1}={F_{k}}^{\mathrm {-T} }Y_{k-1|k-1}F_{k}^{-1}$

$A_{k}=\left(G_{k}^{\textrm {T}}{\tilde {Y}}_{k|k-1}G_{k}+Q_{k}^{-1}\right)^{-1}G_{k}^{\textrm {T}}{\tilde {Y}}_{k|k-1}$

$C_{k}=F_{k}^{-1}\left(\mathrm {I} -G_{k}A_{k}\right)$

$Y_{k|k-1}=C_{k}^{\textrm {T}}Y_{k-1|k-1}F_{k}^{-1}=C_{k}^{\textrm {T}}Y_{k-1|k-1}C_{k}+A_{k}^{\textrm {T}}Q_{k}^{-1}A_{k}$

${\hat {\boldsymbol {y}}}_{k|k-1}=C_{k}^{\textrm {T}}{\hat {\boldsymbol {y}}}_{k-1|k-1}+Y_{k|k-1}{\boldsymbol {u}}_{k}$

なお $Q_{k}=0$ であれば、 $A_{k}=0$ である。F は可逆（正則）の必要がある。注意すべきは、もし F, G, Q が時不変(time invariant)ならば、それらの値は保存しておける点である。

固定区間平滑化

固定区間平滑化（fixed-interval smoother）は、平滑化解 ${\hat {\boldsymbol {x}}}_{k|n}$ および $P_{k|n}$ （ $k=1,\,2,\,\ldots ,\,n$ 、 $n$ は固定値とする）を求める。

Rauch–Tung–Striebel^[5]の関係式（ $k\leq l$ ）：

{\boldsymbol {t}}_{k}\triangleq {\hat {\boldsymbol {x}}}_{k|l}-C_{k}{\hat {\boldsymbol {x}}}_{k+1|l}

T_{k}\triangleq P_{k|l}-C_{k}P_{k+1|l}{C_{k}}^{\mathrm {T} }

C_{k}\triangleq P_{k|k}{F_{k+1}}^{\mathrm {T} }{P_{k+1|k}}^{-1}

において、 ${\boldsymbol {t}}_{k}$ 、 $T_{k}$ の右式は $l$ に依存しない。なお $C_{k}$ は情報フィルターのそれに等しい。

これを用いて固定区間平滑化解が求められる。すなわちフィルター計算で $k=l$ における上記の値を求めておき、それらを用いて、

{\hat {\boldsymbol {x}}}_{k|n}=C_{k}{\hat {\boldsymbol {x}}}_{k+1|n}+{\boldsymbol {t}}_{k}

P_{k|n}=C_{k}P_{k+1|n}{C_{k}}^{\mathrm {T} }+T_{k}

を逆方向（backward）すなわち、kが減る方向に逐次計算し平滑化解が求められる。ここで計算が丸め誤差を持っていても、 $P_{k|n}$ は必ず半正定値となる。

また、上記を変形すると、Bryson–Frazierの固定区間平滑化^[6]と等価の式が得られる。すなわち、

{\boldsymbol {\lambda }}_{k}=C_{k}{\boldsymbol {\lambda }}_{k+1}-K_{k}{\boldsymbol {e}}_{k}

\Lambda _{k}=C_{k}\Lambda _{k+1}{C_{k}}^{\mathrm {T} }+K_{k}S_{k}{K_{k}}^{\mathrm {T} }

{\boldsymbol {\lambda }}_{k}\triangleq {\hat {\boldsymbol {x}}}_{k|k-1}-{\hat {\boldsymbol {x}}}_{k|n}

\Lambda _{k}\triangleq P_{k|k-1}-P_{k|n}

{\boldsymbol {\lambda }}_{n+1}={\boldsymbol {0}}

\Lambda _{n+1}=0

また、Biermanによって上記の変形式が得られている^[7]。これは、 ${P_{k+1|k}}^{-1}$ という逆行列計算を必要とせず平滑化解を得られる。すなわち、

{\tilde {\boldsymbol {\lambda }}}_{k}={\tilde {C}}_{k}{\tilde {\boldsymbol {\lambda }}}_{k+1}-{H_{k}}^{\mathrm {T} }{S_{k}}^{-1}{\boldsymbol {e}}_{k}

{\tilde {\Lambda }}_{k}={\tilde {C}}_{k}{\tilde {\Lambda }}_{k+1}{{\tilde {C}}_{k}}^{\mathrm {T} }+{H_{k}}^{\mathrm {T} }{S_{k}}^{-1}H_{k}

{\tilde {C}}_{k}\triangleq \left(\mathrm {I} -K_{k}H_{k}\right)^{\mathrm {T} }{F_{k+1}}^{\mathrm {T} }

{\tilde {\boldsymbol {\lambda }}}_{k}\triangleq {P_{k|k-1}}^{-1}{\boldsymbol {\lambda }}_{k}

{\tilde {\Lambda }}_{k}\triangleq {P_{k|k-1}}^{-1}\Lambda _{k}{P_{k|k-1}}^{-1}

非線形カルマンフィルター

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

拡張カルマンフィルター

ここでは時間発展モデル

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

と、観測モデル

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

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

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

予測

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

$P_{k|k-1}=F_{k}P_{k-1|k-1}F_{k}^{\textrm {T}}+G_{k}Q_{k}G_{k}^{\textrm {T}}$

更新

${\boldsymbol {e}}_{k}={\boldsymbol {z}}_{k}-h({\hat {\boldsymbol {x}}}_{k|k-1},0)$

$S_{k}=H_{k}P_{k|k-1}H_{k}^{\textrm {T}}+R_{k}$

$K_{k}=P_{k|k-1}H_{k}^{\textrm {T}}S_{k}^{-1}$

${\hat {\boldsymbol {x}}}_{k|k}={\hat {\boldsymbol {x}}}_{k|k-1}+K_{k}{\boldsymbol {e}}_{k}$

$P_{k|k}=(\mathrm {I} -K_{k}H_{k})P_{k|k-1}$

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

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

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

Unscented カルマンフィルター

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

予測

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

${\boldsymbol {x}}_{k-1|k-1}^{a}=[{\hat {\boldsymbol {x}}}_{k-1|k-1}^{\textrm {T}}\quad \mathrm {E} ({\boldsymbol {w}}_{k}^{\textrm {T}})\ ]^{\textrm {T}}$

$P_{k-1|k-1}^{a}={\begin{bmatrix}&P_{k-1|k-1}&&0&\\&0&&Q_{k}&\end{bmatrix}}$

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

$\chi _{k-1\|k-1}^{0}$	$={\boldsymbol {x}}_{k-1\|k-1}^{a}$
$\chi _{k-1\|k-1}^{i}$	$={\boldsymbol {x}}_{k-1\|k-1}^{a}+\left({\sqrt {(L+\lambda )P_{k-1\|k-1}^{a}}}\right)_{i}$	$i=1,\ldots L\,\!$
$\chi _{k-1\|k-1}^{i}$	$={\boldsymbol {x}}_{k-1\|k-1}^{a}-\left({\sqrt {(L+\lambda )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 {\boldsymbol {x}}}_{k|k-1}=\sum _{i=0}^{2L}W_{s}^{i}\chi _{k|k-1}^{i}$

$P_{k|k-1}=\sum _{i=0}^{2L}W_{c}^{i}\ [\chi _{k|k-1}^{i}-{\hat {\boldsymbol {x}}}_{k|k-1}][\chi _{k|k-1}^{i}-{\hat {\boldsymbol {x}}}_{k|k-1}]^{\textrm {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 といった値がよく用いられる。

更新

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

${\boldsymbol {x}}_{k|k-1}^{a}=[{\hat {\boldsymbol {x}}}_{k|k-1}^{\textrm {T}}\quad \mathrm {E} ({\boldsymbol {v}}_{k}^{\textrm {T}})\ ]^{\textrm {T}}$

$P_{k|k-1}^{a}={\begin{bmatrix}&P_{k|k-1}&&0&\\&0&&R_{k}&\end{bmatrix}}$

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

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

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

$\chi _{k|k-1}:=[\chi _{k|k-1}\quad \mathrm {E} ({\boldsymbol {v}}_{k}^{\textrm {T}})\ ]^{\textrm {T}}\pm {\sqrt {(L+\lambda )R_{k}^{a}}}$

ここに、

$R_{k}^{a}={\begin{bmatrix}&0&&0&\\&0&&R_{k}&\end{bmatrix}}$

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

$\gamma _{k}^{i}=h(\chi _{k|k-1}^{i})\quad i=0..2L$

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

${\hat {\boldsymbol {z}}}_{k}=\sum _{i=0}^{2L}W_{s}^{i}\gamma _{k}^{i}$

$P_{z_{k}z_{k}}=\sum _{i=0}^{2L}W_{c}^{i}\ [\gamma _{k}^{i}-{\hat {\boldsymbol {z}}}_{k}][\gamma _{k}^{i}-{\hat {\boldsymbol {z}}}_{k}]^{\textrm {T}}$

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

$P_{x_{k}z_{k}}=\sum _{i=0}^{2L}W_{c}^{i}\ [\chi _{k|k-1}^{i}-{\hat {\boldsymbol {x}}}_{k|k-1}][\gamma _{k}^{i}-{\hat {\boldsymbol {z}}}_{k}]^{\textrm {T}}$

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

$K_{k}=P_{x_{k}z_{k}}P_{z_{k}z_{k}}^{-1}$

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

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

$P_{k|k}=P_{k|k-1}-K_{k}P_{z_{k}z_{k}}K_{k}^{\textrm {T}}$

誤差状態カルマンフィルター

真の状態xtをノミナル状態xと誤差状態δxに分解する。

$x_{t}=x+\delta x$

状態方程式

真の状態方程式をfとする。

$x_{t}'=f(x_{t})$

この状態方程式を、ノミナル状態方程式と誤差状態方程式feに分解する。ノミナル状態は真の状態方程式に従うので、以下の式が得られる。

$x'+\delta x'=f(x+\delta x)=f(x)+f_{e}(x,\delta x)$

誤差状態方程式の誤差項の２乗を無視することで、線形な誤差状態方程式を得ることができる。

応用例

学習用参考図書類

有本卓：「カルマン・フィルター」、産業図書、ISBN 978-4782852545（1977年）。
片山徹：「新版　応用カルマンフィルタ」、朝倉書店、ISBN 978-4254201017（2000年2月1日）。
谷荻隆嗣：「カルマンフィルタと適用信号処理」、コロナ社、ISBN 4-339-01125-8 (2005年12月28日)。
津野義道：「Kalman-Bucyのフィルター理論」、共立出版、ISBN 4-320-01820-6 (2006年9月20日).
片山徹：「非線形カルマンフィルタ」、朝倉書店、ISBN 978-4254201482 (2011年11月30日)。
足立修一、丸田一郎：「カルマンフィルタの基礎」、東京電機大学出版局、ISBN 978-4501328900（2012年10月10日）。
野村俊一：「カルマンフィルタ：Rを使った時系列予測と状態空間モデル」、共立出版、ISBN 978-4320112537 (2016年9月8日)。
大住晃、亀山建太郎、松田吉隆：「カルマンフィルタとシステムの同定:動的逆問題へのアプローチ」、森北出版、 ISBN 978-4627922112（2016年11月）。
森平爽一郎：「経済・ファイナンスのためのカルマンフィルター入門」、朝倉書店、ISBN 978-4-254-12841-3（2019年2月1日）。

外部リンク

脚注

^ Steffen L. Lauritzen, Thiele: Pioneer in Statistics, Oxford University Press, 2002. ISBN 0-19-850972-3.
^ 表現式として、 ${\boldsymbol {x}}_{k+1}=F_{k}{\boldsymbol {x}}_{k}+{\boldsymbol {u}}_{k}+G_{k}{\boldsymbol {w}}_{k}$ の形が用いられることも多い。
^ C. Johan Masreliez, R D Martin (1977); Robust Bayesian estimation for the linear model and robustifying the Kalman filter, IEEE Trans. Automatic Control
^ なお、 $A^{\mathrm {-T} }=\left(A^{-1}\right)^{\textrm {T}}$
^ Rauch, H.E.; Tung, F.; Striebel, C. T. (August 1965). “Maximum likelihood estimates of linear dynamic systems”. AIAA J 3 (8): 1445–1450. doi:10.2514/3.3166.
^ Bryson, A. E.; Frazier, M. (1963). Smoothing for linear and nonlinear systems. pp. 353-364.
^ Bierman, G.J. (1973). “Fixed interval smoothing with discrete measurements”. International Journal of Control 8: 65-75.

[1] Steffen L. Lauritzen, Thiele: Pioneer in Statistics, Oxford University Press, 2002. ISBN 0-19-850972-3.

[2] 表現式として、 ${\boldsymbol {x}}_{k+1}=F_{k}{\boldsymbol {x}}_{k}+{\boldsymbol {u}}_{k}+G_{k}{\boldsymbol {w}}_{k}$ の形が用いられることも多い。

[3] C. Johan Masreliez, R D Martin (1977); Robust Bayesian estimation for the linear model and robustifying the Kalman filter, IEEE Trans. Automatic Control

[4] なお、 $A^{\mathrm {-T} }=\left(A^{-1}\right)^{\textrm {T}}$

[5] Rauch, H.E.; Tung, F.; Striebel, C. T. (August 1965). “Maximum likelihood estimates of linear dynamic systems”. AIAA J 3 (8): 1445–1450. doi:10.2514/3.3166.

[bryson-6] Bryson, A. E.; Frazier, M. (1963). Smoothing for linear and nonlinear systems. pp. 353-364.

[bierman-7] Bierman, G.J. (1973). “Fixed interval smoothing with discrete measurements”. International Journal of Control 8: 65-75.

[1]

[2]

[3]

[4]

[5]

[6]

[7]

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

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

表話編歴制御理論
分野	適応制御（英語版）制御理論デジタル制御 en:Energy-shaping control ファジィ制御ハイブリッド制御知能制御（英語版）モデル予測制御（英語版）多変量制御ニューラル制御非線形制御最適制御リアルタイム制御ロバスト制御（英語版）確率制御（英語版）
系特性	ボード線図ブロック図閉ループ伝達関数可制御性（英語版）フーリエ変換周波数特性ラプラス変換ネガティブフィードバック可観測性（英語版）ポジティブフィードバック根軌跡法（英語版）サーボ機構 en:Signal-flow graph 状態空間表現安定性理論定常状態解析・設計システムダイナミクス伝達関数法
デジタル制御	離散時間信号デジタル信号処理量子化（英語版）リアルタイムソフトウェア標本化データシステム同定 Z変換
先進技術	ニューラルネットワーク係数図法（英語版）制御再構成（英語版）分布定数系（英語版）分数次数制御（英語版）ファジィ論理 en:H-infinity loop-shaping ハンケル特異値（英語版）カルマンフィルター en:Krener's theorem 最小二乗法リアプノフ安定 en:Minor loop feedback 知覚制御理論（英語版）状態観測器ベクトル制御
制御器	組み込み制御器閉ループ制御器 en:Lead-lag compensator 数値制御 PID制御プログラマブルロジックコントローラ
制御応用	en:Automation and Remote Control 分散制御システム電動機工業制御システム（英語版）メカトロニクス運動制御（英語版）プロセス制御ロボット工学 SCADA
カテゴリ

カルマンフィルター

実用例

歴史

用いられる動的システム

カルマンフィルター

予測

更新

不偏量

設定例

導出

更新後の共分散行列

カルマンゲインの導出

更新後の誤差共分散行列

再帰ベイズ推定との関係

情報フィルター

固定区間平滑化

非線形カルマンフィルター

拡張カルマンフィルター

Unscented カルマンフィルター

誤差状態カルマンフィルター

応用例

関連項目

学習用参考図書類

外部リンク

脚注