一种基于矩阵稀疏性的Kalman滤波算法优化的方法技术

本发明专利技术涉及基于矩阵稀疏性的Kalman滤波算法优化的方法，其步骤为：建立组合导航卡尔曼滤波模型，对解算矩阵进行分块，包括状态系数矩阵F，一步状态转移阵Φ，噪声驱动阵G，噪声方差阵Q，状态一步预测量阵X

An optimization method of Kalman filter algorithm based on matrix sparsity

【技术实现步骤摘要】
一种基于矩阵稀疏性的Kalman滤波算法优化的方法
本专利技术涉及导航
，特别是涉及一种基于矩阵稀疏性的Kalman滤波算法优化的方法。
技术介绍
Kalman滤波是应用最为广泛的一种估计滤波技术，自从Kalman滤波提出以来，它已成为信号处理、通信和控制等最基本、最重要的工具，但随着组合导航系统传感器种类的增加，系统的规模也日趋复杂，相应地，Kalman滤波器结构形式也日趋庞大，利用高性能硬件计算机去提高计算速度和增大存储机制的方案越来越多被适用于Kalman滤波器的设计中，但嵌入式硬件的发展远远不能满足软件算法的需求，硬件性能已经成为Kalman滤波技术发展的掣肘，由于滤波本身计算量大和所需程序空间大，同时还要应对实时状态估计的需要，严重制约了其应用。本专利技术根据Kalman滤波状态方程和观测方程的稀疏特性对状态转移矩阵和观测矩阵进行分块处理，利用分块矩阵的零元素矩阵进行理论推导，避免了大量有关零元素的运算，降低了高维的存储空间需求，在解算的过程中将某些中间结果存储于数据缓冲区，并复用缓存结果，在此基础上，还采用一定策略对矩阵再次进行稀疏压缩存储，进一步降低对连续存储单元的需求。该专利技术能提高解算效率、降低存储空间要求、减少连续存储单元的使用。在硬件条件受限制的情况下，实现复杂Kalman滤波算法的有效应用。
技术实现思路
本专利技术的目的在于克服现有技术的不足，提供一种实现简单、解算快捷高效的基于矩阵稀疏性的Kalman滤波算法优化的方法。本专利技术解决其技术问题是通过以下技术方案实现的：一种基于矩阵稀疏性的Kalman滤波算法优化的方法，包括如下步骤：1)建立一种组合导航卡尔曼滤波模型，对解算矩阵进行分块，包括状态系数矩阵F，一步状态转移阵Φ，噪声驱动阵G，噪声方差阵Q，状态一步预测量阵Xkk，预测均方误差阵Pkk，滤波增益阵Kk，状态估计量阵Xk，估计均方误差阵Pk；2)利用F阵和G阵的零元素进行简化推导，避免了大量有关零元素的运算，进行了降维处理，降低了存储空间，得出Φ阵和Q阵的简化计算过程；3)对卡尔曼滤波的五个步骤Xkk、Pkk、Kk、Xk、Pk进行计算分块矩阵推导，并在计算过程中采用复用存储单元和压缩存储单元的方法降低存储空间的占用，并利用计算机解算的特点降低计算开销。而且，步骤2)中所述Φ阵简化推导模型，包括：只需计算第一行分块矩阵且可以将F12和F1·F2进行缓存并在后面的计算中予以复用，统计F2和F3的非零元素个数，并考虑采用三元稀疏矩阵进行存储；对于一步状态转移阵Φ也按二分法对矩阵进行分块，首先计算后半部分如上述分析，只计算第一行分块矩阵，然后仅针对J矩阵的对角元素进行加1操作，再后将Φ分块矩阵的第2行第2列用单位矩阵I2进行替换，由此计算可得Φ。而且，步骤2)中所述Q的简化推导模型，包括：保留F1G1QG1T作为中间结果用于计算(FM)TMi+1＝FMi+(FMi)T，记作：上述结果均采用8×8矩阵进行存储，并视矩阵非零元素个数采用三元式稀疏矩阵存储。而且，步骤3)中，所述Xkk阵简化推导模型，包括：在计算过程中，对于Xkk矩阵第1行第1列分块，Φ1Xk1+Φ2Xk2仅为2个8×8矩阵的乘法和加法。对于Xkk矩阵第2行第1列分块，只需将Xk分块矩阵的第2行第1列直接遍历赋值，而不需要进行解算。而且，步骤3)中，所述Pkk包括：其中FT4＝Φ1Pk2+Φ2Pk4作为中间重复利用计算单元，在与Q矩阵相加时，仅对第1行第1列进行矩阵加法。在与Q矩阵相加时，只对第1行第1列进行矩阵加法，其他行列为零可不进行运算处理，对于第2行第2列的分块矩阵也直接遍历赋值，以减少CPU指令周期。而且，步骤3)中，所述Kk，包括：将并作为中间过程单元保留。进一步地，所述步骤3，所述Xk，包括：进一步地，所述步骤3，所述Pk，包括：对于I-操作，仅需对分块矩阵的对角元素进行减法操作，对于第1行第2列和第2行第1列的数据元素进行取负操作，而且，步骤3)中的降低存储空间的占用，指在Xkk、Pkk、Kk、Xk、Pk的计算过程中，也均采用8×8分块矩阵进行存储，并视非零元素个数采用三元式稀疏矩阵压缩存储。本专利技术的优点和有益效果为：1、本专利技术的基于矩阵稀疏性的Kalman滤波算法优化的方法，解决了组合导航中Kalman滤波器高维矩阵的计算复杂、存储量大、占用大型连续堆栈空间等弱点，提高了算法的效率。具体实施方式下面通过具体实施例对本专利技术作进一步详述，以下实施例只是描述性的，不是限定性的，不能以此限定本专利技术的保护范围。一种基于矩阵稀疏性的Kalman滤波算法优化的方法，其理论推导过程为：对于惯导与电磁计程仪组合导航系统的误差方程，将电磁计程仪与惯导之间误差角扩充为系统状态变量，应用卡尔曼滤波原理形成状态方程：其中，状态变量和噪声矩阵分别为：W＝[εxεyεz▽x▽y▽z]对应的系数矩阵为其中，O为零矩阵，为姿态转换矩阵，其矩阵形式为：Kalman滤波的递推过程为：首先计算预测值、预测值和真实值之间误差协方差矩阵。Xkk＝ΦXkPkk＝ΦPkΦT+Q其中，Φ和Q经连续系统离散化后可得：Mi+1＝FMi+(FMi)T(i＝1,2)M1＝GQ0GT通过上述公式计算卡尔曼增益Kk和估计值Xk，Kk＝PkkHT(HPkkHT+R)-1Xk＝Xkk+Kk(Z-HXkk)最后计算估计值和真实值之间的误差协方差矩阵，为下次递推做准备。Pk＝(I-KkH)Pkk上述即为线性离散化Kalman滤波进行估计状态的步骤。矩阵分块形式可以3*3最小矩阵模块对系统状态方程和观测方程进行分解，但从算法优化和普适性的角度综合考虑，不宜将矩阵分块过细。本专利技术以二分法为例进行说明，故按等分法对矩阵进行分块，将16×16矩阵转为8×8矩阵进行计算和存储对F矩阵进行分块，记作其中F1、F2、O均为8×8矩阵，且O均为零矩阵。对G矩阵进行分块，记作其中为8×6矩阵，且O均为零矩阵。另有，H＝[H1H2](2×16)，利用分块矩阵，并观察矩阵特点，对矩阵进行简化计算处理，且只对非零元素进行计算和存储，可以提高计算效率、节省存储空间，同时状态变量降维后也可避免大的连续空间的占用，提高堆栈空间的使用效率。对于三元组稀疏矩阵的计算机存储，可设计形式如下：StructTriple{unsignedchari,j；//i,j为矩阵元素所在的行和列doubleelement；}Stru本文档来自技高网...

【技术保护点】
1.一种基于矩阵稀疏性的Kalman滤波算法优化的方法，其特征在于：包括如下步骤：/n1)建立一种组合导航卡尔曼滤波模型，对解算矩阵进行分块，包括状态系数矩阵F，一步状态转移阵Φ，噪声驱动阵G，噪声方差阵Q，状态一步预测量阵X

