CN102297695A - 一种深组合导航系统中的卡尔曼滤波处理方法 - Google Patents
一种深组合导航系统中的卡尔曼滤波处理方法 Download PDFInfo
- Publication number
- CN102297695A CN102297695A CN2010102060066A CN201010206006A CN102297695A CN 102297695 A CN102297695 A CN 102297695A CN 2010102060066 A CN2010102060066 A CN 2010102060066A CN 201010206006 A CN201010206006 A CN 201010206006A CN 102297695 A CN102297695 A CN 102297695A
- Authority
- CN
- China
- Prior art keywords
- centerdot
- rho
- observability
- pseudorange
- navigation system
- 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
Images
Landscapes
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
本发明涉及一种深组合导航系统中的卡尔曼滤波处理方法,包括以下步骤:(1)建立滤波状态方程和基于伪距/伪距率量测方程;(2)选取时变系统的第一时间段;(3)计算当前时间段的可观测性矩阵;(4)计算当前的SOM矩阵;(5)计算当前时间段的外观测量;(6)计算当前时间段可观测性矩阵的奇异值;(7)求出每一个奇异值所对应的状态变量X(0)的大小,进行可观测性分析和可观测度计算;(8)循环执行(3)~(8)直至完成分析的全部时间段;(9)根据状态变量X(0)的可观测性和可观测度分析的结果,进行卡尔曼滤波器的改进设计和参数选择。本发明通过对组合导航系统进行可观测性和可观测度分析,根据分析结果对卡尔曼滤波器进行改进设计,提高了组合导航系统的滤波估计效果和导航精度。
Description
技术领域
本发明属于导航技术领域,尤其是一种深组合导航系统中的卡尔曼滤波处理方法。
背景技术
全球定位系统(GPS)与惯性导航系统(INS)具有极强的功能互补性,两者构成的深组合导航系统得到了越来越广泛的应用。卡尔曼滤波处理方法是组合导航系统处理数据最常用的算法。卡尔曼滤波器的有效性就取决于系统的可观测性,对于不完全可观测的系统,不仅需要知道哪些状态变量是可观测的,而且还需要知道每个状态变量的可观测程度。现有的卡尔曼滤波器虽然可以在对时变系统进行卡尔曼滤波处理之前,应用PWCS方法定性地分析系统状态能否全部被观测到,或者判断哪些系统状态(或状态的线性组合)的可观测,哪些系统状态是不可观测的,然而其无法定量地给出某个系统状态在不同时段的可观测度,系统的不完全可观测使得组合导航系统无法获得更高的定位精度。
发明内容
本发明的目的在于克服现有技术的不足,提供一种能够获得高定向精度的深组合导航系统中的卡尔曼滤波处理方法。
本发明解决其技术问题是采取以下技术方案实现的:
一种深组合导航系统中的卡尔曼滤波处理方法,包括以下步骤:
(1)建立卡尔曼滤波器的滤波状态方程和基于伪距/伪距率量测方程:
①按如下公式建立卡尔曼滤波器的滤波状态方程:
在公式(1)中
其中:为姿态误差,δvE,δvN,δvU,为速度误差,为位置误差,εx,εy,εz为陀螺漂移,为加速度计零偏;δtu,δtru为接收机时钟偏差和时钟漂移,fI为INS误差状态矩阵,fG为GPS误差状态矩阵;
②按如下公式建立基于伪距/伪距率的量测方程:
Z=HX+V (2)
在公式(2)中
Zρ=[δρ1 δρ2…δρi]T,
Hρ=[0i×3 0i×3 Hρ1 0i×3 0i×3 Hρ2]i×17,
bj1=cosEljsinAzj,bj2=cosEljcosAzj,bj3=sinElj
其中:i为参与导航定位可见卫星数,j为卫星号(j=1,2,...,i);δρi和分别为伪距差和伪距率差;Vρ和分别为伪距和伪距率残留误差;Azj和Elj分别为第j颗卫星的高度角和方位角;RN地球子午圈曲率半径;h为高度;纬度;e为椭球扁率;
(2)选取时变系统的第一时间段,令时间段变量j=1;
(5)由GPS系统和惯性导航系统直接计算得到当前时间段的外观测量
(7)按照如下公式求出每一个奇异值所对应的状态变量X(0)的大小,并根据状态变量X(0)的大小判断出相应变量的可观测性和可观测度,并且计算出可观测变量的可观测程度:
其中,ηk表示第k个状态变量的可观测度;σ0表示外观测量所对应的奇异值;σi表示使(i=1,2,…n)取得最大的奇异值;
(8)如果当前时间段不是最后的时间段,返回到第(3)步继续循环进行下一时间段的分析,直至完成分析的全部时间段;
(9)根据状态变量X(0)的可观测性和可观测度分析的结果,判断各个状态变量的可观测性以及可观测变量的可观测度,并采用如下三种方式之一进行卡尔曼滤波器的改进设计和参数选择:加入外界观测信息、去除系统状态方程中不可观测的变量和可观测度低的变量、改变系统的机动性。
而且,所述步骤(9)中加入外界观测信息后建立如下的基于伪距/伪距率的量测方程:
在公式(7)中:
其中:bb为载体坐标系中已知基线向量,λ为载波波长,ΔNj分别为差分载波相位及其整周模糊度,j为导航定位卫星号,sj为卫星j至接收机天线的方向余弦向量,be为地心地固坐标系中基线向量,为载波相位测量噪声,与分别为计算姿态矩阵和真是姿态矩阵,且有 φ为姿态误差角。
而且,所述的外界观测信息包括姿态信息、伪距和伪距率信息。
而且,所述的时间段的选值范围为0.5~1秒。
本发明的优点和积极效果是:
本发明通过PWCS方法对组合导航系统进行可观测性和可观测度分析,能够判别出系统的可观测变量和不可观测变量以及可观测变量的可观测程度,并在此基础上对卡尔曼滤波器进行改进设计,提高了组合导航系统的滤波估计效果,在初始对准时能获得高对准精度、在导航阶段能获得高定向精度。本发明处理方法简单,不需要进行实际的卡尔曼滤波计算,使得提前对卡尔曼滤波器进行改进设计成为可能。
附图说明
图1是本发明的深组合导航系统框图;
图2是未加入外界观测信息的深组合导航系统可观测度直方图;
图3是加入外界观测信息后的全组合导航系统可观测度直方图。
具体实施方式
以下结合附图对本发明实施例做进一步详述:
一种深组合导航系统中的卡尔曼滤波处理方法,是在如图1所示的深组合导航系统上实现的,该组合导航系统将GPS定位系统与惯性导航系统组合在一起实现深组合导航功能。惯性导航系统将速度及位置信息传送给INS伪距伪距率计算模块,INS伪距伪距率计算模块同时利用卫星星历计算卫星位置,进而得到INS到GPS的伪距和伪距率,同时将计算得到的伪距和伪距率发送给滤波量程计算模块;GPS接收机控制处理模块通过天线接收GPS信号并将惯性辅助变量处理后也发送给滤波量程计算模块,滤波量程计算模块将计算处理后的伪距伪距率的量测方程送入卡尔曼滤波器中,卡尔曼滤波器处理后将姿态误差、速度误差、位置误差、陀螺漂移、加速度计零偏反馈给GPS接收机控制处理模块和惯性导航系统中。
一种深组合导航系统中的卡尔曼滤波处理方法,包括以下步骤:
(1)建立卡尔曼滤波器的滤波状态方程和基于伪距/伪距率量测方程:
①按照如下公式建立卡尔曼滤波器的滤波状态:
在公式(1)中
w为白噪声,
其中:为姿态误差,δvE,δvN,δvU为速度误差,为位置误差,εx,εy,εz为陀螺漂移,为加速度计零偏;δtu,δtru为接收机时钟偏差和时钟漂移,fI为INS误差状态矩阵,fG为GPS误差状态矩阵;
②按照如下公式建立卡尔曼滤波器的基于伪距/伪距率的量测方程为:
Z=HX+V (2)
在公式(2)中
Zρ=[δρ1 δρ2…δρi]T,
Hρ=[0i×3 0i×3 Hρ1 0i×3 0i×3 Hρ2]i×17,
bj1=cosEljsinAzj,bj2=cosEljcosAzj,bj3=sinElj
其中:i为参与导航定位可见卫星数,j为卫星号(j=1,2,...,i);δρi和分别为伪距差和伪距率差;Vρ和分别为伪距和伪距率残留误差;Azj和Elj分别为第j颗卫星的高度角和方位角;RN地球子午圈曲率半径;h为高度;纬度;e为椭球扁率;
(2)选取时变系统的第一时间段,令时间段变量j=1;
(7)按照如下公式求出每一个奇异值所对应的状态变量X(0)的大小,并根据状态变量X(0)的大小判断出相应变量的可观测性和可观测度,并且计算出可观测变量的可观测程度:
(8)如果当前时间段不是最后的时间段,返回到第(3)步继续循环进行下一时间段的分析,直至完成分析的全部时间段;
(9)根据状态变量X(0)的可观测性和可观测度分析的结果,能够判断出系统的各个变量的可观测性以及可观测变量的可观测度,采用如下三种方式之一进行卡尔曼滤波器的改进设计和参数选择:加入外界观测信息(比如姿态信息、速度信息等)来提高系统滤波的效果,通过去掉系统状态方程中不可观测的变量和可观测度低的变量,即采用降阶滤波的方法来提高滤波估计的效果和减少系统运算量;此外还可以通过改变系统的机动性来提高滤波估计效果;
下面以增加外界观测信息方式提高系统滤波估计效果为例进行说明,即通过加入GPS载波相位姿态观测量后按照如下公式建立全组合导航系统的量测方程如下:
在公式(7)中:
其中:bb为载体坐标系中已知基线向量,λ为载波波长,ΔNj分别为差分载波相位及其整周模糊度,j为导航定位卫星号,sj为卫星j至接收机天线的方向余弦向量,be为地心地固坐标系(简称e系)中基线向量,为载波相位测量噪声。与分别为计算姿态矩阵和真是姿态矩阵,且有 φ为姿态误差角。
通过上述步骤可以实现本发明的卡尔曼滤波处理方法。在表1中给出了在深组合导航系统加入外界观测信息(GPS载波相位姿态观测量)后构成的全组合导航系统和深组合导航系统在第2时间段(2#)的可观测度计算结果,为确保系统矩阵的变化量可以忽略不计,系统分段时间间隔应该取为较小值,例如0.5~1秒,本实施例取时间段为0.5秒。从表1可以看出深组合导航系统为不完全可观测系统,某些误差状态变量无法估计,而全组合导航系统为完全可观测系统。图2和图3分别给出了未加入外界观测信息的深组合导航系统可观测度直方图和加入外界观测信息后的全组合导航系统可观测度直方图,通过对比分析可以看出,加入外界观测信息后的全组合导航系统各状态变量的可观测度明显提高。
从计算结果可以看出,加入GPS姿态测量信息组成的全组合导航系统在任何时刻都是完全可观测系统,相对深组合导航系统具有更好的滤波效果,使得全组合导航系统在初始对准时能获得更高的对准精度、在导航阶段能获得更高的定位和定向精度。
表1全组合导航系统和深组合导航系统可观测度
需要强调的是,本发明所述的实施例是说明性的,而不是限定性的,因此本发明并不限于具体实施方式中所述的实施例,凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式,同样属于本发明保护的范围。
Claims (4)
1.一种深组合导航系统中的卡尔曼滤波处理方法,其特征在于:包括以下步骤:
(1)建立卡尔曼滤波器的滤波状态方程和基于伪距/伪距率量测方程:
①按如下公式建立卡尔曼滤波器的滤波状态方程:
X(t)=A(t)X(t)+w,w~N(0,Q) (1)
在公式(1)中
其中:为姿态误差,δvE,δvN,δvU为速度误差,δλ,δh为位置误差,εx,εy,εz为陀螺漂移,为加速度计零偏;δtu,δtru为接收机时钟偏差和时钟漂移,fI为INS误差状态矩阵,fG为GPS误差状态矩阵;
②按如下公式建立基于伪距/伪距率的量测方程:
Z=HX+V (2)
在公式(2)中
Zρ=[δρ1 δρ2…δρi]T,
Hρ=[0i×3 0i×3 Hρ1 0i×3 0i×3 Hρ2]i×17,
aj3=sinElj
bj1=cosEljsinAzj,bj2=cosEljcosAzj,bj3=sinElj
其中:i为参与导航定位可见卫星数,j为卫星号(j=1,2,...,i);δρi和分别为伪距差和伪距率差;Vρ和分别为伪距和伪距率残留误差;Azj和Elj分别为第j颗卫星的高度角和方位角;RN地球子午圈曲率半径;h为高度;纬度;e为椭球扁率;
(2)选取时变系统的第一时间段,令时间段变量j=1;
(7)按照如下公式求出每一个奇异值所对应的状态变量X(0)的大小,并根据状态变量X(0)的大小判断出相应变量的可观测性和可观测度,并且计算出可观测变量的可观测程度:
(8)如果当前时间段不是最后的时间段,返回到第(3)步继续循环进行下一时间段的分析,直至完成分析的全部时间段;
(9)根据状态变量X(0)的可观测性和可观测度分析的结果,判断各个状态变量的可观测性以及可观测变量的可观测度,并采用如下三种方式之一进行卡尔曼滤波器的改进设计和参数选择:加入外界观测信息、去除系统状态方程中不可观测的变量和可观测度低的变量、改变系统的机动性。
3.根据权利要求1或2所述的一种深组合导航系统中的卡尔曼滤波处理方法,其特征在于:所述的外界观测信息包括姿态信息、伪距和伪距率信息。
4.根据权利要求1所述的一种深组合导航系统中的卡尔曼滤波处理方法,其特征在于:所述的时间段的选值范围为0.5~1秒。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201010206006.6A CN102297695B (zh) | 2010-06-22 | 2010-06-22 | 一种深组合导航系统中的卡尔曼滤波处理方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201010206006.6A CN102297695B (zh) | 2010-06-22 | 2010-06-22 | 一种深组合导航系统中的卡尔曼滤波处理方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN102297695A true CN102297695A (zh) | 2011-12-28 |
CN102297695B CN102297695B (zh) | 2015-05-06 |
Family
ID=45358271
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201010206006.6A Active CN102297695B (zh) | 2010-06-22 | 2010-06-22 | 一种深组合导航系统中的卡尔曼滤波处理方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN102297695B (zh) |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102830415A (zh) * | 2012-08-31 | 2012-12-19 | 西北工业大学 | 一种降维度的基于Carlson滤波算法的快速组合导航方法 |
CN103245963A (zh) * | 2013-05-09 | 2013-08-14 | 清华大学 | 双天线gnss/ins深组合导航方法及装置 |
CN103792561A (zh) * | 2014-02-21 | 2014-05-14 | 南京理工大学 | 一种基于gnss通道差分的紧组合降维滤波方法 |
CN104871032A (zh) * | 2012-12-20 | 2015-08-26 | 大陆-特韦斯贸易合伙股份公司及两合公司 | 用于提供gnss信号的方法 |
CN106996778A (zh) * | 2017-03-21 | 2017-08-01 | 北京航天自动控制研究所 | 误差参数标定方法及装置 |
CN107643534A (zh) * | 2017-09-11 | 2018-01-30 | 东南大学 | 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法 |
CN110646822A (zh) * | 2018-06-26 | 2020-01-03 | 北京自动化控制设备研究所 | 一种基于惯导辅助的整周模糊度Kalman滤波算法 |
CN112346485A (zh) * | 2020-12-24 | 2021-02-09 | 华中光电技术研究所(中国船舶重工集团公司第七一七研究所) | 一种光电跟踪控制方法、系统、电子设备及存储介质 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20050125141A1 (en) * | 2003-12-05 | 2005-06-09 | Honeywell International Inc. | System and method for using multiple aiding sensors in a deeply integrated navigation system |
CN1945212A (zh) * | 2006-11-03 | 2007-04-11 | 北京航空航天大学 | 一种sins/gps组合导航系统的自适应加权反馈校正滤波方法 |
-
2010
- 2010-06-22 CN CN201010206006.6A patent/CN102297695B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20050125141A1 (en) * | 2003-12-05 | 2005-06-09 | Honeywell International Inc. | System and method for using multiple aiding sensors in a deeply integrated navigation system |
CN1945212A (zh) * | 2006-11-03 | 2007-04-11 | 北京航空航天大学 | 一种sins/gps组合导航系统的自适应加权反馈校正滤波方法 |
Non-Patent Citations (3)
Title |
---|
SINPYO HONG ECT.: "Observability Measures and Their Application to GPS/INS", 《IEEE TRANSACTIONS ON VEHICULAR TECHNOLOGY》 * |
帅平等: "GPS/ SINS 组合导航系统状态的可观测度分析方法", 《宇航学报》 * |
黄昆等: "MSINS/GPS 组合导航系统选星算法", 《中国惯性技术学报》 * |
Cited By (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102830415B (zh) * | 2012-08-31 | 2014-03-12 | 西北工业大学 | 一种降维度的基于Carlson滤波算法的快速组合导航方法 |
CN102830415A (zh) * | 2012-08-31 | 2012-12-19 | 西北工业大学 | 一种降维度的基于Carlson滤波算法的快速组合导航方法 |
US10353076B2 (en) | 2012-12-20 | 2019-07-16 | Continental Teves Ag & Co. Ohg | Method for providing a GNSS signal |
CN104871032A (zh) * | 2012-12-20 | 2015-08-26 | 大陆-特韦斯贸易合伙股份公司及两合公司 | 用于提供gnss信号的方法 |
CN103245963A (zh) * | 2013-05-09 | 2013-08-14 | 清华大学 | 双天线gnss/ins深组合导航方法及装置 |
CN103792561A (zh) * | 2014-02-21 | 2014-05-14 | 南京理工大学 | 一种基于gnss通道差分的紧组合降维滤波方法 |
CN103792561B (zh) * | 2014-02-21 | 2016-04-20 | 南京理工大学 | 一种基于gnss通道差分的紧组合降维滤波方法 |
CN106996778A (zh) * | 2017-03-21 | 2017-08-01 | 北京航天自动控制研究所 | 误差参数标定方法及装置 |
CN106996778B (zh) * | 2017-03-21 | 2019-11-29 | 北京航天自动控制研究所 | 误差参数标定方法及装置 |
CN107643534A (zh) * | 2017-09-11 | 2018-01-30 | 东南大学 | 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法 |
CN107643534B (zh) * | 2017-09-11 | 2019-07-12 | 东南大学 | 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法 |
CN110646822A (zh) * | 2018-06-26 | 2020-01-03 | 北京自动化控制设备研究所 | 一种基于惯导辅助的整周模糊度Kalman滤波算法 |
CN112346485A (zh) * | 2020-12-24 | 2021-02-09 | 华中光电技术研究所(中国船舶重工集团公司第七一七研究所) | 一种光电跟踪控制方法、系统、电子设备及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN102297695B (zh) | 2015-05-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102297695B (zh) | 一种深组合导航系统中的卡尔曼滤波处理方法 | |
CN102230971B (zh) | Gps多天线测姿方法 | |
CN102928858B (zh) | 基于改进扩展卡尔曼滤波的gnss单点动态定位方法 | |
CN101793522B (zh) | 基于抗差估计的稳健滤波方法 | |
CN103900565B (zh) | 一种基于差分gps的惯导系统姿态获取方法 | |
CN102819029B (zh) | 一种超紧组合卫星导航接收机 | |
CN105891863B (zh) | 一种基于高度约束的扩展卡尔曼滤波定位方法 | |
CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN106291639A (zh) | 一种gnss接收机实现定位的方法及装置 | |
CN105467415B (zh) | 一种基于差分气压高度约束的小型无人机rtk相对定位方法 | |
CN103900613B (zh) | 一种基于磁力计n阶距检测的mems系统误差估计方法 | |
CN102016631A (zh) | 移动物体间载波相位定位设备和方法 | |
CN101833080A (zh) | 一种利用gps系统附加约束条件的载体姿态测量方法 | |
CN106405592B (zh) | 车载北斗载波相位周跳检测与修复方法及系统 | |
CN102853837A (zh) | 一种mimu和gnss信息融合的方法 | |
CN103017787A (zh) | 适用于摇摆晃动基座的初始对准方法 | |
CN103454664A (zh) | 一种基于陀螺测量信息约束的gnss载波相位模糊度求解方法 | |
CN103424127B (zh) | 一种速度加比力匹配传递对准方法 | |
Wang et al. | Influence of dynamics and trajectory on integrated GPS/INS navigation performance | |
Hsu et al. | Intelligent viaduct recognition and driving altitude determination using GPS data | |
Rahman et al. | Earth-centered Earth-fixed (ECEF) vehicle state estimation performance | |
CN113466909A (zh) | 一种gnss多频系统部分整周模糊度子集选取方法 | |
CN102830410B (zh) | 卫星导航中结合多普勒测速的定位方法 | |
CN102944888A (zh) | 一种基于二阶扩展卡尔曼的低运算量gps定位方法 | |
Farrell | Full integrity testing for GPS/INS |
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 |