CN110729982A - 一种基于矩阵稀疏性的Kalman滤波算法优化的方法 - Google Patents

Abstract

本发明涉及基于矩阵稀疏性的Kalman滤波算法优化的方法，其步骤为：建立组合导航卡尔曼滤波模型，对解算矩阵进行分块，包括状态系数矩阵F，一步状态转移阵Φ，噪声驱动阵G，噪声方差阵Q，状态一步预测量阵X_kk，预测均方误差阵P_kk，滤波增益阵K_k，状态估计量阵X_k，估计均方误差阵P_k；利用F阵和G阵的零元素进行简化推导，得出Φ阵和Q阵的简化计算过程；对卡尔曼滤波的五个步骤X_kk、P_kk、K_k、X_k、P_k进行计算分块矩阵推导，并在计算过程中采用复用存储单元和压缩存储单元的方法降低存储空间的占用，并利用计算机解算的特点降低计算开销。本发明解决了组合导航中Kalman滤波器高维矩阵的计算复杂、存储量大、占用大型连续堆栈空间等弱点，提高了算法效率。

Description

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

技术领域

本发明涉及导航技术领域，特别是涉及一种基于矩阵稀疏性的Kalman滤波算法优化的方法。

背景技术

Kalman滤波是应用最为广泛的一种估计滤波技术，自从Kalman滤波提出以来，它已成为信号处理、通信和控制等最基本、最重要的工具，但随着组合导航系统传感器种类的增加，系统的规模也日趋复杂，相应地，Kalman滤波器结构形式也日趋庞大，利用高性能硬件计算机去提高计算速度和增大存储机制的方案越来越多被适用于Kalman滤波器的设计中，但嵌入式硬件的发展远远不能满足软件算法的需求，硬件性能已经成为Kalman滤波技术发展的掣肘，由于滤波本身计算量大和所需程序空间大，同时还要应对实时状态估计的需要，严重制约了其应用。

本发明根据Kalman滤波状态方程和观测方程的稀疏特性对状态转移矩阵和观测矩阵进行分块处理，利用分块矩阵的零元素矩阵进行理论推导，避免了大量有关零元素的运算，降低了高维的存储空间需求，在解算的过程中将某些中间结果存储于数据缓冲区，并复用缓存结果，在此基础上，还采用一定策略对矩阵再次进行稀疏压缩存储，进一步降低对连续存储单元的需求。该发明能提高解算效率、降低存储空间要求、减少连续存储单元的使用。在硬件条件受限制的情况下，实现复杂Kalman滤波算法的有效应用。

发明内容

本发明的目的在于克服现有技术的不足，提供一种实现简单、解算快捷高效的基于矩阵稀疏性的Kalman滤波算法优化的方法。

本发明解决其技术问题是通过以下技术方案实现的：

一种基于矩阵稀疏性的Kalman滤波算法优化的方法，包括如下步骤：

1)建立一种组合导航卡尔曼滤波模型，对解算矩阵进行分块，包括状态系数矩阵F，一步状态转移阵Φ，噪声驱动阵G，噪声方差阵Q，状态一步预测量阵X_kk，预测均方误差阵P_kk，滤波增益阵K_k，状态估计量阵X_k，估计均方误差阵P_k；

2)利用F阵和G阵的零元素进行简化推导，避免了大量有关零元素的运算，进行了降维处理，降低了存储空间，得出Φ阵和Q阵的简化计算过程；

3)对卡尔曼滤波的五个步骤X_kk、P_kk、K_k、X_k、P_k进行计算分块矩阵推导，并在计算过程中采用复用存储单元和压缩存储单元的方法降低存储空间的占用，并利用计算机解算的特点降低计算开销。

而且，步骤2)中所述Φ阵简化推导模型，包括：

只需计算第一行分块矩阵且可以将F₁ ²和F₁·F₂进行缓存并在后面的计算中予以复用，统计F²和F³的非零元素个数，并考虑采用三元稀疏矩阵进行存储；

对于一步状态转移阵Φ也按二分法对矩阵进行分块，首先计算后半部分

