Pythonで線形動的システムのパラメータ推定を実装してみるもなんかうまくいってない

線形動的システム（線形力学系，Linear dynamical systems; LDS）のEMアルゴリズムによるパラメータ推定を行いました．が，うまくいってない気がします．カルマンフィルタなどで扱う状態方程式（外部入力なし）と観測方程式において，状態と観測が与えられたときに，係数を求めるのが目的です．Ghahramaniらの論文¹の手法を実装します．

状態方程式と観測方程式が

$\begin{align}
{\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}$

というように表され，$\{{\bf y}_1,...,{\bf y}_T\}$が与えられたときに，$A,C,Q,R$と初期状態の平均ベクトル$\pi_1$と分散共分散行列$V_1$を推定します．

論文がとても分かりやすいので，実装に必要なところだけ抜粋します．


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}
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}$

なおN個の観測列があるときは

$\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.