一种基于误差四元数三维矢量分布的高斯粒子滤波数据处理方法技术

本发明专利技术提出的一种基于误差四元数三维矢量分布的高斯粒子滤波数据处理方法，属于数字滤波和多传感器数据融合技术领域，主要用于解决粒子滤波器在姿态估计中存在庞大的计算负担问题，该方法以高斯粒子滤波为框架，融合陀螺仪、加速度计及磁力计数据，将误差四元数三维矢量在三维欧式空间上的高斯分布作为姿态估计误差的后验分布。本发明专利技术将四元数作为全局姿态参数，乘性误差四元数作为局部姿态误差描述的一种间接滤波方法，既可以保证滤波器中四元数的归一化，又可以实现滤波器的序贯估计，并且可实现并行计算加快计算速度，滤波精度稳定，适用于姿态估计、数据融合等应用场合。

【技术实现步骤摘要】
一种基于误差四元数三维矢量分布的高斯粒子滤波数据处理方法
本专利技术为了解决粒子滤波算法在飞行器姿态估计中存在庞大的计算负担问题，提出了一种以高斯粒子滤波为框架的飞行器间接姿态估计算法，属于数据处理和多传感器数据融合
，适用于姿态估计、数据融合等应用场合。
技术介绍
对于姿态估计，使用序贯矢量测量在微型飞行器导航系统中已经得到了深入的研究。目前姿态估计的成熟方案是基于陀螺仪、加速度计及磁力计的矢量观测来确定飞机的姿态。四元数作为全局非奇异最小维数姿态描述形式，在姿态估计问题中得到了广泛的应用。近几十年来，人们已经进行了大量研究，使用各种新算法来提高估计精度。针对强非线性系统，借助于乘性姿态误差描述的思想，J.L.Crassidis和Cheng分别提出了无迹卡尔曼滤波器(USQUE)和粒子滤波器(PF)，将四元数和修正罗德里格斯参数(MRPs)相结合，把误差四元数的四参数转化为修正罗德里格斯三参数，利用MRPs计算均值和协方差，保持了四元数规范化。但这种相互转化增加了计算负担和数值舍入误差，降低了滤波速度；而且USQUE使用固定参数的高斯分布逼近真实分布，因此对随机过程的统计分布非常敏感。当估计多个参数时，PF容易面临维数灾难而导致巨大的计算量。四元数粒子滤波器(QPF)以四元数作为状态量直接对姿态进行估计，无需状态切换。虽然QPF对非线性、非高斯问题比其他算法更加逼近真实值，但是为克服粒子退化和样本匮乏的缺陷，QPF需要对粒子进行重采样进而导致随着粒子数的增加而增大了计算负担。近几十年，各学者对粒子滤波的缺陷做了很多改进，尽管效果变好，但计算变得更加复杂。
技术实现思路
在本专利技术中，提出一种将误差四元数的三维矢量定义为状态量来描述局部姿态误差，四元数作为全局姿态参数的间接高斯粒子滤波器(IndirectGaussianparticlefilter，IGPF)。所提出的滤波器以误差四元数三维矢量在三维欧式空间上的高斯分布作为姿态估计误差的后验分布。将三维空间上的姿态估计问题转化为其切空间上(欧里几德空间)姿态误差的估计问题。该滤波器通过并行计算和线性变换加快了计算速度，比扩展卡尔曼滤波(EKF)具有更快的收敛速度，并且比已知的四元数粒子滤波器(QPF)具有更低的复杂度和更少的计算时间。本专利技术具体过程如下：步骤1：将误差四元数δq的三维矢量δρ定义为状态量并利用陀螺仪输出建立系统状态方程；将加速度计及磁力计的输出作为观测量建立系统观测方程；步骤2：通过加速度计和磁力计的初始观测量获得初始四元数粒子集对系统进行初始化；其中，i表示粒子个数索引，M表示粒子数，上标+表示后验，下标0表示初始时刻；步骤3：已知k时刻状态量的后验分布，从中采样获得后验误差四元数三维矢量粒子集通过k时刻最优四元数估计值叉乘误差四元数粒子集来生成后验四元数粒子集其中，下标k表示k时刻；步骤4：根据步骤3得到的后验四元数粒子集利用步骤1中系统状态方程得到更新后的四元数粒子集其中，上标-表示先验含义，下标k+1表示k+1时刻；步骤5：根据步骤1中系统观测方程对步骤4得到的四元数粒子集进行量测更新得到k+1时刻权值步骤6：计算k+1时刻最优四元数估计值并根据最优四元数估计值计算姿态角；其中，姿态角包括俯仰角θ、横滚角γ、航向角ψ；步骤7：根据步骤5中k+1时刻权值及四元数粒子集叉乘最优四元数估计值的逆生成的误差四元数粒子集估计k+1时刻状态量的后验分布；步骤8：根据步骤6、步骤7得到k+1时刻滤波估计值，循环执行步骤3、4、5、6、7，得到下一时刻滤波估计值。进一步地，步骤1中定义四元数为q≡[q1ρT]T，其中，T表示矩阵转置；q1＝cos(θ/2)，是旋转轴，θ是旋转角度。估计值到真值q的旋转四元数定义为误差四元数δq≡[δq1δρT]T。由于误差四元数对应于小的姿态旋转，所以它的标量δq1→1，姿态信息主要包含在矢量部分δρ，因此定义状态量：用表示四元数乘法，则误差四元数δq和估计值及真值q的关系为：记qk为k时刻的四元数，qk+1为k+1时刻四元数，则系统状态方程为：正交矩阵如下：其中，为k时刻陀螺角速度输出矢量，Δt为采样时间间隔，为的反对称矩阵。k时刻观测量yk由加速度计输出和磁力计输出组成，系统的观测方程为：其中，为导航坐标系到机体坐标系的姿态矩阵，gn为当地重力加速度，mn为当地磁场分量；ηa、ηm分别为加速度计零均值白噪声、磁力计零均值白噪声。进一步地，步骤2中初始四元数粒子集可以由以下两式根据加速度计和磁力计的观测数据进行初始化：Step1：由加速度计在x、y轴方向的输出可求得俯仰角θ和横滚角γ，进一步由磁力计在x、y、z轴方向的输出可求得航向角ψ。具体的计算公式为：其中，||·||表示范数。Step2：由欧拉角和四元数的关系解得初始四元数四参数：进一步地，步骤3中包括以下步骤：Step1：从k时刻状态量的后验分布中采样获得后验误差四元数三维矢量粒子集其中，符号*表示乘法，表示高斯分布，chol(·)为乔里斯基分解函数，randn(3，1)为生成三维标准正态分布随机样本的函数；Step2：通过k时刻最优四元数估计值叉乘误差四元数粒子集生成后验四元数粒子集进一步地，步骤4根据步骤3得到的后验四元数粒子集利用步骤1中系统状态方程得到更新后的四元数粒子集其中，为k时刻陀螺仪偏置误差估计值，为k+1时刻的先验分布。进一步地，步骤5中包括以下步骤：Step1：以步骤4得到的作为重要抽样分布利用步骤1中系统观测方程计算k+1时刻四元数粒子集的量测值Step2：在获得k+1时刻加速度计和磁力计观测值yk+1后，计算粒子的权值Step3：权值归一化得到归一化权值进一步地，步骤6根据步骤5得到的权值估计k+1时刻的最优四元数估计值最优四元数估计值等于四元数二阶矩Pqqk+1的最大特征值λmax对应的单位规范化特征向量：其中，E{·}表示期望。根据最优四元数估计值解算出滤波后姿态角：θ＝arcsin(2(q3q4+q1q2))计算姿态估计误差角：δα＝2arccos(δq1k+1)，δq1k+1是由下式定义：其中，qk+1为k+1时刻的真实四元数。进一步地，步骤7中包括以下步骤：Step1：根据步骤5中四元数粒子集的相应权值以及步骤6中k+1时刻的最优四元数估计值由四元数粒子集叉乘最优四元数估计值的逆生成k+1时刻误差四元数粒子集其中，(·)-1表示矩阵求逆；Step2：k+1时刻状态量的后验分布均值求解：Step3：k+1时刻状态量的后验本文档来自技高网...

