层次贝叶斯量测噪声协方差的估计方法、装置及存储介质与流程

本发明属于状态估计与滤波，涉及工业物联网信息感知、传感技术服务及工业信息和数据处理服务。更具体地说，本发明涉及层次贝叶斯量测噪声协方差的估计方法、装置及存储介质。

背景技术：

1、卡尔曼滤波器是目标跟踪、导航定位、多传感器融合等领域中状态估计的基础工具。当系统为线性且噪声服从高斯分布时，卡尔曼滤波器在最小均方误差意义下提供最优状态估计。然而，该最优性的成立严格依赖于过程噪声协方差矩阵和量测噪声协方差矩阵的精确已知。在实际工程应用中，量测噪声协方差矩阵往往是未知的或随时间变化的。传感器的测距精度会随着目标距离的增加而降低，红外传感器的噪声水平会随着环境温度的变化而漂移，多传感器融合系统中各传感器通道的噪声特性差异可达数个数量级。当量测噪声协方差矩阵不准确时，卡尔曼滤波器的估计精度会严重下降，甚至导致滤波器发散。

2、为解决量测噪声协方差矩阵未知或时变的问题，现有技术中已经发展出多种自适应估计方法。变分贝叶斯r自适应方法将量测噪声协方差矩阵建模为逆威沙特分布，利用变分贝叶斯推断从新息序列中递推更新其后验分布。该方法使用一个自由度参数控制所有量测维度的估计，即各个量测通道的噪声被假定为服从同一先验分布。这一假设在工程中往往不成立。例如，在传感器目标跟踪中，距离测量噪声与角度测量噪声的方差可能相差六个数量级，一个统一的自由度参数无法同时匹配这两种差异悬殊的噪声特性，导致小噪声维度的估计精度受到大噪声维度的拖累。

3、sage-husa型减法估计方法利用新息协方差与理论新息协方差之差来估计量测噪声协方差矩阵。该方法直接使用新息外积作为量测噪声协方差矩阵的经验估计。新息是实际量测值与预测量测值之差，它同时包含由过程噪声引起的预测误差和量测噪声两部分贡献。当直接使用新息外积来估计量测噪声时，预测误差的贡献被不可避免地归入量测噪声估计中，导致量测噪声协方差矩阵被系统性高估。当过程噪声协方差矩阵本身也存在不确定性时，这一问题会进一步恶化，量测噪声估计与过程噪声估计之间形成正反馈，滤波器的一致性难以维持。

4、还有一些方法假设量测噪声协方差矩阵为对角矩阵，对各通道逐维独立估计噪声方差。这类方法放弃了量测通道之间的相关性信息，当量测通道之间存在非对角线协方差时，该假设引入了模型误差。同时，这类方法对异常新息较为敏感，单帧的异常量测可能导致某一维度的噪声方差估计发生剧烈跳变。

5、近期出现了将逆威沙特先验与伽马先验结合为层次贝叶斯框架的方法，但其技术目的主要集中在建模重尾分布或检测异常值。这类方法的层次先验服务于对噪声分布形态的建模，而非对量测噪声协方差矩阵中不同维度之间量级差异的处理。

6、此外，上述各类方法在减法估计出现负特征值时均缺乏合理的修复策略。当滤波器的预测协方差较大时，预测不确定性项在数值上可能超过新息外积，导致减法结果出现负特征值，这在物理意义上是不合理的。现有方法对此类情况的处理方式主要是回退到原始新息外积或采用半衰混合策略，这些做法均会在估计中引入难以控制的系统偏差。

7、综上所述，现有技术中尚缺乏一种能够同时处理各量测维度噪声量级差异、消除过程噪声与量测噪声混淆偏差、并具备合理负定修复策略的量测噪声协方差自适应估计方法。

技术实现思路

