问题描述
我正在尝试使用卡尔曼滤波器来预测下一个对象位置。我的数据由纬度和经度每 1 秒组成,所以,我也可以得到速度。
下面的代码显示了 pykalman 包的尝试以预测进一步的位置。我只是通过添加前三个纬度/经度值来修改测量值。 transition_matrices 和observation_matrices 对吗?我不知道我应该如何设置它们。
#!pip install pykalman
from pykalman import KalmanFilter
import numpy as np
kf = KalmanFilter(transition_matrices = [[1,1],[0,1]],observation_matrices = [[0.1,0.5],[-0.3,0.0]])
measurements = np.asarray([[41.4043467,2.1765616],[41.4043839,2.1766097],[41.4044208,2.1766576]]) # 3 observations
kf = kf.em(measurements,n_iter=5)
(filtered_state_means,filtered_state_covariances) = kf.filter(measurements)
(smoothed_state_means,smoothed_state_covariances) = kf.smooth(measurements)
smoothed_state_means
array([[-1.65091776,23.94730577],[23.15197525,21.2257123 ],[43.96359962,21.9785667 ]])
我该如何解决这个问题?我错过了什么?
使用纬度/经度时路径具有此形状
更新
我尝试过以下转换方式:
1.
R = 6378388.0 # m
rlat1_225 = math.radians(lat_225['message_basicContainer_reference_position_latitude'].values[i-1]/10000000)
rlon1_225 = math.radians(lon_225['message_basicContainer_reference_position_longitude'].values[i-1]/10000000)
dx = R * math.cos(rlat1_225) * math.cos(rlon1_225)
dy = R * math.cos(rlat1_225) * math.sin(rlon1_225)
pos_x = abs(dx*1000)
pos_y= abs(dy*1000)
2.
altitude=0
arc= 2.0*np.pi*(R+altitude)/360.0 #
latitude=lat_225['message_basicContainer_reference_position_latitude']/10000000
longitude=lon_225['message_basicContainer_reference_position_longitude']/10000000
dx = arc * np.cos(latitude*np.pi/180.0) * np.hstack((0.0,np.diff(longitude))) # in m
dy = arc * np.hstack((0.0,np.diff(latitude))) # in m
然而,在应用 EKF 之后,第一种方法似乎是正确的形状(我遵循了 Michel Van Biezen 的解释,其中跟踪平面我可以让它在 python 中工作)。
所以,我按照第一种使用 EKF 的方式进行预测:
然而,当我将预测路径和原始路径重叠时,我得到了这个图
然后,用第二种方法做预测,结果是
解决方法
您需要在局部笛卡尔坐标系中转换经纬度位置。您可以在第一个接收到的位置内设置原点。关于这个系统,你可以估计相对位置和速度。
转移矩阵取决于您选择的状态,例如2 个状态,该轴上的 x 位置和 v 速度:x_k+1 = x_k + v_k * dT 和 v_k+1 = v_k。这是:
transition_matrices = [[1,dT],[0,1]]