CTRV模型

卡尔曼滤波器的模型中,速度作为常量被保存在状态向量中:

这就导致了不能预测速度的方向,也就是不能预测转弯,卡尔曼滤波器中的转弯是靠传感器的纠正.

为了解决这个问题我们使用CTRV模型:

使用位置,,速度,角度,及角速度来作为状态向量:

过程模型

虽然状态向量的变化时非线性的,但是我们可以使用积分准确的求出下一时刻的状态向量

计算积分:

接下来我们需要加入噪声:

其中都符合正态分布.

将噪声结合到过程模型中:

非线性协方差矩阵的计算

我们来回顾一下卡尔曼滤波器与EKF的协方差矩阵是如何计算的:

卡尔曼滤波器是线性模型.根据状态向量是成比例可以推导出协方差矩阵也是成比例的所以:

扩展卡尔曼滤波器EKF用泰勒公式的第一项,也就是最接近非线性曲线斜率的一条直线来模拟非线性.由于是用直线模拟的曲线,所以协方差矩阵也可以按照线性模型的方法进行计算.只不过线性模型中状态向量的比例替换为泰勒公式中的雅可比矩阵.

扩展卡尔曼滤波器因为采用了线性化,所以有精度损失,有没有办法不用线性化直接计算出非线性的协方差矩阵?

无损卡尔曼滤波器UKF采用了一个sigma算子的方案解决了这个问题:
首先,计算前一次协方差矩阵的sigma算子,sigma算子其实就是几个有代表性的点.

然后将这些sigma算子带入到过程模型中得到非线性后的sigma算子.

最后根据这些算计计算出协方差矩阵.

我们发现无损卡尔曼滤波器也不是100%准确的通过计算得到的结果,而是根据几个有代表性的点的变化,重新生成了一个协方差矩阵.如果这几个点取的合理的话无损卡尔曼滤波器的误差是非常小的.

选取算子

首先要确定我们选取算子的个数,通常我们会选择个算子,n为状态向量的纬度.

如图,中间的点是状态向量,然后在对称的在周围取几个点,所以个数为.

算子的公式是这样的:

其中是算子,是状态向量,为状态向量的纬度,可以决定周围的点离中心的距离,通常取.公式中为状态向量,是对称的两个点,通常有n对.

噪声

在预测之前我们还需要考虑噪声.

噪声也是非线性的.无损卡尔曼滤波给出的解决方案是扩充状态向量,这样的话协方差矩阵也会跟着扩充.

预测算子

预测步骤非常简单,只要将我们计算好的算子带入到过程模型即方程(1)中即可得到预测后的算子.

计算协方差矩阵

根据预测后的算子计算状态向量:

根据预测后的算子计算协方差矩阵:

其中的w为是什么?我们可以这样理解:
我们在生成sigma算子的时候,通过分散值调整了sigma距离状态向量的距离

现在我们需要根据sigma算子计算出状态向量与协方差,所以我们需要这个分散值的逆

状态向量转换到测量值

接下来我们需要把状态向量

转换成测量向量

以便后续与测量的数据进行计算.

依然用sigma算子,根据几何关系将sigma算子转换成测量向量.

然后再计算出转换后的状态向量:

计算测量数据的协方差矩阵:

更新

更新部分几乎其实和卡尔曼滤波器一样:

有所不同的地方是卡尔曼增益的计算:

这里的T是状态向量空间与测量向量空间相互关联的矩阵:

滤波器一致性检查(NIS)

我们在引入过几个噪声:

过程噪声,及噪声的协方差矩阵.

测量时也引入测量噪声,及协方差矩阵.

其中测量误差我们可以在传感器的说明书上找到.而模型的过程误差需要我们自己来指定.那么如何确定过程误差呢?

我们可以通过NIS的值来确定误差是否合理:

NIS的全称是归一化新息平方.新息是预测测量值之和与实际测量值之差.归一化是指相对S归一化.

需要注意的是NIS的结果是一个数字,它成卡方分布.

posted @ 2018-07-12 10:06:39
评论加载中...

发表评论