다양한 타임 스텝을 가진 칼만 필터

Question

두 개의 서로 다른 센서로 측정 한 물체의 위치를 ​​나타내는 데이터가 있습니다. 그래서 저는 센서 융합을 할 필요가 있습니다. 더 어려운 문제는 각 센서의 데이터가 본질적으로 임의의 시간에 도착한다는 것입니다. 나는 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가 아닌 값이 누락 된 위치에 있어야합니다. (이것은 관련이 : http://stackoverflow.com.mevn.net/questions/47386426/2-sensor-readings-fusion-yaw-pitch를)

그래서 다음 내 데이터는 다음과 같이 할 수 있습니다 pykalman에 대한

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에 대한 문서는 가변 시간 문제에 대해 명확하지 않습니다. 다큐먼트는 말한다 :

이

"칼만 필터와 칼만 원활한 모두가 시간에 따라 변화 매개 변수를 사용할 수 있습니다 이것을 사용하기 위해, 하나는 최초의 축을 따라 길이 배열 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()  

Answer 1

칼만 필터의 경우 일정한 시간 간격으로 입력 데이터를 나타내는 것이 유용합니다. 센서는 무작위로 데이터를 전송하므로이 단계를 사용하여 시스템에 대해 가장 작은 중요한 시간 단계를 정의하고 시간 축을 이산화 할 수 있습니다.

예를 들어 센서 중 하나는 약 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 궤적을 생성하고 2 개의 센서의 측정 값을 다른 정확도로 무작위로 집계했습니다. 하나는 예측 및 보정 단계를 볼 수 있도록 내가, 목적에 정말 나쁜 매개 변수를 선택

: 여기

Sensor1: mean update time = 1.0s; max error = 0.1m; 
Sensor2: mean update time = 0.7s; max error = 0.3m; 

는 결과이다. 센서 업데이트 사이의 필터는 이전 단계의 일정 속도를 기반으로 궤적을 예측합니다. 업데이트가 발생하면 필터는 센서의 분산에 따라 위치를 수정합니다. 두 번째 센서의 정밀도가 매우 낮으므로 무게가 더 낮은 시스템에 영향을줍니다.

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 파일 here을 넣어 :

여기 내 파이썬 코드입니다.

도와 드리겠습니다.

UPDATE

변수 전환 매트릭스에 대한 귀하의 제안에 일부 정보를 제공합니다. 나는 그것이 당신의 센서의 유용성과 추정 결과에 대한 요구 사항에 달려 있다고 말할 수 있습니다.

: 여기

난 (달리 추정이 높기 때문에 필터 "강성"너무 나빴다, I가 변화 공분산 행렬을 변경) 모두 일정하고, 변수 전이 매트릭스와 같은 추정을 플롯

노란색 마커의 예상 위치는 매우 양호합니다. 하지만! 센서 판독 값 사이에 정보가 없습니다. 가변 전이 행렬을 사용하면 판독 값 사이의 예측 단계를 피할 수 있으며 시스템에 어떤 일이 발생하는지 전혀 알 수 없습니다. 그것은 당신의 독서가 높은 비율로 오는 경우에 충분히 좋을 수 있지만 그렇지 않으면 단점이 될 수 있습니다.

이

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).

흥미로운 주제입니다. 어떤 종류의 추정이 당신의 문제에 가장 잘 맞는지 알려주십시오.