我试图用卡尔曼滤波器来预测下一个物体的位置。我的数据是由纬度和经度组成的,每个1s,所以,我也可以得到速度
下面的代码显示了pykalman包预测进一步位置的尝试。我只是通过添加前三个lat/lon值来修改测量值。转移矩阵和观测矩阵正确吗?我不知道该如何设置它们
#!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 ]])
我怎样才能解决这个问题?我错过了什么
使用lat/long时,路径具有此形状
更新
我尝试过以下转换方式:
一,
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)
二,
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。即:
相关问题 更多 >
编程相关推荐