CN110567455A - 一种求积更新容积卡尔曼滤波的紧组合导航方法 - Google Patents
一种求积更新容积卡尔曼滤波的紧组合导航方法 Download PDFInfo
- Publication number
- CN110567455A CN110567455A CN201910908134.6A CN201910908134A CN110567455A CN 110567455 A CN110567455 A CN 110567455A CN 201910908134 A CN201910908134 A CN 201910908134A CN 110567455 A CN110567455 A CN 110567455A
- Authority
- CN
- China
- Prior art keywords
- error
- state
- navigation
- matrix
- calculating
- 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.)
- Granted
Links
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/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
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)
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明公开了一种求积更新容积卡尔曼滤波的紧组合导航方法,首先对一步预测概率密度函数和状态后验概率密度函数进行建模,并对SINS/GPS紧组合导航***进行动态建模;在更新一步预测状态求积点和下一时刻状态求积点时,分别构建系数矩阵F、G和H,建立以求积点误差矩阵相关的线性方程来辅助状态更新;初始时刻的导航参数直接由惯导解算得到,下一时刻的运载体导航误差、惯性元器件误差和GPS接收机时钟误差将作为滤波初值,实时地完成误差估计任务,再将估计后得到的***状态误差反馈,对下一时刻惯性元件输出进行校正,输出组合导航参数信息。本发明使得组合导航精度有效提高,特别是对于速度和北向位置参数的导航精度提高明显。
Description
技术领域
本发明涉及一种捷联式惯性导航***和全球定位***(SINS/GPS)紧组合导航方法,尤其涉及一种求积更新容积卡尔曼滤波(CKF)的紧组合导航方法,属于组合导航技术领域。
背景技术
SINS和GPS各自具有鲜明的优点和明显的不足。SINS不依赖于外部信息,具有很强的抗干扰能力,可以提供200Hz以上带宽的导航信息,短期稳定性好,提供的导航信息全面。但是,由于在其导航算法中存在积分环节,随着多种误差的累积,SINS的误差随时间不断增加,长期稳定性较差。与SINS形成鲜明对比的是:GPS具有长期高精度的特性,误差在几米以内,而且用户的设备价格较低。但是其短期精度不高,且输出数据率低;同时,在未增设额外的软硬件环境条件下,标准的GPS接收机无法提供姿态信息;除此之外,GPS需要在可视范围内至少有4颗可见卫星,在城市环境中由于存在较高建筑、树木等的遮挡以及大气折射和多路径效应的作用,上述条件经常无法满足。SINS/GPS紧组合导航***利用卡尔曼滤波技术,将SINS和GPS的输出进行信息融合,以获得优于单一导航设备的性能。
在***噪声和量测噪声都满足高斯分布,且对应的统计特性已知的条件下,根据建立的SINS/GPS紧组合导航模型,能够利用CKF实现导航误差的最优估计。然而,在利用CKF进行导航误差估计时,由于需要重复地基于高斯假设产生容积点,这些容积点存在两个问题:(1)由于基于高斯假设产生求积点,导航***状态——导航误差的非高斯信息会被丢失;(2)三阶容积准则只有三阶的计算精度,故导航误差的高阶信息也被忽略。这样会导致导航精度不能得到有效提高。
为了有效地提高导航精度,避免由于求积点重复产生而造成的导航参数的多种信息丢失,前人提出了在组合导航过程中利用一种基于确定模型的求积点更新的卡尔曼滤波方法,它不需要重复地产生求积点,而是直接通过状态一步预测求积点的线性变换来更新状态求积点,这样对导航***状态的信息丢失起到了抑制作用,但是仅仅通过状态一步预测求积点来更新,估计精度有限,且只能在没有***噪声的前提下进行滤波,而组合导航***中噪声存在随机性,故适用范围不大。为了能解决上述方法的多重局限性,本发明提出了一种求积更新容积卡尔曼滤波的SINS/GPS紧组合导航方法,以提高SINS/GPS的组合导航精度。
发明内容
针对上述现有技术,本发明要解决的技术问题是提供一种有效提高导航精度的求积更新容积卡尔曼滤波的SINS/GPS紧组合导航方法。
为解决上述技术问题,本发明的一种求积更新容积卡尔曼滤波的紧组合导航方法,包括以下步骤:
步骤一:建立SINS/GPS紧组合导航***的状态方程和量测方程;
步骤二:建立以状态一步预测误差矩阵和一步预测量测误差矩阵相关的状态求积点更新方程;
步骤三:选取第一个时间间隔后的运载体导航误差,包括姿态误差、速度误差和位置误差、惯性元器件误差和GPS接收机时钟误差作为滤波初值,采用步骤二中状态求积点更新方程,进行滤波的时间更新和量测更新,得到各时刻滤波估计后***状态误差;
步骤四:将步骤三得到的***状态误差作为反馈量,对下一刻的输出进行校正,将估计后的等效时钟钟差和等效时钟漂移反馈回GPS接收机,修正时钟误差;同时将估计得到的导航参数误差作为惯性导航***输出的校正量,输出校正后的导航参数,完成SINS/GPS紧组合导航。
本发明还包括:
1.步骤一所述状态方程为:
其中,x为状态向量, 为实际导航坐标系与地理坐标系之间在地理坐标系下的误差角,分别为载体相对于当地地理坐标系的东向速度误差分量和北向速度误差分量,δλ,δL为载体相对于当地地理坐标系的经度误差和纬度误差分量,为加速度计的零位误差在当地地理坐标系下的误差分量,εx,εy,εz为陀螺仪漂移在当地地理坐标系下的误差分量,δtB,δtD分别表示GPS接收机的等效时钟钟差与等效时钟钟漂,w=[wSINS,wGPS]T为***噪声,wSINS,wGPS分别为惯性导航***和全球定位***的***噪声,A为状态转移矩阵;B为***噪声驱动阵;
步骤一所述量测方程为:
z=h(δλ,δL,δtB,δtD)+vρ
其中,z是量测向量,由各卫星到GPS接收机的伪距和伪距率构成,h(·)为已知的非线性函数,vρ为量测噪声。
2.上述步骤二具体包括:
定义过程中使用的变量:
w=[w1,...,wN]T,W=diag{w}
其中,N为求积点的个数,为k-1时刻更新后的第i个状态求积点,为经函数f(·)传播后的第i个状态求积点,为传播求积点的误差矩阵,为一步预测状态的第i个状态求积点,为状态预测求积点的误差矩阵,为经量测函数h(·)传播后的第i个量测求积点,为量测预测求积点的误差矩阵,为k时刻更新后的第i个状态求积点,为状态更新求积点的误差矩阵,w为由权值构成的列向量,W为一个由权值组成的对角矩阵;
根据求积准则,得到:
他们满足等式:
构建了三个系数矩阵F、G和H,建立状态更新方程,满足:
满足如下的约束条件:
根据约束条件,计算得到:
F=Sk/k-1(S'k/k-1)-1
其中,Sk/k-1为Pk/k-1的方根矩阵,S'k/k-1为(Pk/k-1-Qk-1)的方根矩阵;
选用其中一个特解,具体为:
G=Sk/kD-1
H=Sk/kD-1Wk
其中,Sk/k为Pk/k的方根矩阵,D为的方根矩阵。
3.上述步骤三具体包括:
步骤三(A):初始化:
将第一个时间间隔后的状态向量作为滤波初值P0/0由导航参数的不准确度计算得到,利用容积准则产生初始后验密度函数的求积点和相应的权值wi:
其中,S0/0为P0/0的方根矩阵;N为求积点个数,满足N=2n,n为***的阶次;
步骤三(B):时间更新:
①计算***函数传播后的求积点
②计算状态的一步预测并计算传播求积点的误差矩阵
③计算状态的一步预测误差协方差矩阵Pk/k-1:
④计算矩阵F,并计算状态预测求积点误差矩阵
F=Sk/k-1(S'k/k-1)-1
其中,Sk/k-1为Pk/k-1的方根矩阵,S'k/k-1为(Pk/k-1-Qk-1)的方根矩阵;
⑤计算一步预测状态的求积点
步骤三(C)量测更新:
①计算一步预测量测的求积点
②计算量测的一步预测并计算量测预测求积点的误差矩阵
③计算量测预测误差协方差矩阵Pzz,k/k-1、状态和量测的互协方差矩阵Pxz,k/k-1;
④计算卡尔曼滤波增益Kk,并更新k时刻的状态估计和误差协方差矩阵Pk/k:
⑤计算系数矩阵G和H,并计算更新求积点误差矩阵
G=Sk/kD-1
H=Sk/kD-1Wk
其中,Sk/k为Pk/k的方根矩阵,D为的方根矩阵;
⑥计算更新后的求积点
本发明的有益效果:(1)通过状态更新方程对一步预测状态求积点和后验状态求积点进行实时更新,使得导航***状态的更多高阶和非高斯信息得到保留,有利于提高导航精度;(2)在更新后验状态求积点的过程中,由于增加了量测求积点的修正作用,导航参数的滤波估计会更接近真实值,进而使得反馈校正后的惯性元件输出得到更好的补偿,有效地提高了导航精度。
附图说明
图1是本发明提供的求积更新容积卡尔曼滤波算法流程图;
图2是舰船运行轨迹图;
图3(a)是SINS/GPS紧组合导航的航向角误差曲线图;
图3(b)是SINS/GPS紧组合导航的横摇角误差曲线图;
图3(c)是SINS/GPS紧组合导航的纵摇角误差曲线图;
图4(a)是SINS/GPS紧组合导航的北向速度误差曲线图;
图4(b)是SINS/GPS紧组合导航的东向速度误差曲线图;
图5(a)是SINS/GPS紧组合导航的北向位置误差曲线图;
图5(b)是SINS/GPS紧组合导航的东向位置误差曲线图;
图6是本发明的SINS/GPS紧组合导航原理框图;
图7是本发明的SINS/GPS紧组合导航方框图。
具体实施方式
下面结合附图对本发明具体实施方式做进一步说明。
结合图6和图7,本发明的目的是这样实现的,步骤如下:
步骤一:建立SINS/GPS紧组合导航***的状态方程和量测方程;
步骤二:建立以状态一步预测误差矩阵和一步预测量测误差矩阵相关的状态求积点更新方程,使得更新后的一步预测状态求积点和更新后的后验状态求积点能分别很好地匹配状态的一步预测概率密度和状态后验概率密度的均值和协方差;
步骤三:初始0时刻的导航参数直接由惯导解算得到并输出,选择下一时刻的运载体导航误差(包括姿态误差、速度误差和位置误差)、惯性元器件误差和GPS接收机时钟误差作为滤波初值,基于上述求积更新过程,进行滤波的时间更新和量测更新,最终得到各时刻滤波估计后***状态误差;
步骤四:将估计得到的***状态误差作为反馈量,分别校正惯导输出、惯性元器件输出和GPS的时钟信号误差,完成SINS/GPS紧组合导航,输出校正后的导航参数。
本发明还包括这样一些结构特征:
1、步骤一具体为:
(1)SINS的误差方程
假设在SINS已经初始对准完毕的基础上,计算导航坐标系与导航坐标系之间存在着小角度姿态失准角***的速度误差为位置误差为δp=[δλ,δL,δh]T。
①姿态误差方程
其中,i、b、n和e分别表示地心惯性坐标系、载体坐标系、由当地地理坐标系构成的导航坐标系和地球坐标系;表示导航坐标系相对地心惯性坐标系的旋转角速度在导航坐标系下的投影;为对应的误差项;和为地球自转角速度及其对应误差项;和为导航坐标系相对地球坐标系的旋转角速度及其误差项;为从载体坐标系到导航坐标系的姿态转换矩阵;陀螺漂移误差由随机常值漂移εb和陀螺仪随机噪声组成。
②速度误差方程
其中,表示载体相对于地球坐标系的速度在导航坐标系下的投影;fb和δfb分别表示加速度计输出的比力及其测量误差,δfb由加速度计零偏▽b和加速度计随机噪声组成。
③位置误差方程
其中,L、λ和h分别表示载体的纬度、经度和高度;RN和RM分别表示卯酉圈和子午圈的曲率半径。
(2)GPS的误差方程
在研究组合导航问题时,GPS接收机的时钟钟差和时钟漂移需要作为待估计的状态进行滤波,以避免其对GPS定位精度造成影响。故考虑以GPS接收机的等效时钟钟差δtB和等效时钟漂移δtD为变量,类似一阶马尔可夫过程,建立误差模型。它们满足如下关系:
其中,TD为相关时间常数,wB、wD分别为等效时钟钟差和等效时钟漂移的零均值高斯白噪声。
本发明针对水面航行的运载体建模,忽略天向通道的影响,则可根据SINS和GPS的误差方程,建立SINS/GPS紧组合导航***的状态方程。
其中,为状态向量,分别由姿态、速度、位置、加速度计零偏、陀螺仪漂移误差分量、GPS接收机等效时钟钟差和等效时钟漂移组成;w=[wSINS,wGPS]T为***噪声;A为状态转移矩阵;B为***噪声驱动阵。
本发明将GPS提供的伪距和伪距率信息作为量测量,根据GPS基于载波相位进行伪距和伪距率定位方程,建立量测方程。
其中,ρi表示载体到第i个卫星的伪距;表示载体到第i个卫星的伪距率;(x,y,z)表示载体的真实位置在地球坐标系上的坐标;是由卫星星历给出的第i个卫星位置坐标;m为可见卫星个数;表示测量伪距ρi时的量测噪声;表示测量伪距率时的量测噪声。
由于有#
其中,Re为地球赤道半径。
则最终的SINS/GPS紧组合导航***的量测方程为
z=h(δλ,δL,δtB,δtD)+vρ
其中,z是量测向量,由各卫星到GPS接收机的伪距和伪距率构成,h(·)为已知的非线性函数,vρ为量测噪声。
将SINS/GPS紧组合导航***的状态方程进行离散化,可得离散化后的***状态模型为
xk=Φkxk-1+Gkwk-1
其中,Φk=[I+A(tk)]τs为离散化后的状态转移矩阵;τs为采样周期;k为离散时间序列,tk满足tk=kτs;Gk=B(tk)τs为***噪声驱动阵;***噪声wk为零均值的高斯白噪声,协方差为Qk。
2、步骤二具体为:
(1)定义过程中使用的变量
w=[w1,...,wN]T,W=diag{w}
其中,N为求积点的个数,为k-1时刻更新后的第i个状态求积点,为经函数f(·)传播后的第i个状态求积点,为传播求积点的误差矩阵,为一步预测状态的第i个状态求积点,为状态预测求积点的误差矩阵,为经量测函数h(·)传播后的第i个量测求积点,为量测预测求积点的误差矩阵,为k时刻更新后的第i个状态求积点,为状态更新求积点的误差矩阵,w为由权值构成的列向量,W为一个由权值组成的对角矩阵。
根据现有的求积准则,可得
他们满足以下等式:
在各求积点的权值保持不变的前提下,为了使得更新后的状态求积点和能分别与状态一步预测概率密度和状态后验概率密度的均值和协方差匹配,同时增加量测求积点对状态求积点的修正作用,以提高滤波估计精度,我们构建了三个系数矩阵F、G和H,建立状态更新方程,有
为了能达到均值和协方差分别匹配的目的,它们需要满足如下的约束条件:
根据约束条件,可以计算得到
F=Sk/k-1(S'k/k-1)-1
其中,Sk/k-1为Pk/k-1的方根矩阵,S'k/k-1为(Pk/k-1-Qk-1)的方根矩阵。
由于满足约束条件的系数矩阵G和H有无穷多个,本发明选用其中一个特解,有
G=Sk/kD-1
H=Sk/kD-1Wk
其中,Sk/k为Pk/k的方根矩阵,D为的方根矩阵。
3、步骤三具体为:
根据步骤二,可得本发明的求积更新CKF的具体执行步骤阐述如下:
由于运载体在初始时刻缺少先验信息,我们在导航初始时刻不进行滤波估计,滤波将从第一个时间间隔开始进行,将运载体在该时刻的导航参数误差(姿态误差、速度误差和位置误差)、惯性元件误差、GPS接收机时钟误差作为滤波初值,基于求积点更新方程,进行时间更新和量测更新,得到滤波估计后下一时刻的导航误差、惯性元器件误差以及GPS接收机等效时钟误差。根据一步预测求积点更新方程进行时间更新;根据后验状态求积点更新方程执行相应的量测更新过程:
(1)初始化
①将第一个时间间隔后的导航误差、惯性元器件误差和GPS接收机等效时钟误差作为滤波初值P0/0由导航参数的不准确度计算得到,利用容积准则产生初始后验密度函数的求积点和相应的权值wi。
其中,S0/0为P0/0的方根矩阵;N为求积点个数,满足N=2n,n为***的阶次。
(2)时间更新
①计算***函数传播后的求积点
②计算状态的一步预测并计算传播求积点的误差矩阵
③计算状态的一步预测误差协方差矩阵Pk/k-1;
④计算矩阵F,并计算状态预测求积点误差矩阵
F=Sk/k-1(S'k/k-1)-1
其中,Sk/k-1为Pk/k-1的方根矩阵,S'k/k-1为(Pk/k-1-Qk-1)的方根矩阵。
⑤计算一步预测状态的求积点
(3)量测更新
①计算一步预测量测的求积点
②计算量测的一步预测并计算量测预测求积点的误差矩阵
③计算量测预测误差协方差矩阵Pzz,k/k-1、状态和量测的互协方差矩阵Pxz,k/k-1;
④计算卡尔曼滤波增益Kk,并更新k时刻的状态估计和误差协方差矩阵Pk/k;
⑤计算系数矩阵G和H,并计算更新求积点误差矩阵
G=Sk/kD-1
H=Sk/kD-1Wk
其中,Sk/k为Pk/k的方根矩阵,D为的方根矩阵。
⑥计算更新后的求积点
4、步骤四具体为:
将滤波估计得到的IMU误差将作为反馈量,对下一时刻的IMU输出进行校正;将估计后的等效时钟钟差和等效时钟漂移反馈回GPS接收机,修正时钟误差;同时将估计得到的导航参数误差作为惯性导航***输出的校正量,校正后作为组合导航***的最终导航输出,完成SINS/GPS紧组合导航。
本发明具体实施方式还包括:
下面结合附图与具体实施方式对本发明作进一步详细描述。
(1)首先利用Spirent SimGen产生模拟的船载运动:在三级海况下,船舶载体以10m/s的速度航行,在此期间会进行两次相反的45°转向运动。将船舶的位置、姿态和速度信息作为真实状态导入MATLAB中;
(2)利用SINS导航定位解算算法,设计了SINS模拟发生器;
(3)基于GPS测量伪距和伪距率原理,设计了GPS伪距和伪距率发生器;
(4)通过GPS模拟获得的伪距和伪距率信息,将陀螺仪输出和加速度计输出用于验证SINS/GPS紧组合导航方法。紧组合导航的原理框图如图6所示,通过陀螺仪输出和加速度计输出进行模拟惯导解算,获得导航参数;然后通过建立的***模型和量测模型,初始时刻的导航参数解算没有进行滤波,将利用通过导航定位解算后直接输出,而下一时刻的导航参数误差、惯性元器件误差和GPS接收机参数误差将作为滤波初值,利用图1中的求积更新的容积卡尔曼滤波执行步骤对导航参数误差进行估计;最后将估计的误差对下一时刻的惯性元件和GPS接收机进行反馈校正,同时对SINS模拟输出的导航参数进行误差校正,完成SINS/GPS紧组合导航,输出经误差校正后的导航参数(船舶姿态、速度和位置)。具体实施步骤如下:
步骤一:选取***的姿态、速度、位置、加速度计零偏、陀螺仪漂移误差分量、GPS接收机等效时钟误差作为状态向量,即
建立***的状态方程和量测方程如下:
z=h(δλ,δL,δtB,δtD)+vρ
对***的状态方程进行离散化处理,可得离散后的模型为:
xk=Φkxk-1+Gkwk-1
步骤二:建立以状态一步预测误差矩阵和一步预测量测误差相关的更新方程,使得更新后的一步预测状态求积点和后验状态求积点能很好地匹配一步预测概率密度和状态后验概率密度的均值和协方差。
其中
F=Sk/k-1(S'k/k-1)-1
G=Sk/kD-1
H=Sk/kD-1Wk
这里,Sk/k-1为Pk/k-1的方根矩阵,S'k/k-1为(Pk/k-1-Qk-1)的方根矩阵;Sk/k为Pk/k的方根矩阵,D为的方根矩阵。
步骤三:初始时刻的导航参数直接由惯导解算输出,而下一时刻的导航参数误差、惯性元器件误差和GPS接收机参数误差将作为滤波初值,根据图1中的卡尔曼滤波执行步骤和步骤一中的状态空间模型,对步骤一中的***状态量进行估计。
(1)进行一步预测时间更新,并产生一步预测状态求积点;
(2)执行量测更新,并产生下一时刻的状态求积点;
(3)滤波输出和Pk/k,完成对下一时刻的导航参数误差、惯性元器件误差和GPS接收机等效时钟误差的滤波估计任务。
步骤四:如图6所示,将步骤三滤波估计得到的IMU误差将作为反馈量,对下一时刻的IMU输出进行校正;将估计得到的等效时钟钟差和等效时钟漂移用于校正GPS接收机的时钟误差;同时将估计得到的导航参数误差作为惯性导航***输出的校正量,校正后作为组合导航***的最终导航输出,完成SINS/GPS紧组合导航。
(1)实验条件
为了验证所设计的求积更新CKF的SINS/GPS紧组合导航***的导航性能,进行了一次仿真实验验证。其中陀螺仪的随机常值漂移为0.1°/h,随机噪声为加速度计的零位偏置100μg,随机漂移为其输出频率均为100Hz。GPS的测量伪距误差为10km,伪距率误差为100m/s,它们相应的量测噪声误差分别为1m和0.02m/s;GPS的更新频率为2Hz;***存在小偏差平台失准角实验时长为450s,实验过程中的运行轨迹如图2所示,舰船在三级海况下,以10m/s的速度航行,在此期间会进行两次相反的45°转向运动。
将本发明与现有的EKF和CKF进行SINS/GPS紧组合导航性能对比。假设第1个时间间隔后的导航误差、惯性元器件误差和GPS接收机时钟误差为误差协方差初值为
(2)实验结果
实验结果如图3(a)至图5(b)所示,其均方根误差如表1所示。如图3(a)至图3(c)中可以看出,从图中可以看出,对于载体的姿态估计而言,在存在较大初始航向角ψ0的情况下,基于求积点更新的CKF的滤波误差要明显低于标准的CKF,标准的CKF的滤波误差要低于EKF。同时可以看出,基于求积点的CKF会比标准的CKF、EKF提前进入稳态,但是进入稳态后,标准的CKF误差会更加趋于稳定。从上述结果中,我们可以得出,直接更新状态求积点,并增加量测求积点对状态求积点的修正作用,可以大大地提高滤波精度,但是估计状态的稳定性不佳。如图4(a)至图4(b)中可以看出,对于载体的速度估计而言,由于增加了量测求积点对状态求积点的修正作用,基于求积点更新的CKF的误差要明显低于标准的CKF,同时由于更新状态求积点的方法使得***状态的高阶矩信息和非高斯信息得以保留,也是提高滤波精度的原因;标准的CKF的误差要低于EKF,这是由于***的量测部分存在非线性,而EKF的一阶近似使得***测量的高阶信息丢失,从而造成近似出现较大误差。除此之外,基于求积点更新的CKF要比其他滤波方法更早进入稳态,但标准的CKF的误差稳定性更好。综上所述,我们可以得出,基于求积点更新的CKF具有更高的滤波估计精度,但是估计的稳定性不佳。如图5(a)至图5(b)中可以看出,从图中可以看出,对于载体的位置估计来说,得益于量测求积点的修正作用与***状态信息的完整性,基于求积点更新的CKF的误差在大部分时间要明显低于标准的CKF,存在小范围时间内,标准的CKF具有更低的误差;由于EKF丢失掉了***测量的高阶信息,EKF的误差要高于标准的CKF;在北向位置估计中,基于求积点更新的CKF在整个仿真过程中能基本保持在小范围内波动,而其他非线性滤波方法则波动较大,稳定性较差;在东向位置的估计中,所有非线性滤波估计都存在较大的误差,基于求积点更新的CKF具有更高的稳定性。根据以上分析,可以得到基于求积点更新的滤波方法具有较高的滤波估计精度,在位置上稳定性更佳。
本发明中对紧组合导航***中的速度参数和北向位置参数的估计精度明显要高于其他非线性滤波方法,实验结果表明,本发明应用于SINS/GPS紧组合导航***具有更高的导航精度。
表1 SINS/GPS紧组合导航的导航参数RMSE汇总表;
滤波器 | 偏航角 | 横滚角 | 俯仰角 | 东向速度 | 北向速度 | 东向位置 | 北向位置 |
EKF | 7.4413 | 3.2635 | 4.3248 | 1.2500 | 1.2507 | 200.9194 | 50.3565 |
CKF | 7.3953 | 3.1263 | 4.3083 | 1.1390 | 1.1869 | 200.8351 | 50.2730 |
本发明 | 5.5647 | 0.5388 | 0.7197 | 0.0375 | 0.0389 | 187.5390 | 8.0071 |
综上所述,为了解决传统非线性滤波中重复产生求积点造成非高斯信息和高阶信息丢失的问题,本发明首先采用高斯分布对一步预测概率密度函数和状态后验概率密度函数进行建模,并对SINS/GPS紧组合导航***进行动态建模,其中***的量测方程采用以伪距和伪距率为量测向量的非线性定位方程;然后,在更新一步预测状态求积点和下一时刻状态求积点时,分别构建系数矩阵F、G和H,建立以求积点误差矩阵和相关的线性方程来辅助状态更新,使得更新后的两类求积点至少能匹配状态一步预测概率密度和状态后验概率密度的均值和协方差;最后采用反馈校正方案,初始时刻的导航参数直接由惯导解算得到,下一时刻的运载体导航误差(包括姿态误差、速度误差和位置误差)、惯性元器件误差和GPS接收机时钟误差将作为滤波初值,实时地完成误差估计任务,再将估计后得到的***状态误差反馈回自身:估计后的惯性元件误差将对下一时刻惯性元件输出进行校正,估计后的时钟钟差和时钟钟漂校正GPS接收机,导航参数误差校正惯导输出后,输出组合导航参数信息,完成SINS/GPS紧组合导航。本发明由于增加了量测求积点对状态求积点的修正作用,且更好地捕获了状态在一步预测PDF和状态后验PDF的非高斯信息和高阶矩信息,从而使得组合导航精度有效提高,特别是对于速度和北向位置参数的导航精度提高明显。能够有效地保留导航状态的高阶矩信息和非高斯信息,对于速度参数和北向位置参数的估计具有较为明显的精度提升。
Claims (4)
1.一种求积更新容积卡尔曼滤波的紧组合导航方法,其特征在于,包括以下步骤:
步骤一:建立SINS/GPS紧组合导航***的状态方程和量测方程;
步骤二:建立以状态一步预测误差矩阵和一步预测量测误差矩阵相关的状态求积点更新方程;
步骤三:选取第一个时间间隔后的运载体导航误差,包括姿态误差、速度误差和位置误差、惯性元器件误差和GPS接收机时钟误差作为滤波初值,采用步骤二中状态求积点更新方程,进行滤波的时间更新和量测更新,得到各时刻滤波估计后***状态误差;
步骤四:将步骤三得到的***状态误差作为反馈量,对下一刻的输出进行校正,将估计后的等效时钟钟差和等效时钟漂移反馈回GPS接收机,修正时钟误差;同时将估计得到的导航参数误差作为惯性导航***输出的校正量,输出校正后的导航参数,完成SINS/GPS紧组合导航。
2.根据权利要求1所述的一种求积更新容积卡尔曼滤波的紧组合导航方法,其特征在于:步骤一所述状态方程为:
其中,x为状态向量, 为实际导航坐标系与地理坐标系之间在地理坐标系下的误差角,分别为载体相对于当地地理坐标系的东向速度误差分量和北向速度误差分量,δλ,δL为载体相对于当地地理坐标系的经度误差和纬度误差分量,为加速度计的零位误差在当地地理坐标系下的误差分量,εx,εy,εz为陀螺仪漂移在当地地理坐标系下的误差分量,δtB,δtD分别表示GPS接收机的等效时钟钟差与等效时钟钟漂,w=[wSINS,wGPS]T为***噪声,wSINS,wGPS分别为惯性导航***和全球定位***的***噪声,A为状态转移矩阵;B为***噪声驱动阵;
步骤一所述量测方程为:
z=h(δλ,δL,δtB,δtD)+vρ
其中,z是量测向量,由各卫星到GPS接收机的伪距和伪距率构成,h(·)为已知的非线性函数,vρ为量测噪声。
3.根据权利要求1所述的一种求积更新容积卡尔曼滤波的紧组合导航方法,其特征在于:所述步骤二具体包括:
定义过程中使用的变量:
w=[w1,...,wN]T,W=diag{w}
其中,N为求积点的个数,为k-1时刻更新后的第i个状态求积点,为经函数f(·)传播后的第i个状态求积点,为传播求积点的误差矩阵,为一步预测状态的第i个状态求积点,为状态预测求积点的误差矩阵,为经量测函数h(·)传播后的第i个量测求积点,为量测预测求积点的误差矩阵,为k时刻更新后的第i个状态求积点,为状态更新求积点的误差矩阵,w为由权值构成的列向量,W为一个由权值组成的对角矩阵;
根据求积准则,得到:
他们满足等式:
构建了三个系数矩阵F、G和H,建立状态更新方程,满足:
满足如下的约束条件:
根据约束条件,计算得到:
F=Sk/k-1(S'k/k-1)-1
其中,Sk/k-1为Pk/k-1的方根矩阵,S'k/k-1为(Pk/k-1-Qk-1)的方根矩阵;
选用其中一个特解,具体为:
G=Sk/kD-1
H=Sk/kD-1Wk
其中,Sk/k为Pk/k的方根矩阵,D为的方根矩阵。
4.根据权利要求1所述的一种求积更新容积卡尔曼滤波的紧组合导航方法,其特征在于:步骤三具体包括:
步骤三(A):初始化:
将第一个时间间隔后的状态向量作为滤波初值P0/0由导航参数的不准确度计算得到,利用容积准则产生初始后验密度函数的求积点和相应的权值wi:
其中,S0/0为P0/0的方根矩阵;N为求积点个数,满足N=2n,n为***的阶次;
步骤三(B):时间更新:
①计算***函数传播后的求积点
②计算状态的一步预测并计算传播求积点的误差矩阵
③计算状态的一步预测误差协方差矩阵Pk/k-1:
④计算矩阵F,并计算状态预测求积点误差矩阵
F=Sk/k-1(S'k/k-1)-1
其中,Sk/k-1为Pk/k-1的方根矩阵,S'k/k-1为(Pk/k-1-Qk-1)的方根矩阵;
⑤计算一步预测状态的求积点
步骤三(C)量测更新:
①计算一步预测量测的求积点
②计算量测的一步预测并计算量测预测求积点的误差矩阵
③计算量测预测误差协方差矩阵Pzz,k/k-1、状态和量测的互协方差矩阵Pxz,k/k-1;
④计算卡尔曼滤波增益Kk,并更新k时刻的状态估计和误差协方差矩阵Pk/k:
⑤计算系数矩阵G和H,并计算更新求积点误差矩阵
G=Sk/kD-1
H=Sk/kD-1Wk
其中,Sk/k为Pk/k的方根矩阵,D为的方根矩阵;
⑥计算更新后的求积点
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910908134.6A CN110567455B (zh) | 2019-09-25 | 2019-09-25 | 一种求积更新容积卡尔曼滤波的紧组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910908134.6A CN110567455B (zh) | 2019-09-25 | 2019-09-25 | 一种求积更新容积卡尔曼滤波的紧组合导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110567455A true CN110567455A (zh) | 2019-12-13 |
CN110567455B CN110567455B (zh) | 2023-01-03 |
Family
ID=68782043
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910908134.6A Active CN110567455B (zh) | 2019-09-25 | 2019-09-25 | 一种求积更新容积卡尔曼滤波的紧组合导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110567455B (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112197767A (zh) * | 2020-10-10 | 2021-01-08 | 江西洪都航空工业集团有限责任公司 | 一种在线改进滤波误差的滤波器设计方法 |
CN113916220A (zh) * | 2021-08-30 | 2022-01-11 | 西北工业大学 | 一种具有协方差反馈控制的动态自适应导航定位方法 |
CN114459472A (zh) * | 2022-02-15 | 2022-05-10 | 上海海事大学 | 一种容积卡尔曼滤波器和离散灰色模型的组合导航方法 |
CN115265528A (zh) * | 2022-06-29 | 2022-11-01 | 烟台哈尔滨工程大学研究院 | 基于未知输入观测器的组合导航***鲁棒抗扰滤波方法 |
CN116067370A (zh) * | 2023-04-03 | 2023-05-05 | 广东智能无人***研究院(南沙) | 一种imu姿态解算方法及设备、存储介质 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101706284A (zh) * | 2009-11-09 | 2010-05-12 | 哈尔滨工程大学 | 提高船用光纤陀螺捷联惯导***定位精度的方法 |
EP2330382A2 (en) * | 2009-12-03 | 2011-06-08 | Honeywell International Inc. | Method and system for latitude adaptive navigation quality estimation |
CN103134491A (zh) * | 2011-11-30 | 2013-06-05 | 上海宇航***工程研究所 | Geo轨道转移飞行器sins/cns/gnss组合导航*** |
CN103591965A (zh) * | 2013-09-12 | 2014-02-19 | 哈尔滨工程大学 | 一种舰载旋转式捷联惯导***在线标定的方法 |
CN103727941A (zh) * | 2014-01-06 | 2014-04-16 | 东南大学 | 基于载体系速度匹配的容积卡尔曼非线性组合导航方法 |
CN103968843A (zh) * | 2014-05-21 | 2014-08-06 | 哈尔滨工程大学 | 一种gps/sins超紧组合导航***自适应混合滤波方法 |
CN109000642A (zh) * | 2018-05-25 | 2018-12-14 | 哈尔滨工程大学 | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 |
US20190129044A1 (en) * | 2016-07-19 | 2019-05-02 | Southeast University | Cubature Kalman Filtering Method Suitable for High-dimensional GNSS/INS Deep Coupling |
-
2019
- 2019-09-25 CN CN201910908134.6A patent/CN110567455B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101706284A (zh) * | 2009-11-09 | 2010-05-12 | 哈尔滨工程大学 | 提高船用光纤陀螺捷联惯导***定位精度的方法 |
EP2330382A2 (en) * | 2009-12-03 | 2011-06-08 | Honeywell International Inc. | Method and system for latitude adaptive navigation quality estimation |
CN103134491A (zh) * | 2011-11-30 | 2013-06-05 | 上海宇航***工程研究所 | Geo轨道转移飞行器sins/cns/gnss组合导航*** |
CN103591965A (zh) * | 2013-09-12 | 2014-02-19 | 哈尔滨工程大学 | 一种舰载旋转式捷联惯导***在线标定的方法 |
CN103727941A (zh) * | 2014-01-06 | 2014-04-16 | 东南大学 | 基于载体系速度匹配的容积卡尔曼非线性组合导航方法 |
CN103968843A (zh) * | 2014-05-21 | 2014-08-06 | 哈尔滨工程大学 | 一种gps/sins超紧组合导航***自适应混合滤波方法 |
US20190129044A1 (en) * | 2016-07-19 | 2019-05-02 | Southeast University | Cubature Kalman Filtering Method Suitable for High-dimensional GNSS/INS Deep Coupling |
CN109000642A (zh) * | 2018-05-25 | 2018-12-14 | 哈尔滨工程大学 | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 |
Non-Patent Citations (3)
Title |
---|
ZHAO XIN: "Robust Adaptive Cubature Kalman Filters and Its Application to Ultra-Tightly Coupled SINS-GPS Navigation System", 《SENSORS》 * |
张召友等: "3种确定性采样非线性滤波算法的复杂度分析", 《哈尔滨工业大学学报》 * |
高健: "基于CKF的北斗/SINS紧组合导航算法研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112197767A (zh) * | 2020-10-10 | 2021-01-08 | 江西洪都航空工业集团有限责任公司 | 一种在线改进滤波误差的滤波器设计方法 |
CN113916220A (zh) * | 2021-08-30 | 2022-01-11 | 西北工业大学 | 一种具有协方差反馈控制的动态自适应导航定位方法 |
CN113916220B (zh) * | 2021-08-30 | 2023-06-23 | 西北工业大学 | 一种具有协方差反馈控制的动态自适应导航定位方法 |
CN114459472A (zh) * | 2022-02-15 | 2022-05-10 | 上海海事大学 | 一种容积卡尔曼滤波器和离散灰色模型的组合导航方法 |
CN115265528A (zh) * | 2022-06-29 | 2022-11-01 | 烟台哈尔滨工程大学研究院 | 基于未知输入观测器的组合导航***鲁棒抗扰滤波方法 |
CN116067370A (zh) * | 2023-04-03 | 2023-05-05 | 广东智能无人***研究院(南沙) | 一种imu姿态解算方法及设备、存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN110567455B (zh) | 2023-01-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110567455B (zh) | 一种求积更新容积卡尔曼滤波的紧组合导航方法 | |
CN109211276B (zh) | 基于gpr与改进的srckf的sins初始对准方法 | |
CN108226980B (zh) | 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法 | |
CN108827310B (zh) | 一种船用星敏感器辅助陀螺仪在线标定方法 | |
CN107525503B (zh) | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 | |
Jiancheng et al. | Study on innovation adaptive EKF for in-flight alignment of airborne POS | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
CN106871928B (zh) | 基于李群滤波的捷联惯性导航初始对准方法 | |
CN104635251B (zh) | 一种ins/gps组合定位定姿新方法 | |
CN105737823B (zh) | 一种基于五阶ckf的gps/sins/cns组合导航方法 | |
CN108120994B (zh) | 一种基于星载gnss的geo卫星实时定轨方法 | |
CN109724599A (zh) | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 | |
CN108387227B (zh) | 机载分布式pos的多节点信息融合方法及*** | |
CN108680942B (zh) | 一种惯性/多天线gnss组合导航方法及装置 | |
CN110702143B (zh) | 基于李群描述的sins捷联惯性导航***动基座快速初始对准方法 | |
CN110514203B (zh) | 一种基于isr-ukf的水下组合导航方法 | |
CN104655131A (zh) | 基于istssrckf的惯性导航初始对准方法 | |
CN109945895B (zh) | 基于渐消平滑变结构滤波的惯性导航初始对准方法 | |
CN104062672A (zh) | 基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法 | |
Xue et al. | In-motion alignment algorithm for vehicle carried SINS based on odometer aiding | |
CN105806363A (zh) | 基于srqkf的sins/dvl水下大失准角对准方法 | |
CN114777812B (zh) | 一种水下组合导航***行进间对准与姿态估计方法 | |
CN115616643B (zh) | 一种城市区域建模辅助的定位方法 | |
CN111795708B (zh) | 晃动基座条件下陆用惯性导航***的自适应初始对准方法 | |
CN112857398A (zh) | 一种系泊状态下舰船的快速初始对准方法和装置 |
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 |