カルマンフィルターは、 離散的な誤差のある観測から、時々刻々と時間変化する量（例えばある物体の位置と速度）を推定するために用いられる。レーダー やコンピュータビジョン など、工学分野で広く用いられる。例えば、カーナビゲーション では、機器内蔵の加速度計 や人工衛星 からの誤差のある情報を統合して、時々刻々変化する自動車 の位置を推定するのに応用されている。カルマンフィルターは、目標物の時間変化を支配する法則を活用して、目標物の位置を現在（フィルター）、未来（予測）、過去（内挿あるいは平滑化）に推定することができる。
カルマンフィルターは時間領域 において、連続時間線形動的システム 、もしくは離散化された離散時間線型 動的システムに基づいて駆動する。以降に導入される解説は、後者の立場のものである。それらはガウス白色雑音 によって励振をうける線形演算子 からなるマルコフ連鎖 モデルにより表現される。より端的にいえば、システムは状態空間モデル (state space model) で表現されるということである。
対象のシステムに定義された「状態 (state)」は、そのシステムの過去の動特性の遷移を保持する役割を果たし、動特性の遷移を保持する線形空間が状態空間 として定義される。この空間は実数空間であるため、システムの状態は一般に、任意の次元の状態空間に含まれる実数ベクトル として与えられる。状態の変化は、現在の状態と、それに付加する雑音の影響と、場合によってはシステムの状態の制御に関与する既知の制御入力の線形結合によって記述される。したがって、状態はシステムの因果性に寄与する存在である。上記の理念は、以下に記述する状態方程式 (state equation) によって表現される。
状態が直接観測できない場合には、システムの出力は一般に状態と観測雑音の線形結合として観測が可能なものとして与えられる。この理念は観測方程式 (observation equation) として、以下に示すような線形モデルで表現される。
カルマンフィルターは、システムの状態が直接観測できない問題に対する状態推定法であるから、一般的に観測方程式を伴う問題に適用される。
カルマンフィルターは隠れマルコフモデル (hidden Markov model) の類似であると考えることができる。両者の主な差異は隠れマルコフモデルにおける状態変数が、連続 であるか離散 であるかである。また、隠れマルコフモデルでは状態変数の未来への変化を任意の分布に従う形式で統計的に与えることができる一方で、カルマンフィルターでは、ガウス分布 に従う雑音によって未来の状態変数が統計的に記述される点が異なっている。したがって、カルマンフィルターと隠れマルコフモデルの間には強固な双対性が存在する。ちなみに、カルマンフィルターの導出過程においては、「システムに付随する雑音の性質はガウス分布に従う」という仮定の下に行われるのが一般的であるが、雑音の性質がガウス分布に従わない場合であっても、カルマンフィルターは線形なクラスにおける最適な推定値、すなわち線形最小分散推定値を導くことができる点で、汎用性に富んでいるといえる。
観測が可能な唯一の、雑音の影響を受けた出力過程に基づいて（制御問題であれば、入力も観測可能な過程となる）、システムの状態をカルマンフィルターを用いて推定するためには、対象とするシステムに対して、カルマンフィルターの理念と合致するような状態の遷移（すなわち状態過程）に関するモデルを与えなければならない。これは、時変 (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}}
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}}
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) も表現できる。
実際の多くの動的システムでは上記の状態空間モデルは厳密には適合しないが、カルマンフィルターは雑音の影響を考慮して設計されているため、上記のモデルは対象システムに近似的に適合するものと考えられ、そのことを理由としてカルマンフィルターの有用性は十分に認められている。カルマンフィルターは洗練された様々な拡張がなされており、それは以降に述べられる。
まっすぐで無限の長さを持つ摩擦の無いレールの上に乗っているトロッコを考えよう。初期条件は、トロッコは位置 0 に静止している。トロッコにはランダムな 力（加速度）が与えられる。Δt 秒ごとにトロッコの位置 x を観測する。ただしこの観測には誤差が混入している。トロッコの位置と速度のモデルを考えると、以下の様に設定すると、カルマンフィルターを用い得る。
制御の必要はないから、 u k 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}}
σ
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 が大きければカルマンフィルターは、初期条件より、それ以降の観測に重みを置くようになる。
時間を進めるための予測 と更新 の手続きのうち、更新が終わったあとの共分散行列 P k |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})}
観測誤差 v k
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}}}
して、前述の不偏量 P k |k -1R k
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}}}
を得る。この式は、 K k K k
カルマンフィルターは最小平均二乗誤差(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})}
更新 後の共分散 P k |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 を導くゲインは、P k |k K k
∂
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}
ここからカルマンゲイン K k
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}}
このゲインは、最適カルマンゲインと呼ばれる。
カルマンゲインが上述の値を取るとき、更新後の誤差共分散行列は以下のように簡単になる。カルマンゲインの式の両辺の右から S k K k T
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})}
これらの仮定を用いると、隠れマルコフモデルの観測が z 1 z 2
…
{\displaystyle \ldots }
z k
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)ならば、それらの値は保存しておける点である。
ここまでは線形の仮定が成り立つ系をとりあつかってきたが、実際の系の多くは非線形である。時間発展モデルも観測モデルもどちらも非線形になりうる。
ここでは時間発展モデル
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 カルマンフィルターの予測手続きは更新手続きと別であり、更新手続きに線形カルマンフィルターや拡張カルマンフィルターを用いたり、その逆を行うことも可能である。推定値と共分散には、予測ノイズの平均と共分散項が加えられる。
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}}}
重みは以下のように与えられる。
α = 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)}
誤差状態方程式の誤差項の２乗を無視することで、線形な誤差状態方程式を得ることができる。
^ Steffen L. Lauritzen , Thiele: Pioneer in Statistics Oxford University Press , 2002. ISBN 0-19-850972-3 .^ 表現式として、
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}}
^ C. Johan Masreliez , R D Martin (1977); Robust Bayesian estimation for the linear model and robustifying the Kalman filter ^ なお、
A
−
T
=
(
A
−
1
)
T
{\displaystyle 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 . 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 . ^ 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.
![{\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}}}](https://wikimedia.org/api/rest_v1/media/math/render/svg/346780e45f9457dab0b90801b54caa0e7e77f8b8)
![{\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}}}](https://wikimedia.org/api/rest_v1/media/math/render/svg/77cc5a25f0ab0ae6b93818779b5a3663b923b5cd)
![{\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}}}](https://wikimedia.org/api/rest_v1/media/math/render/svg/5179e14dc0be39c1c58305da3ee9ad0bd8f7d0ab)
![{\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}}}}](https://wikimedia.org/api/rest_v1/media/math/render/svg/04e2ad0af2d933805d24b890b5856579959339ea)
![{\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}}}](https://wikimedia.org/api/rest_v1/media/math/render/svg/c1f077892651304ac4cf3bb16cbadd1e1e07b3ee)
![{\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}}}](https://wikimedia.org/api/rest_v1/media/math/render/svg/d0a16fb2593b3b99f165d1601dc35c1fa5d4c45e)
