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

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

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

技术实现要素：
本发明要解决的技术问题是：克服现有算法的不足，从数值计算的角度出发，提出一种工程上简单易行的，利用SINS/GPS系统数字特征进行离线推导优化和实时并行处理的高效率无损卡尔曼滤波优化算法。本发明解决上述技术问题采用的技术方案为：一种基于SINS/GPS组合导航的卡尔曼滤波数值优化方法，其特征在于包括下列步骤：步骤(1)、建立一种基于位置-速度测量组合的SINS/GPS组合导航松耦合闭环Kalman滤波模型，用3阶方阵展开表示模型中的参数矩阵，包括状态系数An、状态转移矩阵φn、系统噪声系数Gn、量测矢量系数Hn、系统噪声方差Qn、量测噪声方差Rn、误差协方差时间更新值Pn(-)、误差协方差量测更新值Pn(+e)及Kalman滤波增益Kn；步骤(2)、利用系数矩阵An的稀疏性、特殊子分块和中间变量在Matlab环境中使用符号运算功能，对式按3阶子分块展开计算，离线推导出离散滤波器转移矩阵φn的简化计算形式；步骤(3)、利用步骤(2)推导得到的φn的分块稀疏性、特殊子分块及噪声矩阵ΓnQn的对称性，在Matlab环境中离线推导出的简化计算形式；步骤(4)、利用φn的分块稀疏性、特殊子分块、中间变量以及协方差矩阵Pn(-)的对称性，在Matlab环境中对式按3阶子分块展开计算，推导出Pn(-)时间更新的简化计算形式；步骤(5)、利用系统量测方程系数矩阵Hn的分块稀疏性、特殊子分块，在Matlab环境中对式进行离线公式推导，得到滤波增益Kn的简化计算形式；步骤(6)、利用Hn的分块稀疏性、特殊子分块、协方差矩阵Pn(-)的对称性，在Matlab环境中对式Pn(+e)＝(I-KnHn)Pn(-)按3阶子矩阵展开计算，离线推Pn(+e)量测更新的简化计算形式；步骤(7)、细分Pn(+e)和Pn(-)的计算过程，并将耗时矩阵Pn(-)划分“有用”、“无用”信息，通过在子过程层级分析对参数的依赖关系，获得一种新的数值解耦机制，在此基础上实现对滤波过程的无损并行处理。进一步的，所述的步骤(2)对3的计算优化具体包含如下步骤：步骤(21)、利用系数矩阵An的稀疏性及其特殊子分块，离线推导出的简化计算形式并作为中间变量保存，推导结果表明具有分块稀疏性，而第3列分块Si3、对角线上的分块S55、S66为特殊子分块；步骤(22)、利用系数矩阵An、的稀疏性及其特殊子分块，将2按3阶分块展开计算，离线推导出的简化计算形式并作为中间变量保存，指出具有分块稀疏性，而第3列分块Ti3、对角线上的分块T55、T66为特殊子分块；步骤(23)、在步骤(21)、(22)的计算过程中引入数值为O3×3的子矩阵符号A31、S34、S35，以保持计算形式的连贯性，减少实时计算程序的编程复杂度。进一步的，所述的步骤(3)对ΓnQn的计算优化具体包含如下步骤：步骤(31)、利用噪声驱动矩阵G(t)的稀疏性，按3阶分块展开计算Q1n＝G(t)Q(t)G(t)T，离线推导出Q(t)的离散化形式，指出Q1n具有分块对角形式；步骤(32)、利用Q1n的对角形式、φn的稀疏性以及ΓnQnΓTn的对称性，按离线推导出ΓnQn的简化计算形式。进一步的，所述的步骤(5)对Kn的计算优化具体包含如下步骤：步骤(51)、利用Hn的稀疏性，按3阶分块展开计算其结果的逆矩阵保存为中间变量D；步骤(52)、利用Hn的稀疏性以及Pn(-)的对称性，按离线推导出Kn的简化计算形式。进一步的，所述的步骤(7)并行优化处理具体包含如下步骤：步骤(71)、将Pn(+e)的计算过程按Pn(+e)＝Pn(-)-KnHnPn(-)细分，KnHnPn(-)代替Pn(+e)建立新的量测更新；步骤(72)、定义Pn(-)的2、3分块列定义为“有用”信息，其余为“无用”信息，“无用”信息的更新过程与新量测更新过程存在数值解耦关系，这两个过程实时并行处理；步骤(73)、将Pn(-)“有用”信息的计算过程按细分，子过程更新与ΓnQn更新存在数值解耦关系，实时并行处理。本发明与现有技术相比的优点在于：(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分别为平台误差角东、北、天方向分量；δvx、δvy、δvz为东、北、天方向速度误差分量；δL、δλ、δh为经度、纬度、高度误差；εc、εr、▽a分别为随机常值漂移矢量、随机马尔科夫过程漂移矢量以及加速度计随机漂移矢量；ωg、ωr、ωa分别为陀螺仪随机漂移白噪声、一阶马尔科夫驱动白噪声和加速度计随机漂移白噪声；为基体姿态矩阵；O3×3为零矩阵；I3×3为单位矩阵；从数值角度指出，其他分块中Ai3仅第1列向量非零、AIMUii为对角矩阵。直接给出系统的白噪声方阵Q(t)为：式1-6其中Qii为对角矩阵。采用位置、速度组合模式定义滤波器的观测矢量zobs(t)：式1-7直接给出量测方程：式1-8其中系数矩阵H(t)、观测白噪声n(t)为：式1-9aH13＝diag{Rn,RecosL,1}式1-9bn(t)＝(Nn,Ne,Nh,Mn,Me,Mu)T式1-10直接给出测量白噪声协方差阵R(t)：式1-11直接给出系统状态方程(式1-1)及观测方程(式1-8)的离散形式：xn＝φnxn-1+Bnucn+Γnωn式1-12zn＝Hnxn+nn式1-13其中ucn为控制矢量，φn误差转移矩阵，Bn为控制系数矩阵，Γn为状态噪声驱动矩阵。直接给出有控制的闭环Kalman滤波迭代计算过程：式1-14a式1-14bPn(+e)＝(I-KnHn)Pn(-)式1-14cucn＝-Knzobsn式1-14d式中，～表示Kalman滤波器估计(或预测)的参数值；(+e)表示tn时刻估计复位后的参数值；(-)表示tn时刻估计前的参数值；(+c)表示tn时刻控制作用校正后的参数值。总结Kalman滤波的实时计算过程，如图1所示，主要包括参数的离散化，时间更新和量测更新过程。涉及多次高阶矩阵的乘法运算，计算量大，需要进行计算效率的优化。步骤2.φn计算的优化。φn的计算依据下式进行：式2-1计算主要围绕18阶方阵An进行，将An按3阶方阵分块展开，在Matlab环境中利用符号运算功能，推导得到的简化形式，如式2-2、式2-3。式2-2式2-3其中Sij、Tij写成易于计算机编程的形式为：式2-4a式2-4bSi5＝Si4,i,j＝1,2式2-4c式2-4d式2-4e式2-4f式2-4g式2-5a式2-5bT55＝(AIMU22S55)Sim式2-5cT66＝(AIMU33S66)Sim式2-5d式2-4、式2-5中涉及A31、S34、S35的计算项为O3×3，涉及Ai3、Si3、AIMUii、S55、S66等特殊分块的计算项为简化计算项，用()Sim表示。其中Ai3、Si3、Ti3具有如下矩阵形式：式2-6对于任意3阶方阵Xi3有以下性质：式2-7式2-7可用于式2-4、式2-5和后续计算的简化。将An、代入式2-1可得：式2-8其中：式2-9a式2-9b指出φ13、φ23也具有式3.1-3的形式，φ33可分解为具有式3.1-3的形式矩阵和I3×3的和；类似地，知φ55、φ66为对角阵。综上所述，通过使用3阶分块展开推导，在φn的离线推导结果中直接得到约占一半数量的O3×3、I3×3分块；对于其他分块的计算，可以利用特殊分块Ai3、Si3、Ti3和Ajj、Sjj、Tjj的性质进行简化；引入中间变量减少大量的重复计算。统计结果表明离线优化减少了φn更新90％的浮点乘法和94％的加法运算，同时内存花销减少69％。步骤3.Qn计算的优化。直接计算ΓnQn，作二阶近似有：式3-1其中：式3-2式中：式3-3显然，Q1n和ΓnQn均为对称矩阵，对ΓnQn仅需计算上三角部分，把Q1n代入ΓnQn可得：式3-4其中：式3-5a式3-5b式3-5c式3-5d式3-5e对ΓnQn计算的优化主要是利用稀疏矩阵G(t)、对角阵分块Q22、Q33φ55、φ66的特殊性以及ΓnQn的对称性进行离线推导。特别地，式3-5c中Qj-3,j-3φjj、式3-5d中Qn55、式3-5e中Qn66仍为对角阵，可用于简化计算。统计结果表明，本发明减少了ΓnQn更新94％(96％)的乘法(加法)运算和88％的内存花销。步骤4.Pn(-)计算的优化。为推导方便，将Pn(-)、Pn(+e)统一用Pn表示(实际编程中，同一个滤波周期中的Pn(-)、Pn(+e)也只用Pn的不同状态表示)，Pn按3×3阶矩阵分块，写成：式4-1Pn(-)为对称矩阵，则有：式4-2先不考虑ΓnQn，式1-14a写成：式4-3代入式2-8、式4-1得：式4-4a式4-4bPn,66＝(φ66Pn-1,66φ66)Sim式4-4c式中()Temp是为减少重复计算在计算机中保存的中间量、()Rep表示已在()Temp中计算过的重复项，实质是的计算结果。对()Temp的计算可利用的特殊性进行优化。考虑ΓnQn，把式3-4、式4-1代入式1-14a得：Pn,ij＝Pn,ij+Qn,ij,i,j＝1,2,3,5,6；i≥j式4-5综上所述，Pn(-)的计算分为式3-4和式4-3两个步骤，优化过程主要利用φn的分块离散性、φi3及φii(i＝5,6)的特殊性以及引入中间量避免重复计算项。统计结果表明，对Pn(-)的优化减少了69％的计算量和44％的内存开销。步骤5.Kn计算的优化。把式1-9、式1-11和式4-1代入到式1-14b的求逆部分可得：式5-1令式5-2容易证明DT＝D，写成：式5-3则有：式5-4记为：式5-5有：式5-6从上述推导过程可知，采用分块矩阵进行推导优化后，Kn的计算只需要执行式5-1、式5-2、式5-6三个相对简单的过程，利用Hn的稀疏性，避免了式1-14b中5次高阶矩阵乘法。对优化结果的统计表明，本发明减少了Kn更新89％的计算量和61％的内存花销。步骤6.Pn(+e)计算的优化。将式1-14c改写成：Pn(+e)＝Pn(-)-KnHnPn(-)式6-1主要计算量在于KnHnPn(-)，因为Pn(+e)、及Pn(-)都是对称矩阵，则KnHnPn(-)也是对称矩阵，仅需计算上三角部分。记KnHnPn(-)为ΔPn：式6-2代入式1-9、式4-1及式5-5得：ΔPij＝Ki1H13Pn,3j+Ki2Pn,2j,i,j＝1,2,...,6；i≤j式6-3则有：Pn,ij＝Pn,ij-ΔPij,i,j＝1,2,...,6；i≤j式6-4从推导过程可以看到，利用Pn(+e)的对称性以及Hn分块稀疏性，推导出Pn(+e)简洁的计算形式(式6-3、式6-4)，占主要计算量的式6-3中仅需Kn与Pn(+e)第2、3分块行的作少量3阶矩阵乘法，减少大量不必要计算。与常规算法作比较，减少了85％的计算量和61％的内存开销。步骤7.无损解耦并行优化。从优化结果看，Pn(-)的计算量仍约占整个滤波过程的一半，成为进一步提高计算效率的瓶颈。考虑对Pn(-)的更新实施并行处理，以提高实时性。从上述Kalman滤波迭代计算推导过程可知，除Pn(+e)外，其它参量的更新或与Pn(-)无关或仅与第2、3分块行和2、3分块列有关。因此定义Pn(-)第2、3分块行(列)为“有用”信息，其余分块是“无有”信息。更进一步，因为Pn(-)是对称矩阵，因此“有用”分块实际上只有第2、3分块列。“无用”分块用“×”表示，则Pn(-)可写成：式3.6-1在量测更新中，仅Pn(+e)与“无用”数据耦合。将Pn(+e)的更新细分成ΔPij(式6-3)的更新和Pn(-)累加(式6-4)两个过程，并用ΔPij代替Pn(+e)，与式1-14b、式1-14d构成新的量测更新过程。因此，获得Pn(-)“无用”信息更新与新量测更新的数值解耦，可以将这两个过程并行处理，以解决Pn(-)计算耗时的瓶颈问题。类似地，对Pn(-)“有用”分块的更新细分成按式4-3更新和按式4-5更新两个计算步骤，显然，第一个步骤中的计算与时间更新中ΓnQn的计算是可以进行并行处理。综上所述，使用并行处理优化后的Kalman滤波执行过程如图3所示。与无并行优化处理相比，图3所示的并行优化效率提高约25％。更重要的是本发明的解耦过程是利用SINS/GPS组合导航的数字特点，通过严格的数字推导获得，与流行的并行处理方法不同，不需要作“强制滞后”处理，是一种无损的并行处理方法。本发明未详细阐述的部分属于本领域公知技术。