データ同化｜斜方投射のカルマンフィルタをPythonで実装

前回整理した斜方投射のカルマンフィルタで推定する系を、Pythonで実装します🐜

本記事は過去記事の内容を前提とするので、直接こちらにいらっしゃった方は、最初に以下をご覧いただくようお願いします。

斜方投射の条件
サンプルプログラム
- 開発環境
- Pythonスクリプト
解説
パラメタ設定例
- 調整記録
おわりに
参考文献

斜方投射の条件

シミュレーションを回すので、斜方投射の条件として以下のように設定します。

項目	変数名	値
質量[kg]	m	1.0
初速[m/s]	v0	30
初期座標[m]	-	(0, 0)
投射角度[rad]	theta	$\frac{\pi}{4}$
空気抵抗[kg/s]	r	0.1
重力加速度[m/s²]	g	9.80665
開始時刻[s]	ts	0
終了時刻[s]	tf	5
時刻刻み幅[s]	ts	0.01
センサ誤差（分散）[m²]	s_obse	2

500mlペットボトル2本分のボールを、斜め45度の角度で、時速108km（＝秒速30m）で投げる系を考えます。私はきっと腕がもげますｗ

ちなみに、グラフで結果がわかりやすいように、センサ誤差は大きめに設定しました。

サンプルプログラム

Pythonで実装しました。環境さえ整っていれば、コピペで動きます。

開発環境

開発環境は以下の通りです。

項目	内容
OS	Windows 10
Python	3.6.8
NumPy	1.18.5
Pandas	1.0.4
SciPy	1.4.1
Matplotlib	3.2.1

Pythonスクリプト

スクリプト(kf_projectile_motion.py)は次の通りです。出力されるグラフがカルマンフィルタのシミュレーション結果です。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""斜方投射シミュレーション

斜方投射シミュレーション

