一种用于目标跟踪的混合平方根容积卡尔曼滤波方法技术

本发明专利技术提出一种用于目标跟踪的混合平方根容积卡尔曼滤波方法。步骤包括：根据初始化后的状态向量、过程噪声协方差平方根因子、测量噪声协方差平方根因子、状态协方差平方根因子，预测目标先验状态向量和先验状态协方差平方根因子；计算状态向量容积采样点；计算测量向量容积采样点和测量向量容积采样点，获得测量向量；计算卡尔曼增益和后验状态协方差平方根因子；计算后验状态向量。本发明专利技术在保证滤波精度不变的前提下，大大提升了滤波效率，具有很强的实时性能。

A hybrid square root volume Calman filtering method for target tracking

The present invention proposes a hybrid square root volume Calman filtering method for target tracking. The steps include: according to the state vector, the process noise covariance square root factor and measurement noise covariance square root factor, covariance square root factor after initialization, forecast the prior target state vector and a priori covariance square root factor calculation; state vector volume sampling points; measurement vector volume sampling point and measurement vector volume sampling point. Measurement vector calculation; test covariance square root factor Calman gain and calculate the posterior state vector. Under the premise that the filtering accuracy is not changed, the invention greatly improves the filtering efficiency and has strong real-time performance.

【技术实现步骤摘要】
一种用于目标跟踪的混合平方根容积卡尔曼滤波方法
本专利技术属于目标跟踪
，具体涉及一种基于混合平方根容积卡尔曼滤波器的目标跟踪滤波方法，可广泛应用于汽车辅助驾驶、汽车自主驾驶和汽车主动安全等领域。
技术介绍
卡尔曼滤波器(KalmanFilter，KF)是目标跟踪领域应用广泛的一种跟踪滤波算法。卡尔曼滤波器的作用在于，通过对运动目标运动过程和传感器测量过程建模，利用间接的、含噪声的测量值，尽量准确的估计目标的运动状态。运动状态包括目标的位置、速度和加速度等信息。传感器测量模型中包含噪声，称为测量噪声，这代表着对目标的测量含有一定的不准确性；目标运动过程模型中也包含噪声，称为过程噪声，这代表运动目标的运动过程并不完全符合运动模型的假设。真实世界的很多动态系统都并不确切的符合所假设的运动模型，但是由于卡尔曼滤波器被设计在有噪声的情况下工作，一个近似的符合已经可以使这个滤波器非常有用了。滤波的意义在于，通过假设运动模型和测量模型，减少测量噪声和过程噪声对目标跟踪效果的影响，更精确的估计目标车辆的运动状态。传统的卡尔曼滤波器只适用于线性系统，即运动模型和测量模型必须是线性系统。而在例如汽车目标跟踪领域，这常常是难以达到的。多数情况下，在以传感器为坐标原点的笛卡尔坐标系下，目标运动状态可以用一个六维的状态向量X表示，其中x,表示目标在X方向上的距离、速度和加速度，y,表示目标在Y方向上的距离、速度和加速度。而传感器测量得到的目标通常表示为极坐标下的测量向量Y，Y＝[r,a,v]T，其中r,a,v分别表示目标到传感器的径向距离、方位角和径向速度。测量模型h表示目标状态空间到测量空间的映射，即由于目标状态向量X和测量向量Y之间不满足线性关系，因此测量模型是非线性的。扩展卡尔曼滤波器(ExtendedKalmanFilter，EKF)是为了解决非线性问题而提出的一种滤波方法，它通过对非线性函数进行一阶泰勒展开达到线性化的目的，在非线性特性较强时往往会出现较大误差，甚至出现滤波发散的情况。不敏卡尔曼滤波器(UnscentedKalmanFilter，UKF)是另一种适用于非线性系统的卡尔曼滤波器，它使用非线性函数概率逼近的方式能达到二阶精度，并且与EKF有同阶的计算复杂度。然而在实际使用UKF时，UT变换(UnscentedTransform)中对非正定矩阵的乔利斯基分解将直接导致算法停滞。平方根不敏卡尔曼滤波器(SquareRootUnscentedKalmanFilter，SRUKF)成功的避免了矩阵非正定的问题成为UKF理想的替代方法。平方根容积卡尔曼滤波器(SquareRootCubatureKalmanFilter，SRCKF)同样使用非线性函数概率密度逼近的方式，理论上比SRUKF具有更高的滤波精度。然而，SRCKF的高时间复杂度往往难以满足系统高实时性需求。
技术实现思路
本专利技术提出一种用于目标跟踪的混合平方根容积卡尔曼滤波方法，其结合了线性卡尔曼滤波器KF的预测技术和非线性卡尔曼滤波器SRCKF的更新技术，在取得与SRCKF相同的滤波精度的情况下，大大提升了滤波效率，具有很强的实时性能。为了解决上述技术问题，本专利技术提供一种用于目标跟踪的混合平方根容积卡尔曼滤波方法，包括如下步骤：步骤1，初始化参数，包括：k＝0时刻的目标状态向量X0，过程噪声协方差平方根因子Qsqrt，测量噪声协方差平方根因子Rsqrt，状态协方差平方根因子S0；步骤2，根据k-1时刻目标后验状态向量Xk-1、k-1时刻后验状态协方差平方根因子Sk-1、目标运动模型Ak和过程噪声协方差平方根因子Qsqrt，预测k时刻目标先验状态向量和k时刻先验状态协方差平方根因子步骤3，根据k时刻目标先验状态向量和k时刻先验状态协方差平方根因子计算k时刻状态向量容积采样点σx,k；步骤4，根据k时刻状态向量容积采样点σx,k和非线性测量模型h(·)计算k时刻的测量向量容积采样点σy,k以及k时刻先验测量向量步骤5，根据k时刻先验状态向量状态向量容积采样点σxk、测量向量容积采样点σy,k和先验测量向量计算k时刻卡尔曼增益Kk和后验状态协方差平方根因子Sk；步骤6，根据k时刻先验状态向量k时刻卡尔曼增益Kk、k时刻传感器获得的测量向量Yk以及k时刻先验测量向量计算k时刻后验状态向量Xk。进一步，定义k时刻的目标状态向量其中，xk、分别表示k时刻目标在X方向上的距离、速度和加速度，yk、分别表示k时刻目标在Y方向上的距离、速度和加速度，T表示矩阵转置；定义k时刻的目标测量向量Yk＝[rk,ak,vk]T，其中rk、ak、vk分别表示k时刻目标到传感器的径向距离、方位角和径向速度。进一步，步骤1中，根据k＝0时刻的目标测量向量Y0初始化k＝0时刻的目标状态向量X0，初始化方法如公式(1)所示：X0＝[r0cos(a0),v0cos(a0),0,r0sin(a0),v0sin(a0),0]T公式(1)公式(1)中，r0,a0,v0分别表示k＝0时刻传感器测量的目标径向距离、方位角和径向速度；根据运动模型初始化过程噪声协方差平方根因子Qsqrt，初始化方法如公式(2)所示：公式(2)中，过程噪声协方差平方根因子Qsqrt为6维方阵，chol(·)表示矩阵的乔利斯基分解，ζx、分别表示目标运动模型在X方向距离x、速度和加速度的建模误差方差；ζy、分别表示目标运动模型在Y方向距离y、速度和加速度的建模误差方差；根据传感器的测量精度初始化测量噪声协方差平方根因子Rsqrt，初始化方法如公式(3)所示：公式(3)中测量噪声协方差平方根因子Rsqrt为3维方阵，ξr、ξa、ξv分别测量模型在径向距离r、方位角a和径向速度v的测量误差方差；根据经验初始化状态协方差平方根因子S0，初始化如公式(4)所示：进一步，步骤2中，预测k时刻目标先验状态向量和k时刻先验状态协方差平方根因子的方法如公式(5)和公式(6)所示：公式(6)中，qr(·)表示矩阵的QR分解；公式(5)中，Ak为目标运动模型，表示目标状态向量从k-1时刻到k时刻的变换规律。进一步，步骤3中，计算k时刻状态向量容积采样点σx,k的方法如公式(7)所示：公式(7)中，表示k时刻状态向量容积采样点σx,k第i行，表示k时刻先验状态协方差平方根因子第i列，L表示目标状态向量的维数。进一步，步骤4中，计算k时刻的测量向量容积采样点σy,k以及k时刻先验测量向量的方法如公式(8)和公式(9)所示：σy,k＝h(σx,k)公式(8)公式(8)中，h(·)为非线性测量模型，其定义如公式(10)所示：进一步，步骤5中，计算k时刻卡尔曼增益Kk和后验状态协方差平方根因子Sk的过程如公式(11)-(14)所示：其中，Syy,k为测量协方差平方根因子，Pxy,k为互协方差，表示状态向量容积采样点σx,k第1列到第2L列，表示测量向量容积采样点σy,k第1列到第2L列。进一步，步骤6中，计算k时刻后验状态向量Xk的方法如公式(15)所示：本专利技术与现有技术相比，其显著优点在于：(1)本专利技术提出的混合平方根容积卡尔曼滤波方法是基于KF的线性预测和SRCKF的非线性更新，在保证与SRCKF具有相同的滤波精度的情况下，混合平方根容积本文档来自技高网...

