导航卡尔曼滤波器中的投影问题

Question

我目前正在研究一种用于 GPS 导航的简单而小型的卡尔曼滤波器。我从我的 GPS 传感器获取当前位置、航向角和速度。因此，假设速度和航向角恒定，卡尔曼滤波器应该融合当前测量和从先前位置开始的线性运动。

我的问题是选择一个有用的空间，让卡尔曼滤波器能够以良好的方式执行。

局部坐标系方法：

如果我选择一个本地坐标系（北[米]，东[米]），以前的位置在原点，我将能够轻松预测新位置，但是如何将新测量值（纬度/经度）转换为我的本地坐标系使用 wgs-84 椭球？以及如何使用 wgs-84 椭球将我在本地坐标系中的新预测转换为纬度/经度？

所以我需要两个函数：

f:=(lat_ref, lng_ref, lat, lng) -> (x,y)

g:=(lat_ref, lng_ref, x, y) -> (lat, lng)（这也可以使用Vincenty 完成）

全局坐标系方法：

我找到了Vincenty-Algorithm，它根据任何椭球上的参考位置、距离和航向角计算新位置。该算法工作正常，但我看不出如何在全局坐标系中工作的卡尔曼滤波器中使用该算法。

有什么想法或建议可以解决我的一个问题吗？

Answer 1

我研究了许多科学论文并在wikipedia找到了答案

主要技巧是将纬度/经度/高度 (llh) 转换为地心地地固定 (ecef)，然后转换为我的本地坐标系 earth/north/up (enu)，反之亦然。

我发现了一些其他有趣的链接到这个主题。

• direct and faster conversion from llh to enu in matlab
• ecef2llh and llh2ecef in matlab

Answer 2

查看 FWTOOLS，尤其是 cs2cs 和 proj... 他们可以处理从投影坐标系到地理坐标系的坐标系转换。重要的是要记住考虑任何基准偏移。