状態方程式と観測方程式が
$\begin{align}というように表され，$\{{\bf y}_1,...,{\bf y}_T\}$が与えられたときに，$A,C,Q,R$と初期状態の平均ベクトル$\pi_1$と分散共分散行列$V_1$を推定します．
{\bf x}_{t+1}&=A{\bf x}_t + {\bf w}_t\\
{\bf y}_{t}&=C{\bf x}_t + {\bf v}_t\\
{\bf w}_t &\sim N(0,Q)\\
{\bf v}_t &\sim N(0,R)\\
\end{align}$
論文がとても分かりやすいので，実装に必要なところだけ抜粋します．
Eステップ
以下のように定義します．
$\begin{align}
{\bf x}_t^{\tau}&=E({\bf x}_t|\{{\bf y}_1^{\tau}\})\\
V_t^{\tau}&=Var({\bf x}_t|\{{\bf y}_1^{\tau}\})\\
\end{align}$
Kalman filter
$\begin{align}
{\bf x}_1^0 &= {\bf \pi}_1\\
V_1^0 &= V_1\\
{\bf x}_t^{t-1} &= A {\bf x}_{t-1}^{t-1}\\
V_t^{t-1} &= A V_{t-1}^{t-1} A' + Q\\
K_t &= V_t^{t-1}C'(CV_t^{t-1}C'+R)^{-1}\\
{\bf x}_t^t &= {\bf x}_t^{t-1} + K_t({\bf y}_t-C{\bf x}_t^{t-1})\\
V_t^t &= V_t^{t-1} - K_t C V_t^{t-1}\\
\end{align}$
Kalman smoother
$\begin{align}
J_{t-1} &= V_{t-1}^{t-1}A'(V_t^{t-1})^{-1}\\
{\bf x}_{t-1}^T &= {\bf x}_{t-1}^{t-1} + J_{t-1}({\bf x}_t^T - A{\bf x}_{t-1}^{t-1})\\
V_{t-1}^T &= V_{t-1}^{t-1} + J_{t-1}(V_t^T - V_t^{t-1})J_{t-1}'\\
V_{T,T-1}^T &= (I - K_T C)AV_{T-1}^{T-1}\\
V_{t-1,t-2}^T &= V_{t-1}^{t-1}J_{t-2}' + J_t-1(V_{t,t-1}^T - AV_{t-1}^{t-1})J_{t-2}'\\
\end{align}$
これらを用いて，Mステップへの入力を求めます．
$\begin{align}
{\hat {\bf x}}_t & = {\bf x}_t^T \\
P_t & = V_t^T + {\bf x}_t^T {{\bf x}_t^T}' \\
P_{t,t-1} & = V_{t,t-1}^T + {\bf x}_t^T{{\bf x}_{t-1}^T}' \\
\end{align}$
Mステップ
$\begin{align}なおN個の観測列があるときは
C^{new} & = (\sum_{t=1}^T {\bf y}_t {\hat{\bf x}}_t')(\sum_{t=1}^T P_t)^{-1} \\
R^{new} & = \frac{1}{T}\sum_{t=1}^T({\bf y}_t{\bf y}_t' - C^{new}{\hat{\bf x}}_t{\bf y}_t') \\
A^{new} & = (\sum_{t=2}^T P_{t,t-1})(\sum_{t=2}^T P_{t-1})^{-1} \\
Q^{new} & = \frac{1}{T-1}(\sum_{t=2}^T P_{t} - A^{new}\sum_{t=2}^T P_{t-1,t}) \\
{\bf \pi}_1^{new} & = {\hat {\bf x}}_1 \\
V_1^{new} & = P_1 - {\hat {\bf x}}_1{\hat {\bf x}}_1'
\end{align}$
$\begin{align}
{\bar {\hat {\bf x}}}_t & = \frac{1}{N}\sum_{i=1}^N {\hat {\bf x}}_t^{(i)} \\
V_1^{new} & = P_1 - {\bar {\hat {\bf x}}}_1{\bar {\hat {\bf x}}}_1' + \frac{1}{N}\sum_{i=1}^N [{\bar {\hat {\bf x}}}_1^{(i)}-{\bar {\hat {\bf x}}}_1][{\bar {\hat {\bf x}}}_1^{(i)}-{\bar {\hat {\bf x}}}_1]' \\
\end{align}$
実験
Wikipediaのカルマンフィルタの例にあるシステムで実験してみました．トロッコが摩擦なしの無限レール上でランダムな力によって動くシステムです．
データを生成するために用意したパラメータは
A [[ 1. 0.1] [ 0. 1. ]] C [[ 1. 0.]] Q [[ 2.50000000e-05 5.00000000e-04] [ 5.00000000e-04 1.00000000e-02]] R [[ 1.]]でした．
が，下のコードによって推定された結果は
A [[ 0.62656567 0.64773696] [ 0.56148647 0.0486408 ]] C [[ 0.19605285 1.14560846]] Q [[ 0.15479875 -0.12660499] [-0.12665761 0.31297647]] R [[ 0.59216228]] pi_1 [[-0.39133729] [-0.89786661]] V_1 [[ 0.18260503 -0.07688508] [-0.07691364 0.20206351]]でした．下のコードでは生成されるデータは毎回異なるので結果はその都度変わります．がどうもうまくいかない．
どこがおかしいのだろう(:D)|￣|_ =3
1. Z. Ghahramani and G.E. Hinton. Parameter estimation for linear dynamical systems. University of Toronto technical report CRG-TR-96-2, 6, 1996.