【技术保护点】
一种用于目标跟踪的混合平方根容积卡尔曼滤波方法，其特征在于，包括如下步骤：步骤1，初始化参数，包括：k＝0时刻的目标状态向量X0，过程噪声协方差平方根因子Qsqrt，测量噪声协方差平方根因子Rsqrt，状态协方差平方根因子S0；步骤2，根据k‑1时刻目标后验状态向量Xk‑1、k‑1时刻后验状态协方差平方根因子Sk‑1、目标运动模型Ak和过程噪声协方差平方根因子Qsqrt，预测k时刻目标先验状态向量

【技术特征摘要】
1.一种用于目标跟踪的混合平方根容积卡尔曼滤波方法，其特征在于，包括如下步骤：步骤1，初始化参数，包括：k＝0时刻的目标状态向量X0，过程噪声协方差平方根因子Qsqrt，测量噪声协方差平方根因子Rsqrt，状态协方差平方根因子S0；步骤2，根据k-1时刻目标后验状态向量Xk-1、k-1时刻后验状态协方差平方根因子Sk-1、目标运动模型Ak和过程噪声协方差平方根因子Qsqrt，预测k时刻目标先验状态向量和k时刻先验状态协方差平方根因子步骤3，根据k时刻目标先验状态向量和k时刻先验状态协方差平方根因子计算k时刻状态向量容积采样点σx,k；步骤4，根据k时刻状态向量容积采样点σx,k和非线性测量模型h(·)计算k时刻的测量向量容积采样点σy,k以及k时刻先验测量向量步骤5，根据k时刻先验状态向量状态向量容积采样点σx，k、测量向量容积采样点σy,k和先验测量向量计算k时刻卡尔曼增益Kk和后验状态协方差平方根因子Sk；步骤6，根据k时刻先验状态向量k时刻卡尔曼增益Kk、k时刻传感器获得的测量向量Yk以及k时刻先验测量向量计算k时刻后验状态向量Xk。2.如权利要求1所述用于目标跟踪的混合平方根容积卡尔曼滤波方法，其特征在于，定义k时刻的目标状态向量其中，xk、分别表示k时刻目标在X方向上的距离、速度和加速度，yk、分别表示k时刻目标在Y方向上的距离、速度和加速度，T表示矩阵转置；定义k时刻的目标测量向量Yk＝[rk,ak,vk]T，其中rk、ak、vk分别表示k时刻目标到传感器的径向距离、方位角和径向速度。3.如权利要求2所述用于目标跟踪的混合平方根容积卡尔曼滤波方法，其特征在于，步骤1中，根据k＝0时刻的目标测量向量Y0初始化k＝0时刻的目标状态向量X0，初始化方法如公式(1)所示：X0＝[r0cos(a0),v0cos(a0),0,r0sin(a0),v0sin(a0),0]T公式(1)公式(1)中，r0,a0,v0分别表示k＝0时刻传感器测量的目标径向距离、方位角和径向速度；根据运动模型初始化过程噪声协方差平方根因子Qsqrt，初始化方法如公式(2)所示：公式(2)中，过程噪声协方差平方根因子...

【专利技术属性】
技术研发人员：刘华军，赖少发，
申请(专利权)人：南京理工大学，
类型：发明
国别省市：江苏,32