# coding:utf-8
# カルマンフィルタの実装

import numpy as np


class KalmanFilter(object):
    def __init__(self, dim_x, dim_z):
        self.z = None  # 状態
        self.F = None  # 遷移行列
        self.H = None  # 観測行列

        self.Q = None  # プロセスノイズの分散共分散行列
        self.R = None  # 観測ノイズの分散共分散行列

        self.K = None  # カルマンゲイン
        self.m = None  # 状態推定値
        self.P = None  # 推定誤差分散共分散行列

    def _update(self, observerd_data):
        """
        観測されたデータに応じて、状態と推定共分散行列を更新する
        :param observerd_data (1d ndarray): 観測値
        :return: 状態の確率密度関数の平均値
        """

        x = observerd_data
        F, H = self.F, self.H
        Q, R = self.Q, self.R
        m = np.copy(self.m)
        P = np.copy(self.P)
        # prediction step
        m = F @ m
        P = F @ P @ F.T + Q
        # update step
        S = H @ P @ H.T + R
        K = P @ H.T @ np.linalg.inv(S)
        m = m + K @ (x - H @ m)
        P = P - K @ S @ K.T
        # update parameter
        self.m = m
        self.P = P
        return self.m


if __name__ == "__main__":
    dt = 10 ** -3
    sigma1 = 0.5
    sigma2 = 0.5
    q1 = 0.5
    q2 = 0.5
    # transition matrix
    A = np.array([[1, 0, dt, 0],
                  [0, 1, 0, dt],
                  [0, 0, 1, 0],
                  [0, 0, 0, 1]])
    # process noise covariance
    Q = np.array([[q1 * dt ** 3 / 3, 0, q1 * dt ** 2 / 2, 0],
                  [0, q1 * dt ** 3 / 3, 0, q2 * dt ** 2 / 2],
                  [q1 * dt ** 2 / 2, 0, q1 * dt, 0],
                  [0, q2 * dt ** 2 / 2, 0, q2 * dt]])

    # observe matrix
    H = np.array([[1, 0, 0, 0],
                  [0, 1, 0, 0]])
    # measurement noise covariance
    R = np.array([[sigma1 ** 2, 0],
                  [0, sigma2 ** 2]])

    m = np.random.normal(size=4)
    P = np.eye(4)

    # set parameter
    kf = KalmanFilter(4, 2)

    kf.F = A
    kf.H = H

    kf.Q = Q
    kf.R = R

    kf.m = m
    kf.P = P

    trajectory = np.random.normal(size=[1000, 2])
    state_list = []
    var_list = []

    for y in trajectory:
        state_list.append(np.copy(m))
        var_list.append(np.array(P[0, 0]))

    X = np.array(state_list)
    v = np.array(var_list)
    print(X)

@@ -42,16 +42,48 @@
 y as np%0A
+import matplotlib.pyplot as plt%0A
 %0A%0Aclass 
@@ -146,36 +146,8 @@
 z):%0A
-        self.z = None  # %E7%8A%B6%E6%85%8B%0A
     
@@ -394,17 +394,16 @@
     def 
-_
 update(s
@@ -551,16 +551,19 @@
 %E5%AF%86%E5%BA%A6%E9%96%A2%E6%95%B0%E3%81%AE%E5%B9%B3%E5%9D%87%E5%80%A4
+%E3%81%A8%E5%88%86%E6%95%A3
 %0A       
@@ -1018,16 +1018,24 @@
 n self.m
+, self.P
 %0A%0A%0Aif __
@@ -1072,17 +1072,17 @@
  10 ** -
-3
+1
 %0A    sig
@@ -1285,24 +1285,25 @@
  0, 0, 1%5D%5D)%0A
+%0A
     # proces
@@ -1798,16 +1798,21 @@
 p.eye(4)
+ * 10
 %0A%0A    # 
@@ -1944,51 +1944,152 @@
    t
-rajectory = np.random.normal(size=%5B1000, 2%5D
+ = np.linspace(0, 30, 1000)%0A    trajectory = np.c_%5Bnp.cos(t) + t, np.sin(t) * t%5D%0A%0A    trajectory += np.random.normal(0, 1, size=trajectory.shape
 )%0A  
@@ -2150,16 +2150,44 @@
 ectory:%0A
+        m, P = kf.update(y)%0A
         
@@ -2243,20 +2243,19 @@
 pend(np.
-arra
+cop
 y(P%5B0, 0
@@ -2323,13 +2323,245 @@
     
-print(X
+%0A    plt.scatter(X%5B:, 0%5D, X%5B:, 1%5D, s=v ** 0.5 * 10, alpha=0.25)%0A%0A    plt.plot(trajectory%5B:, 0%5D, trajectory%5B:, 1%5D, %22bo%22, label=%22observerd%22)%0A    plt.plot(X%5B:, 0%5D, X%5B:, 1%5D, %22r-%22, linewidth=5, label=%22estimated%22)%0A    plt.legend()%0A%0A    plt.show(
 )%0A

add test code