我必须过滤GPS提供的航向。航向以0-360度提供。当在0-360或360-0之间转换时,卡尔曼会变得疯狂,我的滤波航向完全错误。
有没有可能对这种数据使用卡尔曼滤波?
我已经通过明智地使用(C数学库(函数余数处理了类似的问题。余数(a,b(给出了a除以b的余数,并且在-b/2和b/2之间。不在0和b.之间。
例如,在一个简单的情况下,你只有航向作为一个状态,我将把它写为的通用卡尔曼测量更新
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(航向和航向变化率(,那么在时间更新中也值得做类似的事情。这是你在更新时间时应该有的dt说
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^进行"重新规范化">
X^ = remainder( X^, 360)
在测量更新之后