import numpy as np
from pykalman import KalmanFilter
# import matplotlib.pyplot as plt
# kf = KalmanFilter(transition_matrices=np.array([[1,0], [0,1]]),
#                   observation_matrices =np.array([[1,0],[0,1]]),
#                   transition_covariance= 0.2*np.eye(2))
def kalman_calc(kf,x,y,circle_count_flag):
    global filtered_state_means0,filtered_state_covariances0,lmx,lmy,lpx,lpy
    current_measurement = np.array([np.float32(x),np.float32(y)])
    cmx, cmy = current_measurement[0], current_measurement[1]
    # print(current_measurement)
    if circle_count_flag ==1:
        filtered_state_means0=np.array([x,y])
        filtered_state_covariances0=np.eye(2)
        lmx, lmy = x, y#上次测量位置
        lpx, lpy = x, y#上次预测位置

 
    filtered_state_means, filtered_state_covariances= (kf.filter_update( filtered_state_means0,filtered_state_covariances0,current_measurement))
    cpx,cpy= filtered_state_means[0], filtered_state_means[1]
    
    filtered_state_means0, filtered_state_covariances0=filtered_state_means, filtered_state_covariances
    lpx, lpy = filtered_state_means[0], filtered_state_means[1]
    lmx, lmy =current_measurement[0], current_measurement[1]
    return cpx,cpy

# For test and show.
# def func(x, a, b, c):
#     return a  * x  + b

# xdata = np.linspace(0,1,20)
# y = func(xdata,3,2,1)
# error = 0.3
# y_noise = error*np.random.randn(len(xdata))
# y_data = y + y_noise

# z = func(xdata,5,3,4)
# error = 0.3
# z_noise = error*np.random.randn(len(xdata))
# z_data = z + z_noise

# circle_count_flag = 0

# x_predict = []
# y_predict = []


# for idx in range(len(xdata)):
#     circle_count_flag += 1
#     cpx,cpy = kalman_calc(z[idx],y[idx],circle_count_flag)
#     x_predict.append(cpx)
#     y_predict.append(cpy)
# plt.figure('data')
# plt.plot(z_data,y_data,'b+')
# plt.plot(x_predict,y_predict,'ro')
# plt.show()