如上述分析，只计算第一行分块矩阵，然后仅针对J矩阵的对角元素进行加1操作，再后将Φ分块矩阵的第2行第2列用单位矩阵I₂进行替换，由此计算可得Φ。

而且，步骤2)中所述Q的简化推导模型，包括：

保留F₁G₁QG₁ ^T作为中间结果用于计算(FM)^T

M_i+1＝FM_i+(FM_i)^T，

记作：

上述结果均采用8×8矩阵进行存储，并视矩阵非零元素个数采用三元式稀疏矩阵存储。

而且，步骤3)中，所述X_kk阵简化推导模型，包括：

在计算过程中，对于X_kk矩阵第1行第1列分块，Φ₁X_k1+Φ₂X_k2仅为2个8×8矩阵的乘法和加法。对于X_kk矩阵第2行第1列分块，只需将X_k分块矩阵的第2行第1列直接遍历赋值，而不需要进行解算。

而且，步骤3)中，所述P_kk包括：

其中FT4＝Φ₁P_k2+Φ₂P_k4作为中间重复利用计算单元，在与Q矩阵相加时，仅对第1行第1列进行矩阵加法。在与Q矩阵相加时，只对第1行第1列进行矩阵加法，其他行列为零可不进行运算处理，对于第2行第2列的分块矩阵也直接遍历赋值，以减少CPU指令周期。

而且，步骤3)中，所述K_k，包括：

将

并作为中间过程单元保留。

进一步地，所述步骤3，所述X_k，包括：

进一步地，所述步骤3，所述P_k，包括：

对于I-操作，仅需对分块矩阵的对角元素进行减法操作，对于第1行第2列和第2行第1列的数据元素进行取负操作，

而且，步骤3)中的降低存储空间的占用，指在X_kk、P_kk、K_k、X_k、P_k的计算过程中，也均采用8×8分块矩阵进行存储，并视非零元素个数采用三元式稀疏矩阵压缩存储。

本发明的优点和有益效果为：

1、本发明的基于矩阵稀疏性的Kalman滤波算法优化的方法，解决了组合导航中Kalman滤波器高维矩阵的计算复杂、存储量大、占用大型连续堆栈空间等弱点，提高了算法的效率。

具体实施方式

下面通过具体实施例对本发明作进一步详述，以下实施例只是描述性的，不是限定性的，不能以此限定本发明的保护范围。

一种基于矩阵稀疏性的Kalman滤波算法优化的方法，其理论推导过程为：对于惯导与电磁计程仪组合导航系统的误差方程，将电磁计程仪与惯导之间误差角扩充为系统状态变量，应用卡尔曼滤波原理形成状态方程：

其中，状态变量和噪声矩阵分别为：

W＝[ε_x ε_y ε_z ▽_x ▽_y ▽_z]

对应的系数矩阵为

其中，

O为零矩阵，

为姿态转换矩阵，其矩阵形式为：

Kalman滤波的递推过程为：

首先计算预测值、预测值和真实值之间误差协方差矩阵。X_kk＝ΦX_k

P_kk＝ΦP_kΦ^T+Q

其中，Φ和Q经连续系统离散化后可得：

M_i+1＝FM_i+(FM_i)^T(i＝1,2)

M₁＝GQ₀G^T

通过上述公式计算卡尔曼增益K_k和估计值X_k，

K_k＝P_kkH^T(HP_kkH^T+R)^-1

X_k＝X_kk+K_k(Z-HX_kk)

最后计算估计值和真实值之间的误差协方差矩阵，为下次递推做准备。

P_k＝(I-K_kH)P_kk

上述即为线性离散化Kalman滤波进行估计状态的步骤。

矩阵分块形式可以3*3最小矩阵模块对系统状态方程和观测方程进行分解，但从算法优化和普适性的角度综合考虑，不宜将矩阵分块过细。本发明以二分法为例进行说明，故按等分法对矩阵进行分块，将16×16矩阵转为8×8矩阵进行计算和存储对F矩阵进行分块，记作

其中F₁、F₂、O均为8×8矩阵，且O均为零矩阵。

对G矩阵进行分块，记作

其中

为8×6矩阵，且O均为零矩阵。