【技术特征摘要】
1.一种基于矩阵稀疏性的Kalman滤波算法优化的方法，其特征在于：包括如下步骤：
1)建立一种组合导航卡尔曼滤波模型，对解算矩阵进行分块，包括状态系数矩阵F，一步状态转移阵Φ，噪声驱动阵G，噪声方差阵Q，状态一步预测量阵Xkk，预测均方误差阵Pkk，滤波增益阵Kk，状态估计量阵Xk，估计均方误差阵Pk；
2)利用F阵和G阵的零元素进行简化推导，得出Φ阵和Q阵的简化计算过程；
3)对卡尔曼滤波的五个步骤Xkk、Pkk、Kk、Xk、Pk进行计算分块矩阵推导，并在计算过程中采用复用存储单元和压缩存储单元的方法降低存储空间的占用，并利用计算机解算的特点降低计算开销。


2.根据权利要求1所述的基于矩阵稀疏性的Kalman滤波算法优化的方法，其特征在于：步骤2)中所述Φ阵简化推导模型，包括：









只需计算第一行分块矩阵且可以将F12和F1·F2进行缓存并在后面的计算中予以复用，统计F2和F3的非零元素个数，并考虑采用三元稀疏矩阵进行存储；
对于一步状态转移阵Φ也按二分法对矩阵进行分块，首先计算后半部分如上述分析，只计算第一行分块矩阵，然后仅针对J矩阵的对角元素进行加1操作，再后将Φ分块矩阵的第2行第2列用单位矩阵I2进行替换，由此计算可得Φ。


3.根据权利要求1所述的基于矩阵稀疏性的Kalman滤波算法优化的方法，其特征在于：步骤2)中所述Q的简化推导模型，包括：




保留F1G1QG1T作为中间结果用于计算(FM)T
Mi+1＝FMi+(FMi)T，
记作：
上述结果均采用8×8矩阵进行存储，并视矩阵非零元素个数...

【专利技术属性】
技术研发人员：郭猛，娄癸阳，胡小毛，姚琪，于文涛，
申请(专利权)人：中国船舶重工集团公司第七零七研究所，
类型：发明
国别省市：天津;12