[发明专利]未知测量噪声方差的平方根高阶容积卡尔曼滤波方法

说明书

技术领域

本发明属于非线性系统的滤波领域，特别涉及一种处理含有未知测量噪声方差的平方根高阶容积卡尔曼滤波方法。

背景技术

在诸多领域中，系统状态的动态估计问题一直是人们关注的焦点。线性高斯系统的状态估计问题，一般采用Kalman滤波方法。但是在处理实际问题时，比如目标跟踪、导航定位以及视频监控等，系统的状态方程或测量方程通常表现出较强的非线性特征。因此，研究非线性系统的状态估计即非线性滤波问题具有重要的理论意义和实际应用价值。扩展Kalman滤波(EKF)是一种最直接、最简单的非线性滤波方法。EKF方法通过对非线性函数的泰勒转换开始进行一阶线性化截断，将非线性问题转化为线性问题，然后利用Kalman滤波方法进行处理。对于一般的非线性系统而言，EKF不能保证其收敛，状态估计的误差较大。无迹Kalman滤波是一类用采样策略逼近非线性分布的方法。其核心是通过一种非线性变换——U变化来进行非线性模型的状态与误差协方差的递推和更新。与EKF不同。UKF不是对非线性模型做近似，而是对状态的概率密度函数做近似，因而能获得比EKF更高的估计精度。为了解决UKF应对高维状态效果不理想的问题，有人提出了3阶容积Kalman滤波(CKF)。该方法利用球形积分准则和径向积分准则优化了UKF中的sigma点采样策略和权重分配，改善了滤波性能。为了进一步估计精度，又有人提出了高阶容积Kalman滤波方法，它能获得与粒子滤波相当的性能，但在计算开销上小于后者。然而，标准的UKF、CKF和HCKF算法由于数值计算舍入误差、可观测性弱(初值误差较大)和观测噪声大等因素影响，可能引起误差协方差矩阵负定，而导致滤波器不稳定，甚至不能工作。为此，众多学者通过采用误差协方差阵的平方根代替协方差阵参加递推运算，提出一系列平方根UKF和平方根CK算法，较好地解决了滤波器不稳定问题。

然而，上述非线性滤波应用的一个先决条件是己知噪声的统计特性，由于实际系统噪声统计特性往往具有不确定性，这就会导致滤波性能下降。对实际应用系统而言，量测噪声方差总是时变未知的，这是因为量测系统受到内外部各种因素的干扰，包括测量误差和环境扰动。也就是说，噪声统计特性未知或者知道的不确切，此时需要在滤波过程中进行估计。

发明内容

针对上述的这些问题，一种以处理未知测量噪声方差的平方根高阶容积Kalman滤波方法应运而生，该方法以HCKF为基础，首先利用QR分解，Cholesky因子更新和高效最小二乘法等矩阵分解技术，提高了滤波算法的运行效率和数值稳定性。接着，在SHCKF基础上，通过采用Sage-Husa估计器实时估算量测噪声的统计特性，有效解决了测量噪声方差位置的非线性系统滤波问题。

本发明是SHCKF的改进形式，包括计算预测状态估计值以及计算预测误差方差阵的平方根S(k|k-1)、计算测量与测量计算互协方差阵P_xz(k|k-1)、计算噪声方差计算信息协方差阵的平方根S_zz(k|k-1)、计算增益阵K(k)、估计状态及协方差阵的平方根S(k|k)。具体内容如下：

步骤1滤波系统初始化：S₀＝chol(p₀)、

步骤2预测过程，具体包括：

(2.1)计算高阶容积点：x_i(k-1|k-1)

(2.2)计算传播后的容积点

(2.3)状态预测估计值

(2.4)计算预测误差方差阵的平方根S(k|k-1)

步骤3更新过程

(3.1)计算容积点(i＝0,1,…2n²)，x_i(k|k-1)

(3.2)计算传播后的容积点z_i(k|k-1)

(3.3)测量预测

(3.4)计算互协方差阵P_xz(k|k-1)和增益阵K(k)(下标xz表示状态和测量值的互协方差)

(3.5)在线估计v(k)的方差

(3.6)计算新息方差阵的平方根S_zz(k|k-1)

(3.7)计算增益阵K(k)

(3.8)计算估计状态及其协方差的平方根S(k|k)。

本发明的有益效果：首先和HCKF相比，提高了滤波算法的运行效率和数值稳定性。并且在HCKF的基础上通过采用Sage-Husa估计器实时估算擦亮噪声的统计特性，有效解决了测量噪声方差位置的非线性系统滤波问题。

附图说明