1、本发明的目的是提供一种层次贝叶斯量测噪声协方差的估计方法、装置及存储介质，通过构造全局逆威沙特分布层和逐维伽马分布层组成的双层层次贝叶斯先验结构，实现对各量测维度的独立自适应建模，解决各量测维度噪声量级差异问题；通过无偏新息分解显式从新息外积中减去预测不确定性项，消除过程噪声与量测噪声的混淆偏差；通过特征值投影正半定修复，在仅丢弃负定方向上的非物理解的同时保留所有正半定方向上的有效信息，保证估计的渐近无偏性，解决负定结果缺乏无偏修复策略的问题；通过层次融合实现全局结构约束与逐维独立自适应的自动平衡，无需人工设定各维度的独立学习率，从而获得量测噪声协方差矩阵的无偏且各向异性的在线自适应估计，适用于目标跟踪、导航定位、传感器融合等需要递推状态估计且量测噪声统计特性未知或时变的应用场景，可应用于工业物联网信息感知与传感技术服务及工业信息和数据处理服务领域。

2、为了解决上述问题，并实现本发明的目的和其它优点，提供了一种层次贝叶斯量测噪声协方差的估计方法，包括以下步骤：

3、s1，构造层次贝叶斯先验结构：为量测噪声协方差矩阵构造双层层次贝叶斯先验，其中m为量测向量的维数；

4、所述双层层次贝叶斯先验包括：

5、全局逆威沙特分布层：量测噪声协方差矩阵服从逆威沙特分布，其中为全局尺度矩阵，为全局自由度；

6、逐维伽马分布层：对于每个量测维度i，量测噪声协方差矩阵的对角元素对应的精度服从伽马分布，其中为形状参数，为速率参数，为第i个量测维度伽马分布的形状参数分子，满足，为第i个量测维度伽马分布的速率参数分子；

7、s2，无偏新息分解：在每个递推时刻k，获取当前帧的新息向量，其中为当前量测向量，为量测预测向量；

8、根据新息协方差的定义：

9、；

10、其中上标t表示矩阵转置，为量测雅可比矩阵，为预测状态协方差矩阵；

11、通过以下子步骤计算无偏经验估计矩阵：

12、s21，获取当前帧的预测协方差cholesky因子，满足，以及量测-状态互协方差矩阵；

13、s22，由，通过前向代入求解三角方程组，得到中间变量矩阵，则：

14、；

15、s23，计算无偏经验估计矩阵：

16、；

17、s3，特征值投影正半定修复：对s23得到的进行对称化处理，随后对对称化后的矩阵执行特征分解，将分解出的负特征值投影至零，重构得到满足正半定要求的经验估计矩阵；

18、s4，全局逆威沙特分布层后验递推更新：以遗忘因子ρ驱动全局逆威沙特先验的超参数进行指数滑动平均递推更新：

19、，；

20、其中和分别为当前时刻k的全局自由度和全局尺度矩阵；和分别为上一时刻k-1的全局自由度和全局尺度矩阵；

21、若，则依据逆威沙特分布的均值公式计算当前帧的全局逆威沙特分布层后验期望全矩阵估计：

22、；

23、若，则当前帧的全局逆威沙特分布层后验期望全矩阵估计保持上一时刻的值不变；

24、输出当前帧的全局逆威沙特分布层后验期望全矩阵估计；

25、s5，逐维伽马分布层后验递推更新：对于每个量测维度i，使用新息向量的第i个分量的平方值进行底层先验参数的递推更新：

26、，；

27、其中和分别为当前时刻k和上一时刻k-1的第i个量测维度的形状参数分子；和分别为当前时刻k和上一时刻k-1的第i个量测维度的速率参数分子；

28、若，则计算该维度的逐维后验期望估计：

29、；

30、s6，层次融合：s6接收s4输出的全局逆威沙特分布层后验期望全矩阵估计，构建当前帧最终输出的量测噪声协方差矩阵，其中表示量测噪声协方差矩阵的第i个对角元素，表示量测噪声协方差矩阵的第i行第j列非对角元素；表示全局逆威沙特分布层后验期望全矩阵估计的第i个对角元素；表示全局逆威沙特分布层后验期望全矩阵估计的第i行第j列非对角元素；

31、对于每个量测维度i，确定的第i个对角元素：

32、若，则：

33、；

34、其中融合权重系数ω∈[0.5,0.8]；

35、若，则：

36、；

37、对于所有非对角元素，即i ≠ j且i,j∈{1,…,m}，由全局逆威沙特分布层后验期望全矩阵估计提供：

38、；

