我有一些数据表示从两个不同传感器测量的物体的位置。所以，我需要做传感器融合。更困难的问题是来自每个传感器的数据基本上是随机到达的。我想使用 pykalman 来融合和平滑数据。pykalman 如何处理可变时间戳数据？
数据的简化示例如下所示：
import pandas as pd
data={'time':\
['10:00:00.0','10:00:01.0','10:00:05.2','10:00:07.5','10:00:07.5','10:00:12.0','10:00:12.5']\
,'X':[10,10.1,20.2,25.0,25.1,35.1,35.0],'Y':[20,20.2,41,45,47,75.0,77.2],\
'Sensor':[1,2,1,1,2,1,2]}
df=pd.DataFrame(data,columns=['time','X','Y','Sensor'])
df.time=pd.to_datetime(df.time)
df=df.set_index('time')
和这个：
df
Out[130]:
X Y Sensor
time
2017-12-01 10:00:00.000 10.0 20.0 1
2017-12-01 10:00:01.000 10.1 20.2 2
2017-12-01 10:00:05.200 20.2 41.0 1
2017-12-01 10:00:07.500 25.0 45.0 1
2017-12-01 10:00:07.500 25.1 47.0 2
2017-12-01 10:00:12.000 35.1 75.0 1
2017-12-01 10:00:12.500 35.0 77.2 2
对于传感器融合问题，我认为我可以重塑数据，以便我的位置 X1、Y1、X2、Y2 有一堆缺失值，而不仅仅是 X、Y。（这是相关的： https ://stackoverflow.com/questions/47386426/2-sensor-readings-fusion-yaw-pitch ）
那么我的数据可能如下所示：
df['X1']=df.X[df.Sensor==1]
df['Y1']=df.Y[df.Sensor==1]
df['X2']=df.X[df.Sensor==2]
df['Y2']=df.Y[df.Sensor==2]
df
Out[132]:
X Y Sensor X1 Y1 X2 Y2
time
2017-12-01 10:00:00.000 10.0 20.0 1 10.0 20.0 NaN NaN
2017-12-01 10:00:01.000 10.1 20.2 2 NaN NaN 10.1 20.2
2017-12-01 10:00:05.200 20.2 41.0 1 20.2 41.0 NaN NaN
2017-12-01 10:00:07.500 25.0 45.0 1 25.0 45.0 25.1 47.0
2017-12-01 10:00:07.500 25.1 47.0 2 25.0 45.0 25.1 47.0
2017-12-01 10:00:12.000 35.1 75.0 1 35.1 75.0 NaN NaN
2017-12-01 10:00:12.500 35.0 77.2 2 NaN NaN 35.0 77.2
pykalman 的文档表明它可以处理丢失的数据，但这是正确的吗？
但是，pykalman 的文档并不清楚可变时间问题。文档只是说：
“卡尔曼滤波器和卡尔曼平滑器都能够使用随时间变化的参数。为了使用它，只需沿其第一个轴传入长度为 n_timesteps 的数组：”
>>> transition_offsets = [[-1], [0], [1], [2]]
>>> kf = KalmanFilter(transition_offsets=transition_offsets, n_dim_obs=1)
我找不到任何使用具有可变时间步长的 pykalman Smoother 的示例。因此，任何使用我上述数据的指导、示例甚至示例都会非常有帮助。我没有必要使用 pykalman，但它似乎是一个有用的工具来平滑这些数据。
*****在@Anton 下面添加了其他代码我制作了一个使用平滑功能的有用代码版本。奇怪的是，它似乎以相同的权重对待每一次观察，并且轨迹贯穿每一次。即使，如果传感器方差值之间存在很大差异。我预计在 5.4、5.0 点附近，过滤后的轨迹应该更接近传感器 1 点，因为该点的方差较小。相反，轨迹准确地到达每个点，并大转弯到达那里。
from pykalman import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
# reading data (quick and dirty)
Time=[]
RefX=[]
RefY=[]
Sensor=[]
X=[]
Y=[]
for line in open('data/dataset_01.csv'):
f1, f2, f3, f4, f5, f6 = line.split(';')
Time.append(float(f1))
RefX.append(float(f2))
RefY.append(float(f3))
Sensor.append(float(f4))
X.append(float(f5))
Y.append(float(f6))
# Sensor 1 has a higher precision (max error = 0.1 m)
# Sensor 2 has a lower precision (max error = 0.3 m)
# Variance definition through 3-Sigma rule
Sensor_1_Variance = (0.1/3)**2;
Sensor_2_Variance = (0.3/3)**2;
# Filter Configuration
# time step
dt = Time[2] - Time[1]
# transition_matrix
F = [[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]]
# observation_matrix
H = [[1, 0, 0, 0],
[0, 1, 0, 0]]
# transition_covariance
Q = [[1e-4, 0, 0, 0],
[ 0, 1e-4, 0, 0],
[ 0, 0, 1e-4, 0],
[ 0, 0, 0, 1e-4]]
# observation_covariance
R_1 = [[Sensor_1_Variance, 0],
[0, Sensor_1_Variance]]
R_2 = [[Sensor_2_Variance, 0],
[0, Sensor_2_Variance]]
# initial_state_mean
X0 = [0,
0,
0,
0]
# initial_state_covariance - assumed a bigger uncertainty in initial velocity
P0 = [[ 0, 0, 0, 0],
[ 0, 0, 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]]
n_timesteps = len(Time)
n_dim_state = 4
filtered_state_means = np.zeros((n_timesteps, n_dim_state))
filtered_state_covariances = np.zeros((n_timesteps, n_dim_state, n_dim_state))
import numpy.ma as ma
obs_cov=np.zeros([n_timesteps,2,2])
obs=np.zeros([n_timesteps,2])
for t in range(n_timesteps):
if Sensor[t] == 0:
obs[t]=None
else:
obs[t] = [X[t], Y[t]]
if Sensor[t] == 1:
obs_cov[t] = np.asarray(R_1)
else:
obs_cov[t] = np.asarray(R_2)
ma_obs=ma.masked_invalid(obs)
ma_obs_cov=ma.masked_invalid(obs_cov)
# Kalman-Filter initialization
kf = KalmanFilter(transition_matrices = F,
observation_matrices = H,
transition_covariance = Q,
observation_covariance = ma_obs_cov, # the covariance will be adapted depending on Sensor_ID
initial_state_mean = X0,
initial_state_covariance = P0)
filtered_state_means, filtered_state_covariances=kf.smooth(ma_obs)
# extracting the Sensor update points for the plot
Sensor_1_update_index = [i for i, x in enumerate(Sensor) if x == 1]
Sensor_2_update_index = [i for i, x in enumerate(Sensor) if x == 2]
Sensor_1_update_X = [ X[i] for i in Sensor_1_update_index ]
Sensor_1_update_Y = [ Y[i] for i in Sensor_1_update_index ]
Sensor_2_update_X = [ X[i] for i in Sensor_2_update_index ]
Sensor_2_update_Y = [ Y[i] for i in Sensor_2_update_index ]
# plot of the resulted trajectory
plt.plot(RefX, RefY, "k-", label="Real Trajectory")
plt.plot(Sensor_1_update_X, Sensor_1_update_Y, "ro", label="Sensor 1")
plt.plot(Sensor_2_update_X, Sensor_2_update_Y, "bo", label="Sensor 2")
plt.plot(filtered_state_means[:, 0], filtered_state_means[:, 1], "g.", label="Filtered Trajectory", markersize=1)
plt.grid()
plt.legend(loc="upper left")
plt.show()
