用经纬度实现卡尔曼滤波器

时间:2014-06-12 10:51:40

标签: java filtering kalman-filter

我来自非数学背景,在我的java应用程序中实现kalman过滤器需要一些帮助。 基本上,我从地图上的特定实体的各种传感器获得输入。输入是纬度和经度项的位置及其准确性。 现在基于我为同一实体收到的几个输入,我需要预测实体在地图上的准确位置。请注意,我不必持续跟踪它。

如何为卡尔曼滤波器提供纬度,长度和精度的输入,以便获得融合的预测值作为输出,从中可以获得lat& long以在地图上绘制相同的背面?

1 个答案:

答案 0 :(得分:2)

由于您没有提供有关此问题的大量信息,我将提供一个仅具有纬度/经度/准确度的简单解决方案:

我首先将卡尔曼滤波器动力学定义为具有随机噪声的速度的随机游动

x = [rx, ry, vx, vy],

其中x是卡尔曼滤波器的状态。现在,模型的预测部分变为

x_{k+1} = A*x_k + Q,

其中

A = [1, 0, dt, 0;0, 1, 0, dt;0, 0, 1, 0;0, 0, 0, 1]

和Q可以在Applied Stochastic Differential Equations

的第56页找到

为了在纬度/经度/精度上应用观测,我首先将WGS84坐标转换为局部ENU coordinates(度量)。这使得系统更加健壮且易于理解。

更新成为

y = H*x + R,

其中y是观察位置[rx,ry]和

H = [1,0,0,0;0,1,0,0]

并且你在观察噪音中使用你的准确度:

R = diag([accuracyx, accuracy]),

其中精确度现在是位置的变化。

这是进行此类2D跟踪的标准方法。由于您只使用纬度/经度/精度,结果可能不如您想要的那么准确。

现在您可以应用上述系统来生成卡尔曼预测和更新方法