另有，H＝[H₁H₂]_(2×16)，

利用分块矩阵，并观察矩阵特点，对矩阵进行简化计算处理，且只对非零元素进行计算和存储，可以提高计算效率、节省存储空间，同时状态变量降维后也可避免大的连续空间的占用，提高堆栈空间的使用效率。

对于三元组稀疏矩阵的计算机存储，可设计形式如下：

StructTriple{

unsignedchari,j；//i,j为矩阵元素所在的行和列

doubleelement；

}

StructSparseMatrix

{

Tripledata[]；//矩阵元素数组，数组个数根据非零元素个数动态分配

int*rowpos[]；//行向量头指针，数组个数根据矩阵行数动态分配

unsignedcharrolw,col,elemNum；//矩阵行数、列数、非零元素个数

}

例如对于8×8矩阵，采用数组进行存储的方式固定占用8×8×8＝512字节的连续存储单元，而三元组矩阵存储占用elemNum×10+row×4+3＝elemNum×10+35，若考虑elemNum＝22，可知对存储空间要求将降低50％。即若非零元素低于22个，采用稀疏存储可以降低至少50％的连续存储单元要求。因此可设定：若非零元素低于22个，可采用稀疏存储的方式。三元组稀疏矩阵存储的方式可以进一步减少连续存储空间的占用。另由于稀疏矩阵计算具有不确定的时间复杂度，而惯导系统又是定时解算系统，故不采用稀疏矩阵进行解算。

对于Φ阵，有：

对于式1和式2，只需计算第一行分块矩阵，故16×16矩阵计算可转化为2个8×8矩阵解算。且可以将式1的F₁ ²和F₁·F₂进行缓存并在式2的计算中予以复用，解算结果将分别保存于8×8矩阵存储空间，在此基础上，统计F²和F³的非零元素个数，并根据上述策略决定是否采用三元稀疏矩阵进行8×8矩阵存储。

同样地，对于一步状态转移阵Φ也按二分法对矩阵进行分块，首先计算后半部分

如上述分析，只计算第一行分块矩阵。然后执行I+计算时，仅针对J矩阵的对角元素进行加1操作，而不进行调用矩阵加函数，节省矩阵遍历消耗，也降低了函数调用消耗，再后将Φ分块矩阵的第2行第2列用单位矩阵I₂进行替换，由此计算可得Φ，并同时对Φ记作：

其中Φ₁、Φ₂、I₂、O均为8×8矩阵，Φ₁、Φ₂具体内容不是本发明重点内容，不再详细列出。同样考虑对Φ₁、Φ₂、I₂进行三元组稀疏矩阵8×8方式存储。

对于噪声方差阵Q，有：

观察可知，可转为8×8矩阵进行解算和存储，并视矩阵非零元素个数考虑进一步采用三元组稀疏矩阵存储。

保留F₁G₁QG₁ ^T作为中间结果用于计算(FM)^T，也转为8×8矩阵进行解算存储，并视矩阵非零元素个数采用三元式稀疏矩阵存储。

同样地，对于M_i+1＝FM_i+(FM_i)^T，也可转为8×8矩阵进行矩阵加法和存储。

对于

也可转为8×8矩阵进行处理。同时记作：

其中Q₁和O均为8×8矩阵,且O均为零矩阵。

至此，已将Φ矩阵和Q矩阵从16×16矩阵解算转化为8×8矩阵运算和存储，同时在计算过程中，也采用一定策略对矩阵进行三元式稀疏矩阵降维存储，这样提高了运算效率，降低了计算维数、节省了存储空间、减少了连续存储单元的占用。复用缓存更能减少本轮分块过程中数据存储空间的总占用。

对于X_kk，有：

在计算过程中，对于X_kk矩阵第1行第1列分块，Φ₁X_k1+Φ₂X_k2仅为2个8×8矩阵的乘法和加法。对于X_kk矩阵第2行第1列分块，只需将X_k分块矩阵的第2行第1列直接遍历赋值，而不需要进行解算。

对于P_kk，有：

其中FT4＝Φ₁P_k2+Φ₂P_k4，则：

