[发明专利]一种基于KLD采样的混合粒子联邦滤波数据处理方法

说明书

本发明提出的一种基于KLD采样的混合粒子联邦滤波数据处理方法，属于数字滤波和多传感器数据融合技术领域，主要用于解决多传感器组合导航系统既包含线性子系统又包含非线性子系统时传统联邦卡尔曼滤波无法获得有效精度的问题，该方法以联邦滤波为框架，融合GPS、磁力计及气压高度计数据，将线性子系统采用卡尔曼滤波算法解算，非线性子系统采用改进的粒子滤波进行解算。本发明相较于传统的联邦卡尔曼滤波算法有更高的精度和可靠性，适用于数据融合等应用场合。

技术领域

本发明为了解决多传感器组合导航系统既包含线性子系统又包含非线性子系统时传统的联邦卡尔曼滤波(Federated Kalman Filtering，FKF)无法获得有效精度的问题，提出了在联邦滤波算法的基础上，线性子系统采用卡尔曼滤波(Kalman Filter，KF)算法解算，非线性子系统采用KLD(Kullback-Lerbler Distance)采样的无迹粒子滤波(UnscentedParticle Filter，UPF)算法进行解算，属于数据处理和数据融合技术领域，适用于导航数据融合等应用场合。

背景技术

导航技术从诞生发展至今，没有任何一个导航系统能够同时满足精度要求和可靠性要求，为解决这一不足，诞生出了组合导航系统。组合导航本质是多传感器系统，多传感器系统进行组合导航时常见的方式有集中式滤波和联邦式滤波两种。集中式滤波在高维系统中存在很多不足，比如计算量大、容错性差等，而联邦式滤波应用于多传感器系统具有较为突出的优势。经典的联邦卡尔曼滤波算法是1998年由Carlson提出的，由于该算法具备计算量小、可靠性高等优点，在数据处理领域得到了较为广泛的应用。但是当系统中包含非线性环节时，联邦卡尔曼滤波算法的性能会大大降低。

发明内容

在本发明中，提出一种将线性子系统采用卡尔曼滤波算法、非线性子系统采用KLD-UPF算法进行数据融合的方法，总称为混合粒子联邦滤波(Hybrid Particle FederalFilter，HPFF)算法，该算法相比较传统的联邦卡尔曼滤波算法具有更高的稳定性和可靠性。

本发明具体过程如下：

步骤1：根据三轴加速度计和三轴陀螺仪的数据，应用姿态更新、速度更新、位置更新算法分别对姿态角、速度和位置进行更新；

步骤2：根据捷联惯性导航系统(Strap-down Inertial Navigation System，SINS)状态变量和系统噪声建立SINS系统状态方程；根据全球定位系统(GlobalPositioning System，GPS)误差状态变量和系统噪声建立GPS状态方程；

步骤3：利用GPS伪距作为观测量建立SINS/GPS子系统非线性量测方程，根据磁力计信息和SINS系统解算的姿态信息可以得到SINS/磁力计子系统量测方程，根据气压高度计信息和SINS系统解算的姿态信息可以得到SINS/气压高度计子系统量测方程；

步骤4：将系统信息按照信息分配因子进行分配，信息分配因子按照无重置系统β_m＝0，β_i＝1/3的原则来确定，其中，β_m和β_i分别为主滤波器和子滤波器信息分配系数；

步骤5：对线性子系统采用卡尔曼滤波、非线性系统采用KLD采样的无迹粒子滤波算法进行时间更新和量测更新，得到各子系统的局部最优估计和协方差矩阵；其中，对非线性子系统SINS/GPS采用KLD采样的UPF算法处理如下：

Step1：在k＝0时刻，对先验概率密度p(X₀)进行采样得到初始粒子群其中N表示采样粒子个数，初始化每个粒子权值为1/N；

Step2：进行KLD采样，设定k时刻当前采样粒子数n＝0，非空子空间数m＝0，预测粒子集合对应权值集合进行以下循环：