CN104020480B - 一种带自适应因子的交互式多模型ukf的卫星导航方法 - Google Patents
一种带自适应因子的交互式多模型ukf的卫星导航方法 Download PDFInfo
- Publication number
- CN104020480B CN104020480B CN201410270532.7A CN201410270532A CN104020480B CN 104020480 B CN104020480 B CN 104020480B CN 201410270532 A CN201410270532 A CN 201410270532A CN 104020480 B CN104020480 B CN 104020480B
- Authority
- CN
- China
- Prior art keywords
- model
- time
- moment
- matrix
- representing
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 81
- 230000003044 adaptive effect Effects 0.000 title claims abstract description 36
- 230000002452 interceptive effect Effects 0.000 title claims abstract description 13
- 239000011159 matrix material Substances 0.000 claims abstract description 107
- 230000008569 process Effects 0.000 claims abstract description 40
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 30
- 238000009499 grossing Methods 0.000 claims abstract description 8
- 230000001133 acceleration Effects 0.000 claims description 43
- 238000001914 filtration Methods 0.000 claims description 42
- 238000013179 statistical model Methods 0.000 claims description 26
- 230000007704 transition Effects 0.000 claims description 17
- 238000004364 calculation method Methods 0.000 claims description 7
- 238000003491 array Methods 0.000 claims description 6
- 230000008859 change Effects 0.000 claims description 6
- 230000004927 fusion Effects 0.000 claims description 3
- 238000012886 linear function Methods 0.000 claims description 3
- 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 2
- 239000006185 dispersion Substances 0.000 claims description 2
- 238000010606 normalization Methods 0.000 claims description 2
- 238000012546 transfer Methods 0.000 claims description 2
- 238000005259 measurement Methods 0.000 description 6
- 239000002245 particle Substances 0.000 description 4
- 238000005070 sampling Methods 0.000 description 4
- 239000000969 carrier Substances 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000007476 Maximum Likelihood Methods 0.000 description 1
- 238000000342 Monte Carlo simulation Methods 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000001902 propagating effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000012360 testing method 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明涉及一种带自适应因子的交互式多模型UKF的卫星导航方法,属于导航控制领域。本发明方法首先利用残差序列的自协方差矩阵作为校正量,设计平滑滤波器将此校正量与陈旧自适应因子融合,得到一个新的自适应因子,并利用此自适应因子实时调整过程噪声协方差矩阵,有效地降低了***中噪声统计特性未知造成的定位误差。然后,采用交互式多模型算法(IMM)设置模型集M,并根据测量残差实时调整模型概率实现模型间的软切换,降低了***模型不精确造成的定位误差。本发明方法有效地改善了卫星导航***中复杂机动载体在模型和噪声统计特性未知的条件下的定位精度。
Description
技术领域
本发明涉及一种带自适应因子的交互式多模型UKF的卫星导航方法,属于导航控制领域。
背景技术
接收机定位算法是影响卫星导航***定位精度的关键。长期以来最小二乘法以其原理简单,易于实现等优点得到了广泛的应用。但是最小二乘法没有把不同时刻的估计值联系起来,定位结果显得分散、杂乱无章,这对导航算法的优化制造了困难。为克服最小二乘法在估计性能方面的不足,学者们将非线性滤波算法应用于导航领域。对于非线性***的滤波问题,由于需要处理无穷维的积分运算,在理论上很难找到严格的最优解,一般都是通过近似的方法来求解。当前非线性滤波方法主要分为三类,第一类是函数近似的方法,主要采用泰勒级数展开或插值多项式展开的方法对非线性函数进行近似,其代表为扩展卡尔曼滤波和插值滤波;第二类是确定性采样近似,它通过挑选一组sigma点作为样本,并通过非线性函数传播获得***的验后均值和方差。其典型代表是无迹卡尔曼滤波;第三类是随机采样近似,主要指基于蒙特卡洛方法的仿真,其代表是粒子滤波。在理论上粒子滤波是最优的,且适用于非高斯噪声的情况。但是粒子滤波存在粒子退化、计算量大、实时性差等问题。
扩展卡尔曼滤波(ExtendKalmanFilter,EKF)是函数近似方法的典型代表,也是应用最为广泛的滤波算法之一,在很多领域得到了广泛的应用。但是EKF采用一阶泰勒级数展开式近似线性化的方法会产生模型误差,导致滤波精度下降。为克服EKF在滤波精度方面的不足,学者们提出了无迹卡尔曼滤波(UnsignedKalmanFilter,UKF),成为第二类方法-确定性采样近似的典型方法之一。UKF挑选一组sigma点作为样本,并通过非线性函数传播获得***的验后均值和方差,避免了非线性***的线性化过程,其估计精度可以达到二阶泰勒级数展开的精度,而与之计算量相当的EKF只能达到一阶展开的精度。
但是EKF和UKF存在共同的不足即需要准确的***模型和噪声统计特性,对于很多***,这些条件都难以满足,基于此,学者们提出了自适应滤波算法。目前自适应滤波算法主要有噪声方差匹配法和模型匹配法。⑴噪声方差匹配法一般采用实时调整过程噪声协方差矩阵的方法实现,其中以Sage-Husa自适应算法为代表。Sage-Huse自适应滤波算法以其原理简单得到了一定的利用,但它对滤波初值比较敏感,且极易引起滤波发散,因此在应用时要进行滤波发散的抑制,导致算法繁琐,计算量大;A.H.Mohamed提出基于极大似然估计准则的自适应算法,该算法根据估计残差推导了Q阵和R阵的更新公式。HalilErsinSoken为矩阵Q增加自适应因子来实时调整矩阵Q的值,当估计精确时Q阵保持不变,当估计发生偏差时,由测量残差重新确定噪声矩阵的值。这两种方法实时性好,但都需要计算雅克比矩阵,增加了UKF的计算量。而且,这两种方法都完全依靠测量残差确定噪声协方差矩阵,估计稳定性弱;为增强以上算法的稳定性,学者们提出了主从式AUKF,它在从UKF中根据测量残差更新噪声协方差矩阵,并利用该方差矩阵去校正主UKF,这种算法稳定性较好但是计算量大;模糊自适应滤波算法以测量残差作为输入来制定模糊规则,实时调整Q阵的值,算法简单但灵活度差;汪秋婷在其博士论文中通过给过程噪声矩阵增加自适应因子来增加模型的精确性,原理简单,计算量小,但对测量残差的统计不够精确。⑵模型匹配法主要采用IMM算法,该算法设置模型集M,并根据估计残差实时调整模型概率,实现模型间的软切换。该方法解决了单一模型对载体运动状态描述不全面而导致的滤波精度低的问题。IMM算法最开始应用于目标跟踪,现在在导航领域也得到了越来越多的应用。实验证明,当***模型不确定时,该方法能明显提高导航***的定位精度,但该算法无法校正噪声统计特性未知造成的误差。
发明内容
本发明针对卫星导航***中复杂机动载体运动模型和噪声统计特性未知造成滤波精度低的问题提出了一种带自适应因子的交互式多模型UKF的卫星导航方法,有效地改善了卫星导航***中复杂机动载体在模型和噪声统计特性未知的条件下的定位精度。
本发明的目的是通过以下技术方案实现的。
一种带自适应因子的交互式多模型UKF的卫星导航方法,具体包括如下步骤:
步骤1:建立***模型
步骤1.1:建立基于全球定位***(GPS)的地面载体的状态方程。
为全面描述载体的运动状态和钟差漂移频率,用符号X表示全球定位***(GPS)状态方程的状态变量, 其中,x,分别代表世界大地坐标***(WorldGeodeticSystem一1984CoordinateSystem,WGS-84坐标系)下XT轴上的位置、速度和加速度;y,分别代表WGS-84坐标系下YT轴上的位置、速度和加速度;z,分别代表WGS-84坐标系下ZT轴上的位置、速度和加速度;δtu和分别代表载体接收机钟差和钟差漂移频率。建立模型集(用符号M表示),模型集M包含描述载体运动状态的当前统计模型(用符号CS表示)、常加速度模型(用符号CA表示)和常速转弯模型(用符号CT表示),即M=[CS,CA,CT]。接收机钟差变化率用一阶马尔科夫模型描述,当前统计模型CS的状态方程如公式(1)所示;常加速度模型CA的状态方程如公式(2)所示;常速转弯模型CT的状态方程如公式(3)所示。
其中,分别表示***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻的状态,其中i=1,2,3,即:表示***在当前统计模型CS下k时刻的状态;表示***在常加速度模型CA下k时刻的状态;表示***在常速转弯模型CT下k时刻的状态;k为正整数;Φi为当前统计模型CS、常加速度模型CA和常速转弯模型CT下的状态转移矩阵,Φi=diag(Φi,x,Φi,y,Φi,z,Φt,u),Φi,x表示WGS-84坐标系下XT轴上的状态转移矩阵;表示WGS-84坐标系下YT轴上的状态转移矩阵;Φi,z表示WGS-84坐标系下ZT轴上的状态转移矩阵;Φt,u表示描述地面载体接收机钟差和钟差漂移的状态转移矩阵;是***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下的过程噪声协方差矩阵自适应因子;是***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下的过程噪声矩阵。
用符号分别表示***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下(k-1)时刻的过程噪声协方差矩阵, E[·]表示期望值运算,表示WGS-84坐标系下描述XT轴上载体运动状态的过程噪声协方差矩阵;表示WGS-84坐标系下描述YT轴上载体运动状态的过程噪声协方差矩阵;表示WGS-84坐标系下描述ZT轴上载体运动状态的过程噪声协方差矩阵;表示描述地面载体接收机钟差和钟差漂移的过程噪声协方差矩阵;
步骤1.2:建立基于全球定位***(GPS)的地面载体的观测方程
采用伪距作为观测量建立观测方程。在k时刻,接收机观测到的GPS卫星数量用N表示,则建立此时刻CS模型,CA模型和CT模型的伪距观测方程如公式(4)所示。
其中,表示在k时刻观测到的第n颗GPS卫星的伪距,n=1,2…N,N表示k时刻观测到的GPS卫星的总数;表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下XT轴上的位置;表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下YT轴上的位置;表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下ZT轴上的位置;分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下XT轴上的位置;分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下YT轴上的位置;分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下ZT轴上的位置;表示k时刻地面载体接收机钟差;表示k时刻观测到的第n颗GPS卫星的观测误差。
为下文描述算法方便,建立CS模型,CA模型和CT模型的观测方程为公式(5)。
其中,,为***的观测量, 观测方程中的非线性函数,为***的观测误差。
用Rk表示公式(5)的观测误差的协方差矩阵,
步骤2:设置CS模型,CA模型和CT模型0时刻的滤波初始值,同时设置当前定位任务的结束时间。
在步骤1操作的基础上,分别设置CS模型、CA模型和CT模型的状态的估计初值,用符号和表示;然后设置CS模型、CA模型和CT模型的估计误差协方差矩阵初值,用符号和表示;并设置***运行的结束时间。
步骤3:得到CS模型,CA模型和CT模型在k时刻的滤波初始值。
使用步骤2或步骤4得到的CS模型、CA模型和CT模型第(k-1)时刻的状态的估计初值和,以及CS模型、CA模型和CT模型的估计误差协方差矩阵初值和,通过公式(6)和公式(7)进行交互混合,分别得到CS模型,CA模型和CT模型中k时刻的状态的估计初值,分别用 和表示,以及CS模型,CA模型和CT模型中k时刻的估计误差协方差矩阵初值,分别用和表示。
其中,分别表示CS模型,CA模型和CT模型在k时刻滤波的状态初值;j=1,2,3,分别代表CS模型、CA模型和CT模型;i=1,2,3,分别表示CS模型,CA模型和CT模型在k-1时刻状态的最优估计值;表示k-1时刻模型i和模型j的混合概率,πij为模型i到模型j的模型转移概率,其值由人为设定;为模型i在k时刻的概率,的初值由人为设定,并在以后各个时刻由步骤6更新;代表模型j的归一化常数,
其中,i=1,2,3,分别表示CS模型、CA模型和CT模型在k-1时刻滤波误差的方差矩阵。分别表示CS模型、CA模型和CT模型在k时刻滤波误差方差矩阵初值。
步骤4:对CS模型,CA模型和CT模型进行自适应无迹卡尔曼滤波(AdaptiveUnsignedKalmanFilter,AUKF),得到各模型在k时刻的滤波值。具体操作步骤为:
步骤4.1:用表示***状态的维数,即步骤1中得到的CS模型,CA模型和CT模型中状态方程的维数,通过公式(8)计算模型j在k时刻个初始西格马(sigma)点,其中第r个sigma点用用符号表示,且r为整数;
其中,表示平方根矩阵的第r列(或第r行)。
用公式(9)计算第一类权系数,该权系数用于求每个sigma点的一阶统计特性。模型j的第r个sigma点的第一类权系数用符号表示,即:表示CS模型的第r个sigma点第一类权系数,表示CA模型的第r个sigma点第一类权系数,表示CT模型的第r个sigma点第一类权系数;用公式(10)计算第二类权系数,该权系数用于求每个sigma点的二阶统计特性。模型j的第r个sigma点的第二类权系数用符号表示,即:表示CS模型的第r个sigma点第二类权系数,表示CA模型的第r个sigma点第二类权系数,表示CT模型的第r个sigma点第二类权系数;
其中,λ为一个复合刻度参数,其值通过公式(11)求解,参数α决定sigma点的散布程度,其值由人为设定,1e-4≤α≤1;参数β用来描述全球定位***状态方程的状态变量X的分布信息,在高斯情况下,β的最优值为2。
其中,参数κ取0。
步骤4.2:通过公式(12)至公式(14)进行时间更新。
其中,表示模型j在k时刻经过一步转移后的第r个西格马(sigma)点;Φj为模型j的状态转移矩阵。
其中,是模型j在k时刻的一步状态预测值;
其中,为模型j在k时刻的一步预测误差方差矩阵。表示模型j在(k-1)时刻的自适应矩阵,的初始值由人为设定。
步骤4.3:通过公式(15)-(17)进行观测更新
其中表示模型j在k时刻观测估计值,r表示第0个sigma点到第个sigma点的标识,h(·)为在步骤1中获取的观测方程中的非线性函数。
y为模型j在k时刻的时间更新误差协方差矩阵,为模型j在k时刻交叉误差协方差矩阵。
步骤4.4:通过公式(18)至公式(20)进行滤波更新
其中为模型j在k时刻的滤波增益矩阵。
为模型j中k时刻的状态估计值,为k时刻的该GPS***接收机的观测值,为已知量。
为k时刻模型j的估计误差方差矩阵。
步骤5:计算模型j在k时刻的过程噪声协方差矩阵阵的自适应矩阵
步骤5.1:用公式(21)至公式(23)计算模型j在k时刻的的估计偏差
其中为模型j在k时刻的残差序列。残差序列的协方差矩阵用表示,用公式(22)计算。
其中,s表示时刻从1到k的变化指示。
模型j在k时刻的估计偏差矩阵用符号表示,并利用公式(23)求取。
其中trace(·)表示对矩阵求迹。
步骤5.2:采用平滑滤波器求取模型j的过程噪声协方差矩阵的自适应因子
过程噪声协方差矩阵中起作用的主要为对角线上的值,因此设计自适应因子为对角矩阵,即
其中表示模型j在k时刻过程噪声协方差的自适应因子的对角线元素,用公式(25)和公式(26)求取。自适应矩阵中的后两项为1表示接收机钟差和钟差漂移的过程噪声协方差矩阵无校正。
其中为求取对角线元素的中间变量。λ1和λ2为平滑滤波系数,且0<λ1<1,λ1+λ2=1。η为门限参数,其值由人为设定。
经过步骤4和步骤5的操作,完成了模型j的自适应UKF滤波。
步骤6:在步骤5操作基础上,通过公式(27)至公式(29)对模型概率更新。
其中,为模型j在k时刻的似然函数,可通过公式(28)计算得到;c为归一化常数,可通过公式(29)计算得到。
其中exp[·]表示以自然常数e为底的指数函数。
为模型j的归一化常数,πij为模型i到模型j的转移概率,其值由认为设定,为k-1时刻的模型概率,在0时刻由人为设定,并在以后每一时刻通过公式(27)更新。
步骤7:在步骤6操作基础上,进行信息融合,得到k时刻滤波器的输出。
其中为采用交互式多模型自适应UKF(IMM-AUKF)算法得到的***最优估计值,Pk为采用IMM-AUKF算法得到的状态最优估计值的误差协方差矩阵。
步骤8:判断当前时刻是否到达步骤2设置的当前定位任务的结束时间,如果到达,则结束操作;否则,在步骤6操作基础上,将步骤4得到的模型j中k时刻的状态估计值模型j中k时刻的估计误差方差矩阵步骤5得到的模型j在k时刻过程噪声协方差矩阵阵的自适应矩阵以及步骤6得到的模型j的概率作为输入值,重复执行步骤2至步骤7的操作,计算下一时刻滤波器的输出值。
有益效果
本发明提出的应用于卫星导航的自适应估计方法与以有技术相比较,其优势在于:针对卫星导航***中复杂机动载体运动模型和噪声统计特性未知导致定位精度低的问题从两个方面改善了载体定位定速的精度:首先利用残差序列的自协方差矩阵作为校正量,设计平滑滤波器将此校正量与陈旧自适应因子融合,得到一个新的自适应因子,并利用此自适应因子实时调整过程噪声协方差矩阵,有效地降低了***中噪声统计特性未知造成的定位误差。其次,采用交互式多模型算法(IMM)设置模型集M,并根据测量残差实时调整模型概率实现模型间的软切换,降低了***模型不精确造成的定位误差。
附图说明
图1为本发明具体实施方式中一种带自适应因子的交互式多模型UKF的卫星导航方法的流程示意图。
具体实施方式
为了更好的说明本发明的目的和优点,下面结合附图和实施实例对本发明做进一步说明。
本实施例要完成300秒时间内地面车载的定位任务。该车载在0至60秒时间内做匀速运动,在60至102秒时间内做匀速转弯运动,在103至162秒时间内做匀速直线运动,在183至195秒内做匀速转弯运动,196秒至205秒的时间内做匀加速直线运动,206秒至300秒时间内做匀减速直线运动。
使用本发明提出的应用于卫星导航的自适应估计方法进行定位,其流程如图1所示,具体步骤如下:
步骤1:建立***模型
步骤1.1:建立基于全球定位***(GPS)的地面载体的状态方程。
为全面描述载体的运动状态和钟差漂移频率,用符号X表示全球定位***(GPS)状态方程的状态变量, 其中,分别代表世界大地坐标***(WorldGeodeticSystem一1984CoordinateSystem,WGS-84坐标系)下XT轴上的位置、速度和加速度;分别代表WGS-84坐标系下YT轴上的位置、速度和加速度;分别代表WGS-84坐标系下ZT轴上的位置、速度和加速度;δtu和分别代表载体接收机钟差和钟差漂移频率。建立模型集M,模型集M包含描述载体运动状态的当前统计模型CS、常加速度模型CA和常速转弯模型CT,即M=[CS,CA,CT]。其中“当前”统计模型假设下一时刻的加速度为当前时刻的加速度加上一个有限值,即下一时刻加速度的取值是有限的,且只能在当前加速度的附近变化。“当前”统计模型CS对环境有较强的适应能力,是描述载体运动状态常用的模型之一;常加速模型CA加速在载体运动过程中,加速度的取值是常值;常速转弯模型CT假设载体的在一个平面内做角速度恒定的转弯运动。
接收机钟差变化率用一阶马尔科夫模型描述,当前统计模型CS的状态方程如公式(1)所示;常加速度模型CA的状态方程如公式(2)所示;常速转弯模型CT的状态方程如公式(3)所示。公式(1)中Φ1=diag{Φ1,xΦ1,yΦ1,zΦt,u}其中
其中λτ表示WGS-84坐标系下各个坐标轴上的加速度时间常数的倒数,取为0.1,T为采样时间间隔,取为1;λt为一阶马尔科夫时间常数倒是,取为0.1。
公式(2)中Φ2=diag{Φ2,xΦ2,yΦ2,zΦt,u},其中
公式(3)中,
Φ3=diag{Φ3,xΦ3,yΦ3,zΦt,u},其中 ω代表载体的角度度,设为0.2。
用符号分别表示***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下(k-1)时刻的过程噪声协方差矩阵, E[·]表示期望值运算,表示WGS-84坐标系下描述XT轴上载体运动状态的过程噪声协方差矩阵;表示WGS-84坐标系下描述YT轴上载体运动状态的过程噪声协方差矩阵;表示WGS-84坐标系下描述ZT轴上载体运动状态的过程噪声协方差矩阵;Qt,k-1表示描述地面载体接收机钟差和钟差漂移的过程噪声协方差矩阵;
其中,
步骤1.2:建立基于全球定位***(GPS)的地面载体的观测方程
采用伪距作为观测量建立观测方程。在1秒至300秒内,接收机观测到的5颗GPS卫星,并利用这5颗GPS卫星进行定位,则建立此时刻CS模型,CA模型和CT模型的伪距观测方程如公式(4)所示。其中公式(4)中n的取值范围为n=1,2,3,4,5
为下文描述算法方便,建立CS模型,CA模型和CT模型的观测方程为公式(5)。
用Rk表示公式(5)的观测误差的协方差矩阵,
步骤2:设置CS模型,CA模型和CT模型0时刻的滤波初始值,其中位置、速度和接收机钟差的初始值由最小二乘法获得,加速度和接收机钟差变化量的初值设为0;同时设置当前定位任务的结束时间为300秒。
步骤3:得到CS模型,CA模型和CT模型在k时刻的滤波初始值。
使用步骤2或步骤4得到的CS模型、CA模型和CT模型第(k-1)时刻的状态的估计初值和以及CS模型、CA模型和CT模型的估计误差协方差矩阵初值和通过公式(6)和公式(7)进行交互混合,分别得到CS模型,CA模型和CT模型中k时刻的状态的估计初值和以及CS模型,CA模型和CT模型中k时刻的估计误差协方差矩阵初值和
步骤4:对CS模型,CA模型和CT模型进行自适应无迹卡尔曼滤波(AdaptiveUnsignedKalmanFilter,AUKF),得到各模型在k时刻的滤波值。具体操作步骤为:
步骤4.1:用表示步骤1中得到的CS模型,CA模型和CT模型的状态方程中的维数,通过公式(8)计算模型j在k时刻个初始西格马(sigma)点;
用公式(9)计算第一类权系数该权系数用于求每个sigma点的一阶统计特性;用公式(10)计算第二类权系数该权系数用于求每个sigma点的二阶统计特性;其中公式(9)和(10)中的β取值为2,α取值为0.01,公式(11)中的参数κ取0。
步骤4.2:通过公式(12)至公式(14)进行时间更新。
其中公式(14)中的初始值设为单位矩阵。
步骤4.3:通过公式(15)-(17)进行观测更新
步骤4.4:通过公式(18)至公式(20)进行滤波更新
步骤5:计算模型j在k时刻的过程噪声协方差矩阵阵的自适应矩阵
步骤5.1:用公式(21)至公式(22)计算模型j在k时刻的残差序列的协方差矩阵。并利用公式(23)求模型j在k时刻的估计偏差矩阵。
步骤5.2:采用公式(25)设计的平滑滤波器求取模型j的过程噪声协方差矩阵的自适应矩阵
通过公式(25)至公式(26)设计的平滑滤波器实现Δk与陈旧自适应因子的融合。
公式(26)中平滑滤波系数设置为λ1=0.98,λ2=0.02,门限参数设置为10。
经过步骤4和步骤5的操作,完成了模型j的自适应UKF滤波,j=1,2,3,分别表示CS模型,CA模型和CT模型。
步骤6:在步骤5操作基础上,通过公式(27)至公式(29)对模型概率更新
其中模型i到模型j的转移概率πij设置为
步骤7:在步骤6操作基础上,通过公式(30)和公式(31)进行信息融合,得到k时刻滤波器的输出。
步骤8:判断当前时刻是否到达步骤2设置的当前定位任务的结束时间300秒,如果到达,则结束操作;否则,在步骤6操作基础上,将步骤4得到的模型j中k时刻的状态估计值模型j中k时刻的估计误差方差矩阵步骤5得到的模型j在k时刻过程噪声协方差矩阵阵的自适应矩阵以及步骤6得到的模型j的概率作为输入值,重复执行步骤2至步骤7的操作,计算下一时刻滤波器的输出值。
为了说明本发明的效果,分别使用已有的UKF算法、IMM-UKF以及本发明提出的IMM-AUKF方法对实施例中的定位任务进行定位,得到的误差对表结果如表1所示。
表1UKF算法、IMM-UKF和IMM-AUKF算法的实验对比结果
从表1中可以看出,与传统方法相比,本发明提出的IMM-AUKF方法的定位精度最高。
Claims (3)
1.一种带自适应因子的交互式多模型UKF的卫星导航方法,其特征在于:具体包括如下步骤:
步骤1:建立***模型;
步骤1.1:建立基于全球定位***GPS的地面载体的状态方程;
为全面描述载体的运动状态和钟差漂移频率,用符号X表示全球定位***GPS状态方程的状态变量,其中,分别代表WGS-84坐标系下XT轴上的位置、速度和加速度;分别代表WGS-84坐标系下YT轴上的位置、速度和加速度;分别代表WGS-84坐标系下ZT轴上的位置、速度和加速度;δtu和分别代表载体接收机钟差和钟差漂移频率;建立模型集M,模型集M包含描述载体运动状态的当前统计模型CS、常加速度模型CA和常速转弯模型CT,即M=[CS,CA,CT];接收机钟差变化率用一阶马尔科夫模型描述,当前统计模型CS的状态方程如公式(1)所示;常加速度模型CA的状态方程如公式(2)所示;常速转弯模型CT的状态方程如公式(3)所示;
其中,分别表示***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻的状态,其中i=1,2,3,即:表示***在当前统计模型CS下k时刻的状态;表示***在常加速度模型CA下k时刻的状态;表示***在常速转弯模型CT下k时刻的状态;k为正整数;Φi为当前统计模型CS、常加速度模型CA和常速转弯模型CT下的状态转移矩阵,Φi=diag(Φi,x,Φi,y,Φi,z,Φt,u),Φi,x表示WGS-84坐标系下XT轴上的状态转移矩阵;Φi,y表示WGS-84坐标系下YT轴上的状态转移矩阵;Φi,z表示WGS-84坐标系下ZT轴上的状态转移矩阵;Φt,u表示描述地面载体接收机钟差和钟差漂移的状态转移矩阵;是***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下的过程噪声协方差矩阵自适应因子;是***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下的过程噪声矩阵;
用符号分别表示***在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k-1时刻的过程噪声协方差矩阵, i=1,2,3,E[·]表示期望值运算,表示WGS-84坐标系下描述XT轴上载体运动状态的过程噪声协方差矩阵;表示WGS-84坐标系下描述YT轴上载体运动状态的过程噪声协方差矩阵;表示WGS-84坐标系下描述ZT轴上载体运动状态的过程噪声协方差矩阵;Qt,k-1表示描述地面载体接收机钟差和钟差漂移的过程噪声协方差矩阵;
步骤1.2:建立基于全球定位***GPS的地面载体的观测方程;
采用伪距作为观测量建立观测方程;在k时刻,接收机观测到的GPS卫星数量用N表示,则建立此时刻CS模型,CA模型和CT模型的伪距观测方程如公式(4)所示;
其中,表示在k时刻观测到的第n颗GPS卫星的伪距,n=1,2…N,N表示k时刻观测到的GPS卫星的总数;表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下XT轴上的位置;表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下YT轴上的位置;表示k时刻观测到的第n颗GPS卫星在WGS-84坐标系下ZT轴上的位置;分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下XT轴上的位置;分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下YT轴上的位置;分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS-84坐标系下ZT轴上的位置;表示k时刻地面载体接收机钟差;表示k时刻观测到的第n颗GPS卫星的观测误差;
为下文描述算法方便,建立CS模型,CA模型和CT模型的观测方程为公式(5);
其中,为***的观测量,观测方程中的非线性函数,为***的观测误差;
用Rk表示公式(5)的观测误差的协方差矩阵,
步骤2:设置CS模型,CA模型和CT模型0时刻的滤波初始值,同时设置当前定位任务的结束时间;
在步骤1操作的基础上,分别设置CS模型、CA模型和CT模型的状态的估计初值,用符号和表示;然后设置CS模型、CA模型和CT模型的估计误差协方差矩阵初值,用符号和表示;并设置***运行的结束时间;
步骤3:得到CS模型,CA模型和CT模型在k时刻的滤波初始值;
使用CS模型、CA模型和CT模型第k-1时刻的状态的估计初值和以及CS模型、CA模型和CT模型的估计误差协方差矩阵初值和通过公式(6)和公式(7)进行交互混合,分别得到CS模型,CA模型和CT模型中k时刻的状态的估计初值,分别用和表示,以及CS模型,CA模型和CT模型中k时刻的估计误差协方差矩阵初值,分别用和表示;
其中,分别表示CS模型,CA模型和CT模型在k时刻滤波的状态初值;j=1,2,3,分别代表CS模型、CA模型和CT模型;i=1,2,3,分别表示CS模型,CA模型和CT模型在k-1时刻状态的最优估计值;表示k-1时刻模型i和模型j的混合概率,πij为模型i到模型j的模型转移概率,其值由人为设定;为模型i在k时刻的概率,的初值由人为设定,并在以后各个时刻由步骤6更新;代表模型j的归一化常数,
其中,i=1,2,3,分别表示CS模型、CA模型和CT模型在k-1时刻滤波误差的方差矩阵;分别表示CS模型、CA模型和CT模型在k时刻滤波误差方差矩阵初值;
步骤4:对CS模型,CA模型和CT模型进行自适应无迹卡尔曼滤波AUKF,得到各模型在k时刻的滤波值;
步骤5:计算模型j在k时刻的过程噪声协方差矩阵阵的自适应矩阵步骤6:在步骤5操作基础上,通过公式(8)至公式(10)对模型概率更新;
其中,为模型j在k时刻的似然函数,可通过公式(9)计算得到;c为归一化常数,可通过公式(10)计算得到;
其中exp[·]表示以自然常数e为底的指数函数;
为模型j的归一化常数,πij为模型i到模型j的转移概率,其值由人为设定,为k-1时刻的模型概率,在0时刻由人为设定,并在以后每一时刻通过公式(8)更新;
步骤7:在步骤6操作基础上,进行信息融合,得到k时刻滤波器的输出;
其中为采用交互式多模型自适应UKF算法得到的***最优估计值,Pk为采用IMM-AUKF算法得到的状态最优估计值的误差协方差矩阵;
步骤8:判断当前时刻是否到达步骤2设置的当前定位任务的结束时间,如果到达,则结束操作;否则,在步骤6操作基础上,将步骤4得到的模型j中k时刻的状态估计值模型j中k时刻的估计误差方差矩阵步骤5得到的模型j在k时刻过程噪声协方差矩阵阵的自适应矩阵以及步骤6得到的模型j的概率作为输入值,重复执行步骤2至步骤7的操作,计算下一时刻滤波器的输出值。
2.如权利要求1所述的一种带自适应因子的交互式多模型UKF的卫星导航方法,其特征在于:其步骤4中所述对CS模型,CA模型和CT模型进行自适应无迹卡尔曼滤波AUKF,得到各模型在k时刻的滤波值的具体操作步骤为:
步骤4.1:用表示***状态的维数,即步骤1中得到的CS模型,CA模型和CT模型中状态方程的维数,通过公式(13)计算模型j在k时刻个初始sigma点,其中第r个sigma点用用符号表示,且r为整数;
其中,表示平方根矩阵的第r列或第r行;
用公式(14)计算第一类权系数,该权系数用于求每个sigma点的一阶统计特性;模型j的第r个sigma点的第一类权系数用符号表示,即:表示CS模型的第r个sigma点第一类权系数,表示CA模型的第r个sigma点第一类权系数,表示CT模型的第r个sigma点第一类权系数;用公式(15)计算第二类权系数,该权系数用于求每个sigma点的二阶统计特性;模型j的第r个sigma点的第二类权系数用符号表示,即:表示CS模型的第r个sigma点第二类权系数,表示CA模型的第r个sigma点第二类权系数,表示CT模型的第r个sigma点第二类权系数;
其中,λ为一个复合刻度参数,其值通过公式(16)求解,参数α决定sigma点的散布程度,其值由人为设定,1e-4≤α≤1;参数β用来描述全球定位***状态方程的状态变量X的分布信息,在高斯情况下,β的最优值为2;
其中,参数κ取0;
步骤4.2:通过公式(17)至公式(19)进行时间更新;
其中,表示模型j在k时刻经过一步转移后的第r个sigma点;Φj为模型j的状态转移矩阵;
其中,是模型j在k时刻的一步状态预测值;
其中,为模型j在k时刻的一步预测误差方差矩阵;表示模型j在k-1时刻的自适应矩阵,的初始值由人为设定;
步骤4.3:通过公式(20)至(22)进行观测更新;
其中表示模型j在k时刻观测估计值,r表示第0个sigma点到第个sigma点的标识,h(·)为在步骤1中获取的观测方程中的非线性函数;
步骤4.4:通过公式(23)至公式(25)进行滤波更新
其中为模型j在k时刻的滤波增益矩阵;
为模型j中k时刻的状态估计值,yk为k时刻的该GPS***接收机的观测值,为已知量;
为k时刻模型j的估计误差方差矩阵。
3.如权利要求1或2所述的一种带自适应因子的交互式多模型UKF的卫星导航方法,其特征在于:其步骤5中所述计算模型j在k时刻的过程噪声协方差矩阵的自适应矩阵的具体操作步骤为:
步骤5.1:用公式(26)至公式(28)计算模型j在k时刻的的估计偏差
其中为模型j在k时刻的残差序列;残差序列的协方差矩阵用表示,用公式(27)计算;
其中,s表示时刻从1到k的变化指示;
模型j在k时刻的估计偏差矩阵用符号表示,并利用公式(28)求取;
其中trace(·)表示对矩阵求迹;
步骤5.2:采用平滑滤波器求取模型j的过程噪声协方差矩阵的自适应因子
过程噪声协方差矩阵中起作用的主要为对角线上的值,因此设计自适应因子为对角矩阵,即
其中表示模型j在k时刻过程噪声协方差的自适应因子的对角线元素,用公式(30)和公式(31)求取;自适应矩阵中的后两项为1表示接收机钟差和钟差漂移的过程噪声协方差矩阵无校正;
其中为求取对角线元素的中间变量;λ1和λ2为平滑滤波系数,且0<λ1<1,λ1+λ2=1;η为门限参数,其值由人为设定。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410270532.7A CN104020480B (zh) | 2014-06-17 | 2014-06-17 | 一种带自适应因子的交互式多模型ukf的卫星导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410270532.7A CN104020480B (zh) | 2014-06-17 | 2014-06-17 | 一种带自适应因子的交互式多模型ukf的卫星导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104020480A CN104020480A (zh) | 2014-09-03 |
CN104020480B true CN104020480B (zh) | 2016-07-06 |
Family
ID=51437336
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410270532.7A Expired - Fee Related CN104020480B (zh) | 2014-06-17 | 2014-06-17 | 一种带自适应因子的交互式多模型ukf的卫星导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104020480B (zh) |
Families Citing this family (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104392136B (zh) * | 2014-11-28 | 2017-12-19 | 东南大学 | 一种面向高动态非高斯模型鲁棒测量的高精度数据融合方法 |
CN105353367A (zh) * | 2015-11-26 | 2016-02-24 | 中国人民解放军63921部队 | 一种双基地mimo雷达空间机动目标跟踪方法 |
CN105372652A (zh) * | 2015-12-04 | 2016-03-02 | 中国人民解放军63921部队 | 基于接收线阵的mimo雷达空间机动目标跟踪方法 |
CN105716844B (zh) * | 2016-01-30 | 2018-06-08 | 西北工业大学 | 建立机电作动器的卡尔曼滤波模型及故障诊断方法 |
WO2019218291A1 (zh) * | 2018-05-17 | 2019-11-21 | 罗伯特·博世有限公司 | 用于对定位数据滤波的方法和装置 |
CN109000642A (zh) * | 2018-05-25 | 2018-12-14 | 哈尔滨工程大学 | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 |
CN109376642B (zh) * | 2018-10-16 | 2022-03-04 | 长安大学 | 一种基于驾驶员行为的运动车辆状态预测方法 |
CN109990786B (zh) * | 2019-02-28 | 2020-10-13 | 深圳大学 | 机动目标跟踪方法及装置 |
CN109781118A (zh) * | 2019-03-08 | 2019-05-21 | 兰州交通大学 | 一种无人车的位置跟踪方法 |
CN110307841B (zh) * | 2019-06-14 | 2023-02-03 | 南京工程学院 | 一种基于信息量测不完全的车辆运动参数估计方法 |
CN110261859B (zh) * | 2019-06-25 | 2023-10-31 | 北京中科海讯数字科技股份有限公司 | 一种水下机动静止交替状态目标跟踪方法 |
CN110350996B (zh) * | 2019-06-28 | 2020-10-23 | 中国科学院声学研究所 | 基于交互式多模型滤波器的时钟漂移率跟踪方法及*** |
CN110501732B (zh) * | 2019-07-24 | 2021-09-24 | 北京航空航天大学 | 一种多卫星分布式导航滤波计算方法 |
CN113064154B (zh) * | 2021-03-18 | 2023-09-01 | 沈阳理工大学 | 一种空中目标跟踪方法 |
CN114488227B (zh) * | 2022-01-26 | 2023-10-20 | 西南交通大学 | 一种基于空间相关性的多路径误差改正方法 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP1271102B1 (en) * | 1996-04-25 | 2012-09-05 | SiRF Technology, Inc. | Spread spectrum receiver with multi-bit correlator |
US8259012B2 (en) * | 2010-04-14 | 2012-09-04 | The Boeing Company | Software GNSS receiver for high-altitude spacecraft applications |
CN102608631B (zh) * | 2011-10-28 | 2014-09-03 | 北京航空航天大学 | 基于模糊逻辑的自适应强跟踪ukf定位滤波算法 |
CN103575298A (zh) * | 2013-11-14 | 2014-02-12 | 哈尔滨工程大学 | 基于自调节的ukf失准角初始对准方法 |
-
2014
- 2014-06-17 CN CN201410270532.7A patent/CN104020480B/zh not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
CN104020480A (zh) | 2014-09-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104020480B (zh) | 一种带自适应因子的交互式多模型ukf的卫星导航方法 | |
CN109211276B (zh) | 基于gpr与改进的srckf的sins初始对准方法 | |
Meng et al. | Covariance matching based adaptive unscented Kalman filter for direct filtering in INS/GNSS integration | |
CN106291645B (zh) | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 | |
CN103644903B (zh) | 基于分布式边缘无味粒子滤波的同步定位与地图构建方法 | |
CN104469942B (zh) | 一种基于隐马尔科夫模型的室内定位方法 | |
CN107677272B (zh) | 一种基于非线性信息滤波的auv协同导航方法 | |
CN102322858B (zh) | 用于地磁/捷联惯导组合导航***的地磁匹配导航方法 | |
CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
CN104392136B (zh) | 一种面向高动态非高斯模型鲁棒测量的高精度数据融合方法 | |
CN110231636B (zh) | Gps与bds双模卫星导航***的自适应无迹卡尔曼滤波方法 | |
CN103940433B (zh) | 一种基于改进的自适应平方根ukf算法的卫星姿态确定方法 | |
CN110567455B (zh) | 一种求积更新容积卡尔曼滤波的紧组合导航方法 | |
CN104035110B (zh) | 应用于多模卫星导航***中的快速卡尔曼滤波定位方法 | |
CN103776453A (zh) | 一种多模型水下航行器组合导航滤波方法 | |
CN107290742B (zh) | 一种非线性目标跟踪***中平方根容积卡尔曼滤波方法 | |
CN103439731A (zh) | 基于无迹卡尔曼滤波的gps/ins组合导航方法 | |
CN109507706B (zh) | 一种gps信号丢失的预测定位方法 | |
CN105180938A (zh) | 一种基于粒子滤波的重力采样矢量匹配定位方法 | |
CN108873029A (zh) | 一种实现导航接收机钟差建模的方法 | |
CN105157704A (zh) | 一种基于贝叶斯估计的粒子滤波重力辅助惯导匹配方法 | |
CN103218482B (zh) | 一种动力学***中不确定参数的估计方法 | |
CN103776449B (zh) | 一种提高鲁棒性的动基座初始对准方法 | |
EP2330382B1 (en) | Method and system for latitude adaptive navigation quality estimation | |
CN103973263A (zh) | 一种新的逼近滤波方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20160706 |