CN114396941B - 一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法 - Google Patents
一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法 Download PDFInfo
- Publication number
- CN114396941B CN114396941B CN202111562355.6A CN202111562355A CN114396941B CN 114396941 B CN114396941 B CN 114396941B CN 202111562355 A CN202111562355 A CN 202111562355A CN 114396941 B CN114396941 B CN 114396941B
- Authority
- CN
- China
- Prior art keywords
- satellite
- carrier
- error
- noise
- code
- 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 45
- 238000000034 method Methods 0.000 title claims abstract description 31
- 238000005259 measurement Methods 0.000 claims abstract description 57
- 239000011159 matrix material Substances 0.000 claims abstract description 38
- 238000001514 detection method Methods 0.000 claims abstract description 23
- 230000008569 process Effects 0.000 claims abstract description 9
- 238000000926 separation method Methods 0.000 claims abstract description 7
- 230000010354 integration Effects 0.000 claims abstract description 6
- 239000013598 vector Substances 0.000 claims description 25
- 238000011045 prefiltration Methods 0.000 claims description 16
- 230000008859 change Effects 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 9
- 238000012886 linear function Methods 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 7
- 230000002159 abnormal effect Effects 0.000 claims description 6
- 230000004044 response Effects 0.000 claims description 6
- 230000001427 coherent effect Effects 0.000 claims description 5
- 238000013461 design Methods 0.000 claims description 4
- 238000005311 autocorrelation function Methods 0.000 claims description 3
- 230000005284 excitation Effects 0.000 claims description 3
- 238000005562 fading Methods 0.000 claims description 3
- 230000014509 gene expression Effects 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 230000003595 spectral effect Effects 0.000 claims description 3
- 238000012795 verification Methods 0.000 claims 2
- 230000003044 adaptive effect Effects 0.000 abstract description 2
- 238000012937 correction Methods 0.000 description 3
- 238000012546 transfer Methods 0.000 description 3
- 238000009499 grossing Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
Classifications
-
- 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
-
- 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/24—Acquisition or tracking or demodulation of signals transmitted by the system
- G01S19/25—Acquisition or tracking or demodulation of signals transmitted by the system involving aiding data received from a cooperating element, e.g. assisted GPS
- G01S19/256—Acquisition or tracking or demodulation of signals transmitted by the system involving aiding data received from a cooperating element, e.g. assisted GPS relating to timing, e.g. time of week, code phase, timing offset
-
- 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/24—Acquisition or tracking or demodulation of signals transmitted by the system
- G01S19/27—Acquisition or tracking or demodulation of signals transmitted by the system creating, predicting or correcting ephemeris or almanac data within the receiver
-
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
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)
- Power Engineering (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明公开了一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法，来解决卫星信号受到周围复杂电磁环境干扰难以准确定位的问题。级联式惯性/卫星超紧组合方法将输入中频信号经过载波和伪码分离、相关积分、预滤波和组合导航滤波过程，将定位信息反馈到载波NCO和码NCO。相比于传统的深组合方法，本发明中预滤波过程采用自适应强跟踪卡尔曼滤波（ASTKF），量测噪声协方差矩阵则根据各个通道的载噪比自适应地调整，同时引入基于平滑窗口的x2故障检测方法。组合导航滤波结合惯性、卫星信息，将定位结算结果反馈到载波NCO和码NCO。本发明的组合导航方法在噪声环境发生变化或者弱信号条件下自适应能力增强，提高了组合导航抗干扰能力和定位精度。
Description
技术领域
本发明属于惯性/卫星组合导航技术领域，具体涉及一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法。
背景技术
随着科技技术的发展，需要导航定位的应用场景越来越多，对定位的精度和鲁棒性要求越来越高，不仅局限于信号良好、视野开阔的场景，还可能处于一些复杂的电磁环境中，多变的气候，建筑物遮挡、电磁干扰都可能导致载体难以接受到稳定的外源信息，导航定位进度下降，将影响载体的任务完成情况，因此，如何实现导航定位系统在复杂电磁环境下的高精度导航与定位是一个值得研究的问题。
全球卫星导航系统(Global Navigation Satellite System，GNSS)作为一种空间上的无线定位系统，为用户实时提供三维位置、速度和时间信息的精确信息，但是卫星接收机天线容易受到周围复杂电磁环境的干扰导致定位精度恶化甚至无法定位。捷联惯导系统(SINS)具有自主性、隐蔽性等特点，能够连续提供位置、速度、姿态信息，常用于与GNSS进行组合保证卫星拒止条件下的导航定位精度，但是在缺乏外源信息校正的情况下捷联惯导系统的定位误差将随着时间积累。
因此，设计一种能够解决传统惯性/卫星组合导航方法在强电磁干扰下定位精度差的问题是十分必要的。
发明内容
为解决上述问题，本发明公开了一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法，建立基于强跟踪Kalman滤波的预滤波算法，引入基于平滑窗口的的χ2故障检测模型，排除故障量测值，同时基于各通道载噪比值自适应的调整量测方差，建立惯性/卫星紧组合导航算法，用组合定位结果复现本地码相位和载波频率反馈到码NCO和载波NCO。本发明有效提高了卫星接收机跟踪环路抗干扰能力，最终提高了导航定位的精度。
为达到上述目的，本发明的技术方案如下：
一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法，包括以下步骤：
(1)推导卫星导航接收机跟踪环路实现步骤，引入强跟踪Kalman滤波(STKF)，对码相位误差、载波频率误差进行预滤波处理；
(2)推导基于平滑窗口的χ2故障检测模型和基于载噪比的量测噪声计算方程。并应用于步骤(1)模型；
(3)由步骤(1)的预滤波输出和捷联惯导系统接收的数据信息推导惯性/卫星紧组合导航模型；
(4)在每次步骤(3)滤波结束后对系统误差状态进行校正，输出校正后的组合导航的姿态、速度、位置等信息并将其反馈到载波NCO和码NCO。
进一步地，所述步骤(1)中，推导卫星导航接收机跟踪环路实现步骤，引入强跟踪Kalman滤波。卫星信号跟踪过程需要解决的核心问题是对码相位和载波频率的精确跟踪，卫星信号经过射频前端处理后得到中频信号，中频信号在完成载波分离和伪码分离后产生同相I支路信号和正交相Q支路信号，I支路信号和Q支路信号的数学表达式如下：
其中，A是GPS信号幅度；N是相关器中的信号采样点数量；R(·)是伪随机码的自相关函数；δτ是本地码相位误差；δω是本地载波的角频率误差；T是相干积分时间；是本地载波相位误差平均值，其计算公式可以表示为：
设超前滞后相关器超前码、滞后码与即时码的间隔为0.5码片，则码相位误差δτ的计算公式为：
其中IE、QE、IL、QL分别为超前码、滞后码相关积分输出的I、Q分量。以上运算通过鉴相器完成。
卫星跟踪环路预滤波器的状态方程和量测方程为：
其中，φk,k-1为tk-1时刻到tk时刻的一步状态状态转移矩阵；Xk为状态向量；Γk-1为系统噪声矩阵；Wk-1为系统激励噪声序列,其对应方差为Qk；Zk为量测向量；Vk为量测噪声序列，其对应方差为Rk。kalman滤波更新方程如下：
Pk＝(I-KkHk)Pk,k-1
其中Pk为状态均方误差，Kk为滤波增益矩阵。在实际工程中，为了便于处理，会对卡尔曼滤波的观测、状态模型进行简化，且实际工程中我们很难得到准确的噪声统计特性，因此上述模型存在不确定性，为了解决这个问题，此处以强跟踪Kalman滤波进行卫星跟踪环路设计。强跟踪成立需要满足的条件为，选择一个合适的增益矩阵Kk使得
E[(γk+j)γk T]＝0,k＝0,1,2...,j＝1,2,...
其中γk＝Zk-HXk,k-1为观测量残差。我们可以对增益矩阵Kk进行调整，强迫残差序列保持正交，使上式始终成立，从而达到强迫滤波器对系统实际状态保持跟踪的目的。将一步预测方差矩阵改为
其中，λk为时变渐消因子，
其中，0<ρ≤1为遗忘因子，一般取ρ＝0.95；为量测方差估计。
选取卫星中频信号的和码相位误差δτ、载波相位误差载波角频率误差δω和载波角频率变化率误差δa作为STKF的状态向量，载波相位可视为载波频率的一次函数，载波频率可视为载波频率变化率的一次函数，载波频率变化率可模型化为缓慢变化的偏差，码相位误差也可视为载波频率的一次函数，具体形式如下：
其中，对于GPS L1 C/A信号来说，α＝β＝1/1540。wmp、wclock、wdrift、wacc分别为各个状态量对应的过程噪声，其协方差矩阵如下：
其中c为光速，qb、qd、qa分别为载波相位、载波频率、载波频率变化率的噪声功率谱密度，大小与接收机射频前端振荡器噪声特性有关。由于STKF是一个线性滤波器，可以将鉴相器输出的码相位和载波相位作为观测量，则观测方程如下：
进一步地，所述步骤(2)中，推导基于平滑窗口的χ2故障检测模型和基于载噪比的量测噪声计算方程，具体如下：
量测方差可以近似表示为长度为μ的滑动窗口内各个历元新息方差估计的平均值，
窗口宽度μ不宜设置过大，否则会产生较大的时延，故障检测能力将变弱，一般取3～5适宜。定义为故障检测函数，则εk服从自由度为m的χ2分布即
εk～χ2(m)
式中，m为量测向量的维度。当卫星伪距/伪距率量测信息异常时，故障检测函数不再服从上述分布，可以通过χ2分布上分为点进行故障检测，上分为点与量测维度有关，例如，当量测维度m＝6时，也就是说在量测信息正常时，认为/>的概率只有0.5％，而当m＝3时，/>该故障检测函数能够准确判断量测信息是否出现异常。
量测噪声主要来源于跟踪环路的相位抖动和动态响应误差，因此可以通过各个卫星通道的载噪比值来计算当前时刻量测噪声：
其中，载噪比可通过窄带宽带功率比方法求得。
进一步地，所述步骤(3)中，由步骤(1)的预滤波输出和捷联惯导系统接收的数据信息推导惯性/卫星紧组合导航模型，具体如下：
将预滤波器输出的码相位和载波频率经过一定的运算之后可以得到各个卫星通道的伪距、伪距率信息，由卫星的广播星历文件可以计算得到卫星在地心地固坐标系(ECEF)下的位置速度信息，结合捷联惯导系统接收的包括三轴陀螺仪的角增量或角速度数据和三轴加速度计的比力或比力积分增量数据可以算得另一组伪距、伪距率信息，二者作差作为紧组合的量测值。
组合导航滤波的状态方程为：
其中，FTC为状态转移矩阵，GTC为系统噪声分配矩阵，WTC为系统噪声，XTC为状态变量，表示为：
XTC＝[(φn)T (δvn)T (δpn)T (εb)T (△b)T cδtu cδtru]T
其中φn为姿态误差角向量，δvn、δpn为东北天坐标系(ENU)下的速度误差向量、位置误差向量，εb、△b分别是陀螺仪和加速度计的常值零偏，cδtu、cδtru分别是卫星导航接收机钟差等效距离误差和钟漂等效距离误差；
WTC＝[(ωbg)T (ωba)T ωtu ωtru]T
其中ωbg、ωba、ωtu、ωtru分别表示陀螺零偏噪声、加速度计零偏噪声、钟差等效距离误差的白噪声、钟漂等效距离误差的白噪声；
量测方程为ZTC＝HTCXTC+VTC，其中量测值ZTC为卫星接收机输出伪距、伪距率数据和SINS推出的各个卫星相应的伪距、伪距率数据之差，HTC为量测更新矩阵，VTC为量测噪声。
量测噪声方差的大小可以由预滤波器的状态噪声方差来确定，即伪距、伪距率误差方差可以通过各个通道的预滤波器中码相位误差、载波频率误差对应的状态方差量来调整：
Rρ,TC＝Pτ·(λcarr/α)2
其中，Rρ,TC、分别为伪距量测量、伪距率量测量对应的噪声方差矩阵，Pτ、/>为对应卫星通道预滤波器码相位误差和载波频率误差状态方差矩阵，λcarr为载波波长，对于GPS L1 C/A信号来说，α＝1/1540。
进一步地，所述步骤(4)中，对系统误差状态进行校正，输出校正后的组合导航的姿态、速度、位置等信息并将其反馈到载波NCO和码NCO，具体如下：
在步骤(4)一个组合导航滤波周期结束后得到各误差量的估计值，并用估计值对导航参数进行补偿，得到各导航参数的“精确值”，补偿公式为：
位置：
速度：
钟差：
钟漂：
码相位：
载波频率：
其中aj,k从接收机指向各通道卫星方向的单位矢量。通过更新的位置速度信息复现的本地码相位和载波频率反馈到码NCO和载波NCO，就构成了闭合的跟踪环路，同时输出的位置速度信息也能够在复杂电磁干扰环境下实现高精度。
本发明的有益效果为：
本发明所述的一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法，首先推导卫星导航接收机跟踪环路实现步骤，引入自适应强跟踪Kalman滤波(ASTKF)，对码相位误差、载波相位误差进行预滤波处理；其次，推导基于平滑窗口的χ2故障检测模型和基于载噪比的量测噪声计算方程，并应用于与滤波算法中；然后由上述预滤波地输出和捷联惯导系统接收的数据信息推导惯性/卫星紧组合导航模型；最后，在每个组合导航滤波周期结束后对系统误差状态进行校正，输出校正后的组合导航的姿态、速度、位置等信息并将其反馈到载波NCO和码NCO，从而提高卫星接收机在复杂电磁环境下的定位精度。
附图说明
图1为本发明基于强跟踪Kalman滤波的级联式惯性/卫星深组合流程图；
图2为惯导辅助卫星跟踪环路传递函数框图；
图3为惯导辅助与无辅助的卫星跟踪环路传递函数幅频响应曲线比较图。
具体实施方式
下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
如图1所示，本发明实施提出的一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合，用自适应地强跟踪Kalman滤波对卫星接收机跟踪环路中鉴相器输出的码相位、载波相位等进行预滤波，滤波过程中引入基于平滑窗口的χ2故障检测算法，将与滤波的结果和惯导定位信息结合得到最终的位置速度修正量，修正量反馈到载波NCO、码NCO构成回路。图2和图3显示惯导辅助锁相环的等效传递函数模型，以及幅频响应曲线，可以定性地判断惯导的辅助能够有效提高卫星跟踪锁相环的动态响应特性和噪声抑制能力。下面进行详细的分析：
步骤1：推导卫星导航接收机跟踪环路实现步骤，引入强跟踪Kalman滤波作为预滤波器。
卫星信号跟踪过程需要解决的核心问题是对码相位和载波频率的精确跟踪。GPS信号在L1波段可按如下形式表示：
其中，s(t)是波段上的GPS信号；Pc是C/A码信号的功率；PPL1是L1波段上P码的信号功率，C(t)是C/A码的相位；D(t)表示数据码；P(t)是P码相位；fL1是L1波段的频率。
卫星信号经过射频前端处理后得到中频信号，中频信号在完成载波分离和伪码分离后产生同相I支路信号和正交相Q支路信号，I支路信号和Q支路信号的数学表达式如下：
其中，A是GPS信号幅度；N是相关器中的信号采样点数量；R(·)是伪随机码的自相关函数；δτ是本地码相位误差；δω是本地载波的角频率误差；T是相干积分时间；是本地载波相位误差平均值，由上述公式设计鉴相器或鉴频器可以算得载波相位误差或码相位误差。载波环中使用的鉴相器和鉴频器主要有以下几种，其中比较常用的是两象限的反正切函数arctan[Q(t)/I(t)]。
其中cross＝I(t1)×Q(t2)-I(t2)×Q(t1),dot＝I(t1)×Q(t2)+I(t2)×Q(t1)。
码相位误差则通过超前码、滞后码算得。由伪码生成器生成三个移位版本的本地伪码，分别是超前、滞后和即时码，相干间隔为d/2个码片，超前和滞后码的四路相关积分输出分别如下：
设超前滞后相关器超前码、滞后码与即时码的间隔为0.5码片，采用非相干超前滞后包络法计算码相位误差δτ公式如下：
其中IE、QE、IL、QL分别为超前码、滞后码相关积分输出的I、Q分量。
卫星跟踪环路预滤波器的状态方程和量测方程为：
其中，φk,k-1为tk-1时刻到tk时刻的一步状态状态转移矩阵；Xk为状态向量；Γk-1为系统噪声矩阵；Wk-1为系统激励噪声序列,其对应方差为Qk；Zk为量测向量；Vk为量测噪声序列，其对应方差为Rk。kalman滤波更新方程如下：
Pk＝(I-KkHk)Pk,k-1
其中Pk为状态均方误差，Kk为滤波增益矩阵。在实际工程中，为了便于处理，会对卡尔曼滤波的观测、状态模型进行简化，且实际工程中我们很难得到准确的噪声统计特性，因此上述模型存在不确定性，为了解决这个问题，此处以强跟踪Kalman滤波进行卫星跟踪环路设计。强跟踪成立需要满足的条件为，选择一个合适的增益矩阵Kk使得
E[(γk+j)γk T]＝0,k＝0,1,2...,j＝1,2,...
其中γk＝Zk-HXk,k-1为观测量残差。我们可以对增益矩阵Kk进行调整，强迫残差序列保持正交，使上式始终成立，从而达到强迫滤波器对系统实际状态保持跟踪的目的。将一步预测方差矩阵改为
其中，λk为时变渐消因子，
其中，0<ρ≤1为遗忘因子，一般取ρ＝0.95；为量测方差估计。
选取卫星中频信号的和码相位误差δτ、载波相位误差载波角频率误差δω和载波角频率变化率误差δa作为STKF的状态向量，载波相位可视为载波频率的一次函数，载波频率可视为载波频率变化率的一次函数，载波频率变化率可模型化为缓慢变化的偏差，码相位误差也可视为载波频率的一次函数，具体形式如下：
其中，对于GPS L1 C/A信号来说，α＝β＝1/1540。wmp、wclock、wdrift、wacc分别为各个状态量对应的过程噪声，其协方差矩阵如下：
其中c为光速，qb、qd、qa分别为载波相位、载波频率、载波频率变化率的噪声功率谱密度，大小与接收机射频前端振荡器噪声特性有关。由于STKF是一个线性滤波器，可以将鉴相器输出的码相位和载波相位作为观测量，则观测方程如下：
步骤2：推导基于平滑窗口的χ2故障检测模型和基于载噪比的量测噪声计算方程，具体如下：
量测方差可以近似表示为长度为μ的滑动窗口内各个历元新息方差估计的平均值，
窗口宽度μ不宜设置过大，否则会产生较大的时延，故障检测能力将变弱，一般取3～5适宜。定义为故障检测函数，则εk服从自由度为m的χ2分布即
εk～χ2(m)
式中，m为量测向量的维度。当卫星伪距/伪距率量测信息异常时，故障检测函数不再服从上述分布，可以通过χ2分布上分为点进行故障检测，上分为点与量测维度有关，例如，当量测维度m＝6时，也就是说在量测信息正常时，认为/>的概率只有0.5％，而当m＝3时，/>该故障检测函数能够准确判断量测信息是否出现异常。
量测噪声主要来源于跟踪环路的相位抖动和动态响应误差，因此可以通过各个卫星通道的载噪比值来计算当前时刻量测噪声：
其中，载噪比可通过窄带宽带功率比方法求得：
其中WBP、NBP分别为窄带信号能量和宽带信号能量，T1为相干积分时间。
步骤3：由步骤(1)的预滤波输出和捷联惯导系统接收的数据信息推导惯性/卫星紧组合导航模型，具体如下：
将预滤波器输出的码相位和载波频率经过一定的运算之后可以得到各个卫星通道的伪距、伪距率信息，由卫星的广播星历文件可以计算得到卫星在地心地固坐标系(ECEF)下的位置速度信息，结合捷联惯导系统接收的包括三轴陀螺仪的角增量或角速度数据和三轴加速度计的比力或比力积分增量数据可以算得另一组伪距、伪距率信息，二者作差作为紧组合的量测值。
组合导航滤波的状态方程为：
其中，FTC为状态转移矩阵，GTC为系统噪声分配矩阵，WTC为系统噪声，XTC为状态变量，表示为：
XTC＝[(φn)T (δvn)T (δpn)T (εb)T (△b)T cδtu cδtru]T
其中φn为姿态误差角向量，δvn、δpn为东北天坐标系(ENU)下的速度误差向量、位置误差向量，εb、△b分别是陀螺仪和加速度计的常值零偏，cδtu、cδtru分别是卫星导航接收机钟差等效距离误差和钟漂等效距离误差；
WTC＝[(ωbg)T (ωba)T ωtu ωtru]T
其中ωbg、ωba、ωtu、ωtru分别表示陀螺零偏噪声、加速度计零偏噪声、钟差等效距离误差的白噪声、钟漂等效距离误差的白噪声；
其中，/>
Fap＝F1+F2
Fvp＝(vn×)(2F1+F2)
量测方程为ZTC＝HTCXTC+VTC，其中量测值ZTC为卫星接收机输出伪距、伪距率数据和SINS推出的各个卫星相应的伪距、伪距率数据之差，HTC为量测更新矩阵，VTC为量测噪声。
量测更新矩阵为：
其中a为从接收机指向各通道卫星方向的单位矢量。T1、T2分别为从ENU系到ECEF系位置、速度的转换矩阵。
量测噪声方差的大小可以由预滤波器的状态噪声方差来确定，即伪距、伪距率误差方差可以通过各个通道的预滤波器中码相位误差、载波频率误差对应的状态方差量来调整：
Rρ,TC＝Pτ·(λcarr/α)2
其中，Rρ,TC、分别为伪距量测量、伪距率量测量对应的噪声方差矩阵，Pτ、Pfcarr为对应卫星通道预滤波器码相位误差和载波频率误差状态方差矩阵，λcarr为载波波长，对于GPS L1 C/A信号来说，α＝1/1540。
步骤4：对系统误差状态进行校正，输出校正后的组合导航的姿态、速度、位置等信息并将其反馈到载波NCO和码NCO，具体如下：
在步骤(3)一个组合导航滤波周期结束后得到各误差量的估计值，并用估计值对导航参数进行补偿，得到各导航参数的“精确值”，补偿公式为：
位置：
速度：
钟差：
钟漂：
码相位：
载波频率：
其中aj,k从接收机指向各通道卫星方向的单位矢量。通过更新的位置速度信息复现的本地码相位和载波频率反馈到码NCO和载波NCO，就构成了闭合的跟踪环路，同时输出的位置速度信息也能够在复杂电磁干扰环境下实现高精度。
Claims (1)
1.一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法，其特征在于：包括惯性/卫星深组合演示验证平台，具体步骤如下：
(1)推导卫星导航接收机跟踪环路实现步骤，引入强跟踪Kalman滤波，对码相位误差、载波相位误差进行预滤波处理；
具体如下：
卫星信号跟踪过程需要解决的核心问题是对码相位和载波频率的精确跟踪，卫星信号经过射频前端处理后得到中频信号，中频信号在完成载波分离和伪码分离后产生同相I支路信号和正交相Q支路信号，I支路信号和Q支路信号的数学表达式如下：
其中，A是GPS信号幅度；N是相关器中的信号采样点数量；R(·)是伪随机码的自相关函数；δτ是本地码相位误差；δω是本地载波的角频率误差；T是相干积分时间；是本地载波相位误差平均值，其计算公式表示为：
设超前滞后相关器超前码、滞后码与即时码的间隔为0.5码片，则码相位误差δτ的计算公式为：
其中IE、QE、IL、QL分别为超前码、滞后码相关积分输出的I、Q分量，以上运算通过鉴相器完成，
卫星跟踪环路预滤波器的状态方程和量测方程为：
其中，φk,k-1为tk-1时刻到tk时刻的一步状态状态转移矩阵；Xk为状态向量；Γk-1为系统噪声矩阵；Wk-1为系统激励噪声序列,其对应方差为Qk；Zk为量测向量；Vk为量测噪声序列，其对应方差为Rk，kalman滤波更新方程如下：
Pk＝(I-KkHk)Pk,k-1
其中Pk为状态均方误差，Kk为滤波增益矩阵；在实际工程中，为了便于处理，会对卡尔曼滤波的观测、状态模型进行简化，且实际工程中我们很难得到准确的噪声统计特性，因此上述模型存在不确定性，为了解决这个问题，此处以强跟踪Kalman滤波进行卫星跟踪环路设计，强跟踪成立需要满足的条件为，选择一个合适的增益矩阵Kk使得
E[(γk+j)γk T]＝0,k＝0,1,2...,j＝1,2,...
其中γk＝Zk-HXk,k-1为观测量残差，我们对增益矩阵Kk进行调整，强迫残差序列保持正交，使上式始终成立，从而达到强迫滤波器对系统实际状态保持跟踪的目的，将一步预测方差矩阵改为
其中，λk为时变渐消因子，
其中，0<ρ≤1为遗忘因子，取ρ＝0.95；为量测方差估计，
选取卫星中频信号的和码相位误差δτ、载波相位误差载波角频率误差δω和载波角频率变化率误差δa作为STKF的状态向量，载波相位可视为载波频率的一次函数，载波频率可视为载波频率变化率的一次函数，载波频率变化率可模型化为缓慢变化的偏差，码相位误差也可视为载波频率的一次函数，具体形式如下：
其中，对于GPS L1 C/A信号来说，α＝β＝1/1540，wmp、wclock、wdrift、wacc分别为各个状态量对应的过程噪声，其协方差矩阵如下：
其中c为光速，ωrf为载波频率，qb、qd、qa分别为载波相位、载波频率、载波频率变化率的噪声功率谱密度，大小与接收机射频前端振荡器噪声特性有关，由于STKF是一个线性滤波器，将鉴相器输出的码相位和载波相位作为观测量，则观测方程如下：
(2)推导基于平滑窗口的χ2故障检测模型和基于载噪比的量测噪声计算方程，并应用于步骤(1)里算法形成的模型；具体如下：
量测方差近似表示为长度为μ的滑动窗口内各个历元新息方差估计的平均值，
窗口宽度μ不宜设置过大，否则会产生较大的时延，故障检测能力将变弱，为3～5；定义为故障检测函数，则εk服从自由度为m的χ2分布即
εk～χ2(m)
式中，m为量测向量的维度，当卫星伪距/伪距率量测信息异常时，故障检测函数不再服从上述分布，通过χ2分布上分为点进行故障检测，上分为点与量测维度有关，当量测维度m＝6时，也就是说在量测信息正常时，认为/>的概率只有0.5％，该故障检测函数能够准确判断量测信息是否出现异常，
量测噪声主要来源于跟踪环路的相位抖动和动态响应误差，通过各个卫星通道的载噪比值来计算当前时刻量测噪声：
其中，载噪比通过窄带宽带功率比方法求得；
(3)由步骤(1)的预滤波输出和捷联惯导系统接收的数据信息推导惯性/卫星紧组合导航模型；具体如下：
将预滤波器输出的码相位和载波频率经过一定的运算之后得到各个卫星通道的伪距、伪距率信息，由卫星的广播星历文件计算得到卫星在地心地固坐标系下的位置速度信息，结合捷联惯导系统接收的包括三轴陀螺仪的角增量或角速度数据和三轴加速度计的比力或比力积分增量数据算得另一组伪距、伪距率信息，二者作差作为紧组合的量测值，
组合导航滤波的状态方程为：
其中，FTC为状态转移矩阵，GTC为系统噪声分配矩阵，WTC为系统噪声，XTC为状态变量，表示为：
XTC＝[(φn)T (δvn)T (δpn)T (εb)T (Δb)T cδtu cδtru]T
其中φn为姿态误差角向量，δvn、δpn为东北天坐标系(ENU)下的速度误差向量、位置误差向量，εb、Δb分别是陀螺仪和加速度计的常值零偏，cδtu、cδtru分别是卫星导航接收机钟差等效距离误差和钟漂等效距离误差；
WTC＝[(ωbg)T (ωba)T ωtu ωtru]T
其中ωbg、ωba、ωtu、ωtru分别表示陀螺零偏噪声、加速度计零偏噪声、钟差等效距离误差的白噪声、钟漂等效距离误差的白噪声；
量测方程为ZTC＝HTCXTC+VTC，其中量测值ZTC为卫星接收机输出伪距、伪距率数据和SINS推出的各个卫星相应的伪距、伪距率数据之差，HTC为量测更新矩阵，VTC为量测噪声，
量测噪声方差的大小由预滤波器的状态噪声方差来确定，即伪距、伪距率误差方差通过各个通道的预滤波器中码相位误差、载波频率误差对应的状态方差量来调整：
Rρ,TC＝Pτ·(λcarr/α)2
其中，Rρ,TC、分别为伪距量测量、伪距率量测量对应的噪声方差矩阵，Pτ、/>为对应卫星通道预滤波器码相位误差和载波频率误差状态方差矩阵，λcarr为载波波长，对于GPSL1 C/A信号来说，α＝1/1540；
(4)在每次步骤(3)滤波周期结束后对系统误差状态进行校正，输出校正后的组合导航的姿态、速度、位置信息并将其反馈到载波NCO和码NCO；
具体如下：
在步骤(4)一个组合导航滤波周期结束后得到各误差量的估计值，并用估计值对导航参数进行补偿，得到各导航参数的“精确值”，补偿公式为：
位置：
速度：
钟差：
钟漂：
码相位：
载波频率：
其中aj,k从接收机指向各通道卫星方向的单位矢量，通过更新的位置速度信息复现的本地码相位和载波频率反馈到码NCO和载波NCO，就构成了闭合的跟踪环路，同时输出的位置速度信息也能够在复杂电磁干扰环境下实现高精度。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111562355.6A CN114396941B (zh) | 2021-12-20 | 2021-12-20 | 一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111562355.6A CN114396941B (zh) | 2021-12-20 | 2021-12-20 | 一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114396941A CN114396941A (zh) | 2022-04-26 |
| CN114396941B true CN114396941B (zh) | 2023-12-19 |
Family
ID=81227951
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111562355.6A Active CN114396941B (zh) | 2021-12-20 | 2021-12-20 | 一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114396941B (zh) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115342820A (zh) * | 2022-10-18 | 2022-11-15 | 深圳市诚王创硕科技有限公司 | 一种汽车夜间行驶导航定位系统 |
| CN116774263B (zh) * | 2023-06-12 | 2024-04-05 | 同济大学 | 面向组合导航系统的导航定位方法及装置 |
| CN116577808B (zh) * | 2023-07-11 | 2023-09-29 | 中国人民解放军战略支援部队航天工程大学 | 一种基于接收机相关器输出的导航欺骗干扰检测方法 |
| CN116625360B (zh) * | 2023-07-19 | 2023-09-29 | 苏州精源创智能科技有限公司 | 一种基于强跟踪模型的扫地机打滑识别和修正方法 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101666868A (zh) * | 2009-09-30 | 2010-03-10 | 北京航空航天大学 | 一种基于sins/gps深组合数据融合的卫星信号矢量跟踪方法 |
| CN106501832A (zh) * | 2016-12-16 | 2017-03-15 | 南京理工大学 | 一种容错矢量跟踪gnss/sins深组合导航方法 |
| WO2018014602A1 (zh) * | 2016-07-19 | 2018-01-25 | 东南大学 | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 |
| CN108761512A (zh) * | 2018-07-28 | 2018-11-06 | 南京理工大学 | 一种弹载bds/sins深组合自适应ckf滤波方法 |
| CN109000642A (zh) * | 2018-05-25 | 2018-12-14 | 哈尔滨工程大学 | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 |
| CN113253325A (zh) * | 2021-06-24 | 2021-08-13 | 中国人民解放军国防科技大学 | 惯性卫星序贯紧组合李群滤波方法 |
-
2021
- 2021-12-20 CN CN202111562355.6A patent/CN114396941B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101666868A (zh) * | 2009-09-30 | 2010-03-10 | 北京航空航天大学 | 一种基于sins/gps深组合数据融合的卫星信号矢量跟踪方法 |
| WO2018014602A1 (zh) * | 2016-07-19 | 2018-01-25 | 东南大学 | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 |
| CN106501832A (zh) * | 2016-12-16 | 2017-03-15 | 南京理工大学 | 一种容错矢量跟踪gnss/sins深组合导航方法 |
| CN109000642A (zh) * | 2018-05-25 | 2018-12-14 | 哈尔滨工程大学 | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 |
| CN108761512A (zh) * | 2018-07-28 | 2018-11-06 | 南京理工大学 | 一种弹载bds/sins深组合自适应ckf滤波方法 |
| CN113253325A (zh) * | 2021-06-24 | 2021-08-13 | 中国人民解放军国防科技大学 | 惯性卫星序贯紧组合李群滤波方法 |
Non-Patent Citations (1)
| Title |
|---|
| 基于矢量跟踪的SINS/GPS深组合导航方法;王新龙;于洁;;中国惯性技术学报(第06期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114396941A (zh) | 2022-04-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114396941B (zh) | 一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法 | |
| CN108415042B (zh) | 改善gnss接收机载波相位连续性的相位预测方法及系统 | |
| US8364401B2 (en) | Highly integrated GPS, Galileo and inertial navigation system | |
| CN110823217B (zh) | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 | |
| US6950059B2 (en) | Position estimation using a network of a global-positioning receivers | |
| CN107656300B (zh) | 基于北斗/gps双模软件接收机的卫星/惯性超紧组合方法 | |
| EP0986733B1 (en) | Robust accurate gps time reference for space application | |
| US20060161329A1 (en) | System and method for advanced tight coupling of GPS and inertial navigation sensors | |
| CN113359170A (zh) | 一种惯导辅助北斗单频动对动高精度相对定位方法 | |
| Wang et al. | An innovative scheme for SINS/GPS ultra-tight integration system with low-grade IMU | |
| Sun et al. | An improved adaptive unscented Kalman filter with application in the deeply integrated BDS/INS navigation system | |
| Skaloud | Reducing the GPS ambiguity search space by including inertial data | |
| US10534087B1 (en) | Differential vector phase locked loop GPS reception method | |
| Chi et al. | Enabling robust and accurate navigation for UAVs using real-time GNSS precise point positioning and IMU integration | |
| US20060015250A1 (en) | GPS navigation with integrated phase track filter | |
| Davari et al. | Multirate adaptive Kalman filter for marine integrated navigation system | |
| CN106199652A (zh) | 一种gps接收机的自适应矢量跟踪方法 | |
| CN114114360B (zh) | 基于多通道协同长时相干积分的gnss载波相位跟踪方法 | |
| CN114019543B (zh) | 用于改善伪距观测质量的弹性增强gnss伪码跟踪方法 | |
| CN115097508A (zh) | 一种带有多径误差估计器的卫星/惯性深耦合方法 | |
| CN117955554B (zh) | 基于预报拼接的低轨卫星实时钟差确定方法及系统 | |
| Lashley et al. | Impact of carrier to noise power density, platform dynamics, and IMU quality on deeply integrated navigation | |
| Banuelos et al. | Centralized Processing of multiple smartphone raw measurements with fixed and known position onboard a vehicle | |
| Wu et al. | Design and performance evaluation of an adaptive hybrid coherent and non-coherent GNSS vector tracking loop | |
| Li et al. | Performance analysis of the ultra-Tight GPS/INS integration based on an improved Kalman filter design for tracking loops |
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 |