[发明专利]基于噪声补偿的迭代平方根CKF的汽车雷达目标跟踪方法

权利要求书

1.一种基于噪声补偿的迭代平方根CKF的汽车雷达目标跟踪方法，其特征在于，包括以下步骤：

步骤1：建立车载雷达目标跟踪的数学模型；

步骤2：初始化步骤1中数学模型的汽车雷达目标跟踪系统参数；

步骤3：汽车雷达目标跟踪系统的滤波算法在时间更新阶段使用SRCKF的预测方法，即进行容积采样和非线性传递后，对汽车雷达目标跟踪系统的状态和误差协方差平方根进行一步预测；

步骤4：汽车雷达目标跟踪滤波算法的量测更新阶段，在SRCKF的基础上引入Gauss-Newton迭代方法和噪声补偿方法，以步骤3中状态和误差协方差平方根的预测值为初值进行迭代更新，在迭代状态估计中引入噪声补偿因子；直至连续二次迭代的状态估计差值小于预设的阈值或者迭代次数达到预设最大迭代次数，迭代即停止；

步骤1-4的具体步骤如下：

步骤一、在车体直角坐标系下建立车载汽车雷达目标跟踪系统的运动学模型和观测模型如下：

汽车雷达目标跟踪系统在以下简称系统，其中，k为离散时间变量，X_k表示k时刻系统的状态向量；z_k表示k时刻系统的观测值；F和h(·)分别为系统的状态方程和非线性观测方程；w_k-1和v_k分别表示过程噪声和观测噪声，其方差分别为Q_k-1和R_k，Q_k-1和R_k分别服从关于N(0，Q_k-1)和N(0，R_k)的高斯分布；F采用常加速模型；

步骤二、初始化系统参数：即待测目标的初始状态变量x₀、状态误差协方差平方根S₀、过程噪声平方根Q₀和观测噪声平方根R₀；

步骤三、假设k-1时刻系统状态的后验密度函数：已知，P_k-1＝(S_k-1)^T(S_k-1)；其中x_k-1是k-1时刻系统的状态，是此刻状态的后验估计，P_k-1为状态误差协方差矩阵，S_k-1为P_k-1的平方根因子，上标T为转置，记n维单位向量为e＝[1，0，...，0]^T，用[1]表示对e的元素进行全排列和改变元素符号产生的点集，即完整全对称点集，[1]_j表示点集的第j个点，j＝1，2，...，m，m＝2n，n为系统状态维度；预测k时刻系统的状态和协方差平方根矩阵，利用容积法则计算系统当前状态的2n个容积点：式中ξ_j为容积点向量，容积点的权值w_j＝1/m，X_j，k-1为状态的第j个容积点；

步骤四、通过步骤一中的状态方程F传播容积点：其中为第j个容积点的一步预测值；

步骤五、计算k时刻加权平均得到的系统状态的一步预测值

步骤六、计算k时刻状态误差协方差平方根的预测值其中，qr(·)为QR分解函数，计算时取QR分解返回值的转置矩阵，为计算用到的中间变量或矩阵，Q_k是k时刻的过程噪声方差；

步骤七、量测更新是以预测得到的和为初始值的迭代过程，初始误差协方差矩阵记k时刻第i次迭代的状态估计和协方差平方根分别为和当i＝0时，令i＝1；

步骤八、计算更新的容积点：是第i-1次迭代状态估计的第j个容积点，和分别为第i-1次迭代的状态误差协方差平方根估计和状态估计；

步骤九、通过步骤一中的h(·)传播容积点：为第i-1次迭代时的第j个容积点量测值；

步骤十、计算量测估计值：为第i-1次迭代时的容积点量测值加权平均所得的量测估计值；

步骤十一、估计新息协方差平方根和互协方差矩阵：和分别是第i-1次迭代的信息协方差平方根和互协方差矩阵估计值，和为计算过程中的中间变量或矩阵；

步骤十二、估计第i-1次迭代时的滤波增益

步骤十三、用步骤一中的h(·)传播第i-1次迭代得到的状态估计：

步骤十四、基于k时刻系统的观测值zk，更新第i次迭代时系统状态估计值：其中，λ是噪声补偿因子，上标(-1)表示求倒；

步骤十五、更新系统第i次迭代的状态误差协方差平方根

步骤十六、判断迭代终止条件或i＝N_iter，其中，ε和N_iter分别为预先设置的阈值和最大迭代次数；若迭代终止时迭代次数为N，则k时刻系统状态估计值为：状态误差协方差平方根矩阵为：若未达到迭代终止条件，则迭代次数i＝i+1，即进行下一次迭代，重复步骤八到步骤十六的过程。