CN104408315B - 一种基于sins/gps组合导航的卡尔曼滤波数值优化方法 - Google Patents

Abstract

本发明公开了一种基于SINS/GPS组合导航的卡尔曼滤波数值优化方法，在建立SINS/GPS组合导航常规Kalman滤波模型的基础上，该方法采用分块矩阵技术，对滤波计算中涉及的高阶矩阵运算在Matlab环境中按子分块展开推导，避免大量无意义的重复项运算；并通过细分耗时参数的计算过程，在子过程层级获得与其他更新过程的数值解耦，从而给出一种新的无损并行处理机制，极大地提高了滤波计算的实时性。本发明克服了常用分解滤波优化算法推导复杂不易于工程实现的缺点，避免了传统并行滤波中的有损精度解耦，实现了SINS/GPS组合导航***的高效率无损数据滤波。

Description

一种基于SINS/GPS组合导航的卡尔曼滤波数值优化方法

技术领域

本发明涉及组合导航***数据滤波的技术领域，具体涉及一种基于SINS/GPS组合导航的卡尔曼滤波数值优化方法。

背景技术

在SINS/GPS组合导航***的数据滤波环节，往往包含对“有色噪声”的处理，这时已经不符合经典Kalman滤波的适用条件，为了仍然能够使用最优估计技术，需要进行状态扩充。但是随着状态维数的增加，计算量急剧膨胀，数值稳定性下降，甚至会出现“维数灾难”。对于这个问题，目前一般的方法是，利用平方根信息滤波SRIF、U-D分解滤波、SVD滤波等分解滤波优化算法来减少计算量和增强数值鲁棒性，并利用并行算法提高滤波实时性。但是，分解算法的推导过程需要用到复杂的矩阵理论知识，在工程应用上有一定的难度；而且分解滤波算法复杂度仍为O(n³)，随着状态维数的进一步增加，实时性也受到了挑战。并行Kalman滤波虽然大幅提升了实时性，但是其解耦过程基于量测更新强制滞后一个滤波周期来实现，这会带来一定的精度损害，在多次迭代后可能会引起数值不稳定问题。此外，作为一类通用的优化算法，上述几种滤波器都没有基于SINS/GPS组合导航***的特点进行专门设计，国内外也鲜有研究利用***的数值特征对Kalman滤波过程进行深入的优化。

发明内容

本发明要解决的技术问题是：克服现有算法的不足，从数值计算的角度出发，提出一种工程上简单易行的，利用SINS/GPS***数字特征进行离线推导优化和实时并行处理的高效率无损卡尔曼滤波优化算法。

本发明解决上述技术问题采用的技术方案为：一种基于SINS/GPS组合导航的卡尔曼滤波数值优化方法，其特征在于包括下列步骤：

步骤(1)、建立一种基于位置-速度测量组合的SINS/GPS组合导航松耦合闭环Kalman滤波模型，用3阶方阵展开表示模型中的参数矩阵，包括状态系数A_n、状态转移矩阵φ_n、***噪声系数G_n、量测矢量系数H_n、***噪声方差Q_n、量测噪声方差R_n、误差协方差时间更新值P_n(-)、误差协方差量测更新值P_n(+_e)及Kalman滤波增益K_n；

步骤(2)、利用系数矩阵A_n的稀疏性、特殊子分块和中间变量在Matlab环境中使用符号运算功能，对式按3阶子分块展开计算，离线推导出离散滤波器转移矩阵φ_n的简化计算形式；

步骤(3)、利用步骤(2)推导得到的φ_n的分块稀疏性、特殊子分块及噪声矩阵Γ_nQ_n 的对称性，在Matlab环境中离线推导出的简化计算形式；

步骤(4)、利用φ_n的分块稀疏性、特殊子分块、中间变量以及协方差矩阵P_n(-)的对称性，在Matlab环境中对式按3阶子分块展开计算，推导出P_n(-)时间更新的简化计算形式；

步骤(5)、利用***量测方程系数矩阵H_n的分块稀疏性、特殊子分块，在Matlab环境中对式进行离线公式推导，得到滤波增益K_n的简化计算形式；

步骤(6)、利用H_n的分块稀疏性、特殊子分块、协方差矩阵P_n(-)的对称性，在Matlab环境中对式P_n(+_e)＝(I-K_nH_n)P_n(-)按3阶子矩阵展开计算，离线推P_n(+_e)量测更新的简化计算形式；

步骤(7)、细分P_n(+_e)和P_n(-)的计算过程，并将耗时矩阵P_n(-)划分“有用”、“无用”信息，通过在子过程层级分析对参数的依赖关系，获得一种新的数值解耦机制，在此基础上实现对滤波过程的无损并行处理。

