CN106597507B - Gnss/sins紧组合滤波平滑的高精度快速算法 - Google Patents
Gnss/sins紧组合滤波平滑的高精度快速算法 Download PDFInfo
- Publication number
- CN106597507B CN106597507B CN201611065215.7A CN201611065215A CN106597507B CN 106597507 B CN106597507 B CN 106597507B CN 201611065215 A CN201611065215 A CN 201611065215A CN 106597507 B CN106597507 B CN 106597507B
- Authority
- CN
- China
- Prior art keywords
- smoothing
- sins
- gnss
- state
- rtss
- 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
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 141
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 117
- 230000010354 integration Effects 0.000 title abstract description 5
- 238000009499 grossing Methods 0.000 claims abstract description 220
- 238000000034 method Methods 0.000 claims abstract description 57
- 238000004364 calculation method Methods 0.000 claims abstract description 39
- 230000004927 fusion Effects 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 108
- 230000008569 process Effects 0.000 claims description 28
- 239000013598 vector Substances 0.000 claims description 27
- 230000007704 transition Effects 0.000 claims description 24
- 238000012545 processing Methods 0.000 claims description 13
- 239000011541 reaction mixture Substances 0.000 claims description 11
- 239000005436 troposphere Substances 0.000 claims description 10
- 230000002457 bidirectional effect Effects 0.000 claims description 9
- 238000012937 correction Methods 0.000 claims description 9
- 238000011156 evaluation Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 5
- 238000012935 Averaging Methods 0.000 claims description 4
- 238000013461 design Methods 0.000 claims description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 238000009825 accumulation Methods 0.000 claims description 3
- 238000010924 continuous production Methods 0.000 claims description 3
- XEBWQGVWTUSTLN-UHFFFAOYSA-M phenylmercury acetate Chemical compound CC(=O)O[Hg]C1=CC=CC=C1 XEBWQGVWTUSTLN-UHFFFAOYSA-M 0.000 claims description 3
- 230000009467 reduction Effects 0.000 claims description 3
- 238000005457 optimization Methods 0.000 claims description 2
- 238000012546 transfer Methods 0.000 claims description 2
- 230000007306 turnover Effects 0.000 abstract 1
- 238000005070 sampling Methods 0.000 description 12
- 230000014509 gene expression Effects 0.000 description 6
- 238000005259 measurement Methods 0.000 description 6
- 238000012805 post-processing Methods 0.000 description 5
- 230000008901 benefit Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 230000006872 improvement Effects 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 238000005295 random walk Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
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/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Complex Calculations (AREA)
Abstract
本发明涉及一种GNSS/SINS紧组合滤波平滑的高精度快速算法。采用状态方程一步预测的近似方法优化了Kalman滤波的计算效率；通过剔除GNSS状态参数和分解SINS状态参数，加快了RTSS和FBC平滑算法的计算效率；将RTSS嵌入FBC中，融合得到新的平滑算法，提高了平滑算法的解算精度；采用低频观测更新率保存滤波结果并进行平滑处理，减小内存消耗，再以SINS机械编排高频内插的方法，从低频的平滑结果中恢复出任意时刻的紧组合解。本发明可以使GNSS/SINS紧组合快速生成高精度的解算结果。
Description
技术领域
本发明属于GNSS/SINS组合导航领域，是一种GNSS/SINS紧组合滤波平滑的高精度快速算法。
背景技术
以全球导航卫星系统(GNSS,Global Navigation Satellite System)辅助捷联惯导(SINS，Strapdown Inertial Navigation System)的模式已成为组合导航的标准，它结合了GNSS长时间内的高精度定位与SINS的运动学信息，融合互补以提供连续、高带宽、长时和短时精度均较高的、完整的导航参数。在军用和民用上，可为全球性的航空、航海活动提供高可靠的导航服务。在基础测绘测量作业中，组合导航系统是地理信息数据大范围、高效、快速获取的关键，它维持着整个数据采集过程中的坐标系统，实现各类数据在现实世界中的正确表达，如航空摄影测量，车载移动测量系统等。另外，矢量航空重力测量技术的突破性进展也得益于组合导航系统以及高灵敏度的重力传感器的成功应用。
GNSS/SINS组合导航采用Kalman滤波器估计，为了获取高精度的导航解，一般采用带有平滑算法的后处理模式。其中，后处理的平滑算法主要分为两类：1.RTSS(Rauch，Tungand Striebel Smoother)平滑算法；2.前向(Forward)—后向(Backward)滤波组合(Combination)平滑算法，简称称FBC平滑算法。在GNSS/SINS组合导航后处理中，RTSS和FBC平滑算法是一种提高精度的有效方法，RTSS算法向后平滑可以解决GNSS观测中断时的导航接续问题，并大幅度提高速度和姿态的解算精度，而FBC算法可以有效提高紧组合滤波的位置解算精度。加拿大卡尔加里大学开发的高精度组合导航软件KINGSDAP、Novatel公司商业软件IE(Inertial Explorer)和Applanix公司的商业软件POSPac，这些著名的组合导航解算软件均包含了平滑算法模块。
目前GNSS/SINS紧组合后处理平滑算法主要存在以下问题：
1)在组合导航中，SINS数据采样率远高于GNSS数据采样率，以SINS采样率进行标准Kalman滤波的状态预报更新耗时严重；
2)标准的紧组合RTSS平滑算法需要对所有状态进行平滑处理，而其中GNSS模糊度参数过多且随卫星升降而变化频繁，极大的增加了算法操作的复杂度；
3)紧组合中，单独的RTSS平滑对速度和姿态的精度提高十分明显，而单独的FBC平滑对位置精度提高十分明显，因此，单一的平滑算法不能有效提高所有导航参数的精度；
4)标准的平滑算法均以SINS高频采样率储存滤波结果，再按此频率进行平滑解算，对于典型的200HZ的SINS数据，平滑算法的储存空间可达GB级，计算时间在30min左右，十分消耗硬件资源；
针对以上问题，本发明给出了一种GNSS/SINS紧组合滤波平滑的快速算法，有效的减少了算法耗时，提高了紧组合解算的精度。
发明内容
本发明提供了一种GNSS/SINS紧组合滤波平滑的快速算法，具有数据利用率高、算法时间和内存消耗低，解算精度高的特点。为达到上述目的，具体技术方案如下：
一种GNSS/SINS紧组合滤波平滑的高精度快速算法，将Kalman滤波的状态预报进行效率优化，在平滑过程中剔除GNSS状态参数和分解SINS状态参数，利用状态降维后的RTSS和FBC相融合的平滑算法，采用低频保存的滤波结果进行平滑处理，以SINS机械编排高频内插获取紧组合解；
具体包括以下步骤；
步骤1，采用双线程设置前后向两个独立的滤波器，将SINS观测值、GNSS观测值和前后向初始信息分别输入两个滤波器中进行初始化，双向滤波器共用同一内存中的观测数据；
步骤2，采用效率优化的Kalman滤波算法进行GNSS/SINS组合滤波；
在GNSS/SINS组合滤波过程中，两个GNSS观测更新历元间通过SINS机械编排传递，对应的误差信息采用Kalman滤波一步预测协方差矩阵传递；在机械编排历元时采用效率优化的Kalman滤波处理，使用状态转移矩阵累积和近似过程噪声计算取代预报方差传递；
步骤3，在步骤2中，按照GNSS观测更新频率，逐历元保存GNSS/SINS组合滤波结果及其中间信息，完成前后向滤波后，将其输入到改进的前向和后向RTSS平滑器，利用改进的GNSS/SINS紧组合RTSS平滑算法进行低频率的双向RTSS平滑处理，分别得到低频率的前向和后向RTSS平滑处理的结果；
在进行RTSS平滑处理时，将GNSS相关的钟差、对流层和模糊度参数剔除，以降低状态维数，避免模糊度参数过多带来的计算负担，加速RTSS计算效率；从SINS状态参数中只分解出位置、速度和姿态这三类参数，对这三类参数分别进行后续FBC平滑处理；
步骤4，将步骤3中的前向和后向RTSS平滑结果输入到改进的FBC平滑器，利用改进的GNSS/SINS紧组合FBC平滑算法得到低频率的FBC平滑处理的结果；
步骤5，将SINS观测值和步骤4中的低频率FBC平滑结果输入高频内插模块，通过SINS机械编排高频内插算法得到平滑结果；
首先使用平滑结果对SINS原始数据进行系统误差改正，然后在两个低频平滑结果间进行前后向SINS机械编排，取均值后获取内插时刻高精度的紧组合导航结果。
所述步骤2中效率优化的Kalman滤波算法的具体实施步骤如下：
设起始历元为k0，下一观测更新历元为k200，当前历元为k，具体实施为：
步骤2.1，将上一历元的导航解作为初值，使用SINS机械编排算法计算当前历元k的导航解；
步骤2.2，将步骤2.1所得机械编排导航解以及观测信息带入状态微分方程系数矩阵F中，并对F矩阵使用离散化公式得到当前历元的状态转移矩阵Φk,k-1；
步骤2.3，判断当前历元是否存在GNSS观测信息：
若没有GNSS观测信息，则按照式(1)计算由起始历元k0至当前历元k的状态转移矩阵
式(1)中，为起始历元k0至前一历元k-1的状态转移矩阵；
若有GNSS观测信息，则按照式(2)计算Kalman滤波的一步预测状态及其方差，且由于预报状态为起始历元k0至当前历元k，所以式(2)中第一式的状态转移矩阵为过程噪声计算使用式(2)中第三式近似，其中Δtk＝tk-t0，t0为起始时刻，tk为当前时刻；计算得到预报状态及其方差后，按照式(3)进行观测更新，得到Kalman滤波的最终解；
式(2)和式(3)如下：
其中，Xk-1为上一时刻的状态，Xk,k-1为预报状态，Φk,k-1为状态转移矩阵，Qk-1为过程噪声，Δtk为两个历元间的时间间隔，Pk-1为上一时刻的状态方差，Pk,k-1为预报方差，I为单位矩阵，Fk,k-1为状态微分方程系数矩阵，为过程噪声矩阵；
其中，zk为观测值，Hk为观测设计矩阵，Kk为增益系数，Rk为观测噪声，Xk和Pk为滤波状态及其方差。
所述步骤3中，改进的GNSS/SINS紧组合RTSS平滑算法的具体步骤如下：
步骤3.1，按照式(4)和式(5)构建状态方程和观测方程，并且设置机械编排的初始位置、速度和姿态，Kalman滤波的初始状态X0,P0和Q；
步骤3.2，按照效率优化的GNSS/SINS紧组合滤波方法运行Kalman滤波，从状态向量Xk中删去GNSS状态，提取SINS状态向量XSINS,k，并且按照式(8)计算RTSS增益采用临时文件保存XSINS,k和
步骤3.3，一旦Kalman滤波达到数据结尾，启动RTSS算法，以和为初始状态按照式(6)和式(7)后向进行平滑处理；
式(4)、式(5)、式(6)、式(7)、式(8)如下：
δz＝HδX+η (5)
式(4)中，δXSINS，δXGNSS分别为SINS和GNSS的状态参数，δXSINS包含位置，速度，姿态，零偏参数，δXGNSS包含模糊度和对流层，GNSS接收机钟差已被消去；FSINS，FGNSS分别为SINS和GNSS的状态微分方程系数矩阵；wSINS，wGNSS分别为SINS和GNSS的状态的连续过程噪声；
式(5)中，δz为观测值残差，δX为包含δXSINS和δXGNSS的状态向量，H为观测设计矩阵，η为观测噪声；
其中，为k时刻的状态平滑值，为k+1时刻的状态平滑值，Ck为平滑历元的RTSS平滑增益矩阵，为状态转移矩阵的转置，为状态预报方差；
其中，为Kalman滤波估值，式(7)所得状态向量即为最终所得RTSS平滑结果；
式(11)中，
所述步骤4中，改进的GNSS/SINS紧组合FBC平滑算法的具体步骤如下：
步骤4.1.1，按照式(4)、式(5)和式(9)构建前后向滤波的紧组合状态方程和观测方程，并且设置机械编排的初始位置、速度和姿态，Kalman滤波的初始状态X0,P0和Q；
式(9)如下：
式(13)中，δre为位置误差，δve为速度误差，φ为失准角，为b系到e系的旋转矩阵，ab为加表零偏，εb为陀螺零偏，为地球自转角速度，δγe为重力误差，ξr、ξv和ξφ分别为位置、速度和失准角的过程噪声；
步骤4.1.2，采用双线程，独立运行前向和后向Kalman滤波，在GNSS观测更新时刻，从Kalman滤波状态向量中分解出位置、速度和失准角及其协方差矩阵，并保存；
步骤4.1.3，滤波完成以后，逐历元进行FBC平滑，使用式(10)对位置和速度进行平滑，使用式(11)至式(16)对姿态进行平滑，若俯仰角为±90°时，采用式(17)至式(21)的四元数法对姿态进行平滑；
式(10)至式(21)如下：
式(14)中，上标f和b表示前向(forward)和后向(backward)滤波，c表示平滑，r为位置结果，δr、Pδr分别为从Kalman滤波状态向量中分解出的位置修正和相应方差矩阵，rmech为机械编排值；
式中的φ×为失准角φ的反对成矩阵，为机械编排所得姿态旋转矩阵，R为经过失准角修正的姿态旋转矩阵；
其中，y,p和r分别表示航向角、俯仰角和横滚角；从式(12)中计算得到姿态角，如下：
式(17)中，R21、R22、R23、R13、R33表示旋转矩阵R中的元素；结合式(11)、式(12)和式(13)，可得如下关系
式(14)中，φx、φy和φz为失准角， 表示旋转矩阵中的元素；对式(14)求偏导可得
(δy δp δr)T＝F3×3(δφx δφy δφz)T (15)
其中，F3×3为方差协方差传播矩阵，利用误差传播定律，由失准角协方差矩阵Pφ得到姿态角的协方差矩阵Patt，如下：
设机械编排得到的相应的四元素为失准角φ对应的四元素为qφ，经过修正后的四元素为其关系为：
qφ表示如下：
由于失准角φ很小，上式近似为：
展开可得：
式(24)中，表示的各分量元素，表示的各分量元素，φx、φy和φz为失准角；
因此，可得的方差为：
其中，P(φ)为失准角的方差矩阵，L为4×3矩阵。
所述步骤4中，RTSS和FBC相融合的平滑算法具体步骤如下：
步骤4.2.1，设置前向和后向滤波的初值信息与观测信息，使用效率优化的Kalman滤波算法，采用双线程独立运行前向和后向Kalman滤波，同时存储RTSS平滑所需的中间结果；
步骤4.2.2，双向Kalman滤波完成以后，启动改进的RTSS平滑模块，采用双线程独立运行前向和后向RTSS平滑器，并保存中间结果用于FBC平滑解算；
步骤4.2.3，当双向RTSS平滑完成后，启动FBC平滑模块，采用改进的FBC平滑算法对前向和后向RTSS平滑结果再次进行组合平滑，得到高精度的平滑结果。
所述步骤5中，SINS机械编排高频内插平滑结果模块的具体步骤如下：
步骤5.1，确定内插时刻，从低频平滑结果中获取与内插时刻相邻的两个平滑结果，如果内插时刻等于平滑结果时刻，则直接输出平滑结果；
步骤5.2，使用平滑结果中SINS系统误差的平滑值，对SINS原始数据进行系统误差改正，以获取准确的加表和陀螺数据，为机械编排做准备；
步骤5.3，对相邻的两个平滑结果进行精度评定，过程如下：从其中的一个平滑结果作为初值开始，进行机械编排至另一个平滑结果时刻，得到机械编排的导航结果，与平滑结果做差，如果三维位置差异小于0.1mm，表明平滑结果通过精度评定，否则提示精度不达标，不进行内插；
步骤5.4，通过精度评定后，以两个平滑结果作为前后向机械编排的初值，独立的从前后两个方向向内插时刻进行机械编排，对两个机械编排结果取均值后得到最终的内插结果。
所述步骤2至步骤4中，在Kalman滤波过程中直接计算RTSS平滑增益矩阵，并只保存状态向量、状态方差和平滑增益矩阵，保存频率由原来的SINS状态更新频率降低为观测更新频率；当FBC平滑算法遇到姿态奇点处时，将姿态转为四元数后进行FBC平滑。
本发明使用效率优化的Kalman滤波模块、基于改进的RTSS和FBC相融合的平滑算法模块和利用SINS机械编排高频内插平滑结果模块设计了一种GNSS/SINS紧组合导航数据后处理平滑的快速算法，具有以下优点：
1)采用双线程独立运行前后向Kalman滤波，且共用SINS和GNSS观测值存储区，降低了计算时间和内存空间的消耗；
2)效率优化的Kalman滤波算法，极大的提高了状态预报更新的计算效率；
3)改进的RTSS平滑算法模块避免了GNSS状态参数过多带来的计算复杂度，RTSS和FBC相融合的平滑算法可以提高GNSS/SINS紧组合所有的导航参数的解算精度；
4)基于SINS机械编排高频内插的方法，从低频的平滑结果中恢复出任意时刻的紧组合解，在保证导航解精度的基础上，极大的降低了现有平滑方法带来的内存和时间消耗。
附图说明
图1为本发明实例的GNSS/SINS紧组合滤波平滑的快速算法总体结构图；
图2为本发明实例的GNSS/SINS紧组合结构图；
图3为本发明实例的ECEF系下的SINS机械编排图；
图4为本发明实例的效率优化的GNSS/SINS组合Kalman滤波流程图；
图5为本发明实例的改进的紧组合RTSS平滑算法流程图；
图6为本发明实例的改进的紧组合FBC平滑算法流程图；
图7为本发明实例的RTSS和FBC相融合的平滑算法流程图；
图8为本发明实例的基于SINS机械编排的平滑结果内插算法流程图。
具体实施方式
本发明的算法总体结构如图1所示，将Kalman滤波的状态预报进行效率优化，在平滑过程中剔除GNSS状态参数和分解SINS状态参数，设计了状态降维以后的RTSS和FBC相融合的平滑算法，采用低频保存的滤波结果进行平滑处理，以SINS机械编排高频内插获取任意时刻紧组合解的方法；技术方案如下所述：
步骤1，采用双线程设置前后向两个独立的滤波器，将SINS观测值、GNSS观测值和前后向初始信息输入滤波器中进行初始化，双向滤波器共用同一内存中的观测数据，避免内存消耗和多次读取带来的时间消耗；
步骤2，在GNSS/SINS组合滤波时，使用效率优化的Kalman滤波处理。SINS观测值输出率远高于GNSS观测值，在GNSS/SINS组合滤波中，两个GNSS观测更新历元间需要通过SINS机械编排传递，对应的误差信息使用Kalman滤波一步预测协方差矩阵传递。由于一步预测协方差矩阵计算极为耗时，本发明中在机械编排历元时采用效率优化的Kalman滤波处理，使用状态转移矩阵累积和近似过程噪声计算取代预报方差传递；
当没有GNSS观测信息时，无需以SINS采样率高频更新Kalman滤波的预报方差Pk,k-1，仅需计算起始历元k0至当前历元k的状态转移矩阵即可；当有GNSS观测信息时，采用近似公式计算过程噪声其余部分严格按照标准Kalman公式进行计算；
步骤3，在步骤2中，按照GNSS观测更新频率，逐历元保存GNSS/SINS组合滤波结果及其中间信息，完成前后向滤波后，将其输入到改进的前向和后向RTSS平滑器，进行低频率的双向RTSS平滑处理，分别得到低频率的前向和后向RTSS平滑处理的结果；
由于GNSS状态与SINS状态耦合程度不高，在进行RTSS平滑时，将GNSS相关的钟差、对流层和模糊度参数剔除，以降低状态维数，避免模糊度参数过多带来的计算负担，加速RTSS计算效率；从SINS状态参数中只分解出位置、速度和姿态这三类参数，对这三类参数分别进行FBC平滑；
步骤4，采用两个线程进行独立的前后向滤波并做RTSS平滑，再将前后向RTSS结果进行FBC平滑得到最终结果；将步骤3中的前向和后向RTSS平滑结果输入到改进的FBC平滑器，得到低频率的FBC平滑处理的结果；
步骤5，将SINS观测值和步骤4中的低频率FBC平滑结果输入高频内插模块，首先使用平滑结果对SINS原始数据进行系统误差改正，然后在两个低频平滑结果间进行前后向SINS机械编排，取均值后获取内插时刻高精度的紧组合导航参数。
步骤2-4中，在Kalman滤波过程中直接计算RTSS平滑增益矩阵，并只保存状态向量、状态方差和平滑增益矩阵，保存频率由原来的SINS状态更新频率降低为观测更新频率。当FBC平滑算法遇到姿态奇点处时(俯仰角±90度)，将姿态转为四元数后进行FBC平滑。
以下将结合具体实施方式对本发明的各算法进行详细说明。
本发明是一种GNSS/SINS紧组合滤波平滑的快速算法，紧组合模式如图2所示，导航坐标系选为ECEF系，相应的SINS机械编排如图3所示。紧组合中，GNSS和SINS的原始观测值共同输入到一个Kalman滤波器中，联合估计导航参数(位置、速度和姿态)、SINS系统误差以及GNSS相关参数(对流层和模糊度)，并且采用闭环修正技术，对SINS系统误差进行反馈校正。GNSS/SINS紧组合状态模型和观测模型，分别如下：
δz＝HδX+η (23)
式(4)中，δXSINS，δXGNSS分别为SINS和GNSS的状态参数，δXSINS包含了位置，速度，姿态，零偏参数，δXGNSS包含了模糊度和对流层，由于采用了单差或双差的定位模式，GNSS接收机钟差已被消去；FSINS，FGNSS分别为SINS和GNSS的状态微分方程系数矩阵；wSINS，wGNSS分别为SINS和GNSS的状态的连续过程噪声；式(5)中，δz为观测值残差，δX为包含δXSINS和δXGNSS的状态向量，H为观测设计矩阵，η为观测噪声。
按照上述紧组合数学模型，采用Kalman滤波进行估计，其一般解算步骤包括一步状态预测和观测更新两部分，具体如下：
一步状态预测
观测更新
式(2)中，Xk-1为上一时刻的状态，Xk,k-1为预报状态，Φk,k-1为状态转移矩阵，Qk-1为过程噪声，Δtk为两个历元间的时间间隔，Pk-1为上一时刻的状态方差，Pk,k-1为预报方差，I为单位矩阵，Fk,k-1为状态微分方程系数矩阵，为过程噪声矩阵。式(3)中，zk为观测值，Hk为观测设计矩阵，Kk为增益系数，Rk为观测噪声，Xk和Pk为滤波状态及其方差。
以上紧组合数学模型和Kalman滤波一般解算步骤是本发明算法的基础，下面将结合图1所示的技术路线，对本发明中各模块的关键技术及其实施方法展开详细叙述。
一、效率优化的Kalman滤波算法
GNSS/SINS组合导航中，SINS数据采样率(通常200Hz)远高于GNSS数据采样率(通常1Hz)，在没有GNSS观测更新时，以SINS采样率进行Kalman状态一步预测时，需计算200次预报方差Pk,k-1，即公式(2)，耗时严重。本方法采用了一种效率优化的Kalman滤波算法，避免了预报方差的直接计算，算法流程如图4所示，具体实施步骤如下所述：
设起始历元为k0，下一观测更新历元为k200，当前历元为k，具体实施为：
步骤2.1，将上一历元的导航解作为初值，使用SINS机械编排算法计算当前历元k的导航解；
步骤2.2，将步骤1所得机械编排导航解以及观测信息带入状态微分方程系数矩阵F中，并对F矩阵使用离散化公式得到当前历元的状态转移矩阵Φk,k-1；
步骤2.3，判断当前历元是否存在GNSS观测信息：
若没有GNSS观测信息，则不使用经典Kalman滤波算法的状态一步预测，而是按照式(1)计算由起始历元k0至当前历元k的状态转移矩阵
式(1)中，为起始历元k0至前一历元k-1的状态转移矩阵。
若有GNSS观测信息，则按照式(2)计算Kalman滤波的一步预测状态及其方差，由于预报状态为起始历元k0至当前历元k，所以式(2)中第一式的状态转移矩阵为过程噪声计算使用式(2)中第三式近似，其中Δtk＝tk-t0，t0为起始时刻，tk为当前时刻。计算得到预报状态及其方差后，按照式(3)进行观测更新，即可得到Kalman滤波的最终解。
由上述算法步骤可知，当无GNSS观测信息时，无需计算Kalman滤波计算一步预测方差Pk,k-1，仅需计算起始历元k0至当前历元k的状态转移矩阵即可，显著提高了计算效率；当有GNSS观测信息时，在过程噪声计算时采用了近似处理，其余部分均严格按照Kalman一般解算公式进行。状态方差预报中，主项起到了主要作用，为起始时刻的状态方差。此外，由于过程噪声量级较小且SINS采样间隔很小，因此过程噪声近似对解算精度几乎没有影响。
二、改进的GNSS/SINS紧组合RTSS平滑算法
标准的RTSS平滑算法和Kalman滤波算法类似，同样采用递推算法，但它从最后一个历元开始，后向递推运算。假设共有N个历元，RTSS以第N个历元的状态及其方差作为初始状态后向进行平滑处理。每个平滑历元的RTSS平滑增益矩阵Ck计算如下：
式(27)中，为状态转移矩阵的转置，为状态预报方差。平滑结果的状态向量及其协方差矩阵由下式算得
式(7)中，为k+1时刻的状态平滑值及其方差。在实际平滑处理时，增益矩阵Ck可以在Kalman滤波每个历元完成后计算出并保存，方差矩阵对状态向量计算不起作用，因此，在不需要状态精度信息时可忽略。由于紧组合中采用了闭环修正的结构，每个历元的滤波估计的误差状态均被修正到导航解和IMU原始数据上，下一历元的Kalman滤波状态向量需要被置零。因此，状态向量Xk及其一步预测结果为零，式(28)可以简化如下：
式(6)所得平滑值并非最终的输出结果，由于采用了闭环修正，此时平滑值是相对于状态初值为零的Kalman结果，需要进一步改正到Kalman滤波估值上：
式(7)所得状态向量即为最终所得RTSS平滑结果。以上所述为标准RTSS算法的处理流程及其细节，以此为基础，本发明进一步改进如下所述：
紧组合结构中包括SINS和GNSS两类状态向量，即式(4)中的XSINS和XGNSS。其中SINS状态参数固定为15维，分别为位置、速度、失准角、加计零偏和陀螺零偏；而GNSS状态参数包括对流层和浮点模糊度，其中浮点模糊度参数的维数是不固定的，受到卫星升起和降落的影响而变化，而RTSS很难处理可变状态。另外，对于多系统下的紧组合算法，一般情况下，可见卫星数大于在20颗左右。卫星观测值的冗余度一方面增加了定位解算的精度和可靠性，另一方面也增加了储存和计算负担。假设有20个浮点模糊度参数，则紧组合的状态参数总数为15+1+20＝36，对于一个小时200HZ采样率的SINS数据，若只储存Kalman滤波估值和增益矩阵Ck，其数据量可达7GB，严重影响计算效率和消耗存储空间。
从公式(4)中可以看书，紧组合中SINS与GNSS的状态参数在函数模型上互不相关，SINS状态函数模型是基于刚体运动学规律导出的，状态参数之间具有高度耦合性，而GNSS中的模糊度参数仅具有几何属性，并且对流层和模糊度都被模型化为随机游走，耦合程度很低。同时，RTSS平滑主要依赖于刚体运动学信息而与几何信息无关，因此，可以将GNSS状态参数剔除，只对SINS的15状态参数进行RTSS平滑，可以明显降低RTSS平滑的计算负担。
在没有外部观测更新时，公式(7)中的Kalman滤波估值为零，因此从历元k平滑至历元k0，采用公式(6)可表达为：
将Ck表达式(27)代入得：
式(11)中，上式表明，在Kalman滤波观测更新历元k0和k之间只需累乘状态转移矩阵的转置与效率优化的Kalman滤波具有相似性，最后只需一步计算并储存即可，而并非原来的多步计算Ck。在RTSS平滑时，可直接从求出并加上Kalman滤波估值得到k0时刻的平滑值此时，平滑结果的频率与GNSS观测更新频率一致，解决了标准RTSS算法按SINS状态更新率进行平滑所带来的计算和储存效率问题。
具体算法流程如图5，实施步骤如下：
步骤3.1，按照式(4)和式(5)构建状态方程和观测方程，并且设置机械编排的初始位置、速度和姿态，Kalman滤波的初始状态X0,P0和Q；
步骤3.2，按照效率优化的GNSS/SINS紧组合滤波方法运行Kalman滤波，从状态向量Xk中删去GNSS状态，提取SINS状态向量XSINS,k，并且按照(8)式计算RTSS增益采用临时文件保存XSINS,k和
步骤3.3，一旦Kalman滤波达到数据结尾，启动RTSS算法，以和为初始状态按照式(6)和式(7)后向进行平滑处理。
由以上具体实施步骤可知，本发明改进的RTSS算法，在平滑处理时从状态参数中删去了GNSS状态参数，避免了模糊度参数过多及频繁变化带来的计算难度，并采用一步计算平滑增益矩阵的方法，提高了RTSS平滑算法的运行效率。
三、改进的GNSS/SINS紧组合FBC平滑算法
标准的FBC平滑算法包含前后和方向两个独立的Kalman滤波，降其滤波结果保存后，再按照下式进行组合平滑：
式中，上标f和b表示前向(forward)和后向(backward)滤波，c表示平滑；Xk为状态向量估值，Pk为状态向量估值的协方差矩阵。由式(33)可知，FBC平滑的基本思想是依据双向滤波估值的协方差矩阵对滤波结果进行加权平均。
由于FBC需要进行双向滤波，从图2的GNSS/SINS紧组合结构图可知，在前向滤波状态模型和观测模型的基础上，必须构造后向滤波的相应模型。本发明构造的后向滤波状态模型如下：
式(13)中，δre为位置误差，δve为速度误差，φ为失准角，为b系到e系的旋转矩阵，ab为加表零偏，εb为陀螺零偏，为地球自转角速度，δγe为重力误差，ξr、ξv和ξφ分别为位置、速度和失准角的过程噪声。其它状态量，包括SINS系统误差、GNSS对流层和模糊度，由于都被模型化为随机游走，因此前后向的状态模型一样。对于位置、速度和失准角三个状态量，只需在前向状态微分方程中添加负号即可。而观测模型仅仅表明观测值与状态参数之间的关系，与载体运动时间方向无关，所以后向滤波的观测模型与前向滤波是相同的。
在以上FBC平滑算法流程和细节的基础上，本发明进一步改进如下所述：
在紧组合结构中，所有状态参数均为状态量的扰动量，直接对其进行前后向组合平滑并不完全正确。因为扰动项与众多因素有关，尤其受到前一时刻的导航参数(位置、速度和姿态)的影响，所以前后向滤波在同一时刻的扰动量并不具有一致性，不能直接进行组合平滑。但是，相应的导航参数在同一时刻具有一致性，因为载体的运动轨迹是唯一客观存在的，因此，必须采用导航参数做为式(33)的状态量进行组合平滑。另外，为了排除不同性质状态量之间的相互干扰，将位置、速度和姿态分别从SINS状态向量中分解出来，逐一的进行单独FBC平滑。
例如对于位置参数，从Kalman滤波状态向量中分解出位置修正量δr和相应方差矩阵Pδr，将其改正到机械编排值rmech上得到位置结果，再按照(33)式进行平滑：
式(14)中，上标f和b表示前向(forward)和后向(backward)滤波，c表示平滑，r为位置结果。同样的，对于速度和其它参数均可使用式(10)的形式进行FBC平滑。但是，姿态需要进一步处理，主要是因为Kalman滤波中估计的是失准角，它与姿态存在着非线性关系，其转换关系如下：
式中的φ×为失准角φ的反对成矩阵，为机械编排所得姿态旋转矩阵，R为经过失准角修正的姿态旋转矩阵。姿态旋转矩阵表示如下：
其中，y,p和r分别表示航向角、俯仰角和横滚角。从式(12)中可计算得到姿态角，如下：
式(17)中，R21、R22、R23、R13、R33表示旋转矩阵R中的元素。结合式(11)、式(12)和式(13)，可得如下关系
式(14)中，φx、φy和φz为失准角， 表示旋转矩阵中的元素。该式主要为了求得姿态角与失准角之间的微分关系，以便利用误差传播定律求得姿态的协方差矩阵。为了求导方便，将式(14)中的姿态限定在第一象限。
对式(14)求偏导可得
(δy δp δr)T＝F3×3(δφx δφy δφz)T (40)
其中，F3×3为方差协方差传播矩阵，由于其表达式复杂，可使用Mathematica或MATLAB的符号运算系统得到。然后利用误差传播定律，可以由失准角协方差矩阵Pφ得到姿态角的协方差矩阵Patt，如下：
依据式(16)所得姿态差协方差，可以使用式(33)获得姿态角的FBC平滑结果。需要注意2点：1)定义域不连续问题，前后向滤波结果在角度临界值上会发生突变，如航向角0.1°和359.9°其实是同一个角度，此时要将其归化到同一的区间内，如将0.1°加上360°，才能与359.9°进行FBC平滑；2)奇点问题，当俯仰角为90°时，由旋转矩阵不能得到姿态角，即(13)式不可用，此时采用四元数法。
四元数在表示旋转关系上具有良好的性能，它可以理解为四维空间中的坐标点，其定义域连续，不存在奇点问题，没有姿态角所存在的问题。假设机械编排得到的相应的四元素为失准角φ对应的四元素为qφ，经过修正后的四元素为其关系为：
qφ表示如下：
由于失准角φ很小，上式可近似为：
显然近似化以后，qφ不为规范四元素，这是为了导出φ与的线性关系才这样处理的，但在计算时，仍使用式(18)。
展开可得：
式(24)中，表示的各分量元素，等表示的各分量元素，φx、φy和φz为失准角。
因此，可得的方差为：
其中，P(φ)为失准角的方差矩阵。L为4×3矩阵，根据矩阵秩定理，因此不可逆，是奇异阵。由于不可逆，不能使用FBC平滑公式(33)，这也是四元数的缺点之一。但是四元数的各分量的方差即的对角线仍是一个重要的精度指示量，不考虑各分量之间的相关性，对每个分量单独使用FBC平滑公式，虽然其结果是次优的，但是主对角线元素的值比非对角元素值大，对平滑结果贡献是最大的，该结果可以接受。得到平滑后的四元数，再进行转换得到姿态角。
具体算法流程如图6，实施步骤如下：
步骤4.1.1，按照式(4)、式(5)和式(9)构建前后向滤波的紧组合状态方程和观测方程，并且设置机械编排的初始位置、速度和姿态，Kalman滤波的初始状态X0,P0和Q；
步骤4.1.2，采用双线程，独立运行前向和后向Kalman滤波，在GNSS观测更新时刻，从Kalman滤波状态向量中分解出位置、速度和失准角及其协方差矩阵，并保存；
步骤4.1.3，滤波完成以后，逐历元进行FBC平滑，使用式(10)对位置和速度进行平滑，使用式(11)至式(16)对姿态进行平滑，若俯仰角为±90°时，采用式(17)至式(21)的四元数法对姿态进行平滑。
由以上具体实施步骤可知，本发明改进的FBC平滑算法，构造了后向紧组合滤波模型，从Kalman滤波状态中分解出位置、速度和姿态进行单独平滑，并对姿态奇点进行了处理，保证了紧组合中FBC平滑算法的有效性。
四、RTSS和FBC相融合的平滑算法
紧组合中，单独的RTSS平滑对速度和姿态的精度提高十分明显，而单独的FBC平滑对位置精度提高十分明显，因此，单一的平滑算法不能有效提高所有导航参数的精度。为此，本发明提出了RTSS和FBC相融合的平滑算法，充分利用所有观测信息和运动学信息，以往的做法是将前后向滤波直接输入到FBC平滑器中，而本发明将RTSS平滑器嵌入到FBC平滑器中，先采用两个线程进行独立的前后向滤波并做RTSS平滑，再将前后向RTSS结果输入FBC平滑器中组合，得到高精度的平滑结果。
具体算法流程如图7，实施步骤如下：
步骤4.2.1，设置前向和后向滤波的初值信息与观测信息，使用效率优化的Kalman滤波算法，采用双线程独立运行前向和后向Kalman滤波，同时存储RTSS平滑所需的中间结果；
步骤4.2.2，一旦双向Kalman滤波完成以后，启动改进的RTSS平滑模块，仍采用双线程独立运行前向和后向RTSS平滑器，并保存中间结果用于FBC平滑解算；
步骤4.2.3，当双向RTSS平滑完成后，启动FBC平滑模块，采用改进的FBC平滑算法对前向和后向RTSS平滑结果再次进行组合平滑，得到高精度的平滑结果。
由以上具体实施步骤可知，RTSS和FBC相融合的平滑算法充分结合了两种不同平滑算法的优势，在GNSS/SINS紧组合中，能够有效的一次性提高所有导航参数的解算精度。
五、SINS机械编排高频内插平滑结果模块
在前述效率优化的Kalman滤波算法，改进的RTSS平滑算法，改进的FBC平滑算法以及RTSS和FBC相融合的算法中，为了减小内存消耗和提升运算效率，各模块的计算频率均与GNSS观测频率一致，通常为1s，导致储存的结果频率也较低，这就损失了SINS高采样率、高动态导航的优势，而在飞机、舰船一类高动态导航和移动测量的实际应用中，往往需要高频的、任意采样点上的高精度定位、测速和测姿结果。因此，本发明提出了基于SINS机械编排的方法，对低频平滑进行内插以获取任意时刻高精度的紧组合结果。
具体算法流程如图8，实施步骤如下：
步骤5.1，确定内插时刻，从低频平滑结果中获取与内插时刻相邻的两个平滑结果，如果内插时刻等于平滑结果时刻，则直接输出平滑结果；
步骤5.2，使用平滑结果中SINS系统误差的平滑值，对SINS原始数据进行系统误差改正，以获取准确的加表和陀螺数据，为机械编排做准备；
步骤5.3：对相邻的两个平滑结果进行精度评定，做法如下：从其中的一个平滑结果作为初值开始，进行机械编排至另一个平滑结果时刻，得到机械编排的导航结果，与平滑结果做差，如果三维位置差异小于0.1mm，表明平滑结果通过精度评定，否则提示精度不达标，不进行内插；
步骤5.4，通过精度评定后，以两个平滑结果作为前后向机械编排的初值，独立的从前后两个方向向内插时刻进行机械编排，对两个机械编排结果取均值后得到最终的内插结果。
由以上具体实施步骤可知，基于SINS机械编排的内插算法可以得到任意时刻的高精度导航解，而且机械编排算法简单快速，因而可以高效地生成连续、高采样率、高精度的导航结果，且有对平滑结果精度的评定，增加了内插结果的可靠性。
Claims (7)
1.一种GNSS/SINS紧组合滤波平滑的高精度快速算法，其特征在于：将Kalman滤波的状态预报进行效率优化，在平滑过程中剔除GNSS状态参数和分解SINS状态参数，利用状态降维后的RTSS和FBC相融合的平滑算法，采用低频保存的滤波结果进行平滑处理，以SINS机械编排高频内插获取紧组合解；
具体包括以下步骤；
步骤1，采用双线程设置前后向两个独立的滤波器，将SINS观测值、GNSS观测值和前后向初始信息分别输入两个滤波器中进行初始化，双向滤波器共用同一内存中的观测数据；
步骤2，采用效率优化的Kalman滤波算法进行GNSS/SINS组合滤波；
在GNSS/SINS组合滤波过程中，两个GNSS观测更新历元间通过SINS机械编排传递，对应的误差信息采用Kalman滤波一步预测协方差矩阵传递；在机械编排历元时采用效率优化的Kalman滤波处理，使用状态转移矩阵累积和近似过程噪声计算取代预报方差传递；
步骤3，在步骤2中，按照GNSS观测更新频率，逐历元保存GNSS/SINS组合滤波结果及其中间信息，完成前后向滤波后，将其输入到改进的前向和后向RTSS平滑器，利用改进的GNSS/SINS紧组合RTSS平滑算法进行低频率的双向RTSS平滑处理，分别得到低频率的前向和后向RTSS平滑处理的结果；
在进行RTSS平滑处理时，将GNSS相关的钟差、对流层和模糊度参数剔除，以降低状态维数，避免模糊度参数过多带来的计算负担，加速RTSS计算效率；从SINS状态参数中只分解出位置、速度和姿态这三类参数，对这三类参数分别进行后续FBC平滑处理；
步骤4，将步骤3中的前向和后向RTSS平滑结果输入到改进的FBC平滑器，利用改进的GNSS/SINS紧组合FBC平滑算法以及RTSS和FBC相融合的平滑算法得到低频率的FBC平滑处理的结果；
步骤5，将SINS观测值和步骤4中的低频率FBC平滑结果输入高频内插模块，通过SINS机械编排高频内插算法得到平滑结果；
首先使用平滑结果对SINS原始数据进行系统误差改正，然后在两个低频平滑结果间进行前后向SINS机械编排，取均值后获取内插时刻高精度的紧组合导航结果。
2.根据权利要求1所述的一种GNSS/SINS紧组合滤波平滑的高精度快速算法，其特征在于：所述步骤2中效率优化的Kalman滤波算法的具体实施步骤如下：
设起始历元为k0，下一观测更新历元为k200，当前历元为k，具体实施为：
步骤2.1，将上一历元的导航解作为初值，使用SINS机械编排算法计算当前历元k的导航解；
步骤2.2，将步骤2.1所得机械编排导航解以及观测信息代入状态微分方程系数矩阵F中，并对F矩阵使用离散化公式得到当前历元的状态转移矩阵Φk,k-1；
步骤2.3，判断当前历元是否存在GNSS观测信息：
若没有GNSS观测信息，则按照式(1)计算由起始历元k0至当前历元k的状态转移矩阵
式(1)中，为起始历元k0至前一历元k-1的状态转移矩阵；
若有GNSS观测信息，则按照式(2)计算Kalman滤波的一步预测状态及其方差，且由于预报状态为起始历元k0至当前历元k，所以式(2)中第一式的状态转移矩阵为过程噪声计算使用式(2)中第三式近似，其中Δtk＝tk-t0，t0为起始时刻，tk为当前时刻；计算得到预报状态及其方差后，按照式(3)进行观测更新，得到Kalman滤波的最终解；
式(2)和式(3)如下：
其中，Xk-1为上一时刻的状态，Xk,k-1为预报状态，Φk,k-1为状态转移矩阵，Qk-1为过程噪声，Δtk为两个历元间的时间间隔，Pk-1为上一时刻的状态方差，Pk,k-1为预报方差，I为单位矩阵，Fk,k-1为状态微分方程系数矩阵，为过程噪声矩阵；
其中，zk为观测值，Hk为观测设计矩阵，Kk为增益系数，Rk为观测噪声，Xk和Pk为滤波状态及其方差。
3.根据权利要求2所述的一种GNSS/SINS紧组合滤波平滑的高精度快速算法，其特征在于：所述步骤3中，改进的GNSS/SINS紧组合RTSS平滑算法的具体步骤如下：
步骤3.1，按照式(4)和式(5)构建状态方程和观测方程，并且设置机械编排的初始位置、速度和姿态，Kalman滤波的初始状态X0,P0和Q；
步骤3.2，按照效率优化的GNSS/SINS紧组合滤波方法运行Kalman滤波，从状态向量Xk中删去GNSS状态，提取SINS状态向量XSINS,k，并且按照式(8)计算RTSS增益采用临时文件保存XSINS,k和
步骤3.3，一旦Kalman滤波达到数据结尾，启动RTSS算法，以和为初始状态按照式(6)和式(7)后向进行平滑处理；
式(4)、式(5)、式(6)、式(7)、式(8)如下：
δz＝HδX+η (5)
式(4)中，δXSINS，δXGNSS分别为SINS和GNSS的状态参数，δXSINS包含位置，速度，姿态，零偏参数，δXGNSS包含模糊度和对流层，GNSS接收机钟差已被消去；FSINS，FGNSS分别为SINS和GNSS的状态微分方程系数矩阵；wSINS，wGNSS分别为SINS和GNSS的状态的连续过程噪声；
式(5)中，δz为观测值残差，δX为包含δXSINS和δXGNSS的状态向量，H为观测设计矩阵，η为观测噪声；
其中，为k时刻的状态平滑值，为k+1时刻的状态平滑值，Ck为平滑历元的RTSS平滑增益矩阵，为状态转移矩阵的转置，为状态预报方差；
其中，为Kalman滤波估值，式(7)所得状态向量即为最终所得RTSS平滑结果；
式(8)中，
4.根据权利要求3所述的一种GNSS/SINS紧组合滤波平滑的高精度快速算法，其特征在于：所述步骤4中，改进的GNSS/SINS紧组合FBC平滑算法的具体步骤如下：
步骤4.1.1，按照式(4)、式(5)和式(9)构建前后向滤波的紧组合状态方程和观测方程，并且设置机械编排的初始位置、速度和姿态，Kalman滤波的初始状态X0,P0和Q；
式(9)如下：
式(9)中，δre为位置误差，δve为速度误差，φ为失准角，为b系到e系的旋转矩阵，ab为加表零偏，εb为陀螺零偏，为地球自转角速度，δγe为重力误差，ξr、ξv和ξφ分别为位置、速度和失准角的过程噪声；
步骤4.1.2，采用双线程，独立运行前向和后向Kalman滤波，在GNSS观测更新时刻，从Kalman滤波状态向量中分解出位置、速度和失准角及其协方差矩阵，并保存；
步骤4.1.3，滤波完成以后，逐历元进行FBC平滑，使用式(10)对位置和速度进行平滑，使用式(11)至式(16)对姿态进行平滑，若俯仰角为±90°时，采用式(17)至式(21)的四元数法对姿态进行平滑；
式(10)至式(21)如下：
式(10)中，上标f和b表示前向(forward)和后向(backward)滤波，c表示平滑，r为位置结果，δr、Pδr分别为从Kalman滤波状态向量中分解出的位置修正和相应方差矩阵，rmech为机械编排值；
式中的φ×为失准角φ的反对成矩阵，为机械编排所得姿态旋转矩阵，R为经过失准角修正的姿态旋转矩阵；
其中，y,p和r分别表示航向角、俯仰角和横滚角；从式(12)中计算得到姿态角，如下：
式(13)中，R21、R22、R23、R13、R33表示旋转矩阵R中的元素；结合式(11)、式(12)和式(13)，可得如下关系
式(14)中，φx、φy和φz为失准角， 表示旋转矩阵中的元素；对式(14)求偏导可得
(δy δp δr)T＝F3×3(δφx δφy δφz)T (15)
其中，F3×3为方差协方差传播矩阵，利用误差传播定律，由失准角协方差矩阵Pφ得到姿态角的协方差矩阵Patt，如下：
设机械编排得到的相应的四元素为失准角φ对应的四元素为qφ，经过修正后的四元素为其关系为：
qφ表示如下：
由于失准角φ很小，上式近似为：
展开可得：
式(20)中，表示的各分量元素， 表示的各分量元素，φx、φy和φz为失准角；
因此，可得的方差为：
其中，P(φ)为失准角的方差矩阵，L为4×3矩阵。
5.根据权利要求4所述的一种GNSS/SINS紧组合滤波平滑的高精度快速算法，其特征在于：所述步骤4中，RTSS和FBC相融合的平滑算法具体步骤如下：
步骤4.2.1，设置前向和后向滤波的初值信息与观测信息，使用效率优化的Kalman滤波算法，采用双线程独立运行前向和后向Kalman滤波，同时存储RTSS平滑所需的中间结果；
步骤4.2.2，双向Kalman滤波完成以后，启动改进的RTSS平滑模块，采用双线程独立运行前向和后向RTSS平滑器，并保存中间结果用于FBC平滑解算；
步骤4.2.3，当双向RTSS平滑完成后，启动FBC平滑模块，采用改进的FBC平滑算法对前向和后向RTSS平滑结果再次进行组合平滑，得到高精度的平滑结果。
6.根据权利要求4所述的一种GNSS/SINS紧组合滤波平滑的高精度快速算法，其特征在于：所述步骤5中，SINS机械编排高频内插平滑结果模块的具体步骤如下：
步骤5.1，确定内插时刻，从低频平滑结果中获取与内插时刻相邻的两个平滑结果，如果内插时刻等于平滑结果时刻，则直接输出平滑结果；
步骤5.2，使用平滑结果中SINS系统误差的平滑值，对SINS原始数据进行系统误差改正，以获取准确的加表和陀螺数据，为机械编排做准备；
步骤5.3，对相邻的两个平滑结果进行精度评定，过程如下：从其中的一个平滑结果作为初值开始，进行机械编排至另一个平滑结果时刻，得到机械编排的导航结果，与平滑结果做差，如果三维位置差异小于0.1mm，表明平滑结果通过精度评定，否则提示精度不达标，不进行内插；
步骤5.4，通过精度评定后，以两个平滑结果作为前后向机械编排的初值，独立的从前后两个方向向内插时刻进行机械编排，对两个机械编排结果取均值后得到最终的内插结果。
7.根据权利要求1所述的一种GNSS/SINS紧组合滤波平滑的高精度快速算法，其特征在于：所述步骤2至步骤4中，在Kalman滤波过程中直接计算RTSS平滑增益矩阵，并只保存状态向量、状态方差和平滑增益矩阵，保存频率由原来的SINS状态更新频率降低为观测更新频率；当FBC平滑算法遇到姿态奇点处时，将姿态转为四元数后进行FBC平滑。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611065215.7A CN106597507B (zh) | 2016-11-28 | 2016-11-28 | Gnss/sins紧组合滤波平滑的高精度快速算法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611065215.7A CN106597507B (zh) | 2016-11-28 | 2016-11-28 | Gnss/sins紧组合滤波平滑的高精度快速算法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106597507A CN106597507A (zh) | 2017-04-26 |
| CN106597507B true CN106597507B (zh) | 2019-03-19 |
Family
ID=58593586
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201611065215.7A Active CN106597507B (zh) | 2016-11-28 | 2016-11-28 | Gnss/sins紧组合滤波平滑的高精度快速算法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106597507B (zh) |
Families Citing this family (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107796389A (zh) * | 2017-10-17 | 2018-03-13 | 北京航天发射技术研究所 | 一种基于多核dsp的定位导航方法 |
| CN110106755B (zh) * | 2019-04-04 | 2020-11-03 | 武汉大学 | 利用姿态重构铁轨几何形态的高铁轨道不平顺性检测方法 |
| CN109959952A (zh) * | 2019-04-10 | 2019-07-02 | 成都纵横融合科技有限公司 | 基于顺/反序的gnss/ins组合导航解算方法 |
| CN110031879B (zh) * | 2019-04-17 | 2023-11-17 | 武汉大学 | 模糊度域信息整合的高精度后处理定位方法及系统 |
| CN110058324B (zh) * | 2019-05-09 | 2020-09-08 | 中国人民解放军国防科技大学 | 利用重力场模型的捷联式重力仪水平分量误差修正方法 |
| CN110006427B (zh) * | 2019-05-20 | 2020-10-27 | 中国矿业大学 | 一种低动态高振动环境下的bds/ins紧组合导航方法 |
| CN110432884B (zh) * | 2019-07-08 | 2022-07-01 | 暨南大学 | 基于胎心率减速区面积的胎儿状况测评方法及系统 |
| CN111323796B (zh) * | 2020-03-18 | 2021-11-09 | 中国科学院国家空间科学中心 | 一种gnss接收机高采样钟差解算方法 |
| CN111736183B (zh) * | 2020-07-28 | 2023-12-05 | 中国人民解放军战略支援部队信息工程大学 | 一种联合bds2/bds3的精密单点定位方法和装置 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104613965A (zh) * | 2015-03-02 | 2015-05-13 | 大连理工大学 | 一种基于双向滤波平滑技术的步进式行人导航方法 |
| CN104714244A (zh) * | 2015-03-31 | 2015-06-17 | 东南大学 | 一种基于抗差自适应Kalman滤波的多系统动态PPP解算方法 |
| CN105954783A (zh) * | 2016-04-26 | 2016-09-21 | 武汉大学 | 一种改进gnss/ins实时紧组合导航实时性能的方法 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7498979B2 (en) * | 2006-04-17 | 2009-03-03 | Trimble Navigation Limited | Fast decimeter-level GNSS positioning |
| KR101502721B1 (ko) * | 2014-02-06 | 2015-03-24 | 군산대학교산학협력단 | 적응형 상호작용 다중모델 추정기를 이용한 정밀 위치정보 제공 방법 및 장치 |
-
2016
- 2016-11-28 CN CN201611065215.7A patent/CN106597507B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104613965A (zh) * | 2015-03-02 | 2015-05-13 | 大连理工大学 | 一种基于双向滤波平滑技术的步进式行人导航方法 |
| CN104714244A (zh) * | 2015-03-31 | 2015-06-17 | 东南大学 | 一种基于抗差自适应Kalman滤波的多系统动态PPP解算方法 |
| CN105954783A (zh) * | 2016-04-26 | 2016-09-21 | 武汉大学 | 一种改进gnss/ins实时紧组合导航实时性能的方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106597507A (zh) | 2017-04-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106597507B (zh) | Gnss/sins紧组合滤波平滑的高精度快速算法 | |
| CN107643534B (zh) | 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法 | |
| CN110823217B (zh) | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 | |
| WO2018014602A1 (zh) | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 | |
| US9791575B2 (en) | GNSS and inertial navigation system utilizing relative yaw as an observable for an ins filter | |
| Jiancheng et al. | Study on innovation adaptive EKF for in-flight alignment of airborne POS | |
| Li et al. | A novel backtracking navigation scheme for autonomous underwater vehicles | |
| CN109000642A (zh) | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 | |
| Stančić et al. | The integration of strap-down INS and GPS based on adaptive error damping | |
| Zhang et al. | New optimal smoothing scheme for improving relative and absolute accuracy of tightly coupled GNSS/SINS integration | |
| CN110006427B (zh) | 一种低动态高振动环境下的bds/ins紧组合导航方法 | |
| WO2017215026A1 (zh) | 一种基于高度约束的扩展卡尔曼滤波定位方法 | |
| CN110567455B (zh) | 一种求积更新容积卡尔曼滤波的紧组合导航方法 | |
| CN107607977B (zh) | 一种基于最小偏度单形采样的自适应ukf组合导航方法 | |
| CN110308467A (zh) | 一种基于Zynq-7020的超紧耦合微系统及方法 | |
| Wendel et al. | Time-differenced carrier phase measurements for tightly coupled GPS/INS integration | |
| CN112629526B (zh) | 一种北斗精密单点定位和惯性系统的紧组合导航方法 | |
| Xu et al. | An acoustic ranging measurement aided SINS/DVL integrated navigation algorithm based on multivehicle cooperative correction | |
| Nagui et al. | Improved GPS/IMU loosely coupled integration scheme using two kalman filter-based cascaded stages | |
| CN111854741A (zh) | 一种gnss/ins紧组合滤波器及导航方法 | |
| Geng et al. | Hybrid derivative-free EKF for USBL/INS tightly-coupled integration in AUV | |
| Liu et al. | An improved GNSS/INS navigation method based on cubature Kalman filter for occluded environment | |
| CN112525204B (zh) | 一种航天器惯性和太阳多普勒速度组合导航方法 | |
| CN114323007A (zh) | 一种载体运动状态估计方法及装置 | |
| CN114167472A (zh) | Ins辅助gnss ppp精密动态导航定位方法及系统 |
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 |