偏差感知鲁棒容积卡尔曼滤波方法及系统与流程

本发明涉及非线性状态估计与自适应滤波，涉及工业物联网信息感知、传感技术服务及工业信息和数据处理服务。更具体地说，本发明涉及一种偏差感知鲁棒容积卡尔曼滤波方法及系统。

背景技术：

1、容积卡尔曼滤波器（cubature kalman filter,ckf）是处理非线性系统状态估计问题的常用方法，其采用三阶球面-径向容积规则逼近高斯权重积分，在估计精度与计算效率之间取得了良好平衡，被广泛应用于目标运动监测、多传感器组合导航及空间感知等领域。然而，标准ckf假设量测噪声为零均值高斯白噪声，当系统中存在未知的传感器偏差（如传感器校准误差、平台安装偏差或大气折射系统误差）时，量测模型不再满足零均值假设，导致状态估计出现严重的有偏误差甚至发散。

2、针对上述问题，现有的偏差处理方法主要分为三类：

3、第一类是状态增广法，将偏差作为额外状态变量直接增广到状态向量中，这会显著增加状态维数和计算量，且偏差与状态的耦合在增广协方差中难以正确建模，导致估计不一致；

4、第二类是friedland两阶段滤波器，其将偏差估计与状态估计解耦为两个并行子滤波器，但该方法的根本局限在于，其解耦假设忽略了目标运动状态位置不确定性通过非线性量测函数向偏差量测空间的投影贡献，导致偏差新息协方差被低估。当偏差与状态存在强耦合时，滤波器会出现严重的统计不一致性，表现为归一化估计误差平方（nees）远超理论置信区间；

5、第三类为鲁棒滤波方法，如huber滤波，其虽能抑制异常值影响，但无法直接估计并消除持续存在的传感器偏差。

6、此外，现有方法在处理偏差问题时，还普遍缺乏对偏差协方差矩阵对称正定性的严格数值保证。传统方法在偏差协方差更新过程中，易因有限精度运算中的舍入误差累积而失去对称正定性，导致滤波器数值崩溃。

7、因此，亟需一种既能在偏差新息协方差中有效纳入目标运动状态位置不确定性投影、又能通过数值稳健的更新形式严格保证偏差协方差对称正定性、同时兼顾计算效率的偏差感知鲁棒滤波方法。 技术实现要素：

8、本发明的一个目的是提供了一种偏差感知鲁棒容积卡尔曼滤波方法，包括以下步骤：

9、将系统状态分解为目标运动状态和传感器偏差，分别建立目标运动状态的非线性状态空间模型和传感器偏差的随机游走动态模型，偏差预测状态公式为、偏差协方差预测公式为，其中，为偏差过程噪声协方差；

10、每帧滤波周期中，利用上一帧获得的偏差预测值对原始量测执行偏差校正，校正公式为，其中，为偏差到量测空间的映射函数，使用偏差校正后的量测执行目标运动状态容积卡尔曼滤波的量测更新步骤，获得当前帧的后验状态估计及后验状态协方差矩阵；

11、从后验状态协方差矩阵中提取与角度量测相关的目标位置坐标对应的子协方差块，作为位置子空间协方差块；

12、基于角度量测函数对目标位置坐标的偏导基数构造雅可比矩阵，利用雅可比矩阵将位置子空间协方差块映射至偏差量测空间，获得状态位置协方差投影项；

13、将状态位置协方差投影项、偏差预测协方差和经鲁棒化处理的量测噪声协方差三项矩阵相加，构造偏差新息协方差矩阵；

14、利用偏差新息协方差矩阵和偏差预测协方差计算偏差卡尔曼增益，将偏差卡尔曼增益乘以偏差新息后叠加至偏差预测值，获得当前帧的偏差估计值。

15、优选的是，偏差协方差更新采用joseph对称形式，更新公式为，其中，i为单位矩阵。

16、优选的是，当量测中包含角度观测量时，偏差新息的构造在so(2)旋转群上进行，偏差新息计算公式为，其中g(·)为量测向量的角度分量提取函数，为基于后验状态估计的预测量测，为so(2)流形上的对数映射，对数映射在几何上等价于沿单位圆最短弧方向计算角度差异。

17、优选的是，在目标运动状态容积卡尔曼滤波的量测更新中，采用huber鲁棒损失函数替代平方损失函数，huber损失函数对标准化新息分段定义为：当时，当时，其中，阈值参数对应95%渐近效率，将损失函数对标准化新息的导数作为权重因子乘以标准卡尔曼增益。

18、优选的是，在目标运动状态容积卡尔曼滤波的量测更新中，围绕当前状态估计执行阻尼gauss-newton迭代精化，每次迭代在最新迭代点处重新线性化量测函数，引入阻尼因子λ并使用armijo线搜索条件保证负对数似然损失函数l的充分下降，阻尼因子λ根据损失函数变化按指数规律自适应调整：当时，当时，迭代在标准化新息变化量小于预设精度阈值或达到最大迭代次数时终止。

19、优选的是，在目标运动状态容积卡尔曼滤波的预测步骤中，维护预设的过程噪声协方差缩放因子假设集合，，各假设具有先验权重，每帧滤波周期计算实际归一化新息平方，基于各假设下归一化新息平方的似然函数通过贝叶斯更新递归更新各假设的权重，以各假设权重为加权系数计算缩放因子的加权平均值调整当前帧的过程噪声协方差矩阵。

20、优选的是，在从后验状态协方差矩阵中提取位置子空间协方差块时，利用状态向量中各分量与角度量测之间的相关性结构进行稀疏化处理，仅提取与目标三维空间位置直接对应的3×3子矩阵作为位置子空间协方差块，雅可比矩阵构建为2×3矩阵，其中第一行对应方位角量测对目标三维空间位置坐标的偏导数，第二行对应俯仰角量测对目标三维空间位置坐标的偏导数，投影项计算直接利用2×3雅可比矩阵与3×3位置子空间协方差块进行矩阵乘法，将投影项的计算复杂度从与全状态维数平方成正比降至常数阶。

21、优选的是，在阻尼gauss-newton迭代精化的每次迭代中，对角度量测分量采用流形感知的线性化策略：

22、将当前迭代点的角度预测值视为so(2)流形上的基点，利用对数映射将角度观测值从so(2)流形拉回基点处的切空间获得切空间残差，在该切空间内执行标准欧氏线性化构建角度量测的雅可比行，迭代更新量在切空间中计算，更新后的角度估计值通过指数映射从切空间提升回so(2)流形，阻尼因子λ和armijo线搜索均在切空间的欧氏度量下执行。

23、优选的是，在计算归一化新息平方以驱动过程噪声协方差自适应时，使用经偏差校正后的量测新息替代原始量测新息，偏差校正后的量测新息=原始量测－基于状态预测的量测预测值－偏差预测值经量测函数映射所得的偏差量测分量。

24、提供一种偏差感知鲁棒容积卡尔曼滤波系统，包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时实现所述的方法，所述处理器被配置为包括：

25、状态分解模块，被配置为将系统状态分解为目标运动状态和传感器偏差，分别建立目标运动状态的非线性状态空间模型和传感器偏差的随机游走动态模型；

26、偏差校正模块，被配置为在每帧滤波周期中，利用上一帧获得的偏差预测值对原始量测执行偏差校正；

27、目标运动状态更新模块，被配置为使用偏差校正后的量测执行目标运动状态容积卡尔曼滤波的量测更新，获得当前帧的后验状态估计及后验状态协方差矩阵；

28、协方差投影模块，被配置为从后验状态协方差矩阵中提取位置子空间协方差块，基于角度量测函数对目标位置坐标的偏导数构造雅可比矩阵，将位置子空间协方差块映射至偏差量测空间，获得状态位置协方差投影项；

29、偏差新息协方差构造模块，被配置为将状态位置协方差投影项、偏差预测协方差和经鲁棒化处理的量测噪声协方差三项矩阵相加，构造偏差新息协方差矩阵；

30、偏差更新模块，被配置为利用偏差新息协方差矩阵和偏差预测协方差计算偏差卡尔曼增益，将偏差卡尔曼增益乘以偏差新息后叠加至偏差预测值，获得当前帧的偏差估计值。

31、本发明至少包括以下有益效果：

32、第一、本发明通过状态投影偏差新息协方差矩阵机制，将主状态位置协方差通过量测函数雅可比映射至偏差量测空间，使偏差卡尔曼增益“感知”主状态的估计质量。该机制突破传统friedland两阶段滤波器的解耦假设，使偏差新息协方差能够根据主状态估计质量自动调整，确保在偏差活跃场景下滤波器具有统计一致性，nees保持在卡方95%置信区间内。

33、第二、本发明通过joseph对称形式偏差协方差更新机制，在有限精度运算下天然保持协方差矩阵的对称正定性。该形式通过对残差项和量测噪声项的分别处理，从根本上避免了传统标准形式可能出现的协方差不对称甚至负特征值导致的滤波器数值发散问题。

34、第三、本发明通过so(2)流形对数映射构造角度偏差新息，天然保证角度的循环一致性，克服了传统欧氏减法在跨越±π边界时产生2π跳变的伪影，使偏差估计在角度边界处无跳变、连续可导。

35、第四、本发明通过阻尼gauss-newton迭代精化机制，在每次迭代中围绕当前状态估计重新线性化量测函数，使用armijo线搜索保证损失函数单调下降，在非线性量测下具有超线性收敛速率，显著提高了ckf量测更新的收敛精度。

36、第五、本发明通过两阶段解耦架构将状态估计与偏差估计分离，偏差估计仅涉及低维线性kalman更新，barc-kf总计算量约为ckf的6.5倍，在保持计算效率的同时实现了偏差感知鲁棒估计。

37、本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。

技术实现思路