39、至此，量测噪声协方差矩阵的所有元素均已确定，然后对整个量测噪声协方差矩阵执行对称化，并将每个对角元素限制在预设区间；

40、s7，输出量测噪声协方差矩阵，用于下一帧滤波器的量测更新步骤。

41、优选的是，所述的层次贝叶斯量测噪声协方差的估计方法中，s6中所述对整个量测噪声协方差矩阵执行对称化，具体为将量测噪声协方差矩阵赋值为。

42、优选的是，所述的层次贝叶斯量测噪声协方差的估计方法中，s1中所述各超参数的初始值按以下方式设定：

43、全局自由度的初始值；

44、全局尺度矩阵的初始值，其中为初始量测噪声协方差矩阵；

45、各量测维度伽马分布的初始形状参数；

46、各量测维度伽马分布的初始速率参数，其中为初始量测噪声协方差矩阵的对角元素。

47、优选的是，所述的层次贝叶斯量测噪声协方差的估计方法中，s3中所述特征值投影正半定修复具体包括：

48、对进行特征分解，其中为特征向量矩阵，为矩阵的第i个特征值；

49、将负特征值投影至零：，其中为投影后的特征值；

50、重构正半定矩阵：

51、。

52、优选的是，所述的层次贝叶斯量测噪声协方差的估计方法中，所述遗忘因子ρ的取值范围为 [0.9,0.99]。

53、优选的是，所述的层次贝叶斯量测噪声协方差的估计方法中，指数遗忘因子递推的稳态值为趋近于1/(1-ρ)。

54、优选的是，所述的层次贝叶斯量测噪声协方差的估计方法中，逐维伽马分布层的指数遗忘因子递推 的稳态值为趋近于1/(1-ρ)。

55、优选的是，所述的层次贝叶斯量测噪声协方差的估计方法中，s6中，将每个对角元素限制在预设区间内，其中和分别为第i个量测维度的噪声方差下限和上限；

56、所述预设区间对于传感器测距维度取噪声方差下限，上限，对于传感器测角维度取噪声方差下限，上限。

57、优选的是，所述的层次贝叶斯量测噪声协方差的估计方法中，s6中， 计算出量测噪声协方差矩阵的第i行第j列非对角元素后，将其值约束在区间内，防止全局尺度矩阵递推过程中非对角元素的累积数值爆炸。

58、优选的是，所述的层次贝叶斯量测噪声协方差的估计方法中，s2中，当cholesky三角求解出现数值异常导致无偏经验估计矩阵计算失败时，以最近一次成功计算的经验估计矩阵作为当前帧的经验估计矩阵；若当前为第一帧且无历史帧可用，则以当前帧的新息外积矩阵作为当前帧的无偏经验估计矩阵。

59、优选的是，所述的层次贝叶斯量测噪声协方差的估计方法中，s6中所述融合权重系数ω按以下方式自适应确定：

60、

61、其中为新息归一化统计量的指数移动平均值，τ为nis期望阈值；当滤波器工作正常时，，ω趋近于0.5；当过程噪声协方差矩阵失配导致偏离τ时，ω自适应提高至0.8，加大全局逆威沙特分布层的无偏锚定权重，抑制逐维伽马分布层因过程噪声污染引入的偏差累积。

62、优选的是，所述的层次贝叶斯量测噪声协方差的估计方法中，在前n帧，n≥2，s1至s6各步骤正常执行并获得量测噪声协方差矩阵估计值后，s7中用于下一帧量测更新的操作暂缓执行，下一帧滤波器的量测更新步骤使用初始量测噪声协方差矩阵进行滤波更新，待递推帧数超过n帧后，执行s7步骤将传递至下一帧卡尔曼滤波器的量测更新步骤。

63、一种层次贝叶斯量测噪声协方差的估计装置，包括：

64、先验构造模块，用于在卡尔曼滤波器初始化阶段为量测噪声协方差矩阵构造双层层次贝叶斯先验结构，其中全局层为逆威沙特分布，逐维伽马分布层为各量测维度独立的伽马分布；

65、无偏新息分解模块，用于在每个递推时刻k获取新息向量，获取当前帧的预测协方差cholesky因子，满足，以及量测-状态互协方差矩阵，通过前向代入求解三角方程组得到中间变量矩阵，则，并从新息外积中显式减去预测不确定性项，得到无偏经验估计矩阵；

