# import numpy as np

# measurements = np.array([5., 6., 7., 9., 10.])
# motion = np.array([1., 1., 2., 1., 1.])
# measurement_sigma = 4.
# motion_sigma = 2.
# mu = 0.
# sigma = 1000.

# # Measurement
# def Update( mean1, var1, mean2, var2 ):
#     mean = (var2*mean1 + var1*mean2) / (var1 + var2)
#     var = 1.0 / (1.0/var1 + 1.0/var2)
#     return [mean, var]

# # Motion
# def Predict( mean1, var1, U, varU ):
#     mean = mean1 + U
#     var = var1 + varU
#     return [mean, var]

# for n in range(len(measurements)):
#     [mu, sigma] = Update(mu, sigma, measurements[n], measurement_sigma)
#     print('Update : ', n, [mu, sigma])
#     [mu, sigma] = Predict(mu, sigma, motion[n],motion_sigma)
#     print('Predict: ', n, [mu, sigma])

# print(' ')
# print(Update(1,1,3,1))

# -------------------------------------------------------

import numpy as np

measurements = [ 1., 2., 3. ]
dt = 1.

# Initial state (location and velocity)
x = np.array([[ 0. ],
              [ 0. ]])
# Initial uncertainty
P = np.array([[ 1000.,    0. ],
              [    0., 1000. ]])
# External motion
U = np.array([[ 0. ],
              [ 0. ]]) 
# Next state function
F = np.array([[ 1., dt ], 
              [ 0., 1. ]])
# Measurement function
H = np.array([[ 1., 0. ]])
# Measurement uncertainty
R = np.array([[ 1. ]])
# Identity matrix
I = np.eye(2)


def filter(x, P):

    step = 0
    for z in (measurements):
        step += 1
        print("step = ", step, "  meas. = ", z)
        
        # Measurement
        Htra = H.T
        S = H.dot(P.dot(Htra)) + R
        Sinv = np.linalg.inv(S)
        K = P.dot(Htra.dot(Sinv))
        y = z - H.dot(x)
        xp = x +K.dot(y)
        Pp = P - K.dot(H.dot(P))

        # Prediction
        x = F.dot(xp) + U
        Ftra = F.T
        P = F.dot(Pp.dot(Ftra))

        print('x =')
        print(x)
        print('P =')
        print(P)

filter(x, P)

# # -------------------------------------------------------

# import numpy as np

# # x0 = 4.
# # y0 = 12.
# # measurements = np.array([[  5., 10. ],
# #                          [  6.,  8. ],
# #                          [  7.,  6. ],
# #                          [  8.,  4. ],
# #                          [  9.,  2. ],
# #                          [ 10.,  0. ]])
# # x0 = -4.
# # y0 = 8.
# # measurements = np.array([[  1.,  4. ],
# #                          [  6.,  0. ],
# #                          [ 11., -4. ],
# #                          [ 16., -8. ]])
# # x0 = 1.
# # y0 = 19.
# # measurements = np.array([[  1., 17. ],
# #                          [  1., 15. ],
# #                          [  1., 13. ],
# #                          [  1., 11. ]])
# x0 = 1.
# y0 = 19.
# measurements = np.array([[  2., 17. ],
#                          [  0., 15. ],
#                          [  2., 13. ],
#                          [  0., 11. ]])
# # Time step                         
# dt = 0.1
# # Initial state (location and velocity)
# x = np.array([[ x0 ],
#               [ y0 ],
#               [  0. ],
#               [  0. ]])
# # Initial uncertainty
# P = np.array([[ 0., 0.,    0.,    0. ],
#               [ 0., 0.,    0.,    0. ],
#               [ 0., 0., 1000.,    0. ],
#               [ 0., 0.,    0., 1000. ]])
# # External motion
# U = np.array([[ 0. ],
#               [ 0. ],
#               [ 0. ], 
#               [ 0. ]]) 
# # Next state function
# F = np.array([[ 1., 0., dt, 0. ], 
#               [ 0., 1., 0., dt ],
#               [ 0., 0., 1., 0. ],
#               [ 0., 0., 0., 1. ]])
# # Measurement function
# H = np.array([[ 1., 0., 0., 0. ],
#               [ 0., 1., 0., 0. ]])
# # Measurement uncertainty
# R = np.array([[ 0.1, 0.  ],
#               [ 0. , 0.1 ]])
# # Measurement vector
# z = np.zeros((2,1))


# def filter(x, P):
   
#    for n in range(len(measurements)):

#       z[0][0] = measurements[n][0]
#       z[1][0] = measurements[n][1]

#       # Prediction
#       xp = F.dot(x) + U
#       Ftra = F.T
#       Pp = F.dot(P.dot(Ftra))

#       # Measurement
#       Htra = H.T
#       S = H.dot(Pp.dot(Htra)) + R
#       Sinv = np.linalg.inv(S)
#       K = Pp.dot(Htra.dot(Sinv))
#       y = z - H.dot(xp)
#       x = xp +K.dot(y)
#       P = Pp - K.dot(H.dot(Pp))
#       # print(z)
#       # print('x = ')
#       # print(x)
#       # print('P = ')
#       # print(P)
#       # print(' ')

#    return x, P


# x_final, P_final = filter(x, P)
# print('x = ')
# print(x_final)
# print('P = ')
# print(P_final)