基于状态增广迭代扩展卡尔曼滤波的系统状态估计方法技术方案

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

System state estimation method based on state augmented iterative extended Calman filter

The invention discloses a method to estimate the extended Calman filter algorithm based on iterative augmented system state, the method uses accurate formula to calculate the covariance matrix of measurement prediction, compared with the existing EKF and Modified IEKF algorithm, the invention proposes based on augmented state Diego generation performance extended Calman filter algorithm after calibration to estimate the position error proof velocity and ballistic coefficient, can satisfy the mutual relationship between orthogonal system state and measurement noise, and has faster convergence speed and higher filtering accuracy.

【技术实现步骤摘要】
基于状态增广迭代扩展卡尔曼滤波的系统状态估计方法
本专利技术涉及通信
，特别是涉及一种基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法。
技术介绍
在弹道再入目标跟踪应用中，最常用的算法是扩展卡尔曼滤波(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-1yk＝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算法效果示意图。根据附图可见，本方法可得到更高的估计精度和更快的收敛速度。区别于现有技术，本专利技术的本文档来自技高网...

【技术保护点】
一种基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法，其特征在于，包括：S1：根据系统模型建立非线性系统方程，并对所述非线性系统方程进行初始化滤波，得到初始化系统状态；其中，所述系统状态包括系统状态变量和系统误差协方差更新矩阵；S3：对所述系统模型的非线性系统方程进行标准扩展卡尔曼滤波，计算指定时刻的系统状态；S5：计算所述指定时刻的测量噪声信息，并将计算得到的测量噪声信息增广到所述指定时刻的系统状态中；S7：将所述测量噪声信息对应的状态变量代入所述系统模型中，利用扩展卡尔曼滤波测量更新步骤进行多次迭代，得到经过多次迭代后的系统状态；S9：在所述指定时刻的下一时刻继续进行S3‑S7的步骤，得到多次迭代后的系统状态。

【技术特征摘要】
1.一种基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法，其特征在于，包括：S1：根据系统模型建立非线性系统方程，并对所述非线性系统方程进行初始化滤波，得到初始化系统状态；其中，所述系统状态包括系统状态变量和系统误差协方差更新矩阵；S3：对所述系统模型的非线性系统方程进行标准扩展卡尔曼滤波，计算指定时刻的系统状态；S5：计算所述指定时刻的测量噪声信息，并将计算得到的测量噪声信息增广到所述指定时刻的系统状态中；S7：将所述测量噪声信息对应的状态变量代入所述系统模型中，利用扩展卡尔曼滤波测量更新步骤进行多次迭代，得到经过多次迭代后的系统状态；S9：在所述指定时刻的下一时刻继续进行S3-S7的步骤，得到多次迭代后的系统状态。2.根据权利要求1所述的基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法，其特征在于，所述非线性系统方程为：xk＝f(xk-1)+wk-1yk＝g(xk)+vk；其中，x为系统状态变量；f为非线性系统函数；w为过程噪声；y为测量值；g为非线性测量函数；v为测量噪声；k为时刻值。3.根据权利要求2所述的基于状态增广迭代扩展卡尔曼滤波算法的系统状态估计方法，其特征在于，所述指定时刻为k时刻时，系统的状态预测为：其中，F为非线性系统函数的雅克比矩阵；为k-1时刻的状态误差协方差更新矩阵；T为矩阵转置运算符；Q为过程噪声协方差矩阵。4.根据权利要...

【专利技术属性】
技术研发人员：李伟，柴晶，
申请(专利权)人：太原理工大学，
类型：发明
国别省市：山西,14