66、特征值投影修复模块，用于对所述无偏经验估计矩阵进行对称化处理，执行特征分解，将负特征值投影至零并重构正半定矩阵；

67、全局逆威沙特更新模块，用于以遗忘因子ρ驱动全局逆威沙特分布层的后验参数进行指数滑动平均递推更新，递推公式为和；当时，依据逆威沙特分布的均值公式计算全局逆威沙特分布层后验期望全矩阵估计；当时，所述全局逆威沙特分布层后验期望全矩阵估计保持上一时刻的值不变；并输出当前帧的所述全局逆威沙特分布层后验期望全矩阵估计；

68、逐维伽马更新模块，用于对于每个量测维度i使用新息向量的第i个分量的平方值更新伽马后验参数，递推公式为和；若，则计算逐维后验期望；

69、层次融合模块，用于接收全局逆威沙特分布层后验期望全矩阵估计，构建当前帧最终输出的量测噪声协方差矩阵，其中为量测噪声协方差矩阵的第i个对角元素，为量测噪声协方差矩阵的第i行第j列非对角元素；为全局逆威沙特分布层后验期望全矩阵估计 的第i个对角元素，为全局逆威沙特分布层后验期望全矩阵估计的第i行第j列非对角元素；对于每个量测维度i，当时，将全局逆威沙特分布层后验期望全矩阵估计的对角元素与逐维后验期望进行加权融合，得到；当时，直接取全局逆威沙特分布层后验期望全矩阵估计的对应值；对于所有非对角元素，即i ≠ j且i,j∈{1,…,m}，由全局逆威沙特分布层后验期望全矩阵估计提供；并对所述量测噪声协方差矩阵执行对称化，并将每个对角元素限制在预设区间。

70、一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述的层次贝叶斯量测噪声协方差的估计方法。

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

72、本发明通过构造全局逆威沙特分布层和逐维伽马分布层组成的双层层次贝叶斯先验结构，实现了各量测维度噪声的独立自适应建模。全局逆威沙特分布层捕获全矩阵结构包含非对角线相关性信息，提供整体结构性约束；逐维伽马分布层对各量测通道独立建模，每个维度拥有独立的形状参数和速率参数，允许各维度以不同速率独立自适应。自适应权重融合策略（ω默认取0.5即等权重融合，当nis偏离时ω按偏离程度在[0.5,0.8]范围内线性连续调整至最高0.8），从根本上解决了单一逆威沙特先验中各维度噪声方差跨数量级差异时小噪声维度估计被大噪声维度淹没的问题。

73、本发明通过无偏新息分解消除了过程噪声与量测噪声的混淆偏差。利用新息协方差定义式，显式从新息外积中减去预测不确定性项，使量测噪声的经验估计在期望意义上等于真实的量测噪声协方差矩阵。通过cholesky因子三角求解实现预测不确定性项的高效计算，避免了对预测协方差矩阵的直接求逆，在提高计算效率和数值稳定性的同时消除了过程噪声贡献被错误归因于量测噪声的系统性偏差。

74、本发明通过特征值投影正半定修复提供了无偏的负定修复策略。当减法产生负特征值时，对无偏经验估计矩阵进行特征分解，将负特征值投影至零即，重构得到正半定矩阵。仅丢弃负定方向上的非物理解，保留所有正半定方向上的有效信息，与传统的全矩阵回退策略或半衰混合策略相比不引入正半定子空间上的系统偏差。当滤波器进入稳态后，预测状态协方差矩阵趋于稳定值，出现显著负特征值的概率趋近于零，修复引入的截断偏差逐步消失，保证了估计的渐近无偏性。