进一步的，所述的步骤(2)对3的计算优化具体包含如下步骤：

步骤(21)、利用系数矩阵A_n的稀疏性及其特殊子分块，离线推导出的简化计算形式并作为中间变量保存，推导结果表明具有分块稀疏性，而第3列分块S_i3、对角线上的分块S₅₅、S₆₆为特殊子分块；

步骤(22)、利用系数矩阵A_n、的稀疏性及其特殊子分块，将2按3阶分块展开计算，离线推导出的简化计算形式并作为中间变量保存，指出具有分块稀疏性，而第3列分块T_i3、对角线上的分块T₅₅、T₆₆为特殊子分块；

步骤(23)、在步骤(21)、(22)的计算过程中引入数值为O_3×3的子矩阵符号A₃₁、S₃₄、S₃₅，以保持计算形式的连贯性，减少实时计算程序的编程复杂度。

进一步的，所述的步骤(3)对Γ_nQ_n 的计算优化具体包含如下步骤：

步骤(31)、利用噪声驱动矩阵G(t)的稀疏性，按3阶分块展开计算Q_1n＝G(t)Q(t)G(t)^T，离线推导出Q(t)的离散化形式，指出Q_1n具有分块对角形式；

步骤(32)、利用Q_1n的对角形式、φ_n的稀疏性以及Γ_nQ_nΓ^T _n的对称性，按离线推导出Γ_nQ_n 的简化计算形式。

进一步的，所述的步骤(5)对K_n的计算优化具体包含如下步骤：

步骤(51)、利用H_n的稀疏性，按3阶分块展开计算其结果的逆矩阵保存为中间变量D；

步骤(52)、利用H_n的稀疏性以及P_n(-)的对称性，按离线推导出K_n的简化计算形式。

进一步的，所述的步骤(7)并行优化处理具体包含如下步骤：

步骤(71)、将P_n(+_e)的计算过程按P_n(+_e)＝P_n(-)-K_nH_nP_n(-)细分，K_nH_nP_n(-)代替P_n(+_e)建立新的量测更新；

步骤(72)、定义P_n(-)的2、3分块列定义为“有用”信息，其余为“无用”信息，“无用”信息的更新过程与新量测更新过程存在数值解耦关系，这两个过程实时并行处理；

步骤(73)、将P_n(-)“有用”信息的计算过程按细分，子过程更新与Γ_nQ_n 更新存在数值解耦关系，实时并行处理。

本发明与现有技术相比的优点在于：

(1)本发明充分利用了SINS/GPS的数字特征进行离线优化，其实时计算效率优于流行的通用滤波优化算法。

(2)与通用的分解滤波优化算法相比，本发明没有复杂的矩阵理论推导，更易于工程实现。

(3)本发明的数值解耦过程仅通过***的数字特征推导获得，没有流行并行滤波算法中将量测更新强制滞后一个滤波周期的处理，因此具有更高的滤波精度。

(4)本发明对滤波计算的优化是纯数值离线优化和实时并行优化，能够在通用优化算法的基础上进行二次优化，以达到更高计算效率的目的。

附图说明

图1为本发明数值优化的对象——闭环Kalman滤波周期更新过程。

图2为本发明提出的Kalman滤波数值优化过程。

图3为本发明提出的无损解耦Kalman滤波并行处理机制。

具体实施方式

下面结合图1和图2具体说明本发明的实施过程及主要处理方法。该方法的具体步骤如下：

步骤1.采用间接法滤波，直接给出***状态方程：

式1-1

其中状态误差x(t)、***白噪声ω(t)、系数矩阵A(t)、G(t)为：

式1-2

式1-3

式1-4

式1-5

式中，γ_x、γ_y、γ_z分别为平台误差角东、北、天方向分量；δv_x、δv_y、δv_z为东、北、天方向速度误差分量；δL、δλ、δh为经度、纬度、高度误差；ε_c、ε_r、▽_a分别为随机常值漂移矢量、随机马尔科夫过程漂移矢量以及加速度计随机漂移矢量；ω_g、ω_r、ω_a分别为陀螺仪随机漂移白噪声、一阶马尔科夫驱动白噪声和加速度计随机漂移白噪声；为基体姿态矩阵；O_3×3为零矩阵；I_3×3为单位矩阵；从数值角度指出，其他分块中A_i3仅第1列向量非零、A_IMUii为对角矩阵。

直接给出***的白噪声方阵Q(t)为：

式1-6

其中Q_ii为对角矩阵。

采用位置、速度组合模式定义滤波器的观测矢量z_obs(t)：

