カルマンフィルター

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

実用例

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

歴史

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

用いられる動的システム

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

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

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

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

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

x k = F k x k − 1 + u k + G k w k {\displaystyle {\boldsymbol {x}}_{k}=F_{k}{\boldsymbol {x}}_{k-1}+{\boldsymbol {u}}_{k}+G_{k}{\boldsymbol {w}}_{k}}

ここに、

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

w k ∼ N ( 0 , Q k ) {\displaystyle {\boldsymbol {w}}_{k}\sim N(0,Q_{k})}

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

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

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

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

v k ∼ N ( 0 , R k ) {\displaystyle {\boldsymbol {v}}_{k}\sim N(0,R_{k})}

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

状態方程式と観測方程式を合わせて、状態空間モデルという。上記の状態空間モデルは時変システムを表現しているが、限定的な場合として添字が k {\displaystyle k} の行列を定数と考えることにより、時不変システム (time-invariant) を表現できる。

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

カルマンフィルター

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


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

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

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

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

予測

x ^ k | k − 1 = F k x ^ k − 1 | k − 1 + u k {\displaystyle {\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 T + G k Q k G k T {\displaystyle P_{k|k-1}=F_{k}P_{k-1|k-1}F_{k}^{\textrm {T}}+G_{k}Q_{k}G_{k}^{\textrm {T}}} （今の時刻の予測誤差行列）

更新

e k = z k − H k x ^ k | k − 1 {\displaystyle {\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 T {\displaystyle S_{k}=R_{k}+H_{k}P_{k|k-1}H_{k}^{\textrm {T}}} （観測残差の共分散）
K k = P k | k − 1 H k T S k − 1 {\displaystyle K_{k}=P_{k|k-1}H_{k}^{\textrm {T}}S_{k}^{-1}} （最適 カルマンゲイン）
x ^ k | k = x ^ k | k − 1 + K k e k {\displaystyle {\hat {\boldsymbol {x}}}_{k|k}={\hat {\boldsymbol {x}}}_{k|k-1}+K_{k}{\boldsymbol {e}}_{k}} （更新された状態の推定値）
P k | k = ( I − K k H k ) P k | k − 1 {\displaystyle P_{k|k}=(\mathrm {I} -K_{k}H_{k})P_{k|k-1}} （更新された誤差の共分散）

不偏量

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

  • E ( x k − x ^ k | k ) = E ( x k − x ^ k | k − 1 ) = 0 {\displaystyle \mathrm {E} ({\boldsymbol {x}}_{k}-{\hat {\boldsymbol {x}}}_{k|k})=\mathrm {E} ({\boldsymbol {x}}_{k}-{\hat {\boldsymbol {x}}}_{k|k-1})=0}
  • E ( e k ) = 0 {\displaystyle \mathrm {E} ({\boldsymbol {e}}_{k})=0}

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

  • P k | k = c o v ( x k − x ^ k | k ) {\displaystyle P_{k|k}=\mathrm {cov} ({\boldsymbol {x}}_{k}-{\hat {\boldsymbol {x}}}_{k|k})}
  • P k | k − 1 = c o v ( x k − x ^ k | k − 1 ) {\displaystyle P_{k|k-1}=\mathrm {cov} ({\boldsymbol {x}}_{k}-{\hat {\boldsymbol {x}}}_{k|k-1})}
  • S k = c o v ( e k ) {\displaystyle S_{k}=\mathrm {cov} ({\boldsymbol {e}}_{k})}

設定例

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

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

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

x k = [ x x ˙ ] {\displaystyle {\boldsymbol {x}}_{k}={\begin{bmatrix}x\\{\dot {x}}\end{bmatrix}}}

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

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

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

ここに、

F = [ 1 Δ t 0 1 ] {\displaystyle F={\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}}}

かつ

G = [ Δ t 2 2 Δ t ] {\displaystyle G={\begin{bmatrix}{\frac {\Delta t^{2}}{2}}\\\Delta t\end{bmatrix}}}

w k = [ a k ] {\displaystyle {\boldsymbol {w}}_{k}={\begin{bmatrix}a_{k}\end{bmatrix}}}

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

c o v ( G w k ) = σ a 2 × G G T = σ a 2 × [ Δ t 4 4 Δ t 3 2 Δ t 3 2 Δ t 2 ] {\displaystyle \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 で標準偏差 σ z {\displaystyle \sigma _{z}} の正規分布と仮定する。

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

ここに、

H = [ 1 0 ] {\displaystyle H={\begin{bmatrix}1&0\end{bmatrix}}}

かつ

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

である。

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

x ^ 0 | 0 = [ 0 0 ] {\displaystyle {\hat {\boldsymbol {x}}}_{0|0}={\begin{bmatrix}0\\0\end{bmatrix}}}

P 0 | 0 = [ 0 0 0 0 ] {\displaystyle P_{0|0}={\begin{bmatrix}0&0\\0&0\end{bmatrix}}}

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

P 0 | 0 = [ B 0 0 B ] {\displaystyle P_{0|0}={\begin{bmatrix}B&0\\0&B\end{bmatrix}}}

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

導出

更新後の共分散行列

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

P k | k = c o v ( x k − x ^ k | k ) {\displaystyle P_{k|k}=\mathrm {cov} ({\boldsymbol {x}}_{k}-{\hat {\boldsymbol {x}}}_{k|k})}

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

P k | k = c o v ( x k − ( x ^ k | k − 1 + K k e k ) ) {\displaystyle P_{k|k}=\mathrm {cov} ({\boldsymbol {x}}_{k}-({\hat {\boldsymbol {x}}}_{k|k-1}+K_{k}{\boldsymbol {e}}_{k}))}

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

P k | k = c o v ( x k − ( x ^ k | k − 1 + K k ( z k − H k x ^ k | k − 1 ) ) ) {\displaystyle 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})))}

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

