[发明专利]一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法

说明书

本发明公开一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法，对非线性随机系统的统计测量噪声建立双边限幅的约束模型，根据最大后验估计思想构建目标函数，通过非线性最小二乘牛顿迭代方法近似求解可行域，选择可行域并进行传递和更新，从而避免了非线性系统高阶海森矩阵的计算及状态更新过程中计算新息协方差可能出现的不可逆病态矩阵，保证系统的卡尔曼滤波增益为有界、统计误差收敛。

技术领域

本发明涉及非线性滤波技术领域，具体是一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法。

背景技术

在实际工程应用中，对于非线性随机动态系统状态估计，由于观测系统内在的非线性及外在测量环境的不确定性，最优卡尔曼滤波算法(Kalman filter,KF)得不到解析解。扩展卡尔曼滤波(extended Kalman filter,EKF)算法的基本思想是在卡尔曼滤波估计点对非线性系统函数进行线性化处理。该算法由于实现简单，计算效率高，是目前在实际工程中是应用最广泛的非线性滤波算法。然而，由于线性化误差的存在，在动态系统非线性较强或测量噪声较大时，扩展卡尔曼滤波算法难以获得理论上的稳态解，滤波性能下降甚至发散。针对该问题，正交卡尔曼滤波(Quadrature Kalman filtering,QKF)算法避免点估计近似，相应地计算量增大；另外还有文献提出了多模态正交卡尔曼滤波(MultipleQuadrature Kalman filtering,MQKF)，将原始空间划分为若干子空间，采用Gauss-Hermite正交规则传递有效子空间的点集，利用正交卡尔曼滤波提供的估计不确定性来改善滤波器之间的相互作用，同样存在运算量大的问题。

例如：

专利号CN 103312297 A公开了一种迭代扩展增量卡尔曼滤波方法，通过利用量测方程的增量形式消除了量测方程中未知的系统误差，从而建立了精确的增量量测方程；在此基础之上，利用迭代方法来优化对非线性量测方程进行线性化产生的误差。上述技术方案虽然在一定程度上减少了发散现象，但是同样难以获得理论上的稳态解；

专利号CN102788976A公开了一种高量级扩展卡尔曼滤波方法，将状态的初始滤波值中的元素和距离的量测值均用高量级表示，因为使用了更高的量级，滤波值和距离量测值在数值上减小了，也就是将实际中目标的远距离转换成数据处理中的“近距离”。距离在数值上的减小降低了滤波中坐标转换的非线性，从而在使用扩展卡尔曼滤波时线性化误差减小，滤波精度虽然比没有使用量级转换的经典扩展卡尔曼滤波要高。但是上述方案无法做到控制卡尔曼滤波增益为有界，避免在状态更新过程中计算新息协方差可能出现病态矩阵。

发明内容

针对上述现有技术中的不足，本发明提供一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法。

为实现上述目的，本发明提供一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法，包括如下步骤：

步骤一：建立约束离散非线性随机系统

对于非线性随机系统的状态估计问题，离散时间状态模型包括系统状态过程和系统测量模型，其中，系统状态过程模型将当前时刻状态向量和前向一时刻状态向量相关联，系统测量模型将状态向量和测量向量相关联，具体为：

X_k+1＝F_k(X_k)+V_k (1)

Z_k＝H_k(X_k)+e_k (2)

式(1)-(2)中，k是跟踪时刻；表示k时刻n_X维的系统状态向量；是k时刻n_Z维的量测向量；F_k与H_k分别表示已知的状态转移矩阵和测量矩阵；V_k和e_k分别是过程噪声和测量噪声；