第6章-组合导航方式、方法和滤波器设计

6.2松/紧组合系统状态方程和观测方程6.3GNSS/INS组合导航滤波处理第6章GNSS/INS组合导航方式、方法和滤波器设计6.1GNSS/INS组合导航不同方式及原理1.组合导航原理INSGNSSINS/GNSS组合导航系统完全自主抗干扰全面连续导航参数误差随时间积累实时高精度误差不积累易受干扰、遮挡，高动态下易失锁，周跳高精度高可靠性抗干扰高动态实时性一体化导弹舰船飞机卫星2.组合方式分类位置/速度组合观测量不同组合深度伪距/伪距率组合松组合紧组合超紧组合3.组合方式-松组合原理：以INS和GNSS二者分别输出的速度和位置信息的差值作为观测量，以INS误差方程作为系统方程，通过Kalman滤波对INS的速度、位置、姿态以及传感器的误差进行最优估计，并根据估计结果对INS进行输出或者反馈校正。4.组合方式-紧组合原理：由星历信息和INS输出的位置和速度信息，计算得到相应于INS位置的伪距、伪距率，将其与GNSS接收机测量的伪距和伪距速率相比较，它们的差值作为组合系统的观测量。通过Kalman滤波对INS的误差和GNSS接收机的误差进行最优估计，然后对INS进行输出或者反馈校正。5.组合方式-超紧组合原理：使用滤波技术对INS的误差进行最优估计，同时使用校正后INS的速度信息对接收机的载波环、码环进行辅助，从而减小环路的等效带宽，增加GNSS接收机在高动态或者强干扰环境下的跟踪能力。6.组合方式比较优点：不存在滤波级联；且解算采用原始信息，伪距/伪距率误差不相关；定位精度高于松组合优点：系统结构简单，易于实现，可提高系统导航精度优点：减小环路的等效带宽，提高GNSS接收机在高动态下/强干扰环境下的跟踪能力。松组合紧组合超紧组合缺点：级联滤波；观测量经数据处理得到，噪声时间相关；误差较大缺点：结构比较复杂，实现难度较大。6.2松/紧组合系统状态方程和观测方程6.3GNSS/INS组合导航滤波处理6.1GNSS/INS组合导航不同方式及原理第6章GNSS/INS组合导航方式、方法和滤波器设计6.2.1.状态方程简介下面将从四个方面介绍状态方程：1•状态量的选取2•连续状态方程和系数矩阵3•离散状态方程和状态转移矩阵4•系统驱动噪声方差阵1.状态量的选取松组合：观测量为位置误差、速度误差采用kalman算法滤波估计组合系统的状态量，下面为不同组合方式的状态量选取：下标e,n,u为东、北、天三个轴向；、、为东向、北向、天向平台误差角；、、为经度、纬度、高度位置误差；、、为东向、北向和天向速度误差。enuLhVeVnVu91TLVVhVenuenuX状态量的选取1.状态量的选取紧组合：观测量为伪距差、伪距率差下标e,n,u为东、北、天三个轴向；、、为东向、北向、天向平台误差角；、、为经度、纬度、高度位置误差；、、为东向、北向和天向速度误差。和为GNSS接收机的时钟的钟差和频差。enuLhVeVnVu111TLVVhVttenuenuuruXtutru状态量的选取2.连续状态方程系统状态方程描述了系统状态随时间的运动规律，连续状态方程为：()()()()ttttXFXW松组合系数矩阵F(t)是由捷联惯导的误差传播方程得到，可参见第四章。紧组合状态量为惯导误差参数部分的系数矩阵同松组合；状态量和的系数由以下方程得到：tutru=1=-+ftturuttruruw连续状态方程和系数矩阵3.离散状态方程离散状态方程为：离散状态方程和状态转移矩阵()(,1)(1)(1)kkkkkXΦXW其中，为组合滤波器的状态转移矩阵。F(t)为连续状态方程的系数矩阵，为离散状态方程的状态转移矩阵。二者存在以下关系：(,1)kkΦ(,1)kkΦ23()((k))((k))(,1)=(k)+...2!3!TAkTTkkeTFFΦIFT为组合滤波/预测的时间间隔。4.驱动噪声方程W(t)或者W(k)的方差阵反映的是各个状态量以及状态量间的驱动噪声误差大小。这里以紧组合11维状态量为例。驱动噪声方差阵gyrodrifterrorattitudeerroraccelerationdrifterrorvelocityerrorGNSSclockdrifterrorfrequencyerror4.驱动噪声方程驱动噪声方差阵222222200()[()()]00enuTenufNtEttQWW4.驱动噪声方程驱动噪声方差阵2k-1i12i1i1ii1......i(k)A(k),Q(t)TTTQMMMMAMMMT！1！2！其中，（）连续系统驱动噪声方差阵阶数的取舍可根据估计的精度以及计算机计算速度来判断。6.2.2.观测方程简介将从以下几个方面介绍观测方程：1•观测量的选取2•位置误差观测方程3•速度误差观测方程4•伪距误差观测方程5•伪距率误差观测方程1.观测量选取系统观测方程描述了观测量与状态量之间的关系，观测方程表达式为：()()()()ZkHkkVkX松组合对于松组合，观测量为接收机和惯导二者位置差、速度差。紧组合对于紧组合，观测量为接收机和惯导测量伪距差、伪距率差。观测量的选取及观测方程介绍V(k)为观测噪声，它的噪声方差阵R，反应了观测值的误差大小，也就是说观测信息的质量优劣。2.位置误差观测方程位置误差观测方程()()cos()cosIGMpIGNIGMnNePPuLLRRLhhRLNRLNhNZHXV从上式可以得到观测矩阵332239()0cos010pMNtdiagRRLH3.速度误差观测方程速度误差观测方程IeGeeeInGnnnIuGuuuMMMZHXV3539()01101tdiagH松组合观测方程松组合观测方程将位置观测式和速度观测式合并到一起，得到GNSS/SINS松组合位置、速度组合系统量测方程：()()()()()()()()()ppptttttttttZHVZXHXVZHV对于观测噪声方差阵，分别是根据GNSS接收机的伪距误差、伪距率误差和精度因子HDOP/HVOP得到。4.伪距误差观测方程伪距误差观测方程31223111()000jjjjjjHtaaaH()()()()ZtHtXtVt得到伪距量测阵H(t)式中()jGjIjZt5.伪距率误差观测方程伪距率误差观测方程()()()()ttttZHXV式中：()jGjIjZt5123111()00jjjjjtbbbHH伪距率观测矩阵：紧组合观测方程紧组合观测方程观测量由伪距差与伪距率差组成，形成多维观测矢量，组合系统的量测方程可以表达为：对于观测噪声方差阵，分别是根据GNSS接收机的测量伪距误差、伪距率误差得到。()()tXtHVZHXVHV6.2松/紧组合系统状态方程和观测方程6.3GNSS/INS组合导航滤波处理6.1GNSS/INS组合导航不同方式及原理第6章GNSS/INS组合导航方式、方法和滤波器设计滤波处理将从以下几个方面介绍组合导航滤波处理：1•滤波校正方式的选取2•滤波处理过程同步问题1.滤波校正方式滤波校正方式的选取以下为卡尔曼滤波的输出对原系统的校正方式：开环法或输出校正法闭环法或反馈校正法1.滤波校正方式（1）输出校正输出校正是用滤波估计值去校正原系统的输出数据，图为SINS与GNSS系统开环校正组合原理框图。优点：工程实现简单，不会影响原系统的工作。缺点：长时间工作条件下，SINS误差不再是小量，从而使滤波方程出现模型误差，导致滤波精度下降。1.滤波校正方式（2）反馈校正反馈校正是将滤波器的输出反馈回原系统，来校正其内部的状态参数，原理框图如下所示：优点：其误差始终保持为小量，不会产生滤波方程的模型误差。缺点：将滤波输出引入SINS内部，需改动SINS，增加了工程实现难度。2.滤波同步问题在组合导航系统中，涉及到两套子系统：GPS和惯导系统，那么在组合过程中就必然存在着二者如何进行同步的问题。(1)一般来说，组合系统是以GPS的更新时间为滤波周期；所以当GPS观测数据没有进行更新时，以INS的更新时间进行预测。当GPS出现丢星时，组合系统一直在进行预测过程。滤波同步2.滤波同步问题滤波同步(2)对于时间同步误差，通常滤波器需要根据时间误差进行状态预测，从而保证在GNSS数据更新时刻进行滤波更新。滤波结果Researchresultsinclude:(1)AboelmagdNoureldin,TashfeenB.Karamat,JacquesGeorgy.FundamentalsofInertialNavigation,Satellite-basedPositioningandtheirIntegration.Springer-VerlagBerlinHeidelberg2013(2)TheGPS/INSintegratedsystemdevelopedbyourresearchgroup滤波结果Tightlycoupledintegrationperformance---(1)Theaccuracyofthereferencesystemis2cmwiththeaidofRT-2GPSsoftwareand1.5mforsinglepoint(L1/L2)positioning.滤波结果Tightlycoupledintegrationperformance---(1)滤波结果Tightlycoupledintegrationperformance---(1)HG1700+GPS滤波结果Tightlycoupledintegrationperformance---(1)IMU300CC+GPS滤波结果Tightlycoupledintegrationperformance---(2)USBdatacollectorComputerRFAlgorithmsandanalysissoftwareMEMS-INSAntennaTheMEMS-inertialmeasurementunit(IMU)usedhereisStarNetoXW-5220.TherandomdriftofMEMSgyrois0.05(◦)/sandthatofMEMSaccelerometeris1mg.TheoutputfrequencyofMEMS-IMUis100Hz.TheGPSsoftwarereceiverisdevelopedbyourresearchgroup,andtheoutputfrequencyis25Hz.Itspositioningaccuracyis15m,andthevelocityaccuracyis0.5m/s.滤波结果Tightlycoupledintegrationperformance---(2)Errors(1-δ)GPSMEMS-INS/GPSEastposition(m)4.36463.1204Northposition(m)5.11114.4186Upposition(m)8.86626.0113Eastvelocity(m/s)0.13620.1345Northvelocity(m/s)0.18370.1783Upvelocity(m/s)0.31010.2522BeihangCampus-sta