式1-7

直接给出量测方程：

式1-8

其中系数矩阵H(t)、观测白噪声n(t)为：

式1-9a

H₁₃＝diag{R_n,R_e cosL,1} 式1-9b

n(t)＝(N_n,N_e,N_h,M_n,M_e,M_u)^T 式1-10

直接给出测量白噪声协方差阵R(t)：

式1-11

直接给出***状态方程(式1-1)及观测方程(式1-8)的离散形式：

x_n＝φ_nx_n-1+B_nu_cn+Γ_nω_n 式1-12

z_n＝H_nx_n+n_n 式1-13

其中u_cn为控制矢量，φ_n误差转移矩阵，B_n为控制系数矩阵，Γ_n为状态噪声驱动矩阵。直接给出有控制的闭环Kalman滤波迭代计算过程：

式1-14a

式1-14b

P_n(+_e)＝(I-K_nH_n)P_n(-) 式1-14c

u_cn＝-K_nz_obsn 式1-14d

式中，～表示Kalman滤波器估计(或预测)的参数值；(+_e)表示t_n时刻估计复位后的参数值；(-)表示t_n时刻估计前的参数值；(+_c)表示t_n时刻控制作用校正后的参数值。

总结Kalman滤波的实时计算过程，如图1所示，主要包括参数的离散化，时间更新和量测更新过程。涉及多次高阶矩阵的乘法运算，计算量大，需要进行计算效率的优化。

步骤2.φ_n计算的优化。φ_n的计算依据下式进行：

式2-1

计算主要围绕18阶方阵A_n进行，将A_n按3阶方阵分块展开，在Matlab环境中利用符号运算功能，推导得到的简化形式，如式2-2、式2-3。

式2-2

式2-3

其中S_ij、T_ij写成易于计算机编程的形式为：

式2-4a

式2-4b

S_i5＝S_i4,i,j＝1,2 式2-4c

式2-4d

式2-4e

式2-4f

式2-4g

式2-5a

式2-5b

T₅₅＝(A_IMU22S₅₅)_Sim 式2-5c

T₆₆＝(A_IMU33S₆₆)_Sim 式2-5d

式2-4、式2-5中涉及A₃₁、S₃₄、S₃₅的计算项为O_3×3，涉及A_i3、S_i3、A_IMUii、S₅₅、S₆₆等特殊分块的计算项为简化计算项，用()_Sim表示。其中A_i3、S_i3、T_i3具有如下矩阵形式：

式2-6

对于任意3阶方阵X_i3有以下性质：

式2-7

式2-7可用于式2-4、式2-5和后续计算的简化。将A_n、代入式2-1可得：

式2-8

其中：

式2-9a

式2-9b

指出φ₁₃、φ₂₃也具有式3.1-3的形式，φ₃₃可分解为具有式3.1-3的形式矩阵和I_3×3的和；类似地，知φ₅₅、φ₆₆为对角阵。

综上所述，通过使用3阶分块展开推导，在φ_n的离线推导结果中直接得到约占一半数量的O_3×3、I_3×3分块；对于其他分块的计算，可以利用特殊分块A_i3、S_i3、T_i3和A_jj、S_jj、T_jj的性质进行简化；引入中间变量减少大量的重复计算。统计结果表明离线优化减少了φ_n更新90％的浮点乘法和94％的加法运算，同时内存花销减少69％。

步骤3.Q_n计算的优化。直接计算Γ_nQ_n ，作二阶近似有：

式3-1

其中：

式3-2

式中：

式3-3

显然，Q_1n和Γ_nQ_n 均为对称矩阵，对Γ_nQ_n 仅需计算上三角部分，把Q_1n代入Γ_nQ_n 可得：

式3-4

其中：

式3-5a

式3-5b

式3-5c

式3-5d

式3-5e

对Γ_nQ_n 计算的优化主要是利用稀疏矩阵G(t)、对角阵分块Q₂₂、Q₃₃φ₅₅、φ₆₆的特殊性以及Γ_nQ_n 的对称性进行离线推导。特别地，式3-5c中Q_j-3,j-3φ_jj、式3-5d中Q_n55、式3-5e中Q_n66仍为对角阵，可用于简化计算。统计结果表明，本发明减少了Γ_nQ_n 更新94％(96％)的乘法(加法)运算和88％的内存花销。

步骤4.P_n(-)计算的优化。为推导方便，将P_n(-)、P_n(+_e)统一用P_n表示(实际编程中，同一个滤波周期中的P_n(-)、P_n(+_e)也只用P_n的不同状态表示)，P_n按3×3阶矩阵分块，写成：

式4-1

