[发明专利]一种抗观测数据干扰的多车协同状态估计方法

权利要求书

1.一种抗观测数据干扰的多车协同状态估计方法，其特征在于，包括如下步骤：

步骤1：主车、协同车按照各自频率分别对目标车进行感知，继而分别获得含有噪声的目标车状态观测数据；同时，协同车将协同车的自车状态以及协同车所感知的目标车状态观测数据发送给主车；

步骤2：主车作为信息融合中心，将主车收集到的目标车状态观测数据以及协同车发送的目标车状态观测数据进行时间同步和坐标转换处理，得到经过预处理后的组合观测数据；为抑制异常噪声对观测数据的影响，基于学生t分布模拟重尾、非高斯的观测噪声；

步骤3：在k＝0时刻，初始化状态变量表示x₀服从均值m₀和协方差P₀的高斯分布；初始化观测噪声协方差(u₀Λ₀)^-1的先验超参数其中，u₀，Λ₀分别为初始自举变量值、初始精确度值，均为初始先验超参数；

步骤4：利用扩展卡尔曼滤波器，预测k时刻的目标车状态分布表示x_k|k-1服从均值协方差的高斯分布；预测k时刻观测噪声协方差(u_kΛ_k)^-1的先验超参数和k≥1；

步骤5：变分内循环：利用变分贝叶斯推断理论，计算第t次迭代后的目标后验状态，经过变分推断的均值和协方差为和计算第t次迭代后的观测噪声参数的后验分布，经过变分推断得到精确度、自举变量和自由度分别为和并计算期望(u_kΛ_k)^-1t；

步骤6：判断目标车状态是否收敛，如果收敛则进行下一步骤，否则，令迭代次数t←t+1，返回步骤5继续迭代，直至收敛；收敛的判断标准为：如果两次迭代状态的相对变化量ε小于阈值G，则终止迭代；

步骤7：输出每个时刻估计的目标车状态

步骤8：选择是否继续跟踪，是则返回步骤1继续跟踪，否则结束跟踪；跟踪结束后，输出本次跟踪位置和速度的均方根误差图。

2.根据权利要求1所述的一种抗观测数据干扰的多车协同状态估计方法，其特征在于，所述步骤2中基于学生t分布模拟重尾、非高斯的观测噪声的方法为：

S1、分别构造主车对协同车和目标车的观测方程和协同车对目标车的观测方程

S2、通过组合主车对协同车和目标车的观测结果和协同车对目标车的观测结果得到增广观测向量进而获得增广观测噪声和增广观测噪声H、Jh_k分别是主车传感器的观测矩阵和协同车传感器的雅可比观测矩阵，分别是主车传感器的观测噪声和协同车传感器的观测噪声；

S3、以学生t分布模拟观测噪声v_k，概率密度函数表示为：

其中，S(·)表示学生t分布，d为状态变量维度，v为d维随机向量，且服从学生t分布，μ为均值，Λ为精确度，λ为自由度；

将学生t分布看作多个高斯分布的组合，表示为：

其中，表示高斯分布，Gamma(·)表示伽马分布，u为自举变量；

最终协同跟踪的增广观测模型可以表示为：

协同跟踪的增广观测噪声可以表示为：v_k～S(v_k；μ_k，Λ_k，λ_k)；其中，μ_k，Λ_k，λ_k分别为k时刻的均值，k时刻的精确度和k时刻的自由度。

3.根据权利要求1所述的一种抗观测数据干扰的多车协同状态估计方法，其特征在于，步骤4中计算均值和协方差的方法为：

设置多车协同状态方程x_k＝Fx_k-1+w_k和具体相关参数，F是状态转移矩阵，w_k是过程噪声，x_k是系统状态向量；

基于多车协同状态方程得预测状态和协方差，表示为：

其中，分别为状态一步预测和上一时刻更新后的目标状态，分别为协方差矩阵一步预测和上一时刻更新后的协方差矩阵；Q是过程噪声协方差。