75、本发明通过层次融合实现了全局结构约束与逐维独立自适应的自动平衡。采用自适应权重融合策略（默认0.5:0.5，nis偏离时倾斜至0.5:0.8）将全局逆威沙特分布层后验期望与逐维伽马分布层后验期望进行融合，对于的量测维度执行自适应权重融合，融合权重系数ω默认取0.5（即等权重融合），使全局逆威沙特分布层的无偏锚定与逐维伽马分布层的独立自适应能力各贡献一半；当新息归一化统计量的指数移动平均值偏离期望阈值时，ω自适应提高至最高0.8，加大全局逆威沙特分布层的无偏锚定权重，有效抑制过程噪声协方差矩阵失配时逐维伽马分布层的偏差累积；对于的量测维度直接取全局逆威沙特分布层后验期望对应值，避免因逐维估计不可靠而引入偏差，非对角元素全部由全局逆威沙特分布层提供。当各维度噪声水平相近时，两层估计接近，自适应权重融合退化为简单平均；当某维度噪声异常增大时，逐维伽马后验期望显著偏离全局逆威沙特分布层后验期望对角线，等权重融合自动向偏离方向倾斜。该融合机制无需人工设定各维度的独立学习率，简化了工程部署流程。

76、本发明通过各超参数的初始值设定，保证了方法从第一帧起即可正常工作。全局自由度初始值严格大于m+1，保证了全局逆威沙特分布层后验期望的一阶矩从第一帧起即存在；各量测维度伽马分布的初始形状参数满足大于2的条件，保证了逆伽马分布一阶矩在逐维修正开始前即存在。全局尺度矩阵初始值和初始速率参数的设定使得全局逆威沙特分布层在第一帧的后验期望估计值等于，逐维伽马分布层在第一帧的后验期望估计值等于，即两个层次在第一帧的估计值均等于工程设计人员根据传感器规格预先设定的初始量测噪声协方差矩阵。后续两层根据不同的更新机制独立演进，无需预热阶段或额外的参数调试。

77、本发明通过遗忘因子ρ=0.95的取值，在响应速度与估计稳定性之间取得了最优平衡。全局自由度的稳态值为20，等效记忆窗口约为20帧，既足够长以滤除新息序列中的随机波动，又足够短以跟踪量测噪声协方差矩阵的时变特性。对于量测维数m=3的典型配置，全局自由度稳态值20严格大于4，全局逆威沙特分布层后验期望一阶矩在稳态下恒存在；逐维形状参数稳态值20严格大于2，逐维伽马分布层后验期望一阶矩在稳态下恒存在。全局逆威沙特分布层不冻结而是进入稳态工作区，实现了锚定不僵化的长期运行鲁棒性。

78、本发明通过对称化操作消除了计算过程中因数值舍入引入的非对称误差。在层次融合完成后显式执行对称化，保证最终输出的量测噪声协方差矩阵估计严格满足对称性要求，符合协方差矩阵的基本定义，从而保证了后续卡尔曼滤波量测更新中涉及矩阵求逆、cholesky分解等数值运算的稳定性和可靠性，提高了滤波器在长时运行中的数值鲁棒性。

79、本发明通过将每个对角元素限制在按各量测维度分别独立设置的预设区间内，防止了卡尔曼增益异常放大。当量测噪声协方差矩阵的对角元素被估计得过小时，求逆运算将导致卡尔曼增益异常增大，滤波器过度信任当前量测而忽略动态模型的预测信息，状态估计可能出现剧烈振荡甚至发散。传感器测距维度下限，对应标准差10m，上限，对应标准差100m，测角维度下限，对应标准差0.001rad，上限，对应标准差约0.032rad。下限值既保证了数值稳定性，又不会过高以致抑制正常的噪声方差估计。上限的对应设定防止了因估计过大导致滤波器完全忽略量测信息，保证了滤波器对量测的基本响应能力。

80、本发明计算量可控，工程实用性强。各步骤均采用解析计算方式，每帧的计算开销主要来自一次cholesky三角求解和一次特征分解，在典型量测维数为3的配置下增量计算成本约为基底滤波器的5%-10%，远低于需要迭代优化的变分贝叶斯类方法和蒙特卡洛或粒子滤波类方法。模块化设计便于在嵌入式平台、数字信号处理器、图形处理器或现场可编程门阵列等不同硬件平台上实现，可与容积卡尔曼滤波器、无迹卡尔曼滤波器或扩展卡尔曼滤波器等各类非线性滤波器配合使用，无需修改基底滤波器的核心结构即可实现量测噪声协方差的自适应估计功能，具有良好的可集成性和可推广性，满足飞行控制、自动驾驶、机器人导航等实时系统对处理时间边界确定性的严格要求。

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