P_n(-)为对称矩阵，则有：

式4-2

先不考虑Γ_nQ_n ，式1-14a写成：

式4-3

代入式2-8、式4-1得：

式4-4a

式4-4b

P_n,66＝(φ₆₆P_n-1,66φ₆₆)_Sim 式4-4c

式中()_Temp是为减少重复计算在计算机中保存的中间量、()_Rep表示已在()_Temp中计算过的重复项，实质是的计算结果。对()_Temp的计算可利用的特殊性进行优化。

考虑Γ_nQ_n ，把式3-4、式4-1代入式1-14a得：

P_n,ij＝P_n,ij+Q_n,ij,i,j＝1,2,3,5,6；i≥j 式4-5

综上所述，P_n(-)的计算分为式3-4和式4-3两个步骤，优化过程主要利用φ_n的分块离散性、φ_i3及φ_ii(i＝5,6)的特殊性以及引入中间量避免重复计算项。统计结果表明，对P_n(-)的优化减少了69％的计算量和44％的内存开销。

步骤5.Kn计算的优化。把式1-9、式1-11和式4-1代入到式1-14b的求逆部分可得：

式5-1

令

式5-2

容易证明D^T＝D，写成：

式5-3

则有：

式5-4

记为：

式5-5

有：

式5-6

从上述推导过程可知，采用分块矩阵进行推导优化后，K_n的计算只需要执行式5-1、式5-2、式5-6三个相对简单的过程，利用H_n的稀疏性，避免了式1-14b中5次高阶矩阵乘法。对优化结果的统计表明，本发明减少了K_n更新89％的计算量和61％的内存花销。

步骤6.P_n(+_e)计算的优化。将式1-14c改写成：

P_n(+_e)＝P_n(-)-K_nH_nP_n(-) 式6-1

主要计算量在于K_nH_nP_n(-)，因为P_n(+_e)、及P_n(-)都是对称矩阵，则K_nH_nP_n(-)也是对称矩阵，仅需计算上三角部分。

记K_nH_nP_n(-)为ΔP_n：

式6-2

代入式1-9、式4-1及式5-5得：

ΔP_ij＝K_i1H₁₃P_n,3j+K_i2P_n,2j,i,j＝1,2,...,6；i≤j 式6-3

则有：

P_n,ij＝P_n,ij-ΔP_ij,i,j＝1,2,...,6；i≤j 式6-4

从推导过程可以看到，利用P_n(+_e)的对称性以及H_n分块稀疏性，推导出P_n(+_e)简洁的计算形式(式6-3、式6-4)，占主要计算量的式6-3中仅需K_n与P_n(+_e)第2、3分块行的作少量3阶矩阵乘法，减少大量不必要计算。与常规算法作比较，减少了85％的计算量和61％的内存开销。

步骤7.无损解耦并行优化。从优化结果看，P_n(-)的计算量仍约占整个滤波过程的一半，成为进一步提高计算效率的瓶颈。考虑对P_n(-)的更新实施并行处理，以提高实时性。从上述Kalman滤波迭代计算推导过程可知，除P_n(+_e)外，其它参量的更新或与P_n(-)无关或仅与第2、3分块行和2、3分块列有关。因此定义P_n(-)第2、3分块行(列)为“有用”信息，其余分块是“无有”信息。更进一步，因为P_n(-)是对称矩阵，因此“有用”分块实际上只有第2、3分块列。“无用”分块用“×”表示，则P_n(-)可写成：

式3.6-1

在量测更新中，仅P_n(+_e)与“无用”数据耦合。将P_n(+_e)的更新细分成ΔP_ij(式6-3)的更新和P_n(-)累加(式6-4)两个过程，并用ΔP_ij代替P_n(+_e)，与式1-14b、式1-14d构成新的量测更新过程。因此，获得P_n(-)“无用”信息更新与新量测更新的数值解耦，可以将这两个过程并行处理，以解决P_n(-)计算耗时的瓶颈问题。类似地，对P_n(-)“有用”分块的更新细分成按式4-3更新和按式4-5更新两个计算步骤，显然，第一个步骤中的计算与时间更新中Γ_nQ_n 的计算是可以进行并行处理。

综上所述，使用并行处理优化后的Kalman滤波执行过程如图3所示。与无并行优化处理相比，图3所示的并行优化效率提高约25％。更重要的是本发明的解耦过程是利用SINS/GPS组合导航的数字特点，通过严格的数字推导获得，与流行的并行处理方法不同，不需要作“强制滞后”处理，是一种无损的并行处理方法。

本发明未详细阐述的部分属于本领域公知技术。

Claims (3)