由于Q阵仅存在Q₁阵为非零矩阵，故在计算P_kk时，除保留FT4作为中间结果进行复用外，在与Q分块矩阵相加时，也只对第1行第1列进行矩阵加法，其他行列为零可不进行运算处理，且同上述的计算，对于第2行第2列的分块矩阵也直接遍历赋值，不进行计算，以减少CPU指令周期。

对于K_k，有：

该算式没有明确的能分块的零矩阵，不能进行降维处理，首先计算

并作为中间过程单元进行复用，然后计算K_k。

对于X_k，有：

同样，该算式也没有明确的能分块的零矩阵，无法对算式进行简化处理。故首先对该算式进行观察分析，可以看出

运算结果为2×1矩阵，

为16×2矩阵，该算式运算量不太高，从普适性的角度考虑，可以不强求优化。

对于P_k＝(I-K_kH)P_kk，有：

首先计算出对于I-操作，仅需对该分块矩阵的对角元素进行减法操作，对于第1行第2列和第2行第1列的非对角数据进行取负操作，这样可以不调用矩阵减法函数，以减少计算机指令周期，其他不作优化。

同样地，在K_k，X_k，P_k的计算过程中，也均采用8×8分块矩阵进行存储，并视情况采用三元式稀疏矩阵存储。

本发明提出的基于矩阵稀疏性的Kalman滤波算法优化方法，通过分块矩阵对Kalman模型进行简化推导和矩阵计算单元重复使用，提高了运算效率，降低了计算维数、节省了存储空间、减少了连续存储单元的占用。

下面对本发明的具体实施过程进行详细说明。

步骤1，建立一种组合导航卡尔曼滤波模型，以二分法为例进行说明，按等分对解算矩阵进行分块，包括状态系数矩阵F，一步状态转移阵Φ，噪声驱动阵G，噪声方差阵Q，状态一步预测量阵X_kk，预测均方误差阵P_kk，滤波增益阵K_k，状态估计量阵X_k，估计均方误差阵P_k；

步骤2，利用F阵和G阵的零元素进行简化推导，避免了大量有关零元素的运算，进行了降维处理，降低了存储空间，得出Φ阵和Q阵的简化计算过程；

步骤3，对卡尔曼滤波的五个步骤X_kk、P_kk、K_k、X_k、P_k进行计算分块矩阵推导，并在计算过程中采用复用存储单元和压缩存储单元的方法降低存储空间的占用，并利用计算机解算的特点降低计算开销。

进一步地，所述步骤2，所述Φ阵简化推导模型，推导过程见上述，包括：

只需计算第一行分块矩阵且可以将F₁ ²和F₁·F₂进行缓存并在后面的计算中予以复用。统计F²和F³的非零元素个数，并考虑采用三元稀疏矩阵进行存储。

对于一步状态转移阵Φ也按二分法对矩阵进行分块，首先计算后半部分如上述分析，只计算第一行分块矩阵，然后仅针对J矩阵的对角元素进行加1操作，再后将Φ分块矩阵的第2行第2列用单位矩阵I₂进行替换，由此计算可得Φ。

进一步地，所述步骤2，所述Q的简化推导模型，包括：

保留F₁G₁QG₁ ^T作为中间结果用于计算(FM)^T

记作：

上述结果均采用8×8矩阵进行存储，并视矩阵非零元素个数采用三元式稀疏矩阵存储。

进一步地，所述步骤3，所述X_kk阵简化推导模型，包括：

在计算过程中，对于X_kk矩阵第1行第1列分块，Φ₁X_k1+Φ₂X_k2仅为2个8×8矩阵的乘法和加法。对于X_kk矩阵第2行第1列分块，只需将X_k分块矩阵的第2行第1列直接遍历赋值，而不需要进行解算。

进一步地，所述步骤3，所述P_kk，包括：

其中FT4＝Φ₁P_k2+Φ₂P_k4作为中间重复利用计算单元，在与Q矩阵相加时，仅对第1行第1列进行矩阵加法。在与Q矩阵相加时，只对第1行第1列进行矩阵加法，其他行列为零可不进行运算处理，对于第2行第2列的分块矩阵也直接遍历赋值，以减少CPU指令周期。