P k | k = c o v ( x k − ( x ^ k | k − 1 + K k ( H k x k + v k − H k x ^ k | k − 1 ) ) ) {\displaystyle 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 = c o v ( ( I − K k H k ) ( x k − x ^ k | k − 1 ) − K k v k ) {\displaystyle 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})}

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

P k | k = c o v ( ( I − K k H k ) ( x k − x ^ k | k − 1 ) ) + c o v ( K k v k ) {\displaystyle 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 = ( I − K k H k ) c o v ( x k − x ^ k | k − 1 ) ( I − K k H k ) T + K k c o v ( v k ) K k T {\displaystyle 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}}}

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

P k | k = ( I − K k H k ) P k | k − 1 ( I − K k H k ) T + K k R k K k T {\displaystyle 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}}}

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

カルマンゲインの導出

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

x k − x ^ k | k {\displaystyle {\boldsymbol {x}}_{k}-{\hat {\boldsymbol {x}}}_{k|k}}

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

P k | k {\displaystyle P_{k|k}} = P k | k − 1 − K k H k P k | k − 1 − P k | k − 1 H k T K k T + K k ( H k P k | k − 1 H k T + R k ) K k T {\displaystyle =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 T K k T + K k S k K k T {\displaystyle =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 を導くゲインは、Pk|kのトレースを最小にするから、 必要条件として、Kkによる行列微分は下記が成立しなければならない。

∂ t r ( P k | k ) ∂ K k = − 2 ( H k P k | k − 1 ) T + 2 K k S k = 0 {\displaystyle {\frac {\partial \;\mathrm {tr} (P_{k|k})}{\partial \;K_{k}}}=-2(H_{k}P_{k|k-1})^{\textrm {T}}+2K_{k}S_{k}=0}

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

K k S k = ( H k P k | k − 1 ) T = P k | k − 1 H k T {\displaystyle 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 T S k − 1 {\displaystyle K_{k}=P_{k|k-1}H_{k}^{\textrm {T}}S_{k}^{-1}}

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

更新後の誤差共分散行列

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

K k S k K k T = P k | k − 1 H k T K k T {\displaystyle 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 T K k T + K k S k K k T {\displaystyle 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 = ( I − K k H k ) P k | k − 1 {\displaystyle 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 ( x k | x 0 , … , x k − 1 ) = p ( x k | x k − 1 ) . {\displaystyle p({\boldsymbol {x}}_{k}|{\boldsymbol {x}}_{0},\dots ,{\boldsymbol {x}}_{k-1})=p({\boldsymbol {x}}_{k}|{\boldsymbol {x}}_{k-1}).}

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

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

これらの仮定を用いると、隠れマルコフモデルの観測が z1, z2, … {\displaystyle \ldots } zk と得られる確率は、

p ( x 0 , … , x k , z 1 , … , z k ) = p ( x 0 ) ∏ i = 1 k p ( z i | x i ) p ( x i | x i − 1 ) {\displaystyle 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 ( x k | Z k − 1 ) = ∫ p ( x k | x k − 1 ) p ( x k − 1 | Z k − 1 ) d x k − 1 {\displaystyle 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 までの観測は

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

である。

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

p ( x k | Z k ) = p ( z k | x k ) p ( x k | Z k − 1 ) p ( z k | Z k − 1 ) {\displaystyle 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 ( z k | Z k − 1 ) = ∫ p ( z k | x k ) p ( x k | Z k − 1 ) d x k {\displaystyle 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 ( x k | x k − 1 ) = N ( F k x k − 1 , G k Q k G k T ) {\displaystyle p({\boldsymbol {x}}_{k}|{\boldsymbol {x}}_{k-1})=N(F_{k}{\boldsymbol {x}}_{k-1},G_{k}Q_{k}G_{k}^{\textrm {T}})}

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

p ( x k − 1 | Z k − 1 ) = N ( x ^ k − 1 , P k − 1 ) {\displaystyle p({\boldsymbol {x}}_{k-1}|{\boldsymbol {Z}}_{k-1})=N({\hat {\boldsymbol {x}}}_{k-1},P_{k-1})}

と書ける。


情報フィルター

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

Y k | k ≜ P k | k − 1 {\displaystyle Y_{k|k}\triangleq P_{k|k}^{-1}}
y ^ k | k ≜ P k | k − 1 x ^ k | k {\displaystyle {\hat {\boldsymbol {y}}}_{k|k}\triangleq P_{k|k}^{-1}{\hat {\boldsymbol {x}}}_{k|k}}

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

Y k | k − 1 ≜ P k | k − 1 − 1 {\displaystyle Y_{k|k-1}\triangleq P_{k|k-1}^{-1}}
y ^ k | k − 1 ≜ P k | k − 1 − 1 x ^ k | k − 1 {\displaystyle {\hat {\boldsymbol {y}}}_{k|k-1}\triangleq P_{k|k-1}^{-1}{\hat {\boldsymbol {x}}}_{k|k-1}}

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

I k ≜ H k T R k − 1 H k {\displaystyle I_{k}\triangleq H_{k}^{\textrm {T}}R_{k}^{-1}H_{k}}
i k ≜ H k T R k − 1 z k {\displaystyle {\boldsymbol {i}}_{k}\triangleq H_{k}^{\textrm {T}}R_{k}^{-1}{\boldsymbol {z}}_{k}}

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

Y k | k = Y k | k − 1 + I k {\displaystyle Y_{k|k}=Y_{k|k-1}+I_{k}}
y ^ k | k = y ^ k | k − 1 + i k {\displaystyle {\hat {\boldsymbol {y}}}_{k|k}={\hat {\boldsymbol {y}}}_{k|k-1}+{\boldsymbol {i}}_{k}}

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

Y k | k = Y k | k − 1 + ∑ j = 1 N I k , j {\displaystyle Y_{k|k}=Y_{k|k-1}+\sum _{j=1}^{N}I_{k,j}}
y ^ k | k = y ^ k | k − 1 + ∑ j = 1 N i k , j {\displaystyle {\hat {\boldsymbol {y}}}_{k|k}={\hat {\boldsymbol {y}}}_{k|k-1}+\sum _{j=1}^{N}{\boldsymbol {i}}_{k,j}}

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

Y ~ k | k − 1 = F k − T Y k − 1 | k − 1 F k − 1 {\displaystyle {\tilde {Y}}_{k|k-1}={F_{k}}^{\mathrm {-T} }Y_{k-1|k-1}F_{k}^{-1}}

A k = ( G k T Y ~ k | k − 1 G k + Q k − 1 ) − 1 G k T Y ~ k | k − 1 {\displaystyle 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 ( I − G k A k ) {\displaystyle C_{k}=F_{k}^{-1}\left(\mathrm {I} -G_{k}A_{k}\right)}

Y k | k − 1 = C k T Y k − 1 | k − 1 F k − 1 = C k T Y k − 1 | k − 1 C k + A k T Q k − 1 A k {\displaystyle 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}}

y ^ k | k − 1 = C k T y ^ k − 1 | k − 1 + Y k | k − 1 u k {\displaystyle {\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 {\displaystyle Q_{k}=0} であれば、 A k = 0 {\displaystyle A_{k}=0} である。F は可逆（正則）の必要がある。 注意すべきは、もし F, G, Q が時不変(time invariant)ならば、それらの値は保存しておける点である。

固定区間平滑化

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

Rauch–Tung–Striebel[5]の関係式（ k ≤ l {\displaystyle k\leq l} ）：

t k ≜ x ^ k | l − C k x ^ k + 1 | l {\displaystyle {\boldsymbol {t}}_{k}\triangleq {\hat {\boldsymbol {x}}}_{k|l}-C_{k}{\hat {\boldsymbol {x}}}_{k+1|l}}
T k ≜ P k | l − C k P k + 1 | l C k T {\displaystyle T_{k}\triangleq P_{k|l}-C_{k}P_{k+1|l}{C_{k}}^{\mathrm {T} }}
C k ≜ P k | k F k + 1 T P k + 1 | k − 1 {\displaystyle C_{k}\triangleq P_{k|k}{F_{k+1}}^{\mathrm {T} }{P_{k+1|k}}^{-1}}

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

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

x ^ k | n = C k x ^ k + 1 | n + t k {\displaystyle {\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 T + T k {\displaystyle P_{k|n}=C_{k}P_{k+1|n}{C_{k}}^{\mathrm {T} }+T_{k}}

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

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

λ k = C k λ k + 1 − K k e k {\displaystyle {\boldsymbol {\lambda }}_{k}=C_{k}{\boldsymbol {\lambda }}_{k+1}-K_{k}{\boldsymbol {e}}_{k}}
Λ k = C k Λ k + 1 C k T + K k S k K k T {\displaystyle \Lambda _{k}=C_{k}\Lambda _{k+1}{C_{k}}^{\mathrm {T} }+K_{k}S_{k}{K_{k}}^{\mathrm {T} }}
λ k ≜ x ^ k | k − 1 − x ^ k | n {\displaystyle {\boldsymbol {\lambda }}_{k}\triangleq {\hat {\boldsymbol {x}}}_{k|k-1}-{\hat {\boldsymbol {x}}}_{k|n}}
Λ k ≜ P k | k − 1 − P k | n {\displaystyle \Lambda _{k}\triangleq P_{k|k-1}-P_{k|n}}
λ n + 1 = 0 {\displaystyle {\boldsymbol {\lambda }}_{n+1}={\boldsymbol {0}}}
Λ n + 1 = 0 {\displaystyle \Lambda _{n+1}=0}

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

λ ~ k = C ~ k λ ~ k + 1 − H k T S k − 1 e k {\displaystyle {\tilde {\boldsymbol {\lambda }}}_{k}={\tilde {C}}_{k}{\tilde {\boldsymbol {\lambda }}}_{k+1}-{H_{k}}^{\mathrm {T} }{S_{k}}^{-1}{\boldsymbol {e}}_{k}}
Λ ~ k = C ~ k Λ ~ k + 1 C ~ k T + H k T S k − 1 H k {\displaystyle {\tilde {\Lambda }}_{k}={\tilde {C}}_{k}{\tilde {\Lambda }}_{k+1}{{\tilde {C}}_{k}}^{\mathrm {T} }+{H_{k}}^{\mathrm {T} }{S_{k}}^{-1}H_{k}}
C ~ k ≜ ( I − K k H k ) T F k + 1 T {\displaystyle {\tilde {C}}_{k}\triangleq \left(\mathrm {I} -K_{k}H_{k}\right)^{\mathrm {T} }{F_{k+1}}^{\mathrm {T} }}
λ ~ k ≜ P k | k − 1 − 1 λ k {\displaystyle {\tilde {\boldsymbol {\lambda }}}_{k}\triangleq {P_{k|k-1}}^{-1}{\boldsymbol {\lambda }}_{k}}
Λ ~ k ≜ P k | k − 1 − 1 Λ k P k | k − 1 − 1 {\displaystyle {\tilde {\Lambda }}_{k}\triangleq {P_{k|k-1}}^{-1}\Lambda _{k}{P_{k|k-1}}^{-1}}

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

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

拡張カルマンフィルター

ここでは時間発展モデル

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

と、観測モデル

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

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

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

予測

x ^ k | k − 1 = f ( x ^ k − 1 | k − 1 , u k , 0 ) {\displaystyle {\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 T + G k Q k G k T {\displaystyle P_{k|k-1}=F_{k}P_{k-1|k-1}F_{k}^{\textrm {T}}+G_{k}Q_{k}G_{k}^{\textrm {T}}}

更新

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

S k = H k P k | k − 1 H k T + R k {\displaystyle S_{k}=H_{k}P_{k|k-1}H_{k}^{\textrm {T}}+R_{k}}

K k = P k | k − 1 H k T S k − 1 {\displaystyle K_{k}=P_{k|k-1}H_{k}^{\textrm {T}}S_{k}^{-1}}

x ^ k | k = x ^ k | k − 1 + K k e k {\displaystyle {\hat {\boldsymbol {x}}}_{k|k}={\hat {\boldsymbol {x}}}_{k|k-1}+K_{k}{\boldsymbol {e}}_{k}}

P k | k = ( I − K k H k ) P k | k − 1 {\displaystyle P_{k|k}=(\mathrm {I} -K_{k}H_{k})P_{k|k-1}}

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

F k = ∂ f ∂ x | x ^ k − 1 | k − 1 , u k {\displaystyle F_{k}=\left.{\frac {\partial f}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k}}}

H k = ∂ h ∂ x | x ^ k | k − 1 {\displaystyle H_{k}=\left.{\frac {\partial h}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}}}

Unscented カルマンフィルター

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

予測

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

x k − 1 | k − 1 a = [ x ^ k − 1 | k − 1 T E ( w k T )   ] T {\displaystyle {\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 = [ P k − 1 | k − 1 0 0 Q k ] {\displaystyle P_{k-1|k-1}^{a}={\begin{bmatrix}&P_{k-1|k-1}&&0&\\&0&&Q_{k}&\end{bmatrix}}}

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

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

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

χ k | k − 1 i = f ( χ k − 1 | k − 1 i ) i = 0..2 L {\displaystyle \chi _{k|k-1}^{i}=f(\chi _{k-1|k-1}^{i})\quad i=0..2L}

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

x ^ k | k − 1 = ∑ i = 0 2 L W s i χ k | k − 1 i {\displaystyle {\hat {\boldsymbol {x}}}_{k|k-1}=\sum _{i=0}^{2L}W_{s}^{i}\chi _{k|k-1}^{i}}

P k | k − 1 = ∑ i = 0 2 L W c i   [ χ k | k − 1 i − x ^ k | k − 1 ] [ χ k | k − 1 i − x ^ k | k − 1 ] T {\displaystyle 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 = λ L + λ {\displaystyle W_{s}^{0}={\frac {\lambda }{L+\lambda }}}
W c 0 = λ L + λ + ( 1 − α 2 + β ) {\displaystyle W_{c}^{0}={\frac {\lambda }{L+\lambda }}+(1-\alpha ^{2}+\beta )}
W s i = W c i = 1 2 ( L + λ ) {\displaystyle W_{s}^{i}=W_{c}^{i}={\frac {1}{2(L+\lambda )}}}
λ = α 2 ( L + κ ) − L {\displaystyle \lambda =\alpha ^{2}(L+\kappa )-L\,\!}

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

更新

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

x k | k − 1 a = [ x ^ k | k − 1 T E ( v k T )   ] T {\displaystyle {\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 = [ P k | k − 1 0 0 R k ] {\displaystyle P_{k|k-1}^{a}={\begin{bmatrix}&P_{k|k-1}&&0&\\&0&&R_{k}&\end{bmatrix}}}

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

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

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

χ k | k − 1 := [ χ k | k − 1 E ( v k T )   ] T ± ( L + λ ) R k a {\displaystyle \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 = [ 0 0 0 R k ] {\displaystyle R_{k}^{a}={\begin{bmatrix}&0&&0&\\&0&&R_{k}&\end{bmatrix}}}

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

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

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

z ^ k = ∑ i = 0 2 L W s i γ k i {\displaystyle {\hat {\boldsymbol {z}}}_{k}=\sum _{i=0}^{2L}W_{s}^{i}\gamma _{k}^{i}}

P z k z k = ∑ i = 0 2 L W c i   [ γ k i − z ^ k ] [ γ k i − z ^ k ] T {\displaystyle 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 = ∑ i = 0 2 L W c i   [ χ k | k − 1 i − x ^ k | k − 1 ] [ γ k i − z ^ k ] T {\displaystyle 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 {\displaystyle K_{k}=P_{x_{k}z_{k}}P_{z_{k}z_{k}}^{-1}}

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

x ^ k | k = x ^ k | k − 1 + K k ( z k − z ^ k ) {\displaystyle {\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 T {\displaystyle P_{k|k}=P_{k|k-1}-K_{k}P_{z_{k}z_{k}}K_{k}^{\textrm {T}}}

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

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

x t = x + δ x {\displaystyle x_{t}=x+\delta x}

状態方程式

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

x t ′ = f ( x t ) {\displaystyle x_{t}'=f(x_{t})}

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

x ′ + δ x ′ = f ( x + δ x ) = f ( x ) + f e ( x , δ x ) {\displaystyle 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 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日）。

外部リンク

  • 丸田一郎：「裏口からのカルマンフィルタ入門」

脚注

  1. ^ Steffen L. Lauritzen, Thiele: Pioneer in Statistics, Oxford University Press, 2002. ISBN 0-19-850972-3.
  2. ^ 表現式として、 x k + 1 = F k x k + u k + G k w k {\displaystyle {\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 − T = ( A − 1 ) T {\displaystyle 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. http://pdf.aiaa.org/getfile.cfm?urlX=7%3CWIG7D%2FQKU%3E6B5%3AKF2Z%5CD%3A%2B82%2A%40%24%5E%3F%40%20%0A&urla=%25%2ARL%2F%220L%20%0A&urlb=%21%2A%20%20%20%0A&urlc=%21%2A0%20%20%0A&urld=%21%2A0%20%20%0A&urle=%27%2BB%2C%27%22%20%22KT0%20%20%0A. 
  6. ^ Bryson, A. E.; Frazier, M. (1963). Smoothing for linear and nonlinear systems. pp. 353-364. 
  7. ^ Bierman, G.J. (1973). “Fixed interval smoothing with discrete measurements”. International Journal of Control 8: 65-75. 
典拠管理データベース: 国立図書館 ウィキデータを編集
  • ドイツ
  • イスラエル
  • アメリカ
  • 日本
  • チェコ
  • 表
  • 話
  • 編
  • 歴
分野
系特性
デジタル制御
先進技術
制御器
制御応用
カテゴリ カテゴリ