[发明专利]一种非线性目标跟踪系统中平方根容积卡尔曼滤波方法

摘要

本发明公开了一种非线性目标跟踪系统中平方根容积卡尔曼滤波方法，针对自适应平方根容积卡尔曼滤波算法ASCKF，改进时间更新环节与Sage‑Husa噪声估计器环节，形成ASCKFNS算法；本发明适用于非线性离散系统，在保证算法的稳定性的前提下，进一步提高了算法精确性，且大量减小了计算量，提高了实时性。

主权项

1.一种非线性目标跟踪系统中平方根容积卡尔曼滤波方法，其特征在于：针对自适应平方根容积卡尔曼滤波算法ASCKF，改进时间更新环节与Sage‑Husa噪声估计器环节，形成ASCKFNS算法；在时间更新环节，将非线性状态转移函数f根据求一阶偏导的原理求雅克比矩阵代入第k‑1次采样时刻的状态向量后验估计值到雅克比矩阵中，得到第k次采样时刻将非线性系统近似线性化的等效状态矩阵F_k，简化非线性系统的状态转移函数，从而简化时间更新环节，提高系统实时性；在Sage‑Husa噪声估计器环节，将非线性目标跟踪系统量测函数h根据求一阶偏导的原理求雅克比矩阵代入第k次采样时刻的先验状态向量预测值x_k|k‑1到雅克比矩阵中，得到第k次采样时刻将非线性系统近似线性化的等效量测矩阵H_k，进而得到较为简单的噪声敛散判决，在进行第k次滤波时，让噪声敛散判决同时作用于系统噪声协方差矩阵Q_k与量测噪声协方差矩阵R_k，检验是否需要修正Q_k与R_k，这样不用每次滤波都根据系统噪声协方差矩阵Q_k对量测噪声协方差矩阵R_k进行修正，防止长期同时估计Q_k与R_k造成滤波异常；所述改进时间更新环节，其具体实现公式为：S_k|k‑1＝chol(P_k|k‑1)；式中，为第k‑1次采样时刻得到的滤波后系统噪声协方差矩阵Q_k‑1的估计值；P_k‑1|k‑1为第k‑1次采样时刻得到的误差协方差矩阵后验预测值；P_k|k‑1为第k次采样时刻得到的误差协方差矩阵先验预测值；Chol()表示对矩阵进行Cholesky分解，把一个对称正定的矩阵表示成一个下三角矩阵和其转置的乘积的形式；在所述Sage‑Husa噪声估计器环节中，敛散判决推导过程为：v_k≈z_k‑H_kx_k|k‑1；能得敛散判决为：式中，v_k为第k次采样时刻的新息，z_k为第k次采样时刻系统量测向量；为第k‑1次采样时刻滤波后系统噪声Q_k‑1的估计值；tr[]为对矩阵求迹；若上述判决成立，表明量测噪声引起的状态估计值的实际误差将超过理论预计值，需要对量测噪声协方差矩阵Rk进行修正，则与取值分别为：式中，为第k次采样时刻得到的滤波后量测噪声协方差矩阵R_k的估计值；为第k次采样时刻得到的滤波后系统噪声协方差矩阵Q_k的估计值；d_k‑1为遗忘因子，加强新数据对参数估算的影响力；K_k为第k次采样时刻的卡尔曼滤波增益;若上述判决不成立，表明实际误差没超过理论预计值，不需要对量测噪声协方差矩阵Rk进行修正，则与取值分别为：