【技术保护点】
1.一种基于误差四元数三维矢量分布的高斯粒子滤波数据处理方法，其特征在于，具体包括以下步骤：/n步骤1：将误差四元数δq的三维矢量δρ定义为状态量

【技术特征摘要】
1.一种基于误差四元数三维矢量分布的高斯粒子滤波数据处理方法，其特征在于，具体包括以下步骤：
步骤1：将误差四元数δq的三维矢量δρ定义为状态量并利用陀螺仪输出建立系统状态方程；将加速度计及磁力计的输出作为观测量建立系统观测方程；
步骤2：通过加速度计和磁力计的初始观测量获得初始四元数粒子集对系统进行初始化；其中，i表示粒子个数索引，M表示粒子数，上标+表示后验，下标0表示初始时刻；
步骤3：已知k时刻状态量的后验分布，从中采样获得后验误差四元数三维矢量粒子集通过k时刻最优四元数估计值叉乘误差四元数粒子集来生成后验四元数粒子集其中，下标k表示k时刻；
步骤4：根据步骤3得到的后验四元数粒子集利用步骤1中系统状态方程得到更新后的四元数粒子集其中，上标-表示先验含义，下标k+1表示k+1时刻；
步骤5：根据步骤1中系统观测方程对步骤4得到的四元数粒子集进行量测更新得到k+1时刻权值
步骤6：计算k+1时刻最优四元数估计值并根据最优四元数估计值计算姿态角；其中，姿态角包括俯仰角θ、横滚角γ、航向角ψ；
步骤7：根据步骤5中k+1时刻权值及四元数粒子集叉乘最优四元数估计值的逆生成的误差四元数粒子集估计k+1时刻状态量的后验分布；
步骤8：根据步骤6、步骤7得到k+1时刻滤波估计值，循环执行步骤3、4、5、6、7，得到下一时刻滤波估计值。


2.根据权利要求1所述一种基于误差四元数三维矢量分布的高斯粒子滤波数据处理方法，其...

【专利技术属性】
技术研发人员：周翟和，曾传伟，邹克臣，赖际舟，姚睿，曾庆喜，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：江苏;32