[发明专利]一种用于目标跟踪的混合平方根容积卡尔曼滤波方法

摘要

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

主权项

一种用于目标跟踪的混合平方根容积卡尔曼滤波方法，其特征在于，包括如下步骤：步骤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。