基于状态增广迭代扩展卡尔曼滤波的系统状态估计方法与流程

本发明涉及通信技术领域，特别是涉及一种基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法。

背景技术：

在弹道再入目标跟踪应用中，最常用的算法是扩展卡尔曼滤波(extendedkalmanfilter-ekf)。针对强非线性测量模型，为提高跟踪精度，可采用基于状态增广迭代扩展卡尔曼滤波算法。这不但能提高目标跟踪精度，而且在快速反应及目标拦截等方面有重要的实用意义。

经对现有技术文献的检索发现，bellbradleym.和catheyfrederickw.,撰文《theiteratedkalmanfilterupdateasagauss-newtonmethod》[j]//ieeetransactionsonautomaticcontrol,vol.38,no.2,1993,pp.294-297以及杨争斌、钟丹星,郭福成,周一宇，撰文《一种基于高斯牛顿迭代的单站无源定位算法》[j]//系统工程与电子技术，vol.29,no.12,2007,pp.2006-2009。根据上述文献可知，目前有两种迭代型扩展卡尔曼滤波算法，在测量模型为强非线性时，这两种算法均具有比ekf更快的收敛速度和更高的滤波精度。但后一种修改的迭代算法(modifiediteratedextendedkalmanfilter-modifiediekf)存在以下缺点：在滤波测量更新过程中，经过第一次迭代以后，系统状态与测量噪声之间便不再满足相互正交的关系，此时利用标准的计算测量预测协方差矩阵公式会降低滤波精度。因此，重新设计更高滤波精度和相对稳定输出的迭代型扩展卡尔曼滤波算法具有重要的意义。

技术实现要素：

本发明主要解决的技术问题是提供一种基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法，能够满足系统状态与测量噪声之间相互正交的关系，具有更快的收敛速度和更高的滤波精度。

为解决技术问题，本发明采用的一个技术方案是：提供一种基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法，该方法的步骤包括：s1：根据系统模型建立非线性系统方程，并对非线性系统方程进行初始化滤波，得到初始化系统状态；其中，系统状态包括系统状态变量和系统误差协方差更新矩阵；s3：对系统模型的非线性系统方程进行标准扩展卡尔曼滤波，计算指定时刻的系统状态；s5：计算指定时刻的测量噪声信息，并将计算得到的测量噪声信息增广到指定时刻的系统状态中；s7：将测量噪声信息对应的状态变量代入系统模型中，利用扩展卡尔曼滤波测量更新步骤进行多次迭代，得到经过多次迭代后的系统状态；s9：在指定时刻的下一时刻继续进行s3-s7的步骤，得到多次迭代后的系统状态。

区别于现有技术，本发明的基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法采用准确的公式计算测量预测协方差矩阵，与现有ekf和modifiediekf算法相比，本发明提出的基于状态增广迭代扩展卡尔曼滤波算法性能经过对位置、速度和弹道系数估计误差的校验证明，能够满足系统状态与测量噪声之间相互正交的关系，具有更快的收敛速度和更高的滤波精度。

附图说明

图1是本发明提供的一种基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法的流程示意图；

图2是本发明提供的一种基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法中位置估计误差的性能比较示意图；

图3是本发明提供的一种基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法中速度估计误差的性能比较示意图；

图4是本发明提供的一种基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法中弹道系数估计误差的性能比较图。

具体实施方式

在下面的描述中阐述了很多具体细节以便于充分理解本发明。但是本发明能够以很多不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施的限制。

其次，本发明利用示意图进行详细描述，在详述本发明实施例时，为便于说明，所述示意图只是实例，其在此不应限制本发明保护的范围。

参阅图1，图1是本发明提供的一种基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法的流程示意图。

该方法的步骤包括：

s1：根据系统模型建立非线性系统方程，并对非线性系统方程进行初始化滤波，得到初始化系统状态；其中，系统状态包括系统状态变量和系统误差协方差更新矩阵。

其中，非线性系统方程设为：

xk＝f(xk-1)+wk-1

yk＝g(xk)+vk；

其中，x为系统状态变量；f为非线性系统函数；w为过程噪声；y为测量值；g为非线性测量函数；v为测量噪声；k为时刻值。

且在k＝0时，设定初始化系统状态变量为系统状态误差协方差更新矩阵为

s3：对系统模型的非线性系统方程进行标准扩展卡尔曼滤波，计算指定时刻的系统状态预测。

指定时刻为k时刻时，系统状态预测为：

其中，f为非线性系统函数的雅克比矩阵；为k-1时刻的状态误差协方差更新矩阵；t为矩阵转置运算符；q为过程噪声协方差矩阵。

s5：计算指定时刻的测量噪声信息，并将计算得到的测量噪声信息增广到指定时刻的系统状态中。

将计算得到的测量噪声信息增广到所述指定时刻的系统状态后，得到的系统状态为：

其中，a表示系统状态增广；blkdiag表示矩阵对角化；1表示第一次迭代。

s7：将测量噪声信息对应的状态变量代入系统模型中，利用扩展卡尔曼滤波测量更新步骤进行多次迭代，得到经过多次迭代后的系统状态。

利用扩展卡尔曼滤波测量更新步骤进行多次迭代时的更新公式包括：

其中，k为卡尔曼增益矩阵；h为非线性测量函数的增广雅克比矩阵；为系统状态的测量更新；i为单位矩阵。

经过j次迭代以后，系统状态估计及相应的误差协方差更新矩阵为:

其中：nx为系统状态变量的维数。

s9：在指定时刻的下一时刻继续进行s3-s7的步骤，得到多次迭代后的系统状态。

示例的，以利用雷达测量进行弹道再入目标进行实时跟踪作为实施例进行说明。

动态系统模型为：

其中，x1是弹道再入目标的高度；x2是下落速度；x3是恒定的弹道参数；η是已知的反大气密度标高。

雷达测量方程:

其中，a是雷达站的高度；b是再入目标与雷达之间的水平距离；vk是零均值的测量高斯噪声；tk表示第k时刻。

具体的仿真参数和初始条件分别在表1和表2中所示；

表1

表2

步骤一、初始化，取k＝1，具体参数设置由初始条件表2给出。

步骤二、利用标准扩展卡尔曼滤波公式计算第k时刻的状态预测估计及相应的误差协方差预测矩阵

步骤三、将测量噪声信息增广到系统状态中，即：

步骤四、将测量噪声信息对应的状态包含到测量模型中，即新的测量模型为：利用状态增广扩展卡尔曼滤波测量更新步骤进行3次迭代。

步骤五、经过3次迭代以后，系统状态估计及相应的误差协方差更新矩阵为:

步骤六、k＝k+1，并返回到第二步进行下一时刻的状态估计。

步骤七、对所提出的基于状态增广迭代扩展卡尔曼滤波算法性能经过对位置、速度和弹道系数估计误差进行校验。

如图2-图4所示，本实施例的基于状态增广迭代扩展卡尔曼滤波(stateaugmentation-basedmodifiediteratedkalmanfilter-sm-iekf)算法相比ekf和modifiediekf算法效果示意图。根据附图可见，本方法可得到更高的估计精度和更快的收敛速度。

区别于现有技术，本发明的基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法采用准确的公式计算测量预测协方差矩阵，与现有ekf和modifiediekf算法相比，本发明提出的基于状态增广迭代扩展卡尔曼滤波算法性能经过对位置、速度和弹道系数估计误差的校验证明，能够满足系统状态与测量噪声之间相互正交的关系，具有更快的收敛速度和更高的滤波精度。

本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用所述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。