CN108508463B - 基于Fourier-Hermite正交多项式扩展椭球集员滤波方法 - Google Patents

基于Fourier-Hermite正交多项式扩展椭球集员滤波方法 Download PDF

Info

Publication number
CN108508463B
CN108508463B CN201810266008.0A CN201810266008A CN108508463B CN 108508463 B CN108508463 B CN 108508463B CN 201810266008 A CN201810266008 A CN 201810266008A CN 108508463 B CN108508463 B CN 108508463B
Authority
CN
China
Prior art keywords
ellipsoid
fourier
hermite
state
vector
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201810266008.0A
Other languages
English (en)
Other versions
CN108508463A (zh
Inventor
丁国强
娄泰山
张焕龙
张铎
王晓雷
方洁
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhengzhou University of Light Industry
Original Assignee
Zhengzhou University of Light Industry
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhengzhou University of Light Industry filed Critical Zhengzhou University of Light Industry
Priority to CN201810266008.0A priority Critical patent/CN108508463B/zh
Publication of CN108508463A publication Critical patent/CN108508463A/zh
Application granted granted Critical
Publication of CN108508463B publication Critical patent/CN108508463B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/13—Receivers
    • G01S19/35—Constructional details or hardware or software details of the signal processing chain
    • G01S19/37—Hardware or software details of the signal processing chain

Landscapes

  • Engineering & Computer Science (AREA)
  • Signal Processing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

本发明提出了一种基于Fourier‑Hermite正交多项式扩展椭球集员滤波方法，其步骤为：建立GNSS/INS组合导航系统的模型方程；计算第k‑1步的系统状态参数分量的不确定区间；对状态方程和观测方程实施Fourier‑Hermite级数多项式逼近；计算Fourier‑Hermite级数多项式逼近的误差边界，获取非线性方程的Fourier‑Hermite级数多项式逼近计算误差的外包椭球；利用线性椭球集员滤波算法计算预测状态变量的椭球边界、更新状态椭球边界、计算第k步的状态参数变量估计值和估计方差矩阵。本发明利用Fourier‑Hermite级数多项式获得非线性系统模型的线性化逼近，计算精度较高，降低了计算复杂度，并且有效保证了扩展椭球集员滤波算法的计算稳定性。

Description

基于Fourier-Hermite正交多项式扩展椭球集员滤波方法
技术领域
本发明涉及航空航天系统信息处理方面的导航制导与控制的技术领域，尤其涉及一种基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，可应用于导航定位系统中。
背景技术
传统的随机概率滤波方法一般要求已知过程噪声和观测噪声的统计特性，或者假设其满足一定的分布条件，而实际的非线性系统中系统状态或者参数的统计特性往往是未知的，因此，常规的随机概率滤波算法的应用有很大的局限性。集员滤波算法仅仅要求噪声的有界性，不需要精确获得噪声的统计特性，这一点在实际系统中通常是能够得到保证的，并且在集员滤波计算框架下得到的状态参数估计结果是一个可行解集合，而不是常规滤波计算获得的单个估计值。从控制角度来说，集员滤波方法提供了鲁棒控制和最优控制等理论所要求的状态参数边界，可更好地实现滤波方法与控制策略结合。
考虑到可行状态参数集合形状一般无法准确确定，甚至是非凸的，集员滤波方法在形式上大多采用椭球定界算法。Schweppe和Bertsekas首先提出了可以利用外定界椭球集合来包含系统的真实状态，但是没有考虑椭球的最优化问题。在此基础上，Fogel和Huang给出了最优化定界椭球算法，得到了最小体积和最小迹椭球集合；Maksarov、Kurzhanski和Chernousko等人进一步发展了针对状态和参数估计计算的椭球计算技术；并且Lin针对特定应用情况提出了一种自适应的边界估计计算的椭球算法；Polyak推倒了用于具有模型不确定性系统的椭球算法，进一步扩展了椭球集员滤波算法的应用领域。
但是，上述这些算法都是应用于线性系统的，Scholte和Campell将椭球定界算法推广到非线性系统提出了一种扩展集员滤波算法，其主要思想是首先对非线性系统线性化处理，并采用区间分析技术估计线性化近似后的高阶项误差范围，将其用椭球外包后与噪声椭球集合实施直和计算组成虚拟噪声椭球集合，然后对得到的线性化系统实施线性椭球集员滤波计算，最终得到非线性系统状态参数的估计计算结果。
但是，基于Taylor级数线性化处理得到的扩展集员滤波算法存在着很大的缺陷，首先当系统非线性比较强时，围绕系统状态参数预测估计或者状态参数预估值的一阶Taylor级数展开式往往存在着很大的截断误差，使得该算法存在着数值计算稳定性变差，计算复杂，甚至出现滤波算法发散的现象；再者一阶Taylor级数扩展需要计算Jacobi矩阵，二阶Taylor级数扩展需要计算复杂的Hessian矩阵，计算量巨大，对处理器要求很高，难以满足导航系统快速初始对准的要求。
发明内容
针对现有非线性滤波技术在GNSS/INS紧组合导航系统中系统状态变量或者参数估计计算效能差，基于Taylor级数线性近似的扩展椭球集员滤波算法的计算复杂、效率低下、计算精度不能满足系统要求的技术问题，本发明提出一种基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，基于Fourier-Hermite级数扩展逼近计算非线性椭球集员滤波，充分利用了Fourier-Hermite扩展多项式的正交性特性，有效减小了滤波计算量，提高了系统状态参数估计的计算效率，并且有效改善了扩展集员滤波方法的计算精度。
为了达到上述目的，本发明的技术方案是这样实现的：一种基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，其步骤为：
步骤一：构建GNSS/INS组合导航系统非线性模型方程，包括状态方程和观测方程；
步骤二：根据第k-1步迭代计算获得系统状态变量的估计均值和方差矩阵，确定第k-1步GNSS/INS组合导航系统的状态参数向量的状态分量不确定区间；
步骤三：基于Fourier-Hermite级数多项式逼近法对GNSS/INS组合导航系统的非线性状态方程和观测方程实施Fourier-Hermite级数多项式逼近计算操作，确定Lagrange余子取值区间；
步骤四：计算Fourier-Hermite级数多项式逼近的高阶余项的误差边界，利用椭球将线性化误差外包得到非线性系统状态方程和观测方程的线性化误差的外包椭球；
步骤五：计算虚拟过程的误差椭球，包括虚拟状态噪声误差椭球和虚拟观测噪声误差椭球；
步骤六：利用线性化椭球集员滤波算法的预测计算第k步系统状态向量的预测状态椭球边界；
步骤七：利用线性化椭球集员滤波算法的更新步骤实施系统状态向量的椭球边界更新计算；
步骤八：利用线性化椭球集员算法的状态估计完成系统状态向量的第k步的估计均值计算和估计方差矩阵计算，从而完成GNSS/INS组合导航系统状态向量的估计计算任务。
利用第k-1步的系统状态向量的估计均值和方差矩阵确定当前第k步的系统状态向量各分量的不确定区间，第k-1步系统状态向量的各分量
Figure BDA0001611417870000021
不确定区间为：
Figure BDA0001611417870000022
其中，
Figure BDA0001611417870000023
表示第k-1步椭球包络矩阵Pk-1的(i,i)元素，
Figure BDA0001611417870000024
表示第k-1步的状态变量估计值，l是一个正实数。
所述步骤三中Lagrange余子的取值区间的确定方法为：以第k-1步系统状态向量估计值实施Fourier-Hermite级数多项式扩展，取高阶余项作为非线性系统状态向量的Lagrange余子的不确定区间；
根据GNSS/INS组合导航系统非线性的状态方程xk＝f(xk-1)+wk-1，其中，xk,xk-1∈Rn分别表示系统第k、k-1步的状态向量，Rn表示n维实数空间，下标k表示迭代计算的第k步，f(·)表示系统状态函数、是已知的非线性可导函数，wk-1∈Rn表示系统的过程噪声，且wk∈(0,Qk)，Qk为第k步系统噪声方差矩阵，且满足|wk,i|＜1，i＝1,2,…,n；第k-1步获得的系统状态向量的可行椭球集合为
Figure BDA0001611417870000031
表示k-1步的系统状态变量估计值，Pk-1为k-1步的椭球形状包络矩阵，利用Fourier-Hermite级数多项式获得线性化逼近生成的Lagrange余子的极小化区间，以第k-1步系统状态向量的估计点
Figure BDA0001611417870000032
做Fourier-Hermite级数多项式逼近获得系统状态方程的n阶Fourier-Hermite级数多项式表达式为：
Figure BDA0001611417870000033
其中，H(·)为Hermite多项式展开函数，P为满足正定性的椭球形状包络矩阵，
Figure BDA0001611417870000034
表示Fourier-Hermite级数的高阶余项，根据Fourier-Hermite级数的正交性质，其高阶余项表达式为：
Figure BDA0001611417870000035
利用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级数多项式逼近计算表达式为：
Figure BDA0001611417870000042
其中，Pk,k-1表示第k步系统状态变量的预测方差矩阵，
Figure BDA0001611417870000043
表示利用预测状态变量表达的观测函数，
Figure BDA0001611417870000044
是非线性观测方程的Fourier-Hermite级数多项式的高阶余项，其表达式为
所述步骤四中计算非线性系统状态方程和观测方程的线性化误差的外包椭球的方法为：利用Fourier-Hermite级数多项式逼近操作获得高阶余项算子作为Lagrange余子，计算逼近误差边界，用椭球形状将状态方程的Fourier-Hermite级数多项式逼近线性化误差外包：
Figure BDA0001611417870000046
获得系统状态方程的外包椭球为
Figure BDA0001611417870000047
其中，
Figure BDA0001611417870000048
表示Fourier-Hermite级数多项式逼近的系统状态方程不确定性噪声方差矩阵，表示不确定性噪声方差矩阵的对角元素，
Figure BDA0001611417870000051
表示不确定性噪声方差矩阵的非对角元素；
利用椭球获得的观测方程的Fourier-Hermite级数多项式逼近的线性化误差外包：
Figure BDA0001611417870000052
获得系统状态方程的外包椭球为
Figure BDA0001611417870000053
其中，
Figure BDA0001611417870000054
表示Fourier-Hermite级数多项式逼近的系统观测方程不确定性噪声方差矩阵，
Figure BDA0001611417870000055
表示不确定性噪声方差矩阵的对角元素，
Figure BDA0001611417870000056
表示不确定性噪声方差矩阵的非对角元素。
所述步骤五中获取虚拟状态噪声误差椭球和虚拟观测噪声误差椭球的方法：
计算虚拟过程状态噪声误差椭球为
Figure BDA0001611417870000057
其中，Qk-1为第k-1步系统噪声方差矩阵，
Figure BDA0001611417870000059
表示第k-1步的系统状态噪声误差椭球方差矩阵直和：
Figure BDA00016114178700000510
其中，
Figure BDA00016114178700000511
表示第k-1步的外包椭球的方差矩阵，为过程噪声方差直和计算的过程噪声尺度因子，
Figure BDA00016114178700000513
计算虚拟观测噪声误差椭球
Figure BDA00016114178700000514
Figure BDA00016114178700000515
其中，
Figure BDA00016114178700000516
表示第k-1步的系统观测噪声误差椭球方差矩阵直和：
Figure BDA00016114178700000517
其中，
Figure BDA00016114178700000518
为观测噪声方差矩阵直和计算的观测噪声尺度因子，
Figure BDA00016114178700000519
Figure BDA00016114178700000520
表示第k步观测函数的线性化外包椭球方差矩阵。
所述步骤六中预测计算第k步系统状态向量的预测状态椭球边界的方法为：
线性化预测椭球
Figure BDA00016114178700000521
和虚拟过程噪声直和计算过程，利用第k-1步状态方程获得系统状态向量估计值
Figure BDA00016114178700000522
和Fourier-Hermite级数多项式逼近计算第k步的系统状态向量的预测值，那么根据Fourier-Hermite级数多项式的均值计算公式有：
其中，N(·)表示系统状态向量的概率分布密度函数，
Figure BDA0001611417870000062
是过程函数的Fourier-Hermite多项式的系数向量矩阵的第0行向量；
根据Fourier-Hermite级数多项式逼近计算原理，系统状态向量的方差表达为：
Figure BDA0001611417870000063
其中，βk-1为系统状态向量的方差尺度因子，满足βk-1∈(0，1)；
那么系统状态向量的方差矩阵可表达为：
Figure BDA0001611417870000064
从而获得第k步系统状态向量的预测状态椭球
Figure BDA0001611417870000065
所述步骤七中更新状态椭球边界的方法为：
将预测系统状态向量椭球
Figure BDA0001611417870000066
和观测向量序列集合{Sy}做直和交集计算，其中观测向量序列集合为
Figure BDA0001611417870000071
其中，x表示泛指的系统状态变量；
根据系统状态向量的第k步的预测值采用Fourier-Hermite级数多项式计算系统观测向量的观测更新计算过程，观测向量的预测为：
Figure BDA0001611417870000072
其中，系数向量
Figure BDA0001611417870000073
表示观测函数的Fourier-Hermite多项式的系数矩阵的第0行向量；
系统观测向量的预测方差矩阵为：
那么系统状态向量与观测向量的协方差矩阵可计算为：
其中，ρk为观测向量方差矩阵的调节尺度因子；
那么观测噪声生方差矩阵与观测向量误差方差矩阵的直和为：
Figure BDA0001611417870000082
ρk∈(0,1)，P'z,k,k-1观测噪声方差矩阵。
所述步骤八中第k步系统状态变量的估计计算和估计方差矩阵为：
Figure BDA0001611417870000084
其中，滤波增益矩阵Kk为：
Figure BDA0001611417870000085
Figure BDA0001611417870000086
表示第k步的系统状态向量估计误差包络矩阵计算的中间算子，且
Figure BDA0001611417870000087
P' xz,k,k-1表示系统状态变量与观测向量的协方差矩阵，δk表示算法健康度因子，其表达式为：
Figure BDA0001611417870000088
所述尺度因子
Figure BDA0001611417870000089
需要E(0,Qk-1)和
Figure BDA00016114178700000810
的直和计算，计算准则式为
Figure BDA00016114178700000811
其最优计算式为：
其中，tr(·)为迹；
尺度因子βk-1需要两个椭球
Figure BDA0001611417870000091
和
Figure BDA0001611417870000092
的直和计算，考虑观测向量更新条件下的方差矩阵计算式为：
Figure BDA0001611417870000093
从而，可以得到尺度因子参数βk-1的计算公式为
采用最小化性能指标算法健康度因子δk上界形式来计算尺度因子ρk：
Figure BDA0001611417870000095
尺度因子参数ρk的一种次优计算式为：
Figure BDA0001611417870000096
其中，pm是矩阵Pk,k-1的最大奇异值，cm是矩阵
Figure BDA0001611417870000097
的最大奇异值。
本发明的有益效果：利用Fourier-Hermite级数多项式获得的非线性系统模型方程函数的线性化逼近计算操作，可以以任意阶次的矩信息逼近非线性函数的高阶矩计算，从而获得一类高精度扩展椭球集员滤波方法，应用于GNSS/INS紧组合导航系统模型状态参数的迭代滤波计算过程，实现紧组合导航系统非线性模型状态参数最优滤波计算。本发明计算精度较高，降低了算法的计算复杂度，并且有效保证了扩展椭球集员滤波算法的计算稳定性。。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明GNSS/INS紧组合导航系统模型数据计算流程图。
图2是本发明的流程图。
图3是本发明的载体机动运行轨迹图。
图4是系统仿真位置数据曲线(Fourier-Hermite SMF)。
图5是系统仿真速度数据曲线(Fourier-Hermite SMF)。
图6是系统仿真姿态数据曲线(Fourier-Hermite SMF)。
图7是系统仿真位置数据曲线(EKF)。
图8是系统仿真速度数据曲线(EKF)。
图9是系统仿真姿态数据曲线(EKF)。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明利用Hermite多项式构建非线性GNSS/INS组合导航方程函数。先定义一维Hermite多项式为：
Figure BDA0001611417870000101
Hermite多项式是一组正交多项式，满足正交性性质，
Figure BDA0001611417870000102
并且Hermite多项式具有完备性，也就是在所有满足的函数构成的完备空间中，Hermite多项式序列组成了一组基，其内积定义为<f,g>＝E[f(x)g(x)]，其中，若函数g(x)是有界的，满足E[g(x)2]＜∞，并且其高阶微分也是有界的，也就是满足E[g(k)(x)2]＜∞。另外，若函数g(x)满足标准正态分布N(0,1)，则可利用Hermite多项式构造出函数g(x)的Fourier-Hermite级数扩展表达式为：
Figure BDA0001611417870000104
若函数g(x)满足任意正态分布N(μ,σ2)，那么函数g(x)可表达为：
Figure BDA0001611417870000105
Fourier-Hermite级数多项式也可以推广到高维情形，针对n维函数g(x)，其m阶Hermite多项式中阶次m可分为i1，i2，...，im∈{1,2,…,m}，阶次分为n组，
Figure BDA0001611417870000111
其中有些组内数据可能为空的，每一组的大小qs＝|Js|∈(0,1,…m)，满足
Figure BDA0001611417870000112
那么n维Hermite多项式可表达为各个变量元素的连乘积形式：
举例来说，若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，那么：
[H4(x)]3,2，2，1＝H1(x1)H2(x2)H1(x3)。
在多维情形中多维状态参数变量的方差矩阵为Σ，其Cholesky分解矩阵为L，那么Σ＝LLT，则期望均值μ和方差σ的变换式
Figure BDA0001611417870000114
变换为L-1(x-μ)，那么多维Fourier-Hermite级数扩展表达式为：
Figure BDA0001611417870000115
其中，系数表达式为：
Figure BDA0001611417870000116
对于充分微分函数，则可以获得：
其中系数表达式为：
Figure BDA0001611417870000118
根据Hermite多项式的正交性，系数向量满足Parseval关系式，
Figure BDA0001611417870000119
其中矩阵Γk可表达为：
那么利用非线性函数的Fourier-Hermite级数扩展表达式及其性质可以获得函数的期望均值及其方差矩阵的计算表达式：
E[g(x)]＝c0 (10)
Figure BDA0001611417870000121
其中，矩阵Γk∈Rn×n。随机变量与函数的协方差矩阵可表达为
Cov[x,g(x)]＝LCT＝ΣAT (12)
其中，矩阵
Figure BDA0001611417870000122
矩阵
并且若函数的概率积分具有封闭解形式，如：
Figure BDA0001611417870000124
那么函数的高阶微分可表示为
Figure BDA0001611417870000125
在此基础上可以构建一类Fourier-Hermite级数扩展椭球集员滤波计算方法。
如图2所示，一种基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，根据建立的GNSS/INS组合导航系统非线性的模型方程，计算第k步的系统状态参数分量的不确定区间；基于Fourier-Hermite级数性质对GNSS/INS组合导航系统的状态方程和观测方程实施Fourier-Hermite级数逼近计算操作，确定其Lagrange余子的取值区间；进而计算Fourier-Hermite级数逼近计算的误差边界，获取非线性系统状态方程和观测方程的Fourier-Hermite级数逼近计算误差的外包椭球；开展虚拟过程噪声误差椭球和虚拟观测噪声误差椭球计算；利用线性椭球集员滤波算法的预测步骤计算预测状态变量参数的椭球边界；利用线性椭球集员滤波算法的更新步骤计算系统状态参数的椭球边界；再利用线性椭球集员滤波算法状态变量参数的状态估计计算第k+1步的状态变量参数的估计值和估计方差矩阵，确定第k+1步的椭球形状；经由多步迭代过程完成GNSS/INS组合导航系统模型的状态变量参数的计算任务。
步骤一：构建GNSS/INS组合导航系统非线性模型方程，包括状态方程和观测方程。
设计一类GNSS/INS组合导航系统的状态方程和观测方程，实现状态参数估计。离散状态空间模型为
其中，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，该集合由系统状态的先验知识确定。对于给定的观测向量序列
Figure BDA0001611417870000131
第k步的椭球集员滤波估计计算的状态可行集合为{Xk}，由所有可能的状态点组成的，这些状态点与所有可获取的信息包括系统模型方程、噪声假设和初始状态集合相一致。
定义椭球集合为E(a,P)＝{x∈Rn|(x-a)TP-1(x-a)≤1}，其中，a表示椭球集合的中心，P为满足正定性的椭球形状包络矩阵。定义系统初始状态向量椭球集合为
Figure BDA0001611417870000132
Figure BDA0001611417870000133
表示系统状态变量的初始值，P0表示系统椭球形状包络矩阵的初始矩阵；假设经由k-1步的迭代滤波计算，第k-1步获得的系统状态向量的可行椭球集合为
Figure BDA0001611417870000134
Figure BDA0001611417870000135
表示k-1步的系统状态变量估计值，Pk-1为k-1步的椭球形状包络矩阵，则第k步的非线性椭球集员滤波算法的迭代计算过程由步骤二至步骤八组成。
步骤二：根据第k-1步迭代计算获得系统状态变量的估计均值和方差矩阵，确定第k-1步GNSS/INS组合导航系统的状态参数向量的状态分量不确定区间。
利用第k-1步的系统状态向量的估计均值和方差矩阵确定当前第k步的系统状态向量各分量的不确定区间，第k-1步系统状态向量的各分量不确定区间为：
Figure BDA0001611417870000136
其中，
Figure BDA0001611417870000137
表示第k-1步椭球包络矩阵Pk-1的(i,i)元素，
Figure BDA0001611417870000138
表示第k-1步的状态变量估计值；l是一个正实数，它设置的意义在于保证第k-1步的系统状态向量估计值99.7％的概率落在设定的状态向量取值区间内，其一般取值为l≥3。
步骤三：基于Fourier-Hermite级数多项式逼近法对GNSS/INS组合导航系统的非线性状态方程和观测方程实施Fourier-Hermite级数多项式逼近计算操作，确定Lagrange余子取值区间。
以第k-1步系统状态向量估计值实施Fourier-Hermite级数多项式扩展，取其多项式误差或者称之为高阶余项，Lagrange余子项作为非线性系统状态向量的不确定区间。
根据GNSS/INS组合导航系统非线性的状态方程xk＝f(xk-1)+wk-1，基于Fourier-Hermite级数多项式的高阶余项极小化性质，利用Fourier-Hermite级数多项式获得线性化逼近生成的Lagrange余子的极小化区间，以第k-1步系统状态向量的估计点
Figure BDA0001611417870000141
做Fourier-Hermite级数多项式逼近获得系统状态方程的n阶Fourier-Hermite级数多项式表达式为：
Figure BDA0001611417870000142
其中，Rn+1(xk-1)表示Fourier-Hermite级数的高阶余项，根据fourier-Hermite级数的正交性质，那么其高阶余项表达式为：
Figure BDA0001611417870000143
同样地，利用fourier-Hermite级数多项式对系统非线性的观测方程zk＝h(xk)+vk实施多项式扩展计算，获得Fourier-Hermite级数多项式逼近计算生成Lagrange余子，以第k步状态预测点
Figure BDA0001611417870000144
做Fourier-Hermite级数多项式逼近计算，获得非线性观测方程的Fourier-Hermite级数多项式逼近计算表达式为：
Figure BDA0001611417870000151
其中，Pk,k-1表示第k步系统状态变量的预测方差矩阵，
Figure BDA0001611417870000152
表示利用预测状态变量表达的观测函数，是非线性观测方程的Fourier-Hermite级数多项式的高阶余项，其表达式为
Figure BDA0001611417870000154
步骤四：计算Fourier-Hermite级数多项式逼近的高阶余项的误差边界，利用椭球将线性化误差外包得到非线性系统状态方程和观测方程的线性化误差的外包椭球。
利用Fourier-Hermite级数多项式逼近操作获得高阶余项算子作为Lagrange余子，计算逼近误差边界，用椭球形状将状态方程的Fourier-Hermite级数多项式逼近线性化误差外包：
获得系统状态方程的外包椭球为
Figure BDA0001611417870000156
其中，
Figure BDA0001611417870000157
表示Fourier-Hermite级数多项式逼近的系统状态方程不确定性噪声方差矩阵，
Figure BDA0001611417870000158
表示不确定性噪声方差矩阵的对角元素，表示不确定性噪声方差矩阵的非对角元素。同样地，利用椭球获得的观测方程的Fourier-Hermite级数多项式逼近的线性化误差外包：
Figure BDA00016114178700001510
获得系统状态方程的外包椭球为
Figure BDA00016114178700001511
其中，
Figure BDA00016114178700001512
表示Fourier-Hermite级数多项式逼近的系统观测方程不确定性噪声方差矩阵，表示不确定性噪声方差矩阵的对角元素，
Figure BDA0001611417870000161
表示不确定性噪声方差矩阵的非对角元素。
步骤五：计算虚拟过程的误差椭球，包括虚拟状态噪声误差椭球和虚拟观测噪声误差椭球。
涉及到Fourier-Hermite级数多项式逼近造成的不确定性误差椭球和系统状态噪声相加的两个椭球直和计算，通过逼近不确定性误差和系统状态噪声的直和计算获得虚拟噪声误差椭球。
计算虚拟过程状态噪声误差椭球为
Figure BDA0001611417870000162
Figure BDA0001611417870000163
其中，
Figure BDA0001611417870000164
表示第k-1步的系统状态噪声误差椭球方差矩阵直和，
Figure BDA0001611417870000165
是由椭球形状的系统多项式逼近计算的不确定性误差和状态噪声误差相加获得，涉及到两个椭球直和计算：
Figure BDA0001611417870000166
其中，表示第k-1步的外包椭球的方差矩阵，
Figure BDA0001611417870000168
为过程噪声方差直和计算的过程噪声尺度因子，
Figure BDA0001611417870000169
同样地对非线性观测方程zk＝h(xk)+vk作上述计算步骤，计算虚拟观测噪声误差椭球
Figure BDA00016114178700001610
Figure BDA00016114178700001611
虚拟观测噪声
Figure BDA00016114178700001612
是由椭球的线性化误差和观测噪声相加获得，其中也涉及到两个椭球的直和计算，
其中，
Figure BDA00016114178700001614
为观测噪声方差矩阵直和计算的观测噪声尺度因子，
Figure BDA00016114178700001615
从而获得系统观测噪声的虚拟噪声椭球
Figure BDA00016114178700001616
其中，
Figure BDA00016114178700001617
表示第k步观测函数的线性化外包椭球方差矩阵，
Figure BDA00016114178700001618
表示获得的直和虚拟噪声方差矩阵。
步骤六：利用线性化椭球集员滤波算法的预测步骤计算第k步系统状态向量的预测状态椭球边界。
涉及到线性化预测椭球和虚拟过程噪声椭球的直和计算过程，利用已经获得的第k-1步的系统状态向量估计值，经由Fourier-Hermite级数多项式逼近展开计算第k步的状态预测值，获得状态向量线性化预测值及其预测外包椭球，开展线性化预测椭球和虚拟过程噪声椭球以及系统状态向量预测方差矩阵的直和计算，从而获得系统状态变量的预测椭球边界。
线性化预测椭球
Figure BDA0001611417870000171
和虚拟过程噪声直和计算过程，利用第k-1步状态方程获得系统状态向量估计值
Figure BDA0001611417870000172
和Fourier-Hermite级数多项式逼近计算第k步的系统状态向量的预测值，那么根据Fourier-Hermite级数多项式的均值计算公式有：
Figure BDA0001611417870000173
其中，N(·)表示系统状态向量的概率分布密度函数，系数向量
Figure BDA0001611417870000174
的意义如式(10),(13)和(14)所示，是过程函数的Fourier-Hermite多项式的系数向量矩阵的第0行向量。
根据Fourier-Hermite级数多项式逼近计算原理，系统状态向量的方差表达为：
其中，βk-1为系统状态向量的方差尺度因子，满足βk-1∈(0，1)，那么系统状态向量的方差矩阵可表达为：
Figure BDA0001611417870000181
从而经由上述的式(25)、(26)和(27)的计算过程，获得第k步系统状态向量的预测状态椭球它不同于线性化预测椭球
Figure BDA0001611417870000183
步骤七：利用线性化椭球集员滤波算法的更新步骤实施系统状态向量的椭球边界更新计算。
涉及到预测状态向量椭球和观测向量集合的交集计算。利用系统观测向量序列开展预测椭球和观测向量带的交集计算。即计算观测噪声方差矩阵与观测向量误差方差矩阵的直和，得到滤波增益矩阵。
将预测系统状态向量椭球
Figure BDA0001611417870000184
和观测向量序列集合{Sy}做直和交集计算，其中观测向量序列集合表达为
其中，x表示泛指的系统状态变量。
根据系统状态向量的第k步的预测值，考虑采用Fourier-Hermite级数多项式计算系统观测向量的观测更新计算过程，观测向量的预测计算为：
Figure BDA0001611417870000186
其中，系数向量
Figure BDA0001611417870000187
的意义如式(10),(13)和(14)所示，表示观测函数的Fourier-Hermite多项式的系数矩阵的第0行向量。相应的系统观测向量的预测方差矩阵可由式(26)表达为
那么系统状态向量与观测向量的协方差矩阵可计算为
Figure BDA0001611417870000192
其中，P'xz,k,k-1表示系统状态变量与观测向量的协方差矩阵，ρk为观测向量方差矩阵的调节尺度因子，那么观测噪声生方差矩阵与观测向量误差方差矩阵的直和计算为
Figure BDA0001611417870000193
其中，Pz',k,k-1观测噪声方差矩阵。那么滤波增益矩阵为：
步骤八：利用线性化椭球集员算法的状态估计完成系统状态向量的第k步的估计均值计算和估计方差矩阵计算，从而完成GNSS/INS组合导航系统状态向量的估计计算任务。
Figure BDA0001611417870000201
Figure BDA0001611417870000202
Figure BDA0001611417870000203
其中，
Figure BDA0001611417870000204
表示第k步的系统状态向量估计误差包络矩阵计算的中间算子，δk表示算法健康度因子，其表达式为：
Figure BDA0001611417870000205
在本发明中引入了三个尺度因子参数
Figure BDA0001611417870000206
βk-1和ρk，其数值确定方法如下：
尺度因子参数
Figure BDA0001611417870000207
和βk-1涉及到两个椭球直和运算的外包椭球最优化问题，这里选取外包椭球的最小化计算方法，该方法求解形式简单，相比较于最小化外包椭球体积的优化准则，该方法性能鲁棒性更强。即有
Figure BDA0001611417870000208
从而可以采用式子
Figure BDA0001611417870000209
获得最优的尺度因子参数
Figure BDA00016114178700002010
和βk-1，P1和P2表示泛指的任意两个方差矩阵。
尺度因子参数
Figure BDA00016114178700002011
需要E(0,Qk-1)和
Figure BDA00016114178700002012
的直和计算，那么其计算准则式为
Figure BDA00016114178700002013
其最优计算式为
Figure BDA00016114178700002014
对于尺度因子参数βk-1，需要两个椭球和的直和计算，考虑观测向量更新条件下的方差矩阵计算式为：
Figure BDA00016114178700002017
从而，可以得到尺度因子参数βk-1的计算公式为
Figure BDA00016114178700002018
在迭代计算过程中，观测集合Sy形式一般都比较复杂，从而导致系统状态向量方差矩阵Pk-1的计算复杂性，无论采用最小化椭球体积法还是最小化椭球迹准则，都使尺度因子参数ρk的优化计算很困难，甚至无法获得解析解，若采用数值计算方法计算复杂度很高。在本发明中采用最小化性能指标δk上界形式来计算
Figure BDA0001611417870000211
这样可以获得尺度因子参数ρk的一种次优计算式
Figure BDA0001611417870000212
其中，pm是矩阵Pk,k-1的最大奇异值，cm是矩阵的最大奇异值。
GNSS/INS组合导航系统具有多种组合模式，如松组合、紧组合和深组合模式等。其中，紧组合模式近年来在自动驾驶领域应用越来越多，特别是紧组合模式中GNSS信号在组合导航模型滤波计算过程中可以直接融合到滤波更新操作中去，能够有效改善滤波计算效能。因此，本发明采用GNSS/INS紧组合导航模型验证提出的基于Fourier-Hermite级数多项式扩展的椭球集员滤波方法的计算效能，其中GNSS/INS紧组合导航模型结构示意图如图1所示。
GNSS/INS组合导航系统模型由惯性导航方程和GNSS导航方程组成，其中定义惯性导航的坐标系，为了不失一般性，采用当地水平坐标系(北东地，NED)作为导航坐标系，从而INS速度微分方程可表示为
Figure BDA0001611417870000214
其中，ve表示[3×1]载体运动速度向量；
Figure BDA0001611417870000215
表示从载体系b到导航系e的方向余弦矩阵；
Figure BDA0001611417870000216
表示[3×1]地球旋转向量角速度；fb表示[3×1]载体坐标系b加速度计获得的[3×1]比力向量；
Figure BDA0001611417870000217
表示当地重力向量。
式(44)经一阶离散化和二次欧拉积分可以获得载体速度和位置：
Figure BDA0001611417870000218
其中，Δt表示积分步长，pe表示[3×1]载体位置向量。另外，载体旋转运动利用欧拉角表示，[3×1]载体欧拉角向量分别表示纵摇角、横摇角和航向角，定义欧拉角向量为
Figure BDA0001611417870000219
其微分方程为
Figure BDA0001611417870000221
对式(46)实施一阶离散化和欧拉积分计算，可以获得载体旋转运动的欧拉角向量为
Figure BDA0001611417870000222
式(44)中的方向余弦矩阵
Figure BDA0001611417870000223
利用欧拉角向量表示为
Figure BDA0001611417870000224
方向余弦矩阵微分方程为
Figure BDA0001611417870000225
其中，
Figure BDA0001611417870000226
表示在载体坐标系中惯性组件获得的[3×1]载体旋转角速度向量，[·×]表示三维向量构造的斜对称矩阵；
Figure BDA0001611417870000227
表示在导航坐标系中的[3×1]地球旋转角速度向量。
考虑伪距的量测方程，INS所在位置处获得的伪距可表达为
Figure BDA0001611417870000228
其中，(xsi,ysi,zsi)T表示用户所在真实位置，经由Taylor级数扩展并舍弃高阶项，可以获得
Figure BDA0001611417870000229
其中，
Figure BDA00016114178700002211
GNSS测得的伪距表达式为
ρGi＝ri-δtu-vρi (52)
其中，ri表示卫星和地面运动载体的真实距离，vρi为量测误差。那么，伪距测量方程表示为
Figure BDA0001611417870000234
其中，δtu表示等效时钟误差对应的距离；变量(δXi,δYi,δZi)可以经由坐标变换转化为(δLi,δλi,δhi)参数。同样地，可以获得伪距率方程为
Figure BDA0001611417870000231
其中，δtru表示等效时钟频率误差对应的距离率。并且方程式(53)、(54)中的i＝1,2,3,4，表示取四颗卫星；式(54)中的(δXi,δYi,δZi)可以用(δvE,δvN,δvD)速度表示。
GNSS/INS组合导航系统仿真实例中系统状态向量由位置量δPe、速度量δve、姿态角量δΦ、等效钟差误差δtu、等效钟差变化率误差δtru以及IMU组件中的加速度计误差δfb和陀螺仪误差
Figure BDA0001611417870000232
组成系统状态向量
Figure BDA0001611417870000233
设置GNSS/INS组合导航系统模型的初始参数，构建系统算法仿真平台。观测点坐标北纬32.83°，东经68.793°，高度700m，从卫星接收机获取星历数据和原始伪距以及多普勒频移，从中解算出卫星位置、速度并修正地球自转影响，经由多普勒频移解算出伪距率，其中包含着电离层延时以及卫星钟差校正，在相同观测点INS仿真条件设置为初始平台误差角为5°，初始位置误差为10m；陀螺仪随机常值漂移0.02°/h，噪声误差0.02°/h(1σ)；加速度计随机常值零偏1×10-4g，噪声误差1×10-4g(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算法，并且位置量估计误差获得明显改善且曲线平滑稳定，并且速度误差量收敛很快，导航效果稳定。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，其特征在于，其步骤为：
步骤一：构建GNSS/INS组合导航系统非线性模型方程，包括状态方程和观测方程；
步骤二：根据第k-1步迭代计算获得系统状态变量的估计均值和方差矩阵，确定第k-1步GNSS/INS组合导航系统的状态参数向量的状态分量不确定区间；其中，k＝1,2，···；
步骤三：基于Fourier-Hermite级数多项式逼近法对GNSS/INS组合导航系统非线性的状态方程和观测方程实施Fourier-Hermite级数多项式逼近计算操作，确定Lagrange余子取值区间；
步骤四：计算Fourier-Hermite级数多项式逼近的高阶余项的误差边界，利用椭球将线性化误差外包得到非线性的状态方程和观测方程的线性化误差的外包椭球；
步骤五：计算虚拟过程的误差椭球，包括虚拟状态噪声误差椭球和虚拟观测噪声误差椭球；
步骤六：利用线性化椭球集员滤波算法的预测计算第k步系统的状态向量的预测状态椭球边界；
步骤七：利用线性化椭球集员滤波算法的更新步骤实施系统的状态向量的椭球边界更新计算；
步骤八：利用线性化椭球集员算法的状态估计完成系统的状态向量的第k步的估计均值计算和估计方差矩阵计算，从而完成GNSS/INS组合导航系统状态向量的估计计算任务；
所述步骤三中Lagrange余子的取值区间的确定方法为：以第k-1步系统状态向量估计值实施Fourier-Hermite级数多项式扩展，取高阶余项作为非线性系统状态向量的Lagrange余子的不确定区间；
根据GNSS/INS组合导航系统非线性的状态方程xk＝f(xk-1)+wk-1，其中，xk,xk-1∈Rn分别表示系统第k、k-1步的状态向量，Rn表示n维实数空间，下标k表示迭代计算的第k步，f(·)表示系统状态函数、是已知的非线性可导函数，wk-1∈Rn表示系统的过程噪声，且wk∈(0,Qk)，Qk为第k步系统噪声方差矩阵，且满足|wk,i|＜1，i＝1,2,…,n；第k-1步获得的系统状态向量的可行椭球集合为
Figure FDA0002313764730000011
Figure FDA0002313764730000012
表示k-1步的系统状态变量估计值，Pk-1为k-1步的椭球形状包络矩阵，利用Fourier-Hermite级数多项式获得线性化逼近生成的Lagrange余子的极小化区间，以第k-1步系统状态向量的估计点
Figure FDA0002313764730000013
做Fourier-Hermite级数多项式逼近获得系统状态方程的n阶Fourier-Hermite级数多项式表达式为：
Figure FDA0002313764730000021
其中，H(·)为Hermite多项式展开函数，P为满足正定性的椭球形状包络矩阵，
Figure FDA0002313764730000022
表示Fourier-Hermite级数的高阶余项，根据Fourier-Hermite级数的正交性质，其高阶余项表达式为：
Figure FDA0002313764730000023
利用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级数多项式逼近计算表达式为：
Figure FDA0002313764730000031
其中，Pk,k-1表示第k步系统状态变量的预测方差矩阵，
Figure FDA0002313764730000032
表示利用预测状态变量表达的观测函数，
Figure FDA0002313764730000033
是非线性观测方程的Fourier-Hermite级数多项式的高阶余项，其表达式为
Figure FDA0002313764730000034
2.根据权利要求1所述的基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，其特征在于，利用第k-1步的系统状态向量的估计均值和方差矩阵确定当前第k步的系统状态向量各分量的不确定区间，第k-1步系统状态向量的各分量
Figure FDA0002313764730000035
不确定区间为：
Figure FDA0002313764730000036
其中，
Figure FDA0002313764730000037
表示第k-1步椭球包络矩阵Pk-1的(i,i)元素，
Figure FDA0002313764730000038
表示第k-1步的状态变量估计值，l是一个正实数。
3.根据权利要求1所述的基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，其特征在于，所述步骤四中计算非线性系统状态方程和观测方程的线性化误差的外包椭球的方法为：利用Fourier-Hermite级数多项式逼近操作获得高阶余项算子作为Lagrange余子，计算逼近误差边界，用椭球形状将状态方程的Fourier-Hermite级数多项式逼近线性化误差外包：
Figure FDA0002313764730000039
获得系统状态方程的外包椭球为
Figure FDA00023137647300000310
其中，
Figure FDA00023137647300000311
表示Fourier-Hermite级数多项式逼近的系统状态方程不确定性噪声方差矩阵，
Figure FDA00023137647300000312
表示不确定性噪声方差矩阵的对角元素，
Figure FDA00023137647300000313
表示不确定性噪声方差矩阵的非对角元素；
利用椭球获得的观测方程的Fourier-Hermite级数多项式逼近的线性化误差外包：
Figure FDA0002313764730000041
获得系统状态方程的外包椭球为
Figure FDA0002313764730000042
其中，
Figure FDA0002313764730000043
表示Fourier-Hermite级数多项式逼近的系统观测方程不确定性噪声方差矩阵，表示不确定性噪声方差矩阵的对角元素，表示不确定性噪声方差矩阵的非对角元素。
4.根据权利要求1所述的基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，其特征在于，所述步骤五中获取虚拟状态噪声误差椭球和虚拟观测噪声误差椭球的方法：
计算虚拟过程状态噪声误差椭球为
Figure FDA0002313764730000047
其中，Qk-1为第k-1步系统噪声方差矩阵，
Figure FDA0002313764730000048
表示第k-1步的系统状态噪声误差椭球方差矩阵直和：
Figure FDA0002313764730000049
其中，表示第k-1步的外包椭球的方差矩阵，
Figure FDA00023137647300000411
为过程噪声方差直和计算的过程噪声尺度因子，
Figure FDA00023137647300000412
计算虚拟观测噪声误差椭球
Figure FDA00023137647300000413
Figure FDA00023137647300000414
其中，
Figure FDA00023137647300000415
表示第k-1步的系统观测噪声误差椭球方差矩阵直和：
Figure FDA00023137647300000416
其中，
Figure FDA00023137647300000417
为观测噪声方差矩阵直和计算的观测噪声尺度因子，
Figure FDA00023137647300000418
Figure FDA00023137647300000419
表示第k步观测函数的线性化外包椭球方差矩阵。
5.根据权利要求1所述的基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，其特征在于，所述步骤六中预测计算第k步系统状态向量的预测状态椭球边界的方法为：
线性化预测椭球
Figure FDA00023137647300000420
和虚拟过程噪声直和计算过程，利用第k-1步状态方程获得系统状态向量估计值
Figure FDA0002313764730000051
和Fourier-Hermite级数多项式逼近计算第k步的系统状态向量的预测值，那么根据Fourier-Hermite级数多项式的均值计算公式有：
Figure FDA0002313764730000052
其中，N(·)表示系统状态向量的概率分布密度函数，
Figure FDA0002313764730000053
是过程函数的Fourier-Hermite多项式的系数向量矩阵的第0行向量；
根据Fourier-Hermite级数多项式逼近计算原理，系统状态向量的方差表达为：
Figure FDA0002313764730000054
其中，βk-1为系统状态向量的方差尺度因子，满足βk-1∈(0，1)；
那么系统状态向量的方差矩阵可表达为：
从而获得第k步系统状态向量的预测状态椭球
Figure FDA0002313764730000056
6.根据权利要求5所述的基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，其特征在于，所述步骤七中更新状态椭球边界的方法为：
将预测系统状态向量椭球
Figure FDA0002313764730000061
和观测向量序列集合{Sy}做直和交集计算，其中观测向量序列集合为
其中，x表示泛指的系统状态变量；
根据系统状态向量的第k步的预测值采用Fourier-Hermite级数多项式计算系统观测向量的观测更新计算过程，观测向量的预测为：
Figure FDA0002313764730000063
其中，系数向量
Figure FDA0002313764730000064
表示观测函数的Fourier-Hermite多项式的系数矩阵的第0行向量；
系统观测向量的预测方差矩阵为：
Figure FDA0002313764730000065
那么系统状态向量与观测向量的协方差矩阵可计算为：
Figure FDA0002313764730000071
其中，ρk为观测向量方差矩阵的调节尺度因子；
那么观测噪声生方差矩阵与观测向量误差方差矩阵的直和为：
P′z,k,k-1观测噪声方差矩阵。
7.根据权利要求6所述的基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，其特征在于，所述步骤八中第k步系统状态变量的估计计算和估计方差矩阵为：
Figure FDA0002313764730000073
Figure FDA0002313764730000074
其中，滤波增益矩阵Kk为：
Figure FDA0002313764730000075
Figure FDA0002313764730000076
表示第k步的系统状态向量估计误差包络矩阵计算的中间算子，且P′xz,k,k-1表示系统状态变量与观测向量的协方差矩阵，δk表示算法健康度因子，其表达式为：
8.根据权利要求4所述的基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，其特征在于，所述尺度因子
Figure FDA0002313764730000079
需要E(0,Qk-1)和
Figure FDA00023137647300000710
的直和计算，计算准则式为
Figure FDA00023137647300000711
其最优计算式为：
Figure FDA0002313764730000081
其中，tr(·)为迹；
尺度因子βk-1需要两个椭球
Figure FDA0002313764730000082
和
Figure FDA0002313764730000083
的直和计算，考虑观测向量更新条件下的方差矩阵计算式为：
Figure FDA0002313764730000084
从而，可以得到尺度因子参数βk-1的计算公式为
Figure FDA0002313764730000085
9.根据权利要求6-8中任意一项所述的基于Fourier-Hermite正交多项式扩展椭球集员滤波方法，其特征在于，采用最小化性能指标算法健康度因子δk上界形式来计算尺度因子ρk：
Figure FDA0002313764730000086
尺度因子参数ρk的一种次优计算式为：
Figure FDA0002313764730000087
其中，pm是矩阵Pk,k-1的最大奇异值，cm是矩阵
Figure FDA0002313764730000088
的最大奇异值。
CN201810266008.0A 2018-03-28 2018-03-28 基于Fourier-Hermite正交多项式扩展椭球集员滤波方法 Active CN108508463B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810266008.0A CN108508463B (zh) 2018-03-28 2018-03-28 基于Fourier-Hermite正交多项式扩展椭球集员滤波方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810266008.0A CN108508463B (zh) 2018-03-28 2018-03-28 基于Fourier-Hermite正交多项式扩展椭球集员滤波方法

Publications (2)

Publication Number Publication Date
CN108508463A CN108508463A (zh) 2018-09-07
CN108508463B true CN108508463B (zh) 2020-02-18

Family

ID=63379137

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810266008.0A Active CN108508463B (zh) 2018-03-28 2018-03-28 基于Fourier-Hermite正交多项式扩展椭球集员滤波方法

Country Status (1)

Country Link
CN (1) CN108508463B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110457863B (zh) * 2019-08-23 2021-02-19 江南大学 基于椭球收缩滤波的风力发电机桨距子系统参数估计方法
CN111983926B (zh) * 2020-08-31 2022-04-12 郑州轻工业大学 一种最大协熵扩展椭球集员滤波方法
CN112683265B (zh) * 2021-01-20 2023-03-24 中国人民解放军火箭军工程大学 一种基于快速iss集员滤波的mimu/gps组合导航方法
CN113110548B (zh) * 2021-04-21 2023-05-12 北京控制工程研究所 一种航天器椭球集合演化的设计方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105222780A (zh) * 2015-09-07 2016-01-06 郑州轻工业学院 一种基于Stirling插值多项式逼近的椭球集员滤波方法
CN106767780A (zh) * 2016-11-28 2017-05-31 郑州轻工业学院 基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105222780A (zh) * 2015-09-07 2016-01-06 郑州轻工业学院 一种基于Stirling插值多项式逼近的椭球集员滤波方法
CN106767780A (zh) * 2016-11-28 2017-05-31 郑州轻工业学院 基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Fourier–Hermite Series for Stochastic Stability Analysis of Non-Linear Kalman Filters;Toni Karvonen et al.;《2016 19th International Conference on Information Fusion》;20160708;第1-8页 *
非线性椭球集员滤波及其在故障诊断中的应用;柴伟 等;《航空学报》;20070715;第28卷(第4期);第948-952页 *

Also Published As

Publication number Publication date
CN108508463A (zh) 2018-09-07

Similar Documents

Publication Publication Date Title
CN108508463B (zh) 基于Fourier-Hermite正交多项式扩展椭球集员滤波方法
CN104655131B (zh) 基于istssrckf的惯性导航初始对准方法
CN107110650B (zh) 在能观测性方面受约束的导航状态的估计方法
CN106990426B (zh) 一种导航方法和导航装置
CN109931955B (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN106153073B (zh) 一种全姿态捷联惯导系统的非线性初始对准方法
CN112146655B (zh) 一种BeiDou/SINS紧组合导航系统弹性模型设计方法
CN107063245B (zh) 一种基于5阶ssrckf的sins/dvl组合导航滤波方法
CN105222780A (zh) 一种基于Stirling插值多项式逼近的椭球集员滤波方法
CN107707220A (zh) 一种应用在gnss/ins中的改进型ckf方法
CN110567455A (zh) 一种求积更新容积卡尔曼滤波的紧组合导航方法
Wu iNavFIter: Next-generation inertial navigation computation based on functional iteration
Huang et al. A novel positioning module and fusion algorithm for unmanned aerial vehicle monitoring
CN115932723A (zh) 定位方法、装置、计算机设备、存储介质和程序产品
CN113587926A (zh) 一种航天器空间自主交会对接相对导航方法
CN112013849A (zh) 一种水面船自主定位方法及系统
CN112683265B (zh) 一种基于快速iss集员滤波的mimu/gps组合导航方法
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN110940336B (zh) 捷联惯导仿真定位解算方法、装置及终端设备
CN111024071A (zh) Gnss辅助的加速度计和陀螺仪常值漂移估算的导航方法及系统
CN110610513A (zh) 自主移动机器人视觉slam的不变性中心差分滤波器方法
CN112304309B (zh) 一种基于心动阵列的高超飞行器组合导航信息解算方法
CN113049005A (zh) Gnss位置法辅助dvl误差标定方法及系统
CN111473786A (zh) 一种基于局部反馈的两层分布式多传感器组合导航滤波方法
Zhang et al. Underwater mobile gravity measurement data processing using continuous-discrete Kalman filter

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant