问题描述
我有一些数据，表示从两个不同的传感器测得的物体的位置.因此，我需要进行传感器融合.更为困难的问题是来自每个传感器的数据实际上是在随机时间到达的.我想使用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平滑器的任何示例.因此，使用我上面的数据的任何指导，示例甚至示例都将非常有帮助.我没有必要使用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()
对于卡尔曼滤波器，以恒定的时间步长表示输入数据非常有用.传感器会随机发送数据，因此您可以为系统定义最小的重要时间步长，并通过该步长离散时间轴.
例如，您的一个传感器大约每0.2秒发送一次数据，第二个传感器每0.5秒发送一次数据.因此最小的时间步长可能是0.01秒(在这里，您需要在计算时间和所需的精度之间进行权衡).
您的数据如下所示:
Time Sensor X Y
0,52 0 0 0
0,53 1 0,3417 0,2988
0,54 0 0 0
0,56 0 0 0
0,57 0 0 0
0,55 0 0 0
0,58 0 0 0
0,59 2 0,4247 0,3779
0,60 0 0 0
0,61 0 0 0
0,62 0 0 0
现在，您需要根据观察结果调用Pykalman函数 filter_update .如果没有观察到，则过滤器会根据前一个状态预测下一个状态.如果有发现，它将更正系统状态.
您的传感器可能具有不同的精度.因此，您可以根据传感器的方差指定观察方差.
为了展示这个想法，我生成了一个2D轨迹，并随机放置了两个精度不同的传感器的测量值.
Sensor1: mean update time = 1.0s; max error = 0.1m;
Sensor2: mean update time = 0.7s; max error = 0.3m;
这是结果:
我故意选择了非常糟糕的参数，因此可以看到预测和校正步骤.在传感器更新之间，过滤器基于上一步的恒定速度预测轨迹.更新一到，过滤器就会根据传感器的变化来校正位置.第二个传感器的精度很差，因此以较低的重量影响了系统.
这是我的python代码:
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))
# Kalman-Filter initialization
kf = KalmanFilter(transition_matrices = F,
observation_matrices = H,
transition_covariance = Q,
observation_covariance = R_1, # the covariance will be adapted depending on Sensor_ID
initial_state_mean = X0,
initial_state_covariance = P0)
# iterative estimation for each new measurement
for t in range(n_timesteps):
if t == 0:
filtered_state_means[t] = X0
filtered_state_covariances[t] = P0
else:
# the observation and its covariance have to be switched depending on Sensor_Id
# Sensor_ID == 0: no observation
# Sensor_ID == 1: Sensor 1
# Sensor_ID == 2: Sensor 2
if Sensor[t] == 0:
obs = None
obs_cov = None
else:
obs = [X[t], Y[t]]
if Sensor[t] == 1:
obs_cov = np.asarray(R_1)
else:
obs_cov = np.asarray(R_2)
filtered_state_means[t], filtered_state_covariances[t] = (
kf.filter_update(
filtered_state_means[t-1],
filtered_state_covariances[t-1],
observation = obs,
observation_covariance = obs_cov)
)
# 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()
我将csv文件放在此处，以便您执行代码. >
希望我能为您提供帮助.
更新
一些关于可变转换矩阵的建议信息.我想说这取决于您的传感器的可用性以及对估算结果的要求.
在这里，我用不变的和可变的转移矩阵绘制了相同的估计值(我更改了转移协方差矩阵，否则由于过滤器的刚度"高，估计值太差了):
如您所见，黄色标记的估计位置非常好. 但是！您在传感器读数之间没有任何信息.使用可变转移矩阵可以避免读数之间的预测步骤，并且不知道系统会发生什么.如果您的阅读率很高，可能就足够了，否则可能是不利的.
这是更新的代码:
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-2, 0, 0, 0],
[ 0, 1e-2, 0, 0],
[ 0, 0, 1e-2, 0],
[ 0, 0, 0, 1e-2]]
# 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))
filtered_state_means2 = np.zeros((n_timesteps, n_dim_state))
filtered_state_covariances2 = np.zeros((n_timesteps, n_dim_state, n_dim_state))
# Kalman-Filter initialization
kf = KalmanFilter(transition_matrices = F,
observation_matrices = H,
transition_covariance = Q,
observation_covariance = R_1, # the covariance will be adapted depending on Sensor_ID
initial_state_mean = X0,
initial_state_covariance = P0)
# Kalman-Filter initialization (Different F Matrices depending on DT)
kf2 = KalmanFilter(transition_matrices = F,
observation_matrices = H,
transition_covariance = Q,
observation_covariance = R_1, # the covariance will be adapted depending on Sensor_ID
initial_state_mean = X0,
initial_state_covariance = P0)
# iterative estimation for each new measurement
for t in range(n_timesteps):
if t == 0:
filtered_state_means[t] = X0
filtered_state_covariances[t] = P0
# For second filter
filtered_state_means2[t] = X0
filtered_state_covariances2[t] = P0
timestamp = Time[t]
old_t = t
else:
# the observation and its covariance have to be switched depending on Sensor_Id
# Sensor_ID == 0: no observation
# Sensor_ID == 1: Sensor 1
# Sensor_ID == 2: Sensor 2
if Sensor[t] == 0:
obs = None
obs_cov = None
else:
obs = [X[t], Y[t]]
if Sensor[t] == 1:
obs_cov = np.asarray(R_1)
else:
obs_cov = np.asarray(R_2)
filtered_state_means[t], filtered_state_covariances[t] = (
kf.filter_update(
filtered_state_means[t-1],
filtered_state_covariances[t-1],
observation = obs,
observation_covariance = obs_cov)
)
#For the second filter
if Sensor[t] != 0:
obs2 = [X[t], Y[t]]
if Sensor[t] == 1:
obs_cov2 = np.asarray(R_1)
else:
obs_cov2 = np.asarray(R_2)
dt2 = Time[t] - timestamp
timestamp = Time[t]
# transition_matrix
F2 = [[1, 0, dt2, 0],
[0, 1, 0, dt2],
[0, 0, 1, 0],
[0, 0, 0, 1]]
filtered_state_means2[t], filtered_state_covariances2[t] = (
kf2.filter_update(
filtered_state_means2[old_t],
filtered_state_covariances2[old_t],
observation = obs2,
observation_covariance = obs_cov2,
transition_matrix = np.asarray(F2))
)
old_t = t
# 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", markersize=9)
plt.plot(Sensor_2_update_X, Sensor_2_update_Y, "bo", label="Sensor 2", markersize=9)
plt.plot(filtered_state_means[:, 0], filtered_state_means[:, 1], "g.", label="Filtered Trajectory", markersize=1)
plt.plot(filtered_state_means2[:, 0], filtered_state_means2[:, 1], "yo", label="Filtered Trajectory 2", markersize=6)
plt.grid()
plt.legend(loc="upper left")
plt.show()
我在此代码中未实现的另一个重要点:使用可变转换矩阵时，您还需要更改转换协方差矩阵(取决于当前dt).
这是一个有趣的话题.让我知道哪种估算最适合您的问题.
I have some data that represents the location of an object measured from two different sensors. So, I need to do sensor fusion. The more difficult issue is that the data from each sensor, arrives at essentially a random time. I would like to use pykalman so fuse and smooth the data. How can pykalman handle variable timestamp data?
A simplified sample of the data will look like this:
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')
And this:
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
For the sensor fusing issue, I think that I can just reshape the data so that I have positions X1,Y1,X2,Y2 with a bunch of missing values, instead of just X,Y. (This was related: https://stackoverflow.com/questions/47386426/2-sensor-readings-fusion-yaw-pitch )
So then my data can look like this:
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
The docs for pykalman indicate that it can handle missing data, but is that correct?
But, the docs for pykalman are not at all clear about the variable time issue. The doc just says:
"Both the Kalman Filter and Kalman Smoother are able to use parameters which vary with time. In order to use this, one need only pass in an array n_timesteps in length along its first axis:"
>>> transition_offsets = [[-1], [0], [1], [2]]
>>> kf = KalmanFilter(transition_offsets=transition_offsets, n_dim_obs=1)
I have not been able to find any examples of using a pykalman Smoother with variable time steps. So, any guidance, examples or even an example using my above data, would be very helpful.I is not necessary to use pykalman, but it seems like a useful tool to smooth this data.
*****Additional code added below@Anton I made a version of your helpful code that uses the smooth function. The strange thing is that it seems to treat every observation with the same weight and has the trajectory go through every single one. Even, if I have a large different between the sensor variance values. I would expect that around the 5.4,5.0 point, the filtered trajectory should go closer to the Sensor 1 point, since that one has a lower variance. Instead the trajectory goes exactly to each point, and makes a big turn to get there.
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()
For a Kalman filter it is useful to represent the input data with a constant time step. Your sensors send data randomly, so you can define the smallest significant time step for your system and discretize the time axis with this step.
For example one of your sensors sends data approximately each 0.2 seconds and the second one each 0.5 seconds. So the smallest time step could be 0.01 seconds (here you need to find a trade-off between computational time and desired precision).
Your data would look like this:
Time Sensor X Y
0,52 0 0 0
0,53 1 0,3417 0,2988
0,54 0 0 0
0,56 0 0 0
0,57 0 0 0
0,55 0 0 0
0,58 0 0 0
0,59 2 0,4247 0,3779
0,60 0 0 0
0,61 0 0 0
0,62 0 0 0
Now you need to call the Pykalman function filter_update depending on your observations. If there is no observation, the filter predicts the next state based on the previous one. If there is an observation, it corrects the system state.
Probably your sensors have different accuracy. So you can specify the observation covariance depending on the sensor variance.
To demonstrate the idea I generated a 2D-trajectory and randomly put measurements of 2 sensors with different accuracy.
Sensor1: mean update time = 1.0s; max error = 0.1m;
Sensor2: mean update time = 0.7s; max error = 0.3m;
Here is the result:
I chose really bad parameters on purpose, so one can see the prediction and correction steps. Between the sensor updates the filter predicts the trajectory based on the constant velocity from the previous step. As soon as an update comes the filter corrects the position according to the sensor's variance. The precision of the second sensor is very bad, so it influences the system with a lower weight.
Here is my python code:
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))
# Kalman-Filter initialization
kf = KalmanFilter(transition_matrices = F,
observation_matrices = H,
transition_covariance = Q,
observation_covariance = R_1, # the covariance will be adapted depending on Sensor_ID
initial_state_mean = X0,
initial_state_covariance = P0)
# iterative estimation for each new measurement
for t in range(n_timesteps):
if t == 0:
filtered_state_means[t] = X0
filtered_state_covariances[t] = P0
else:
# the observation and its covariance have to be switched depending on Sensor_Id
# Sensor_ID == 0: no observation
# Sensor_ID == 1: Sensor 1
# Sensor_ID == 2: Sensor 2
if Sensor[t] == 0:
obs = None
obs_cov = None
else:
obs = [X[t], Y[t]]
if Sensor[t] == 1:
obs_cov = np.asarray(R_1)
else:
obs_cov = np.asarray(R_2)
filtered_state_means[t], filtered_state_covariances[t] = (
kf.filter_update(
filtered_state_means[t-1],
filtered_state_covariances[t-1],
observation = obs,
observation_covariance = obs_cov)
)
# 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()
I put the csv-file here so you can execute the code.
I hope I could help you.
UPDATE
Some information to your suggestion about a variable transition matrix. I would say it depends on the availability of your sensors and on the requirements to the estimation result.
Here I plotted the same estimation both with a constant and a variable transition matrix (I changed the transition covariance matrix, otherwise the estimation was too bad because of the high filter "stiffness"):
As you can see the estimated position of the yellow markers is pretty good. BUT! you have no information between the sensor readings. Using a variable transition matrix you avoid the prediction step between readings and have no idea what happens to the system. It can be good enough if your readings come with a high rate, but otherwise it can be a disadvantage.
Here is the updated code:
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-2, 0, 0, 0],
[ 0, 1e-2, 0, 0],
[ 0, 0, 1e-2, 0],
[ 0, 0, 0, 1e-2]]
# 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))
filtered_state_means2 = np.zeros((n_timesteps, n_dim_state))
filtered_state_covariances2 = np.zeros((n_timesteps, n_dim_state, n_dim_state))
# Kalman-Filter initialization
kf = KalmanFilter(transition_matrices = F,
observation_matrices = H,
transition_covariance = Q,
observation_covariance = R_1, # the covariance will be adapted depending on Sensor_ID
initial_state_mean = X0,
initial_state_covariance = P0)
# Kalman-Filter initialization (Different F Matrices depending on DT)
kf2 = KalmanFilter(transition_matrices = F,
observation_matrices = H,
transition_covariance = Q,
observation_covariance = R_1, # the covariance will be adapted depending on Sensor_ID
initial_state_mean = X0,
initial_state_covariance = P0)
# iterative estimation for each new measurement
for t in range(n_timesteps):
if t == 0:
filtered_state_means[t] = X0
filtered_state_covariances[t] = P0
# For second filter
filtered_state_means2[t] = X0
filtered_state_covariances2[t] = P0
timestamp = Time[t]
old_t = t
else:
# the observation and its covariance have to be switched depending on Sensor_Id
# Sensor_ID == 0: no observation
# Sensor_ID == 1: Sensor 1
# Sensor_ID == 2: Sensor 2
if Sensor[t] == 0:
obs = None
obs_cov = None
else:
obs = [X[t], Y[t]]
if Sensor[t] == 1:
obs_cov = np.asarray(R_1)
else:
obs_cov = np.asarray(R_2)
filtered_state_means[t], filtered_state_covariances[t] = (
kf.filter_update(
filtered_state_means[t-1],
filtered_state_covariances[t-1],
observation = obs,
observation_covariance = obs_cov)
)
#For the second filter
if Sensor[t] != 0:
obs2 = [X[t], Y[t]]
if Sensor[t] == 1:
obs_cov2 = np.asarray(R_1)
else:
obs_cov2 = np.asarray(R_2)
dt2 = Time[t] - timestamp
timestamp = Time[t]
# transition_matrix
F2 = [[1, 0, dt2, 0],
[0, 1, 0, dt2],
[0, 0, 1, 0],
[0, 0, 0, 1]]
filtered_state_means2[t], filtered_state_covariances2[t] = (
kf2.filter_update(
filtered_state_means2[old_t],
filtered_state_covariances2[old_t],
observation = obs2,
observation_covariance = obs_cov2,
transition_matrix = np.asarray(F2))
)
old_t = t
# 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", markersize=9)
plt.plot(Sensor_2_update_X, Sensor_2_update_Y, "bo", label="Sensor 2", markersize=9)
plt.plot(filtered_state_means[:, 0], filtered_state_means[:, 1], "g.", label="Filtered Trajectory", markersize=1)
plt.plot(filtered_state_means2[:, 0], filtered_state_means2[:, 1], "yo", label="Filtered Trajectory 2", markersize=6)
plt.grid()
plt.legend(loc="upper left")
plt.show()
Another important point that I did not implement in this code: while using a variable transition matrix you need to vary the transition covariance matrix as well (depending on the current dt).
It's an interesting topic. Let me know what kind of estimation fits to your problem best.
这篇关于随时间变化的卡尔曼滤波器的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持！