# -*- coding: utf-8 -*-
"""Copyright 2014 Roger R Labbe Jr.

bayesian_filters library.
https://github.com/GeorgePearse/bayesian_filters

Documentation at:
https://georgepearse.github.io/bayesian_filters

Supporting book at:
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

This is licensed under an MIT license. See the readme.MD file
for more information.
"""

from __future__ import absolute_import, division, print_function, unicode_literals

from numpy import array
import matplotlib.pyplot as plt
from bayesian_filters.hinfinity import HInfinityFilter


def test_Hinfinity():
    dt = 0.1
    f = HInfinityFilter(2, 1, 0, gamma=0.4)

    f.F = array([[1.0, dt], [0.0, 1.0]])

    f.H = array([[0.0, 1.0]])
    f.x = array([[0.0, 0.0]]).T
    # f.G = array([[dt**2 / 2, dt]]).T

    f.P = 0.01
    f.W = array([[0.0003, 0.005], [0.0050, 0.100]]) / 1000

    f.V = 0.01
    f.Q = 0.01

    xs = []
    vs = []

    for i in range(1, 40):
        f.update(5)
        print(f.x.T)
        xs.append(f.x[0, 0])
        vs.append(f.x[1, 0])
        f.predict()

    plt.subplot(211)
    plt.plot(xs)
    plt.subplot(212)
    plt.plot(vs)


if __name__ == "__main__":
    test_Hinfinity()