进一步地，所述步骤3，所述K_k，包括：

将

并作为中间过程单元保留。

进一步地，所述步骤3，所述X_k，包括：

进一步地，所述步骤3，所述P_k，包括：

对于I-操作，仅需对分块矩阵的对角元素进行减法操作，对于第1行第2列和第2行第1列的数据元素进行取负操作，

进一步地，所述步骤3的降低存储空间的占用，指在X_kk、P_kk、K_k、X_k、P_k的计算过程中，也均采用8×8分块矩阵进行存储，并视非零元素个数采用三元式稀疏矩阵压缩存储。

本发明所采用的基于矩阵稀疏性的Kalman滤波算法优化的方法解决了组合导航中Kalman滤波器高维矩阵的计算复杂、存储量大、占用大型连续堆栈空间等弱点，提高了算法的效率。

尽管为说明目的公开了本发明的实施例，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的精神和范围内，各种替换、变化和修改都是可能的，因此，本发明的范围不局限于实施例所公开的内容。

Claims (7)

1.一种基于矩阵稀疏性的Kalman滤波算法优化的方法，其特征在于：包括如下步骤：

1)建立一种组合导航卡尔曼滤波模型，对解算矩阵进行分块，包括状态系数矩阵F，一步状态转移阵Φ，噪声驱动阵G，噪声方差阵Q，状态一步预测量阵X_kk，预测均方误差阵P_kk，滤波增益阵K_k，状态估计量阵X_k，估计均方误差阵P_k；

2)利用F阵和G阵的零元素进行简化推导，得出Φ阵和Q阵的简化计算过程；

3)对卡尔曼滤波的五个步骤X_kk、P_kk、K_k、X_k、P_k进行计算分块矩阵推导，并在计算过程中采用复用存储单元和压缩存储单元的方法降低存储空间的占用，并利用计算机解算的特点降低计算开销。

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

只需计算第一行分块矩阵且可以将F₁ ²和F₁·F₂进行缓存并在后面的计算中予以复用，统计F²和F³的非零元素个数，并考虑采用三元稀疏矩阵进行存储；

对于一步状态转移阵Φ也按二分法对矩阵进行分块，首先计算后半部分如上述分析，只计算第一行分块矩阵，然后仅针对J矩阵的对角元素进行加1操作，再后将Φ分块矩阵的第2行第2列用单位矩阵I₂进行替换，由此计算可得Φ。

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

保留F₁G₁QG₁ ^T作为中间结果用于计算(FM)^T

M_i+1＝FM_i+(FM_i)^T，

记作：

上述结果均采用8×8矩阵进行存储，并视矩阵非零元素个数采用三元式稀疏矩阵存储。

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

在计算过程中，对于X_kk矩阵第1行第1列分块，Φ₁X_k1+Φ₂X_k2仅为2个8×8矩阵的乘法和加法。对于X_kk矩阵第2行第1列分块，只需将X_k分块矩阵的第2行第1列直接遍历赋值，而不需要进行解算。

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

其中FT4＝Φ₁P_k2+Φ₂P_k4作为中间重复利用计算单元，在与Q矩阵相加时，仅对第1行第1列进行矩阵加法，在与Q矩阵相加时，只对第1行第1列进行矩阵加法，其他行列为零可不进行运算处理，对于第2行第2列的分块矩阵也直接遍历赋值，以减少CPU指令周期。

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

将并作为中间过程单元保留。

进一步地，所述步骤3，所述X_k，包括：

进一步地，所述步骤3，所述P_k，包括：

对于I-操作，仅需对分块矩阵的对角元素进行减法操作，对于第1行第2列和第2行第1列的数据元素进行取负操作。

7.根据权利要求1所述的基于矩阵稀疏性的Kalman滤波算法优化的方法，其特征在于：步骤3)中的降低存储空间的占用，指在X_kk、P_kk、K_k、X_k、P_k的计算过程中，也均采用8×8分块矩阵进行存储，并视非零元素个数采用三元式稀疏矩阵压缩存储。