"""
__author__  = 'ari23(Twitter: @ari23ant)'
__version__ = '0.0.6'
__date__    = '2020/07/15'
__status__  = 'Development'

import numpy as np
import pandas as pd
import scipy
import matplotlib.pyplot as plt


class ProjectileMotion():

    def __init__(self):
        self.msg = 'projectile motion simulation'
        # ------- 設定値 ------- #
        # --- 斜方投射 --- #
        self.m = 1  # 質量[kg]
        self.v0 = 30  # 初速度[m/s]
        self.theta = np.pi / 4  # 投射角度[rad] 45度
        self.r = 0.1  # 空気抵抗[kg/s]
        self.g = 9.80665  # 重力加速度[m/s^2] scipy.constants.gでも良いがVersionで違いが出る可能性がある
        self.ts = 0  # 開始時刻[s]
        self.tf = 5  # 終了時刻[s]
        self.dt = 0.01  # 時刻刻み幅[s]
        # 時間
        self.t = np.arange(self.ts, self.tf, self.dt)

        # --- カルマンフィルタ --- #
        # 可読性を考慮し置き換え
        v0, theta, g, dt = self.v0, self.theta, self.g, self.dt
        # - 初期状態 - #
        # 状態変数 [x座標, x軸速度, x軸加速度, y座標, y軸速度, y軸加速度]
        self.xx_0 = np.array([0, v0*np.cos(theta), 0, 0, v0*np.sin(theta), -g])

        # 状態変数の誤差分散共分散行列
        self.V0 = np.array([[1, 0, 0, 0, 0, 0],
                            [0, 1, 0, 0, 0, 0],
                            [0, 0, 1, 0, 0, 0],
                            [0, 0, 0, 1, 0, 0],
                            [0, 0, 0, 0, 1, 0],
                            [0, 0, 0, 0, 0, 1]])

        # - システムモデル - #
        # システム行列※または遷移行列
        self.Ft = np.array([[1, dt, 0, 0,  0,       0],
                            [0,  1, 0, 0,  0,       0],
                            [0,  0, 1, 0,  0,       0],
                            [0,  0, 0, 1, dt, dt**2/2],
                            [0,  0, 0, 0,  1,      dt],
                            [0,  0, 0, 0,  0,       1]])

        # システムノイズ行列※
        self.Gt = np.ones_like(self.Ft)

        # システムノイズ
        self.Qt = np.array([[.01,   0,   0,   0,   0,   0],
                            [  0, .01,   0,   0,   0,   0],
                            [  0,   0, .01,   0,   0,   0],
                            [  0,   0,   0, .01,   0,   0],
                            [  0,   0,   0,   0, .01,   0],
                            [  0,   0,   0,   0,   0, .01]])

        # - 観測モデル - #
        # 観測行列※ 観測できるのはx座標とy座標のみ
        self.Ht = np.array([[1, 0, 0, 0, 0, 0],
                            [0, 0, 0, 1, 0, 0]])

        # 観測ノイズ
        self.Rt = np.array([[3, 0],
                            [0, 3]])

        # --- その他 --- #
        # 乱数シード
        self.seed = 23
        # 真値から観測値を作る際の分散
        self.s_obse = 2


    def Process(self):
        print(self.msg)
        # ------- 真値、観測値、空気抵抗が無いときの真値を計算 ------- #
        # 可読性を考慮し置き換え
        m, v0, theta, r, g, t = self.m, self.v0, self.theta, self.r, self.g, self.t

        # - 真値 - #
        x_true = m*v0 /r * (1 - np.exp(-r/m * t)) * np.cos(theta)
        y_true = m/r * ((v0*np.sin(theta) + m/r *g) * (1 - np.exp(-r/m *t)) - g*t)

        # - 観測値 - #
        np.random.seed(self.seed)  # 乱数シード設定
        x_obse = x_true + np.random.normal(0, self.s_obse, len(x_true))
        y_obse = y_true + np.random.normal(0, self.s_obse, len(y_true))

        # - 空気抵抗なし - #
        x_no_air = v0*np.cos(theta) * t
        y_no_air = v0*np.sin(theta) * t - 1/2 * g * t**2

        # - グラフ確認 - #
        #fig, ax = plt.subplots(tight_layout=True)
        #ax.plot(x_obse, y_obse, marker='^', lw=0, label='observed')
        #ax.plot(x_true, y_true, label='true')
        #ax.plot(x_no_air, y_no_air, label='no air')
        #ax.set_xlabel('x[m]')
        #ax.set_ylabel('y[m]')
        #ax.legend()
        #ax.grid(b=True)

        # ------- カルマンフィルタで真値を推定 ------- #
        # 可読性を考慮し置き換え
        xx_0, V0, Ft, Gt, Qt, Ht, Rt = self.xx_0, self.V0, self.Ft, self.Gt, self.Qt, self.Ht, self.Rt

        # 観測値を2次元配列に変更
        yy = np.array([ [x_obse[idx], y_obse[idx]] for idx in range(len(x_obse)) ])
        #xx_0[0], xx_0[3] = yy[0,0], yy[0, 1]  # 時刻0の観測値を状態変数の初期値としてもよい

        # カルマンフィルタ
        # xx->各時刻の推定値 ll->尤度（対数尤度関数）
        xx, ll= self.kalman_filter(len(t), xx_0, V0, Ft, Gt, Qt, yy, Ht, Rt)

        print('Likelihood: ' + str(ll))
        self.xx, self.yy = xx, yy  # デバッグ用

        # - グラフ確認 - #
        # figとaxes用意
        fig_kf = plt.figure(figsize=(12.8, 4.8), tight_layout=True)
        gs = fig_kf.add_gridspec(2, 2)
        ax_xy = fig_kf.add_subplot(gs[:, 0])
        ax_y = fig_kf.add_subplot(gs[0, 1])
        ax_x = fig_kf.add_subplot(gs[1, 1])
        # xy座標
        ax_xy.plot(x_obse, y_obse, marker='^', lw=0, label='observed')
        ax_xy.plot(x_true, y_true, label='true')
        ax_xy.plot(x_no_air, y_no_air, label='no air')
        ax_xy.plot(xx[:, 0], xx[:, 3], marker='.', lw=0, label='kalman filter')
        ax_xy.set_title('Likelihood: ' + str(ll))
        ax_xy.set_xlabel('x[m]')
        ax_xy.set_ylabel('y[m]')
        ax_xy.legend()
        ax_xy.grid(b=True)
        # x軸y軸それぞれ
        ax_x.plot(x_obse, label='x_observed', color='tab:blue')
        ax_x.plot(xx[:, 0], label='x_kalmanfiler', color='tab:red')
        ax_x.set_title('Likelihood: ' + str(ll))
        ax_x.set_ylabel('x[m]')
        ax_x.legend()
        ax_x.grid(b=True)
        ax_y.plot(y_obse, label='y_observed', color='tab:blue')
        ax_y.plot(xx[:, 3], label='y_kalmanfiler', color='tab:red')
        ax_y.set_ylabel('y[m]')
        ax_y.legend()
        ax_y.grid(b=True)
 
        # プロット
        plt.show()

        return


    def kalman_filter(self, num, x0, V0, Ft, Gt, Qt, y, Ht, Rt):
        """
        カルマンフィルタ
        """
        # 初期値
        x = x0.copy()  # 状態変数の初期値
        V = V0.copy()  # 状態変数の初期値の分散共分散行列

        # 各時刻の状態変数用意
        xx = np.empty((num, len(x)))
        xx[0] = x.copy()

        # 対数尤度関数
        ll2 = 0.0  # 第2項
        ll3 = 0.0  # 第3項

        # カルマンフィルタの操作
        for i in range(num-1):
            # 一期先予測
            x = Ft @ x
            V = Ft @ V @ Ft.T + Gt @ Qt @ Gt.T

            # 尤度途中計算
            e = y[i+1] - Ht @ x
            d = Ht @ V @ Ht.T + Rt
            ll2 += np.log(np.linalg.det(d))
            ll3 += e.T @ np.linalg.inv(d) @ e

            # フィルタ
            Kt = V @ Ht.T @ np.linalg.inv(Ht @ V @ Ht.T + Rt)
            x = x + Kt @ (y[i+1] - Ht @ x)
            V = V - Kt @ Ht @ V

            xx[i+1] = x.copy()

        # 尤度
        ll = -1/2 * (y.shape[1]*num*np.log(2*np.pi) + ll2 + ll3)

        return xx, ll


if __name__ == '__main__':
    proc = ProjectileMotion()
    proc.Process()

上記スクリプトで出力されるグラフは次の通りです。
カルマンフィルタによって空気抵抗なしのモデルと観測値から、空気抵抗ありの真値に近い値をうまく推定できていることが確認できます。

解説

簡単なものから順にスクリプトを解説します。

斜方投射のモデル式の確認

        # ------- 真値、観測値、空気抵抗が無いときの真値を計算 ------- #
        # 可読性を考慮し置き換え
        m, v0, theta, r, g, t = self.m, self.v0, self.theta, self.r, self.g, self.t

        # - 真値 - #
        x_true = m*v0 /r * (1 - np.exp(-r/m * t)) * np.cos(theta)
        y_true = m/r * ((v0*np.sin(theta) + m/r *g) * (1 - np.exp(-r/m *t)) - g*t)

        # - 観測値 - #
        np.random.seed(self.seed)  # 乱数シード設定
        x_obse = x_true + np.random.normal(0, self.s_obse, len(x_true))
        y_obse = y_true + np.random.normal(0, self.s_obse, len(y_true))

        # - 空気抵抗なし - #
        x_no_air = v0*np.cos(theta) * t
        y_no_air = v0*np.sin(theta) * t - 1/2 * g * t**2

        # - グラフ確認 - #
        fig, ax = plt.subplots(tight_layout=True)
        ax.plot(x_obse, y_obse, marker='^', lw=0, label='observed')
        ax.plot(x_true, y_true, label='true')
        ax.plot(x_no_air, y_no_air, label='no air')
        ax.set_xlabel('x[m]')
        ax.set_ylabel('y[m]')
        ax.legend()
        ax.grid(b=True)

上記で斜方投射の空気抵抗ありのモデル（真値）、空気抵抗なしのモデル（シミュレーションモデル）を計算します。なお、観測値は空気抵抗ありのモデルにガウス分布の乱数を加えた値としました。

上記のようにグラフ確認のコメントアウトを外すと、斜方投射の空気抵抗ありとなしのシミュレーション結果を確認できます。

上記グラフより、うまくモデル化できていることがわかります。

参考として以前の記事から、モデル式を以下に抜粋します。スクリプトと見比べてみてください。

空気抵抗ありモデル式（再掲）

$\displaystyle \begin{align} x &= \frac{m v_0}{r} \biggl( 1 - \exp \Bigl( -\frac{r}{m} t \Bigr) \biggr) \cos\theta \\ y &= \frac{m}{r} \biggl\{ \biggl(v_0 \sin\theta + \frac{m}{r} g \biggr) \biggl( 1 - \exp \Bigl( -\frac{r}{m} t \Bigr) \biggr) - gt \biggr\} \end{align}$

空気抵抗なしモデル式抜粋（再掲）

$\displaystyle \begin{align} x_t &= v_0 \cos\theta \ t \\ y_t &= v_0 \sin\theta \ t - \frac{1}{2} g t^2 \end{align}$

観測モデル

        # - 観測モデル - #
        # 観測行列※ 観測できるのはx座標とy座標のみ
        self.Ht = np.array([[1, 0, 0, 0, 0, 0],
                            [0, 0, 0, 1, 0, 0]])

        # 観測ノイズ
        self.Rt = np.array([[3, 0],
                            [0, 3]])

観測できるのはx座標とy座標だけとし、観測ノイズは対角成分に3を入れました。一般に、この値は使用するセンサのデータシートから精度（誤差）を確認し、それよりも大きい値を設定します。なぜなら、観測ノイズはセンサ誤差だけでなく、表現誤差を含む値を表すためです。

ところで、一見観測ノイズは測定誤差であるように見えるが、それは間違いである。なぜなら、もし観測ノイズが測定誤差と一致するのであれば、このモデル式がセンサ周りの自然現象を完璧に表現できることになってしまい、現実にはありえない再現性を有していることになってしまう。したがって、観測ノイズは測定誤差以外の表現誤差を含めて設定する必要がある。言い換えれば、観測ノイズ＝測定誤差＋表現誤差とも言える。
データ同化｜ざっくり解説 - ari23のブログ

また、観測ノイズだけでなく、状態変数の誤差分散行列の初期値やシステムノイズの非対角成分は普通値を設定できないので、ゼロを入れます。¹

参考として、以前の記事からHtを設定した観測モデルを以下に抜粋します。

観測モデル（観測ノイズ省略）（再掲）

$\displaystyle \begin{align} \boldsymbol{y}_t &= H_t \boldsymbol{x}_{t} \\ \Leftrightarrow \left[ \begin{array}{c} x_t \\ y_t \\ \end{array} \right] &= \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ \end{bmatrix} \left[ \begin{array}{c} x_t \\ \dot{x}_t \\ \ddot{x}_t \\ y_t \\ \dot{y}_t \\ \ddot{y}_t \\ \end{array} \right] \end{align}$

状態変数と状態変数の分散共分散行列

        # - 初期状態 - #
        # 状態変数 [x座標, x軸速度, x軸加速度, y座標, y軸速度, y軸加速度]
        self.xx_0 = np.array([0, v0*np.cos(theta), 0, 0, v0*np.sin(theta), -g])

        # 状態変数の誤差分散共分散行列
        self.V0 = np.array([[1, 0, 0, 0, 0, 0],
                            [0, 1, 0, 0, 0, 0],
                            [0, 0, 1, 0, 0, 0],
                            [0, 0, 0, 1, 0, 0],
                            [0, 0, 0, 0, 1, 0],
                            [0, 0, 0, 0, 0, 1]])

状態変数の座標系は前回の記事で設定した内容を踏まえています。

今回は初期座標がわかっている状況を想定したため、初期のxy座標を(0, 0)としましたが、現実にはわからないことが多いと思います。その場合は、最初に観測した値をそのまま初期値として入れてしまえばよいです（以下のコメントアウトを外せばOKです）。

        #xx_0[0], xx_0[3] = yy[0,0], yy[0, 1]  # 時刻0の観測値を状態変数の初期値としてもよい

次に状態変数の誤差分散共分散行列の値ですが、対角成分の値はかなりエイヤッで決めてしまいます（もちろん、非対角成分はゼロ）。というのも、初期値の誤差を調べる方法そのものが困難であり、またテキトーな値を設定しても時刻が進むにつれてモデルが観測値に馴染んでいくので、そこまで大きな問題にはなりません。

観測ノイズと同じように、少し大きな値を入れておいて、気になるようであれば、のちの尤度の計算結果から調整してあげれば十分です。ちょっと気持ち悪い感じもしますが、この曖昧さを許容しているところがデータ同化らしいと言えますね。

予測とフィルタと尤度

        # カルマンフィルタの操作
        for i in range(num-1):
            # 一期先予測
            x = Ft @ x
            V = Ft @ V @ Ft.T + Gt @ Qt @ Gt.T

            # 尤度途中計算
            e = y[i+1] - Ht @ x
            d = Ht @ V @ Ht.T + Rt
            ll2 += np.log(np.linalg.det(d))
            ll3 += e.T @ np.linalg.inv(d) @ e

            # フィルタ
            Kt = V @ Ht.T @ np.linalg.inv(Ht @ V @ Ht.T + Rt)
            x = x + Kt @ (y[i+1] - Ht @ x)
            V = V - Kt @ Ht @ V

            xx[i+1] = x.copy()

        # 尤度
        ll = -1/2 * (y.shape[1]*num*np.log(2*np.pi) + ll2 + ll3)

カルマンフィルタの操作はkalman_filter()関数で処理しています。
詳細は以前の記事を参考にしてください。以下に一部抜粋します。

カルマンフィルタの操作（再掲）

＜一期先予測＞

$\displaystyle \begin{align} \boldsymbol{x}_{t|t-1} &= F_t \boldsymbol{x}_{t-1|t-1} \\ V_{t|t-1} &= F_t V_{t-1|t-1} {F_t}^{\prime} + G_t Q_t {G_t}^{\prime} \end{align}$

＜フィルタ＞

$\displaystyle \begin{align} \boldsymbol{e}_t &= \boldsymbol{y}_t - H_t \boldsymbol{x}_{t|t-1} \\ D_{t|t-1} &= H_t V_{t|t-1} {H_t}^{\prime} + R_t \\ K_t &= V_{t|t-1} {H_t}^{\prime} {D_{t|t-1}}^{-1} \\ \boldsymbol{x}_{t|t} &= \boldsymbol{x}_{t|t-1} + K_t \boldsymbol{e}_t \\ V_{t|t} &= V_{t|t-1} - K_t H_t V_{t|t-1} \end{align}$

また、カルマンフィルタ操作で、尤度（対数尤度関数）で使う値も計算するので、一緒に尤度も計算しています。

尤度計算（再掲）

$\displaystyle l(\boldsymbol{\theta}) = -\frac{T\ell}{2} \log 2\pi - \frac{1}{2} \sum_{t=1}^{T} \log |D_{t|t-1}| - \frac{1}{2} \sum_{t=1}^{T} {\boldsymbol{e}_t}^{\prime} {D_{t|t-1}}^{-1} {\boldsymbol{e}_t}$

forループ内で、対数尤度関数の第2項と第3項を計算し、ループが終わったあとで残りの第1項を計算したのち、すべて加算しています。

ちなみに、上記のフィルタ操作をすべてコメントアウトすると、以下のような空気抵抗なし、つまりシミュレーションモデルの計算結果が得られます。

システムモデル

        # - システムモデル - #
        # システム行列※または遷移行列
        self.Ft = np.array([[1, dt, 0, 0,  0,       0],
                            [0,  1, 0, 0,  0,       0],
                            [0,  0, 1, 0,  0,       0],
                            [0,  0, 0, 1, dt, dt**2/2],
                            [0,  0, 0, 0,  1,      dt],
                            [0,  0, 0, 0,  0,       1]])

        # システムノイズ行列※
        self.Gt = np.ones_like(self.Ft)

        # システムノイズ
        self.Qt = np.array([[.01,   0,   0,   0,   0,   0],
                            [  0, .01,   0,   0,   0,   0],
                            [  0,   0, .01,   0,   0,   0],
                            [  0,   0,   0, .01,   0,   0],
                            [  0,   0,   0,   0, .01,   0],
                            [  0,   0,   0,   0,   0, .01]])

Ftは、以前の記事で設定済みです。

システムモデル（システムノイズ省略）（再掲）

$\displaystyle \begin{align} \boldsymbol{x}_t &= F_t \boldsymbol{x}_{t-1} \\ \Leftrightarrow \left[ \begin{array}{c} x_t \\ \dot{x}_t \\ \ddot{x}_t \\ y_t \\ \dot{y}_t \\ \ddot{y}_t \\ \end{array} \right] &= \begin{bmatrix} 1 & \Delta t & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & \Delta t & \frac{(\Delta t)^2}{2} \\ 0 & 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix} \left[ \begin{array}{c} x_{t-1} \\ \dot{x}_{t-1} \\ \ddot{x}_{t-1} \\ y_{t-1} \\ \dot{y}_{t-1} \\ \ddot{y}_{t-1} \\ \end{array} \right] \end{align}$

また、Gtは設定したシステムノイズがそのまま状態変数に反映されるように、FtやQtと同じサイズの単位行列としています。

そして、問題のシステムノイズQtです。
非対角成分はゼロで問題ありませんが、対角成分については尤度の値を見ながら設定するしかありません。今回設定した値は、わたしが何度か試した中でもっとも尤度の値が大きいものです。forループを回して全探索のようにしたわけではなく、計算結果にもすこしブレがあることから、もっとよい設定値があると思います。

ただし、あまり細かく設定しても実用上問題なければ十分なので、ほどほどにしておけばよいのかなと考えています。

パラメタ設定例

ここでは、私が実際に行ったパラメタ設定の例を記します。数回しかシミュレーションしていないので、本来はきちんと吟味するべきですので、あくまで例として捉えてください。

調整記録

とりあえず、システムノイズをすべてゼロにして様子を見ます。すると、推定値が空気抵抗なしの場合とほぼ同じ軌道になる結果を得ました。
ただし、実はこのときV0をすべてゼロするミスを犯していて、この事実はあとで気づくことにorz

        # システムノイズ
        self.Qt = np.array([[  0,   0,   0,   0,   0,   0],
                            [  0,   0,   0,   0,   0,   0],
                            [  0,   0,   0,   0,   0,   0],
                            [  0,   0,   0,   0,   0,   0],
                            [  0,   0,   0,   0,   0,   0],
                            [  0,   0,   0,   0,   0,   0]])

xy座標のシステムノイズをそれぞれ1として計算してみると以下のような結果を得ました。
終盤での誤差がかなり大きい様子がわかります。

        # システムノイズ
        self.Qt = np.array([[  1,   0,   0,   0,   0,   0],
                            [  0,   0,   0,   0,   0,   0],
                            [  0,   0,   0,   0,   0,   0],
                            [  0,   0,   0,   1,   0,   0],
                            [  0,   0,   0,   0,   0,   0],
                            [  0,   0,   0,   0,   0,   0]])

速度や加速度にノイズいれてないのはおかしいよなと思い、対角成分にすべて1を入れてみました。あまり変わらないどころか、むしろ尤度の値は小さくなり悪化してしまいました。

        # システムノイズ
        self.Qt = np.array([[  1,   0,   0,   0,   0,   0],
                            [  0,   1,   0,   0,   0,   0],
                            [  0,   0,   1,   0,   0,   0],
                            [  0,   0,   0,   1,   0,   0],
                            [  0,   0,   0,   0,   1,   0],
                            [  0,   0,   0,   0,   0,   1]])

ここで初めて状態変数の分散共分散行列の初期値V0がすべてゼロであることに気づき、対角成分に1を入れました。
やっとそれっぽくなってきましたね。

        # 状態変数の誤差分散共分散行列
        self.V0 = np.array([[1, 0, 0, 0, 0, 0],
                            [0, 1, 0, 0, 0, 0],
                            [0, 0, 1, 0, 0, 0],
                            [0, 0, 0, 1, 0, 0],
                            [0, 0, 0, 0, 1, 0],
                            [0, 0, 0, 0, 0, 1]])

        # システムノイズ
        self.Qt = np.array([[  1,   0,   0,   0,   0,   0],
                            [  0,   1,   0,   0,   0,   0],
                            [  0,   0,   1,   0,   0,   0],
                            [  0,   0,   0,   1,   0,   0],
                            [  0,   0,   0,   0,   1,   0],
                            [  0,   0,   0,   0,   0,   1]])

試しに対角成分にすべて2を入れてみます。
尤度がさらに小さくなり、悪化したのでこれは却下。

        # システムノイズ
        self.Qt = np.array([[  2,   0,   0,   0,   0,   0],
                            [  0,   2,   0,   0,   0,   0],
                            [  0,   0,   2,   0,   0,   0],
                            [  0,   0,   0,   2,   0,   0],
                            [  0,   0,   0,   0,   2,   0],
                            [  0,   0,   0,   0,   0,   2]])

時刻の刻み幅は0.01[s]しかないのに、xy座標が最大1[m]ずれるのは大きすぎると思い、対角成分をすべて0.1にしました。
尤度の値が大きくなり、良くなったが、もう少しいけそう。

        self.Qt = np.array([[ .1,   0,   0,   0,   0,   0],
                            [  0,  .1,   0,   0,   0,   0],
                            [  0,   0,  .1,   0,   0,   0],
                            [  0,   0,   0,  .1,   0,   0],
                            [  0,   0,   0,   0,  .1,   0],
                            [  0,   0,   0,   0,   0,  .1]])

さらに小さく0.01にしてみました。
軌道がすこしぶれている箇所もありますが、今回は要求仕様もないので、ここまでとしました。

        self.Qt = np.array([[.01,   0,   0,   0,   0,   0],
                            [  0, .01,   0,   0,   0,   0],
                            [  0,   0, .01,   0,   0,   0],
                            [  0,   0,   0, .01,   0,   0],
                            [  0,   0,   0,   0, .01,   0],
                            [  0,   0,   0,   0,   0, .01]])

おわりに

カルマンフィルタ3回目の記事にして、やっとプログラムまで出すことができました。

2次元の斜方投射または放物線運動を例としたカルマンフィルタの記事は、私が調べた中ではなかったので、今回取り上げました。
また、下手くそですが、私がパラメタを調整した記録も残しました。初めてカルマンフィルタに触れる方は、ぜひ自分で手を動かしてパラメタ調整をやってみてください。

参考になれば幸いです(^^)

なお、データ同化関連の記事は以下にあります。よろしければ、こちらもご覧ください。

Data assimilation カテゴリーの記事一覧 - ari23の研究ノート

参考文献

参考文献は以下の通りです。

データ同化入門 (予測と発見の科学)
データ同化分野で私にとってのバイブルです。決して簡単ではないですが、数学的議論がかなりきちんとなされています。データ同化に取り組むときは、まずこれを読んでいます。
カルマンフィルタの基礎
工学の視点からカルマンフィルタが丁寧に説明されています。カルマンフィルタならこれがおすすめです。
時系列解析の方法 (統計科学選書)
カルマンフィルタはもちろん、タイトル通り時系列解析の手法が解説されています。比較的読みやすいです。

リンク

非対角成分にゼロ以外を入れる状況が想像できませんが、もしかしてあるのかしら？↩