filter - 长纬度坐标运动的卡尔曼滤波

Question

我正在尝试实现一个简单的卡尔曼滤波器，用于过滤/预测车辆在长坐标/ float 坐标中的运动。

没有来自车辆传感器的测量值，只是对观察到的经/纬度位置的新更新，所以基本上我将尝试预测和更正的状态是车辆在任何给定时间的经度和纬度。

据我所知，该模型是非线性的，因为可能存在方向的随机加速度变化等，但我认为这在很大程度上可以忽略，只要我在我的状态下也跟踪方位。我的问题是我不知道如何根据状态和预测矩阵对该系统建模，除此之外，似乎有必要将 long/lat 坐标转换/投影到某个笛卡尔 xy 系统中，以便这两个变得独立，但我不确定如何去做。

似乎从 xy 转换回 wgs84 并不是那么简单，而且计算量可能有点大。任何人都可以阐明这一点吗？

Answer 1

看起来你的状态变量是一个向量 [lat, long]，你的测量变量是 [lat, < em>长，轴承]。您需要根据这些向量分别为过程和测量模型计算出适当的 f 和 h 函数。由于这是一个非线性问题，您可能需要使用非线性滤波器，例如 EKF、UKF 或 CKF(立方卡尔曼滤波器)。

当使用卡尔曼滤波器处理角度、纬度或经度等模块化值时，只要您的状态或测量值接近不连续的模块化边界，就会出现大问题。比如你的方位角是0到360度的角度，如果你是在1度或者359度测量，滤波器就会有问题。此外，如果您的经度大约是正负 180 度经度，或者您的纬度位于其中一个极点(您可以忽略这些可能性很小)，您可能会遇到问题。

在 David Frederic Crouse 的“Cubature/Unscented/Sigma Point Kalman Filtering with Angular Measurement Models”中介绍了如何处理状态或测量变量中的角度的一个示例，第 18 届国际 session 。关于信息融合(华盛顿特区，2015 年 7 月 6 日至 9 日)，https://ieeexplore.ieee.org/document/7266741 .根据 Crouse 的方法，要将方位角用作测量变量，您需要将环绕函数添加到标准无味或立方体滤波器方程中的几个位置。如果您想处理纬度和经度的不连续性(您可能不会)，则需要在计算协方差矩阵时将坐标转换为欧几里德空间或从欧几里德空间转换。