技术领域
[0001] 本
发明涉及航空航天系统信息处理方面的导航制导与控制的技术领域，尤其涉及一种基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，可应用于导航
定位系统中。
背景技术
[0002] 传统的随机概率滤波方法一般要求已知过程噪声和观测噪声的统计特性，或者假设其满足一定的分布条件，而实际的非线性系统中系统状态或者参数的统计特性往往是未知的，因此，常规的随机概率滤波
算法的应用有很大的局限性。集员滤波算法仅仅要求噪声的有界性，不需要精确获得噪声的统计特性，这一点在实际系统中通常是能够得到保证的，并且在集员滤波计算
框架下得到的状态参数估计结果是一个可行解集合，而不是常规滤波计算获得的单个估计值。从控制
角度来说，集员滤波方法提供了鲁棒控制和最优控制等理论所要求的状态参数边界，可更好地实现滤波方法与控制策略结合。
[0003] 考虑到可行状态参数集合形状一般无法准确确定，甚至是非凸的，集员滤波方法在形式上大多采用椭球定界算法。Schweppe和Bertsekas首先提出了可以利用外定界椭球集合来包含系统的真实状态，但是没有考虑椭球的最优化问题。在此
基础上，Fogel和Huang给出了最优化定界椭球算法，得到了最小体积和最小迹椭球集合；Maksarov、Kurzhanski和Chernousko等人进一步发展了针对状态和参数估计计算的椭球计算技术；并且Lin针对特定应用情况提出了一种自适应的边界估计计算的椭球算法；Polyak推倒了用于具有模型不确定性系统的椭球算法，进一步扩展了椭球集员滤波算法的应用领域。
[0004] 但是，上述这些算法都是应用于线性系统的，Scholte和Campell将椭球定界算法推广到非线性系统提出了一种扩展集员滤波算法，其主要思想是首先对非线性系统线性化处理，并采用区间分析技术估计线性化近似后的高阶项误差范围，将其用椭球外包后与噪声椭球集合实施直和计算组成虚拟噪声椭球集合，然后对得到的线性化系统实施线性椭球集员滤波计算，最终得到非线性系统状态参数的估计计算结果。
[0005] 但是，基于Taylor级数线性化处理得到的扩展集员滤波算法存在着很大的
缺陷，首先当系统非线性比较强时，围绕系统状态参数预测估计或者状态参数预估值的一阶Taylor级数展开式往往存在着很大的截断误差，使得该算法存在着数值计算
稳定性变差，计算复杂，甚至出现滤波算法发散的现象；再者一阶Taylor级数扩展需要计算Jacobi矩阵，二阶Taylor级数扩展需要计算复杂的Hessian矩阵，计算量巨大，对处理器要求很高，难以满足
导航系统快速初始对准的要求。
发明内容
[0006] 针对现有非线性滤波技术在GNSS/INS紧组合导航系统中系统状态变量或者参数估计计算效能差，基于Taylor级数线性近似的扩展椭球集员滤波算法的计算复杂、效率低下、计算
精度不能满足系统要求的技术问题，本发明提出一种基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，基于Fourier-Hermite级数扩展逼近计算非线性椭球集员滤波，充分利用了Fourier-Hermite扩展多项式的正交性特性，有效减小了滤波计算量，提高了系统状态参数估计的计算效率，并且有效改善了扩展集员滤波方法的计算精度。
[0007] 为了达到上述目的，本发明的技术方案是这样实现的：一种基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，其步骤为：
[0008] 步骤一：构建GNSS/INS组合导航系统非线性模型方程，包括
状态方程和观测方程；
[0009] 步骤二：根据第k-1步
迭代计算获得系统状态变量的估计均值和方差矩阵，确定第k-1步GNSS/INS组合导航系统的状态参数向量的状态分量不确定区间；
[0010] 步骤三：基于Fourier-Hermite级数多项式逼近法对GNSS/INS组合导航系统的非线性状态方程和观测方程实施Fourier-Hermite级数多项式逼近计算操作，确定Lagrange余子取值区间；
[0011] 步骤四：计算Fourier-Hermite级数多项式逼近的高阶余项的误差边界，利用椭球将线性化误差外包得到非线性系统状态方程和观测方程的线性化误差的外包椭球；
[0012] 步骤五：计算虚拟过程的误差椭球，包括虚拟状态噪声误差椭球和虚拟观测噪声误差椭球；
[0013] 步骤六：利用线性化椭球集员滤波算法的预测计算第k步系统状态向量的预测状态椭球边界；
[0014] 步骤七：利用线性化椭球集员滤波算法的更新步骤实施系统状态向量的椭球边界更新计算；
[0015] 步骤八：利用线性化椭球集员算法的状态估计完成系统状态向量的第k步的估计均值计算和估计方差矩阵计算，从而完成GNSS/INS组合导航系统状态向量的估计计算任务。
[0016] 利用第k-1步的系统状态向量的估计均值和方差矩阵确定当前第k步的系统状态向量各分量的不确定区间，第k-1步系统状态向量的各分量 不确定区间为：
[0017]
[0018] 其中， 表示第k-1步椭球包络矩阵Pk-1的(i,i)元素， 表示第k-1步的状态变量估计值，l是一个正实数。
[0019] 所述步骤三中Lagrange余子的取值区间的确定方法为：以第k-1步系统状态向量估计值实施Fourier-Hermite级数多项式扩展，取高阶余项作为非线性系统状态向量的Lagrange余子的不确定区间；
[0020] 根据GNSS/INS组合导航系统非线性的状态方程xk＝f(xk-1)+wk-1，其中，xk,xk-1∈Rn分别表示系统第k、k-1步的状态向量，Rn表示n维实数空间，下标k表示迭代计算的第k步，fn(·)表示系统状态函数、是已知的非线性可导函数，wk-1∈R 表示系统的过程噪声，且wk∈(0,Qk)，Qk为第k步系统噪声方差矩阵，且满足|wk,i|＜1，i＝1,2,…,n；第k-1步获得的系统状态向量的可行椭球集合为 表示k-1步的系统状态变量估计值，
Pk-1为k-1步的椭球形状包络矩阵，利用Fourier-Hermite级数多项式获得线性化逼近生成的Lagrange余子的极小化区间，以第k-1步系统状态向量的估计点 做Fourier-Hermite级数多项式逼近获得系统状态方程的n阶Fourier-Hermite级数多项式表达式为：
[0021]
[0022] 其中，H(·)为Hermite多项式展开函数，P为满足正定性的椭球形状包络矩阵，表示Fourier-Hermite级数的高阶余项，根据Fourier-Hermite级数的正交性质，其高阶余项表达式为：
[0023]
[0024] 利用Fourier-Hermite级数多项式对系统非线性的观测方程zk＝h(xk)+vk实施多项式扩展计算，获得Fourier-Hermite级数多项式逼近计算生成Lagrange余子，其中，zk∈Rm表示系统第k步的观测向量，Rm表示m维实数空间，h(·)表示系统观测函数、是已知的非线性可导函数，vk∈Rm系统的观测噪声，且vk∈(0,Rk)，Rk为第k步系统观测噪声方差矩阵，观测噪声满足|vk,j|＜1，j＝1,2,…,m；以第k步状态预测点 做Fourier-Hermite级数多项式逼近计算，获得非线性观测方程的Fourier-Hermite级数多项式逼近计算表达式为：
[0025]
[0026] 其中，Pk,k-1表示第k步系统状态变量的预测方差矩阵， 表示利用预测状态变量表达的观测函数， 是非线性观测方程的Fourier-Hermite级数多项式的高阶余项，其表达式为
[0027]
[0028] 所述步骤四中计算非线性系统状态方程和观测方程的线性化误差的外包椭球的方法为：利用Fourier-Hermite级数多项式逼近操作获得高阶余项算子作为Lagrange余子，计算逼近误差边界，用椭球形状将状态方程的Fourier-Hermite级数多项式逼近线性化误差外包：
[0029]
[0030] 获得系统状态方程的外包椭球为 其中， 表示Fourier-Hermite级数多项式逼近的系统状态方程不确定性噪声方差矩阵， 表示不确定性噪声方差矩阵的对角元素， 表示不确定性噪声方差矩阵的非对角元素；
[0031] 利用椭球获得的观测方程的Fourier-Hermite级数多项式逼近的线性化误差外包：
[0032]
[0033] 获得系统状态方程的外包椭球为 其中， 表示Fourier-Hermite级数多项式逼近的系统观测方程不确定性噪声方差矩阵， 表示不确定性噪声方差矩阵的对角元素， 表示不确定性噪声方差矩阵的非对角元素。
[0034] 所述步骤五中获取虚拟状态噪声误差椭球和虚拟观测噪声误差椭球的方法：
[0035] 计算虚拟过程状态噪声误差椭球为
[0036]
[0037] 其中，Qk-1为第k-1步系统噪声方差矩阵， 表示第k-1步的系统状态噪声误差椭球方差矩阵直和：
[0038]
[0039] 其中， 表示第k-1步的外包椭球的方差矩阵， 为过程噪声方差直和计算的过程噪声尺度因子，
[0040] 计算虚拟观测噪声误差椭球
[0041]
[0042] 其中， 表示第k-1步的系统观测噪声误差椭球方差矩阵直和：
[0043]
[0044] 其中， 为观测噪声方差矩阵直和计算的观测噪声尺度因子， 表示第k步观测函数的线性化外包椭球方差矩阵。
[0045] 所述步骤六中预测计算第k步系统状态向量的预测状态椭球边界的方法为：
[0046] 线性化预测椭球 和虚拟过程噪声直和计算过程，利用第k-1步状态方程获得系统状态向量估计值 和Fourier-Hermite级数多项式逼近计算第k步的系统状态向量的预测值，那么根据Fourier-Hermite级数多项式的均值计算公式有：
[0047]
[0048] 其中，N(·)表示系统状态向量的概率分布
密度函数， 是过程函数的Fourier-Hermite多项式的系数向量矩阵的第0行向量；
[0049] 根据Fourier-Hermite级数多项式逼近计算原理，系统状态向量的方差表达为：
[0050]
[0051] 其中，βk-1为系统状态向量的方差尺度因子，满足βk-1∈(0，1)；
[0052] 那么系统状态向量的方差矩阵可表达为：
[0053]
[0054] 从而获得第k步系统状态向量的预测状态椭球
[0055] 所述步骤七中更新状态椭球边界的方法为：
[0056] 将预测系统状态向量椭球 和观测向量序列集合{Sy}做直和交集计算，其中观测向量序列集合为
[0057]
[0058] 其中，x表示泛指的系统状态变量；
[0059] 根据系统状态向量的第k步的预测值采用Fourier-Hermite级数多项式计算系统观测向量的观测更新计算过程，观测向量的预测为：
[0060]
[0061] 其中，系数向量 表示观测函数的Fourier-Hermite多项式的系数矩阵的第0行向量；
[0062] 系统观测向量的预测方差矩阵为：
[0063]
[0064] 那么系统状态向量与观测向量的协方差矩阵可计算为：
[0065]
[0066] 其中，ρk为观测向量方差矩阵的调节尺度因子；
[0067] 那么观测噪声生方差矩阵与观测向量误差方差矩阵的直和为：
[0068] ρk∈(0,1)，P'z,k,k-1观测噪声方差矩阵。
[0069] 所述步骤八中第k步系统状态变量的估计计算和估计方差矩阵为：
[0070]
[0071]
[0072] 其中，滤波增益矩阵Kk为： 表示第k步的系统状态向量估计误差包络矩阵计算的中间算子，且 P'xz,k,k-1表示系统状
态变量与观测向量的协方差矩阵，δk表示算法健康度因子，其表达式为：
[0073]
[0074] 所述尺度因子 需要E(0,Qk-1)和 的直和计算，计算准则式为其最优计算式为：
[0075] 其中，tr(·)为迹；
[0076] 尺度因子βk-1需要两个椭球 和 的直和计算，考虑观测向量更新条件下的方差矩阵计算式为：
[0077]
[0078] 从而，可以得到尺度因子参数βk-1的计算公式为
[0079]
[0080] 采用最小化性能指标算法健康度因子δk上界形式来计算尺度因子ρk：
[0081]
[0082] 尺度因子参数ρk的一种次优计算式为：
[0083]
[0084] 其中，pm是矩阵Pk,k-1的最大奇异值，cm是矩阵 的最大奇异值。
[0085] 本发明的有益效果：利用Fourier-Hermite级数多项式获得的非线性系统模型方程函数的线性化逼近计算操作，可以以任意阶次的矩信息逼近非线性函数的高阶矩计算，从而获得一类高精度扩展椭球集员滤波方法，应用于GNSS/INS紧组合导航系统模型状态参数的迭代滤波计算过程，实现紧组合导航系统非线性模型状态参数最优滤波计算。本发明计算精度较高，降低了算法的计算复杂度，并且有效保证了扩展椭球集员滤波算法的计算稳定性。。
附图说明
[0086] 为了更清楚地说明本发明
实施例或
现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0087] 图1是本发明GNSS/INS紧组合导航系统模型数据计算
流程图。
[0088] 图2是本发明的流程图。
[0089] 图3是本发明的载体机动运行轨迹图。
[0090] 图4是系统仿真
位置数据曲线(Fourier-Hermite SMF)。
[0091] 图5是系统仿真速度数据曲线(Fourier-Hermite SMF)。
[0092] 图6是系统仿真
姿态数据曲线(Fourier-Hermite SMF)。
[0093] 图7是系统仿真位置数据曲线(EKF)。
[0094] 图8是系统仿真速度数据曲线(EKF)。
[0095] 图9是系统仿真姿态数据曲线(EKF)。
具体实施方式
[0096] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0097] 本发明利用Hermite多项式构建非线性GNSS/INS组合导航方程函数。先定义一维Hermite多项式为：
[0098]
[0099] Hermite多项式是一组正交多项式，满足正交性性质，
[0100]
[0101] 并且Hermite多项式具有完备性，也就是在所有满足 的函数构成的完备空间中，Hermite多项式序列组成了一组基，其内积定义为＝E[f(x)g(x)]，其中，若函数g(x)是有界的，满足E[g(x)2]＜∞，并且其高阶微分也是有界的，也就是(k) 2
满足E[g (x) ]＜∞。另外，若函数g(x)满足标准正态分布N(0,1)，则可利用Hermite多项式构造出函数g(x)的Fourier-Hermite级数扩展表达式为：
[0102]
[0103] 若函数g(x)满足任意正态分布N(μ,σ2)，那么函数g(x)可表达为：
[0104]
[0105] Fourier-Hermite级数多项式也可以推广到高维情形，针对n维函数g(x)，其m阶Hermite多项式中阶次m可分为i1，i2，...，im∈{1,2,…,m}，阶次分为n组，其中有些组内数据可能为空的，每一组的大小qs＝|Js|∈(0,1,…m)，满足 那么n维Hermite多项式可表达为各个变量元素的连乘积形式：
[0106]
[0107] 举例来说，若n＝3，m＝4，i1＝3,i2＝2,i3＝2,i4＝1，那么J1＝{4}，J2＝{2,3}，J3＝{1}，q1＝1，q2＝2，q3＝1，那么：
[0108] [H4(x)]3,2，2，1＝H1(x1)H2(x2)H1(x3)。
[0109] 在多维情形中多维状态参数变量的方差矩阵为Σ，其Cholesky分解矩阵为L，那么Σ＝LLT，则期望均值μ和方差σ的变换式 变换为L-1(x-μ)，那么多维Fourier-Hermite级数扩展表达式为：
[0110]
[0111] 其中，系数表达式为：
[0112]
[0113] 对于充分微分函数，则可以获得：
[0114]
[0115] 其中系数表达式为： 根据Hermite多项式的正交性，系数向量满足Parseval关系式， 其中矩阵Γk可表达为：
[0116]
[0117] 那么利用非线性函数的Fourier-Hermite级数扩展表达式及其性质可以获得函数的期望均值及其方差矩阵的计算表达式：
[0118] E[g(x)]＝c0 (10)
[0119]
[0120] 其中，矩阵Γk∈Rn×n。随机变量与函数的协方差矩阵可表达为
[0121] Cov[x,g(x)]＝LCT＝ΣAT (12)
[0122] 其中，矩阵 矩阵
[0123] 并且若函数的概率积分具有封闭解形式，如：
[0124]
[0125] 那么函数的高阶微分可表示为
[0126]
[0127] 在此基础上可以构建一类Fourier-Hermite级数扩展椭球集员滤波计算方法。
[0128] 如图2所示，一种基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，根据建立的GNSS/INS组合导航系统非线性的模型方程，计算第k步的系统状态参数分量的不确定区间；基于Fourier-Hermite级数性质对GNSS/INS组合导航系统的状态方程和观测方程实施Fourier-Hermite级数逼近计算操作，确定其Lagrange余子的取值区间；进而计算Fourier-Hermite级数逼近计算的误差边界，获取非线性系统状态方程和观测方程的Fourier-Hermite级数逼近计算误差的外包椭球；开展虚拟过程噪声误差椭球和虚拟观测噪声误差椭球计算；利用线性椭球集员滤波算法的预测步骤计算预测状态变量参数的椭球边界；利用线性椭球集员滤波算法的更新步骤计算系统状态参数的椭球边界；再利用线性椭球集员滤波算法状态变量参数的状态估计计算第k+1步的状态变量参数的估计值和估计方差矩阵，确定第k+1步的椭球形状；经由多步迭代过程完成GNSS/INS组合导航系统模型的状态变量参数的计算任务。
[0129] 步骤一：构建GNSS/INS组合导航系统非线性模型方程，包括状态方程和观测方程。
[0130] 设计一类GNSS/INS组合导航系统的状态方程和观测方程，实现状态参数估计。离散
状态空间模型为
[0131]
[0132] 其中，xk∈Rn表示系统第k步的状态向量，zk∈Rm表示系统第k步的观测向量，这里应该说明的是n和m分别表示状态向量和观测向量的维数，Rn和Rm分别表示n维和m维实数空间，下标k表示迭代计算的第k步，本发明的下标表示都一致。f(·)和h(·)分别表示系统状态函数和观测函数，且都是已知的非线性可导函数。wk∈Rn和vk∈Rm分别表示系统的过程噪声和观测噪声，且随时间变化，满足未知但有界(Unknown But Bounded，UBB)的假设条件。记wk∈(0,Qk)和vk∈(0,Rk)，Qk为第k步系统噪声方差矩阵，Rk为第k步系统观测噪声方差矩阵，且过程噪声满足|wk,i|＜1，i＝1,2,…,n，观测噪声满足|vk,j|＜1，j＝1,2,…,m。GNSS/INS组合导航系统状态向量的初始状态x0属于一个已知的有界集合X0：x0∈X0，该集合由系统状态的先验知识确定。对于给定的观测向量序列 第k步的椭球集员滤波估计计算的状态可行集合为{Xk}，由所有可能的状态点组成的，这些状态点与所有可获取的信息包括系统模型方程、噪声假设和初始状态集合相一致。
[0133] 定义椭球集合为E(a,P)＝{x∈Rn|(x-a)TP-1(x-a)≤1}，其中，a表示椭球集合的中心，P为满足正定性的椭球形状包络矩阵。定义系统初始状态向量椭球集合为表示系统状态变量的初始值，P0表示系统椭球形状包络矩阵的初始矩阵；假设经由k-1步的迭代滤波计算，第k-1步获得的系统状态向量的可行椭球集合为表示k-1步的系统状态变量估计值，Pk-1为k-1步的椭球形状包络
矩阵，则第k步的非线性椭球集员滤波算法的迭代计算过程由步骤二至步骤八组成。
[0134] 步骤二：根据第k-1步迭代计算获得系统状态变量的估计均值和方差矩阵，确定第k-1步GNSS/INS组合导航系统的状态参数向量的状态分量不确定区间。
[0135] 利用第k-1步的系统状态向量的估计均值和方差矩阵确定当前第k步的系统状态向量各分量的不确定区间，第k-1步系统状态向量的各分量不确定区间为：
[0136]
[0137] 其中， 表示第k-1步椭球包络矩阵Pk-1的(i,i)元素， 表示第k-1步的状态变量估计值；l是一个正实数，它设置的意义在于保证第k-1步的系统状态向量估计值99.7％的概率落在设定的状态向量取值区间内，其一般取值为l≥3。
[0138] 步骤三：基于Fourier-Hermite级数多项式逼近法对GNSS/INS组合导航系统的非线性状态方程和观测方程实施Fourier-Hermite级数多项式逼近计算操作，确定Lagrange余子取值区间。
[0139] 以第k-1步系统状态向量估计值实施Fourier-Hermite级数多项式扩展，取其多项式误差或者称之为高阶余项，Lagrange余子项作为非线性系统状态向量的不确定区间。
[0140] 根据GNSS/INS组合导航系统非线性的状态方程xk＝f(xk-1)+wk-1，基于Fourier-Hermite级数多项式的高阶余项极小化性质，利用Fourier-Hermite级数多项式获得线性化逼近生成的Lagrange余子的极小化区间，以第k-1步系统状态向量的估计点 做Fourier-Hermite级数多项式逼近获得系统状态方程的n阶Fourier-Hermite级数多项式表达式为：
[0141]
[0142] 其中，Rn+1(xk-1)表示Fourier-Hermite级数的高阶余项，根据fourier-Hermite级数的正交性质，那么其高阶余项表达式为：
[0143]
[0144] 同样地，利用fourier-Hermite级数多项式对系统非线性的观测方程zk＝h(xk)+vk实施多项式扩展计算，获得Fourier-Hermite级数多项式逼近计算生成Lagrange余子，以第k步状态预测点 做Fourier-Hermite级数多项式逼近计算，获得非线性观测方程的Fourier-Hermite级数多项式逼近计算表达式为：
[0145]
[0146] 其中，Pk,k-1表示第k步系统状态变量的预测方差矩阵， 表示利用预测状态变量表达的观测函数， 是非线性观测方程的Fourier-Hermite级数多项式的高阶余项，其表达式为
[0147]
[0148] 步骤四：计算Fourier-Hermite级数多项式逼近的高阶余项的误差边界，利用椭球将线性化误差外包得到非线性系统状态方程和观测方程的线性化误差的外包椭球。
[0149] 利用Fourier-Hermite级数多项式逼近操作获得高阶余项算子作为Lagrange余子，计算逼近误差边界，用椭球形状将状态方程的Fourier-Hermite级数多项式逼近线性化误差外包：
[0150]
[0151] 获得系统状态方程的外包椭球为 其中， 表示Fourier-Hermite级数多项式逼近的系统状态方程不确定性噪声方差矩阵， 表示不确定性噪声方差矩阵的对角元素， 表示不确定性噪声方差矩阵的非对角元素。同样地，利用椭球获得的观测方程的Fourier-Hermite级数多项式逼近的线性化误差外包：
[0152]
[0153] 获得系统状态方程的外包椭球为 其中， 表示Fourier-Hermite级数多项式逼近的系统观测方程不确定性噪声方差矩阵， 表示不确定性噪声方差矩阵的对角元素， 表示不确定性噪声方差矩阵的非对角元素。
[0154] 步骤五：计算虚拟过程的误差椭球，包括虚拟状态噪声误差椭球和虚拟观测噪声误差椭球。
[0155] 涉及到Fourier-Hermite级数多项式逼近造成的不确定性误差椭球和系统状态噪声相加的两个椭球直和计算，通过逼近不确定性误差和系统状态噪声的直和计算获得虚拟噪声误差椭球。
[0156] 计算虚拟过程状态噪声误差椭球为
[0157]
[0158] 其中， 表示第k-1步的系统状态噪声误差椭球方差矩阵直和， 是由椭球形状的系统多项式逼近计算的不确定性误差和状态噪声误差相加获得，涉及到两个椭球直和计算：
[0159]
[0160] 其中， 表示第k-1步的外包椭球的方差矩阵， 为过程噪声方差直和计算的过程噪声尺度因子，
[0161] 同样地对非线性观测方程zk＝h(xk)+vk作上述计算步骤，计算虚拟观测噪声误差椭球
[0162]
[0163] 虚拟观测噪声 是由椭球的线性化误差和观测噪声相加获得，其中也涉及到两个椭球的直和计算，
[0164]
[0165] 其中， 为观测噪声方差矩阵直和计算的观测噪声尺度因子， 从而获得系统观测噪声的虚拟噪声椭球 其中， 表示第k步观测函数的线性化外包椭球方差矩阵， 表示获得的直和虚拟噪声方差矩阵。
[0166] 步骤六：利用线性化椭球集员滤波算法的预测步骤计算第k步系统状态向量的预测状态椭球边界。
[0167] 涉及到线性化预测椭球和虚拟过程噪声椭球的直和计算过程，利用已经获得的第k-1步的系统状态向量估计值，经由Fourier-Hermite级数多项式逼近展开计算第k步的状态预测值，获得状态向量线性化预测值及其预测外包椭球，开展线性化预测椭球和虚拟过程噪声椭球以及系统状态向量预测方差矩阵的直和计算，从而获得系统状态变量的预测椭球边界。
[0168] 线性化预测椭球 和虚拟过程噪声直和计算过程，利用第k-1步状态方程获得系统状态向量估计值 和Fourier-Hermite级数多项式逼近计算第k步的系统状态向量的预测值，那么根据Fourier-Hermite级数多项式的均值计算公式有：
[0169]
[0170] 其中，N(·)表示系统状态向量的概率分布密度函数，系数向量 的意义如式(10),(13)和(14)所示， 是过程函数的Fourier-Hermite多项式的系数向量矩阵的第0行向量。
[0171] 根据Fourier-Hermite级数多项式逼近计算原理，系统状态向量的方差表达为：
[0172]
[0173] 其中，βk-1为系统状态向量的方差尺度因子，满足βk-1∈(0，1)，那么系统状态向量的方差矩阵可表达为：
[0174]
[0175] 从而经由上述的式(25)、(26)和(27)的计算过程，获得第k步系统状态向量的预测状态椭球 它不同于线性化预测椭球
[0176] 步骤七：利用线性化椭球集员滤波算法的更新步骤实施系统状态向量的椭球边界更新计算。
[0177] 涉及到预测状态向量椭球和观测向量集合的交集计算。利用系统观测向量序列开展预测椭球和观测向量带的交集计算。即计算观测噪声方差矩阵与观测向量误差方差矩阵的直和，得到滤波增益矩阵。
[0178] 将预测系统状态向量椭球 和观测向量序列集合{Sy}做直和交集计算，其中观测向量序列集合表达为
[0179]
[0180] 其中，x表示泛指的系统状态变量。
[0181] 根据系统状态向量的第k步的预测值，考虑采用Fourier-Hermite级数多项式计算系统观测向量的观测更新计算过程，观测向量的预测计算为：
[0182]
[0183] 其中，系数向量 的意义如式(10),(13)和(14)所示，表示观测函数的Fourier-Hermite多项式的系数矩阵的第0行向量。相应的系统观测向量的预测方差矩阵可由式(26)表达为
[0184]
[0185] 那么系统状态向量与观测向量的协方差矩阵可计算为
[0186]
[0187] 其中，P'xz,k,k-1表示系统状态变量与观测向量的协方差矩阵，ρk为观测向量方差矩阵的调节尺度因子，那么观测噪声生方差矩阵与观测向量误差方差矩阵的直和计算为[0188]
[0189] 其中，Pz',k,k-1观测噪声方差矩阵。那么滤波增益矩阵为：
[0190]
[0191] 步骤八：利用线性化椭球集员算法的状态估计完成系统状态向量的第k步的估计均值计算和估计方差矩阵计算，从而完成GNSS/INS组合导航系统状态向量的估计计算任务。
[0192]
[0193]
[0194]
[0195] 其中， 表示第k步的系统状态向量估计误差包络矩阵计算的中间算子，δk表示算法健康度因子，其表达式为：
[0196]
[0197] 在本发明中引入了三个尺度因子参数 βk-1和ρk，其数值确定方法如下：
[0198] 尺度因子参数 和βk-1涉及到两个椭球直和运算的外包椭球最优化问题，这里选取外包椭球的最小化计算方法，该方法求解形式简单，相比较于最小化外包椭球体积的优化准则，该方法性能鲁棒性更强。即有 从而可以采用式子
[0199]
[0200] 获得最优的尺度因子参数 和βk-1，P1和P2表示泛指的任意两个方差矩阵。
[0201] 尺度因子参数 需要E(0,Qk-1)和 的直和计算，那么其计算准则式为其最优计算式为
[0202]
[0203] 对于尺度因子参数βk-1，需要两个椭球 和 的直和计算，考虑观测向量更新条件下的方差矩阵计算式为：
[0204]
[0205] 从而，可以得到尺度因子参数βk-1的计算公式为
[0206]
[0207] 在迭代计算过程中，观测集合Sy形式一般都比较复杂，从而导致系统状态向量方差矩阵Pk-1的计算复杂性，无论采用最小化椭球体积法还是最小化椭球迹准则，都使尺度因子参数ρk的优化计算很困难，甚至无法获得解析解，若采用数值计算方法计算复杂度很高。在本发明中采用最小化性能指标δk上界形式来计算
[0208]
[0209] 这样可以获得尺度因子参数ρk的一种次优计算式
[0210]
[0211] 其中，pm是矩阵Pk,k-1的最大奇异值，cm是 矩阵的最大奇异值。
[0212] GNSS/INS组合导航系统具有多种组合模式，如松组合、紧组合和深组合模式等。其中，紧组合模式近年来在自动驾驶领域应用越来越多，特别是紧组合模式中GNSS
信号在组合导航模型滤波计算过程中可以直接融合到滤波更新操作中去，能够有效改善滤波计算效能。因此，本发明采用GNSS/INS紧组合导航模型验证提出的基于Fourier-Hermite级数多项式扩展的椭球集员滤波方法的计算效能，其中GNSS/INS紧组合导航模型结构示意图如图1所示。
[0213] GNSS/INS组合导航系统模型由惯性导航方程和GNSS导航方程组成，其中定义惯性导航的
坐标系，为了不失一般性，采用当地
水平坐标系(北东地，NED)作为导航坐标系，从而INS速度微分方程可表示为
[0214]
[0215] 其中，ve表示[3×1]载体运动速度向量； 表示从载体系b到导航系e的方向余弦b矩阵； 表示[3×1]地球旋转向量
角速度；f 表示[3×1]载体坐标系b
加速度计获得的[3×1]比
力向量； 表示当地重力向量。
[0216] 式(44)经一阶离散化和二次欧拉积分可以获得载体速度和位置：
[0217]
[0218] 其中，Δt表示积分步长，pe表示[3×1]载体位置向量。另外，载体旋转运动利用欧拉角表示，[3×1]载体欧拉角向量分别表示纵摇角、横摇角和航向角，定义欧拉角向量为其微分方程为
[0219]
[0220] 对式(46)实施一阶离散化和欧拉积分计算，可以获得载体旋转运动的欧拉角向量为
[0221]
[0222] 式(44)中的方向余弦矩阵 利用欧拉角向量表示为
[0223]
[0224] 方向余弦矩阵微分方程为
[0225]
[0226] 其中， 表示在载体坐标系中惯性组件获得的[3×1]载体旋转角速度向量，[·×]表示三维向量构造的斜对称矩阵； 表示在导航坐标系中的[3×1]地球旋转角速度向量。
[0227] 考虑伪距的量测方程，INS所在位置处获得的伪距可表达为
[0228]
[0229] 其中，(xsi,ysi,zsi)T表示用户所在真实位置，经由Taylor级数扩展并舍弃高阶项，可以获得
[0230]
[0231] 其中，
[0232]
[0233]
[0234] GNSS测得的伪距表达式为
[0235] ρGi＝ri-δtu-vρi (52)
[0236] 其中，ri表示卫星和地面运动载体的真实距离，vρi为量测误差。那么，伪距测量方程表示为
[0237]
[0238] 其中，δtu表示等效时钟误差对应的距离；变量(δXi,δYi,δZi)可以经由坐标变换转化为(δLi,δλi,δhi)参数。同样地，可以获得伪距率方程为
[0239]
[0240] 其中，δtru表示等效时钟
频率误差对应的距离率。并且方程式(53)、(54)中的i＝1,2,3,4，表示取四颗卫星；式(54)中的(δXiδ, Yiδ, Zi)可以用(δvE,δvNδ, vD)速度表示。
[0241] GNSS/INS组合导航系统仿真实例中系统状态向量由位置量δPe、速度量δve、姿态角b量δΦ、等效钟差误差δtu、等效钟差变化率误差δtru以及IMU组件中的加速度计误差δf 和
陀螺仪误差 组成系统状态向量 设置
GNSS/INS组合导航系统模型的初始参数，构建系统算法仿真平台。观测点坐标北纬32.83°，东经68.793°，高度700m，从卫星接收机获取星历数据和原始伪距以及多普勒频移，从中解算出卫星位置、速度并修正地球自转影响，经由多普勒频移解算出伪距率，其中包含着电离层延时以及卫星钟差校正，在相同观测点INS仿真条件设置为初始平台误差角为5°，初始位置误差为10m；陀螺仪随机常值漂移0.02°/h，噪声误差0.02°/h(1σ)；加速度计随机常值零-4 -4
偏1×10 g，噪声误差1×10 g(1σ)；GNSS初始时钟误差和时钟漂移误差分别取为25m和
0.01m/s；伪距和伪距率初始方差为(3m)2和(3m/s)2，从而可以获得系统噪声方差Qk，用GNSS原始数据统计的伪距和伪距率噪声设置观测噪声方差Rk，从而开展GNSS/INS组合导航系统模型仿真。GNSS/INS紧组合导航系统运动载体的运动轨迹如图3所示，本发明获得的系统仿真位置、速度和姿态的数据曲线分别如图4-6所示。为了比较本发明的计算效能，利用常规的EKF算法开展比较研究，从而获得了如图7-9所示的系统状态变量的仿真数据。很明显，本发明提出的Fourier-Hermite级数多项式扩展方法的计算精度明显优于常规的EKF算法，并且位置量估计误差获得明显改善且曲线平滑稳定，并且速度误差量收敛很快，导航效果稳定。
[0242] 以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何
修改、等同替换、改进等，均应包含在本发明的保护范围之内。
