抗差自适应卡尔曼滤波算法

前言

在林荫道、城市峡谷等复杂环境下，GNSS观测值往往会出现大量粗差（由于环境原因大幅度偏离实际值），影响定位结果精度。同样，我们预设的动力学模型只是载体运动过程的近似描述，有时也会存在显著异常，影响定位精度。
抗差自适应卡尔曼滤波可用于缓解上述问题，其主要包含抗差估计和自适应卡尔曼滤波两部分。抗差估计通过适当扩大异常观测方差（R）以消除和削弱粗差对参数估值的影响。自适应卡尔曼滤波采用自适应因子降低异常动力学模型信息对滤波状态的影响（扩大状态协方差阵P阵）。
本博客推导整理了抗差自适应卡尔曼滤波的抗差及自适应滤波部分，主要参考杨院士的《自适应动态导航定位》一书。

一、观测向量自适应抗差

观测向量有两类，一类不相关观测，一类相关观测。抗差估计通过扩大异常观测方差（降低权重）来消除和削弱粗差对参数估值的影响，对于不相关观测，其协方差阵非对角线元素应为0，直接扩大协方差阵对角线元素即可；对于相关观测，由于各观测之间具有相关性，协方差阵非对角线元素不一定为0，在扩大协方差阵对角线元素的同时，还应保持原有的相关系数ρ不变。

二、抗差自适应卡尔曼滤波

1、抗差自适应卡尔曼滤波方程推导

上式即为状态参数向量的抗差自适应滤波解，可以通过矩阵恒等变换得到我们熟悉的卡尔曼滤波更新公式：

观察上式与标准卡尔曼滤波的区别，不难发现，实际上就一处不同：

即状态一步预测的协方差阵被自适应因子放大了。与观测向量抗差类似，通过状态一步预测协方差阵的膨胀，来降低状态预测信息对状态参数估值的影响。

2、自适应因子

自适应因子的构造常见的有两种方式：

状态不符值先根据观测向量求得状态参数的最小二乘解，然后求最小二乘解与状态一步预测的差别，差别越大自适应因子越大，状态一步预测降权。由于状态不符值需要计算一次最小二乘解，多了一部分计算量的同时，也必须避免最小二乘出现秩亏的情况，同时，由于状态不符值将观测向量最小二乘解作为较准确的值，观测向量应该尽可能可靠。

预测残差（新息向量）反映了状态一步预测与观测向量求得状态之间的差别。对比于状态不符值，预测残差虽然同样依赖观测向量的可靠性，但不需要计算最小二乘，而预测残差在卡尔曼滤波流程中本来就需要计算，只需在原本的预测残差计算与卡尔曼滤波量测更新之间，添加一个计算自适应因子的过程。因此通常采用预测残差构造自适应因子。

自适应因子模型：

三、抗差自适应卡尔曼滤波流程总结

至此，可以总结抗差自适应滤波解算流程如下：

抗差自适应卡尔曼滤波解通用公式：

我们可以根据自适应因子及观测权矩阵的不同情况，得到以下常见的特殊形式：