Filtro Kalman com intervalos de tempo variados
Eu tenho alguns dados que representam a localização de um objeto medido a partir de dois sensores diferentes. Então, eu preciso fazer a fusão do sensor. A questão mais difícil é que os dados de cada sensor chegam essencialmente em um momento aleatório. Eu gostaria de usar o pykalman para fundir e suavizar os dados. Como pykalman pode manipular dados variáveis de timestamp?
Uma amostra simplificada dos dados ficará assim:
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')
E isto:
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
Para o problema de fusão do sensor, acho que posso apenas remodelar os dados para ter as posições X1, Y1, X2, Y2 com um monte de valores ausentes, em vez de apenas X, Y. (Isso foi relacionado:https://stackoverflow.com/questions/47386426/2-sensor-readings-fusion-yaw-pitch )
Então, meus dados podem ficar assim:
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
Os documentos para pykalman indicam que ele pode lidar com dados ausentes, mas isso está correto?
Mas, os documentos para pykalman não são claros quanto à questão do tempo variável. O documento apenas diz:
"O Kalman Filter e o Kalman Smoother são capazes de usar parâmetros que variam com o tempo. Para usar isso, basta passar em uma matriz com n_timesteps de comprimento ao longo de seu primeiro eixo:"
>>> transition_offsets = [[-1], [0], [1], [2]]
>>> kf = KalmanFilter(transition_offsets=transition_offsets, n_dim_obs=1)
Não consegui encontrar nenhum exemplo de uso de um pykalman Smoother com etapas de tempo variáveis. Portanto, qualquer orientação, exemplo ou mesmo um exemplo usando meus dados acima, seria muito útil. Não é necessário usar o pykalman, mas parece uma ferramenta útil para suavizar esses dados.
***** Código adicional adicionado abaixo @Anton Fiz uma versão do seu código útil que usa a função suave. O estranho é que parece tratar todas as observações com o mesmo peso e a trajetória passa por cada uma delas. Mesmo se eu tiver uma grande diferença entre os valores de variação do sensor. Eu esperaria que, em torno do ponto 5.4,5.0, a trajetória filtrada se aproximasse do ponto do Sensor 1, pois esse tem uma variação menor. Em vez disso, a trajetória vai exatamente a cada ponto e faz uma grande virada para chegar lá.
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()