1.一种基于SINS/GPS组合导航的卡尔曼滤波数值优化方法，其特征在于包括下列步骤：

步骤(1)、建立一种基于位置-速度测量组合的SINS/GPS组合导航松耦合闭环Kalman滤波模型，用3阶方阵展开表示模型中的参数矩阵，包括状态系数矩阵A_n、状态转移矩阵φ_n、***噪声系数G_n、量测系数矩阵H_n、***噪声方差Q_n、量测噪声方差R_n、误差协方差时间更新值P_n(-)、误差协方差量测更新值P_n(+_e)及Kalman滤波增益K_n；

步骤(2)、利用状态系数矩阵A_n的稀疏性、特殊子分块矩阵和中间变量在Matlab环境中使用符号运算功能，对式按3阶子分块展开计算，离线推导出离散滤波器转移矩阵φ_n的简化计算形式；

所述的步骤(2)对的计算优化具体包含如下步骤：

步骤(21)、利用状态系数矩阵A_n的稀疏性及其特殊子分块，离线推导出的简化计算形式并作为中间变量保存，推导结果表明具有分块稀疏性，而第3列分块S_i3、对角线上的分块S₅₅、S₆₆为特殊子分块；

步骤(22)、利用状态系数矩阵A_n、的稀疏性及其特殊子分块，将按3阶分块展开计算，离线推导出的简化计算形式并作为中间变量保存，指出具有分块稀疏性，而第3列分块T_i3、对角线上的分块T₅₅、T₆₆为特殊子分块；

步骤(23)、在步骤(21)、(22)的计算过程中引入数值为O_3×3的子矩阵符号A₃₁、S₃₄、S₃₅，以保持计算形式的连贯性；

步骤(3)、利用步骤(2)推导得到的φ_n的分块稀疏性、特殊子分块矩阵及噪声矩阵的对称性，在Matlab环境中离线推导出的简化计算形式,其中Γ_n为状态噪声驱动矩阵；

步骤(4)、利用φ_n的分块稀疏性、特殊子分块矩阵、中间变量以及协方差矩阵P_n(-)的对称性，在Matlab环境中对式按3阶子分块展开计算，推导出P_n(-)时间更新的简化计算形式；

步骤(5)、利用量测系数矩阵H_n的分块稀疏性、特殊子分块矩阵，在Matlab环境中对式进行离线公式推导，得到滤波增益K_n的简化计算形式；

步骤(6)、利用H_n的分块稀疏性、特殊子分块矩阵、协方差矩阵P_n(-)的对称性，在Matlab环境中对式P_n(+_e)＝(I-K_nH_n)P_n(-)按3阶子矩阵展开计算，离线推导P_n(+_e)量测更新的简化计算形式；

步骤(7)、细分P_n(+_e)和P_n(-)的计算过程，并将耗时矩阵P_n(-)划分“有用”、“无用”信息，通过在子过程层级分析对参数的依赖关系，获得一种新的数值解耦机制，在此基础上实现对滤波过程的无损并行处理；

所述的步骤(7)并行优化处理具体包含如下步骤：

步骤(71)、将P_n(+_e)的计算过程按P_n(+_e)＝P_n(-)-K_nH_nP_n(-)细分，K_nH_nP_n(-)代替P_n(+_e)建立新的量测更新；

步骤(72)、P_n(-)的2、3分块列被定义为“有用”信息，其余被定义为“无用”信息，“无用”信息的更新过程与新量测更新过程存在数值解耦关系，这两个过程实时并行处理；

步骤(73)、将P_n(-)“有用”信息的计算过程按细分，子过程更新与更新存在数值解耦关系，实时并行处理。

2.根据权利要求1所述的一种基于SINS/GPS组合导航的卡尔曼滤波数值优化方法，其特征在于：所述的步骤(3)对的计算优化具体包含如下步骤：

步骤(31)、利用***噪声系数G_n的稀疏性，按3阶分块展开计算Q_1n＝G_nQ(t)G_n ^T，离线推导出Q(t)的离散化形式，指出Q_1n具有分块对角形式；

步骤(32)、利用Q_1n的对角形式、φ_n的稀疏性以及的对称性，按离线推导出的简化计算形式。

3.根据权利要求1所述的一种基于SINS/GPS组合导航的卡尔曼滤波数值优化方法，其特征在于：所述的步骤(5)对K_n的计算优化具体包含如下步骤：

步骤(51)、利用H_n的稀疏性，按3阶分块展开计算其结果的逆矩阵保存为中间变量D；

步骤(52)、利用H_n的稀疏性以及P_n(-)的对称性，按离线推导出K_n的简化计算形式。