Qiita Teams that are logged in
You are not logged in to any team

Log in to Qiita Team
Community
OrganizationEventAdvent CalendarQiitadon (β)
Service
Qiita JobsQiita ZineQiita Blog
20
Help us understand the problem. What are the problem?

More than 3 years have passed since last update.

@tttamaki

tensorflowでカルマンフィルタ

Tensorflowでカルマンフィルタを実装してみた.

  • 計算グラフで1ループを実装
  • session.runで呼び出し
  • tensorflowの内部で変数を覚えておく
  • Tips:
    • assignで内部変数に保存する
    • assignの返り値のoperatorをsess.runで呼び出す

numpyバージョン v.s. tensorflowバージョン.

コード

kalman.py
import numpy as np
import matplotlib.pyplot as plt
import time
import tensorflow as tf


def KalmanFilter_tf(T, x_obs, mu0, V0, A, b, Q, R):
    """
    tensorflowで実装したカルマンフィルタ.
    """

    x_predict = np.zeros((T, 2))
    x_filter = np.zeros((T, 2))
    V_predict = np.zeros((T, 2, 2))
    V_filter = np.zeros((T, 2, 2))

    x_predict[0] = mu0
    x_filter[0] = mu0
    V_predict[0] = V0
    V_filter[0] = V0

    g = tf.Graph()

    with g.as_default():  # 計算グラフ準備

        obs_ = tf.placeholder(tf.float32, name="obs", shape=(2, 1))

        A_ = tf.placeholder(tf.float32, name="A", shape=(2, 2))
        b_ = tf.placeholder(tf.float32, name="b", shape=(2, 1))
        Q_ = tf.placeholder(tf.float32, name="Q", shape=(2, 2))
        R_ = tf.placeholder(tf.float32, name="R", shape=(2, 2))

        mu0_ = tf.placeholder(tf.float32, name="mu0", shape=(2, 1))
        V0_ = tf.placeholder(tf.float32, name="V0", shape=(2, 2))

        mu = tf.Variable(tf.zeros((2, 1)), dtype=tf.float32, name="mu")
        V = tf.Variable(tf.zeros((2, 2)), dtype=tf.float32, name="V")

        mu0_init = tf.assign(mu, mu0_)
        V0_init = tf.assign(V, V0_)

        mu_ = A_ @ mu + b_
        V_ = A_ @ V @ tf.transpose(A_) + Q_

        S = V_ + R_
        K = V_ @ tf.matrix_inverse(S)

        # 内部変数で保存しておく
        mu_op = tf.assign(mu, mu_ + K @ (obs_ - mu_))
        V_op = tf.assign(V, V_ - K @ V_)

    with tf.Session(graph=g) as sess:

        # 初期化
        m, v = sess.run([mu0_init, V0_init],
                        feed_dict={mu0_: mu0.reshape((2, 1)), V0_: V0})

        start = time.time()

        for t in range(1, T):

            # 各時刻tでsess.runで呼び出す
            m, v = sess.run([mu_op, V_op],
                            feed_dict={A_: A, b_: b.reshape((2, 1)),
                                       Q_: Q, R_: R,
                                       obs_: x_obs[t].reshape((2, 1))
                                       })

            x_filter[t], V_filter[t] = m.transpose(), v

        elapsed_time = time.time() - start
        print("tensorflow: ", elapsed_time)

    return None, None, x_filter, V_filter


def KalmanFilter(T, x_obs, mu0, V0, A, b, Q, R):
    """
    こちらは普通のカルマンフィルタ.np実装.
    """

    mu = mu0
    V = V0

    x_predict = np.zeros((T, 2))
    x_filter = np.zeros((T, 2))
    V_predict = np.zeros((T, 2, 2))
    V_filter = np.zeros((T, 2, 2))

    x_predict[0] = mu.transpose()
    x_filter[0] = mu
    V_predict[0] = V
    V_filter[0] = V

    start = time.time()

    for t in range(1, T):

        mu_ = A @ mu + b
        V_ = A @ V @ A.transpose() + Q

        x_predict[t] = mu_
        V_predict[t] = V_

        S = V_ + R
        K = V_ @ np.linalg.inv(S)

        mu = mu_ + K @ (x_obs[t] - mu_)
        V = V_ - K @ V_

        x_filter[t] = mu
        V_filter[t] = V

    elapsed_time = time.time() - start
    print("numpy:      ", elapsed_time)

    return x_predict, V_predict, x_filter, V_filter


def main():

    # データ作成
    T = 20
    mu0 = np.array([100, 100])
    V0 = np.array([[10, 0],
                   [0, 10]])

    A = np.array([[1.001, 0.001],
                  [0, 0.99]])
    b = np.array([5, 10])
    Q = np.array([[20, 0],
                  [0, 20]])
    R = np.array([[20, 0],
                  [0, 20]])

    rvq = np.random.multivariate_normal(np.zeros(2), Q, T)
    rvr = np.random.multivariate_normal(np.zeros(2), R, T)
    obs = np.zeros((T, 2))
    obs[0] = mu0
    for i in range(1, T):
        obs[i] = A @ obs[i-1] + b + rvq[i] + rvr[i]
    # 作成終わり

    x_predict, V_predict, x_filter, V_filter = \
        KalmanFilter(T, obs, mu0, V0, A, b, Q, R)
    print(x_filter)

    x_predict_tf, V_predict_tf, x_filter_tf, V_filter_tf = \
        KalmanFilter_tf(T, obs, mu0, V0, A, b, Q, R)
    print(x_filter_tf)

    fig = plt.figure(figsize=(16, 9))
    ax = fig.gca()

    ax.scatter(obs[:, 0], obs[:, 1],
               s=10, alpha=1, marker="o", color='w', edgecolor='k')
    ax.plot(obs[:, 0], obs[:, 1],
            alpha=0.5, lw=1, color='k')

    ax.scatter(x_filter[:, 0], x_filter[:, 1],
               s=10, alpha=1, marker="o", color='r')
    ax.plot(x_filter[:, 0], x_filter[:, 1],
            alpha=0.5, lw=1, color='r')

    ax.scatter(x_filter_tf[:, 0], x_filter_tf[:, 1],
               s=10, alpha=1, marker="o", color='m')
    ax.plot(x_filter_tf[:, 0], x_filter_tf[:, 1],
            alpha=0.5, lw=1, color='m')

    plt.show()


if __name__ == "__main__":
    main()

結果

on Mac

当然,それほど速くない.(on macos, CPU tf 1.4.1, anaconda python 3.6)

numpy:       0.0004937648773193359
tensorflow:  0.00774383544921875

on Ubuntu

GPUならもっと遅い.(Ubuntu 16.04, GPU tf 1.6.0, Geforce 1080Ti, anaconda python 3.6)

numpy:       0.002942800521850586
(...tensorflow messages...)
tensorflow:  0.6563735008239746

(Ubuntu 16.04, CPU tf 1.6.0, anaconda python 3.6)

numpy:       0.002834796905517578
tensorflow:  0.027823209762573242
Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
20
Help us understand the problem. What are the problem?