from KF_V2 import *
# ======================

sensor_id_list = ['AUV01','AUV02']

sensor_data = []
sensor_data.append({
    'data_matrix': np.array([[0, 0, 0], [100, 100, 100]]),
    'id_list': ['001','002'],
    'deviceId': 'AUV01',
    'devicePs':[0.2], #第一个值表示测量误差
    'latest_time': [0],
    'priority':1
})
sensor_data.append({
    'data_matrix': np.array([[0, 0, 0], [100, 100, 100]]),
    'id_list': ['003','004'],
    'deviceId': 'AUV02',
    'deivceProperties':[0.2],
    'latest_time': [0],
    'priority':100
})


fusion_container = DataFusion(25,1,3)
for i in range(15):
    print(i)
    # 在循环开始时，对 sensor_data 中的 data_matrix 进行修改
    if i%5 == 0:
        temp = {
            'data_matrix': np.array([]),
            'id_list': [],
            'deviceId': 'AUV01',
            'devicePs': [0.2],  # 第一个值表示测量误差
            'latest_time': [0]
        }
        c_sensor_data = []
        c_sensor_data.append(temp)
        c_sensor_data.append(temp)
        filted_results = fusion_container.run(c_sensor_data)
    else:
        sensor_data[0]['data_matrix'][0, :] += 1  # 第一行每个元素加1
        sensor_data[0]['data_matrix'][1, :] -= 1  # 第二行每个元素减1
        sensor_data[1]['data_matrix'][0, :] += 1  # 第一行每个元素加1
        sensor_data[1]['data_matrix'][1, :] -= 1  # 第二行每个元素减1
        filted_results = fusion_container.run(sensor_data)




    print("results:\n")
    for d in filted_results:
        print(d)