[发明专利]基于四元数分布的并行高斯粒子滤波数据处理方法

说明书

本发明提出的一种基于四元数分布的并行高斯粒子滤波数据处理方法，属于数字滤波和多传感器数据融合技术领域，主要用于解决粒子滤波器的高维状态产生巨大的计算工作量的问题，该方法以四元数粒子滤波为框架，融合陀螺仪、加速度计及磁力计数据，使用新四元数分布作为单位超球面上的四元数的标准分布，用以估计状态的后验分布。本发明只需在新四元数分布中采样，仅通过线性变换计算四元数分布的二阶矩，加快了计算速度，适用于姿态估计、数据融合等应用场合。

技术领域

本发明为了解决非线性粒子滤波算法在载体姿态估计中的计算量大的缺陷，提出了一种改进算法，属于数据处理和多传感器数据融合技术领域，适用于姿态估计、数据融合等应用场合。

背景技术

序贯姿态估计是导航系统的关键部分，目前姿态估计的成熟方案是用陀螺仪，磁力计和加速度计测量的数据来确定飞机的姿态。由于四元数具有非奇异的特性，因此广泛应用于动力学方程。近几十年来，人们已经进行了大量研究，使用各种新算法来提高估计精度。标准扩展卡尔曼滤波(Extended Kalman Filter，EKF)算法用于估计飞行姿态，然而EKF内的归一化处理没有达到最佳估计结果。为了应对强非线性系统，提出了一种无迹卡尔曼滤波(Unscented Kalman Filter，UKF)算法。同样，UKF也需要归一化来保持四元数的标准化约束。此外，UKF受随机过程的统计分布影响比较大。

修正的罗德里格斯参数(Modified Rodrigues Parameters，MRP)的粒子滤波器(Particle Filter，PF)解决了四元数的归一化问题和非线性非高斯问题，然而高维状态下该粒子滤波器会产生巨大的计算工作量。因此，四元数粒子滤波(Quaternion ParticleFilter，QPF)应运而生，该方法直接使用姿态四元数来工作。虽然QPF比其他算法表现更好，但随着粒子数量的增加，它也有较大的计算负荷。计算负荷的主要部分是由重采样过程引起的。因此，随着多核多线程技术的发展，四元数粒子滤波器的并行实现是必要的。

发明内容

在本发明中，我们介绍了一种新的四元数粒子滤波器，称为并行四元数滤波器(Parallel Quaternion Particle Filter，PQPF)。所提出的滤波器使用新的四元数分布作为单位超球面上的四元数的标准分布，用以估计状态的后验分布。PQPF只需在新的四元数分布中采样，且仅通过线性变换计算四元数分布的二阶矩。因此，所提出的滤波器通过并行计算和线性变换加快了计算速度。

本发明具体过程如下：

步骤1：根据加速度计和磁力计的初始测量值对四元数粒子进行初始化；

步骤2：以四元数为系统状态量，采用陀螺仪数据建立四元数系统状态方程；以加速度计及磁力计的输出为观测量建立量测方程；

步骤3：根据步骤2中建立的状态方程得到四元数的二阶矩，采用高斯分布函数抽样得到四元数粒子并进行状态更新；

步骤4：利用步骤2中的量测方程对四元数粒子进行量测更新，采用并行计算的方式计算新的权值；

步骤5：计算最优四元数估计值，根据四元数估计值计算姿态角。

进一步地，步骤1中定义四元数为q_k≡[q_0k ρ_k]^T，其中是旋转轴，是旋转角度；

初始四元数粒子集可以由以下两式根据加速度计和磁力计的测量数据进行初始化：