Filter 如何使用0-360度的卡尔曼滤波器

我必须过滤GPS提供的航向。航向以0-360度的形式提供。当有0-360或360-0之间的过渡时，卡尔曼会发疯，我的过滤航向完全错误。有没有可能将Kalman用于这种数据？

我通过明智地使用（C数学库）函数余数处理过类似的问题。余数（a，b）给出a除以b的余数，介于-b/2和b/2之间。不在0和b之间

例如，在一个简单的例子中，你只有一个标题作为一个状态，一般的kalman测量更新，我将写为

X^ = X~ + K*(Y-H*X~)

（其中X~为更新状态，X^为测量时的预测状态，Y为测量，H为测量模型矩阵，K为卡尔曼增益）

变成

dY = remainder( Y-X~, 360)
X^ = remainder( X~ + K*dY, 360)

有两个余数可以避免过滤后的航向永远增加，但保留在[-180180]中。虽然严格来说没有必要这么做，但当我的过滤器显示航向是3.6亿度加15度，而不是15度时，我觉得很奇怪

如果你也在模拟航向的变化率，那么你的状态是h，hdot（航向和航向的变化率），那么在时间更新中做类似的事情也是值得的。 也就是说，你应该在更新时间的时候说

h~ = remainder( h + dt*hdot, 360)
hdot~ = hdot

同样，这是将h保持在[-180180]

状态协方差的更新以及K的计算方式保持不变

如果您使用的是实现卡尔曼滤波方程的软件包，那么您可以通过在将观测值传递给测量更新之前改变观测值来实现相同的功能。如果Y为观察值，X为状态，则将Y改为

Y^ = X + remainder( Y-X, 360)

然后我们将有

Y^-X = remainder( Y-X, 360) 

出于上述原因，我还建议通过

X^ = remainder( X^, 360)

测量更新后