53 lines
1.5 KiB
Python
53 lines
1.5 KiB
Python
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) |