[发明专利]局部估计自适应两阶段平方根容积滤波的方法

说明书

本发明公开了一种局部估计自适应两阶段平方根容积滤波的方法，包括：首先推导出采用误差协方差矩阵的平方根代替协方差矩阵参加递推运算的平方根两阶段容积滤波算法；然后提出自适应两阶段平方根容积滤波算法；在两个阶段的滤波器中分别估计该阶段的统计特性未知的噪声，然后分别改善各自滤波器的估计效果，再用新得到的系统状态和误差协方差分别再次修正各自滤波器的噪声统计特征，形成两个滤波器各自的递推循环。本发明通过在局部估计自适应两阶段平方根容积Kalman滤波算法(ATSCKF‑G)中，对于量测噪声统计特性的估计是采用Sage‑Husa滤波器直接进行估计，然后将估计出来的统计特性值作为已知条件。

技术领域

本发明涉及滤波估计领域，具体地说，特别涉及一种局部估计自适应两阶段平方根容积滤波的方法。

背景技术

非线性滤波算法是在非线性系统模型下利用离散的传感器观测量来估计目标的连续状态并滤去随机噪声的过程。目前几种常见的非线性Kalman滤波有各自的优缺点。扩展Kalman滤波(EKF)将非线性函数进行泰勒展开，忽略高维项进行线性化，但这种方法只适合系统模型足够平滑的弱非线性函数，若系统是强非线性系统，则会因为滤波误差较大而失去有效性，同时在计算时需要计算雅克比矩阵，计算量较大。无迹Kalman滤波(UKF)不需要计算Jacobian矩阵，计算量与EKF相当，在强非线性系统下也有较高的滤波精度。但是UKF没有经过严格的数学推导，在状态维数较高时，滤波精度会下降。采用三度 Spherical-Radial容积规则近似非线性函数传递的后验均值和协方差的容积 Kalman滤波，经过了严密的数学推导，在理论上具有严格的保证。在滤波迭代过程中，有些因素的存在会导致误差协方差矩阵负定，比如数值计算中的舍入误差、初始值误差较大和观测噪声较大等，从而导致滤波器不稳定，甚至无法工作。

发明内容

为了解决现有技术的问题，本发明实施例提供了一种局部估计自适应两阶段平方根容积滤波的方法。所述技术方案如下：

一方面，提供了一种局部估计自适应两阶段平方根容积滤波的方法，包括：

首先推导出采用误差协方差矩阵的平方根代替协方差矩阵参加递推运算的平方根两阶段容积Kalman滤波算法；

然后基于Sage-Husa滤波算法，提出自适应两阶段平方根容积Kalman滤波算法；

在两阶段容积Kalman滤波的过程中，整体的对噪声未知统计特性进行估计，将估计出的噪声统计特性作为已知条件进行递推估计。

进一步的，所述平方根两阶段容积Kalman滤波算法中的系统模型具体为；

带有随机偏差的非线性高斯系统模型可表示为：

其中k是离散时间序列，x_k∈R^n×1是系统的状态向量，b_k∈R^p×1是系统偏差向量，z_k∈R^m×1是量测向量，f_k(·)和h_k(·)为已知的非线性状态转移函数和量测函数且在x_k处连续可微，是系统噪声序列、是偏差噪声序列，υ_k是量测噪声序列，均为高斯白噪声序列。其中均值为E(υ_k)＝r，方差和R_k满足如下条件：

令

H_k(X_k)＝h_k(x_k)+F_kb_k

系统模型可以改写为如下形式：