CN103575298A - 基于自调节的ukf失准角初始对准方法 - Google Patents
基于自调节的ukf失准角初始对准方法 Download PDFInfo
- Publication number
- CN103575298A CN103575298A CN201310563483.1A CN201310563483A CN103575298A CN 103575298 A CN103575298 A CN 103575298A CN 201310563483 A CN201310563483 A CN 201310563483A CN 103575298 A CN103575298 A CN 103575298A
- Authority
- CN
- China
- Prior art keywords
- mrow
- misalignment angle
- msub
- filtering
- mover
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 21
- 238000001914 filtration Methods 0.000 claims abstract description 50
- 238000005259 measurement Methods 0.000 claims abstract description 16
- 239000011159 matrix material Substances 0.000 description 11
- 238000004422 calculation algorithm Methods 0.000 description 5
- 230000009466 transformation Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 238000004088 simulation Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000001105 regulatory effect Effects 0.000 description 2
- 241000287196 Asthenes Species 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 230000000875 corresponding effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012886 linear function Methods 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000004451 qualitative analysis Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000011426 transformation method Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- 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/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
Abstract
Description
技术领域
本发明涉及滤波领域,尤其涉及一种基于自调节的UKF失准角初始对准方法。
背景技术
状态估计的主要任务是如何将***的状态向量从含有噪声的观测量中估计出来。非线性滤波方法在近几十年来已经成为各行业研究的热点。特别是随着计算机仿真技术的不断发展,非线性滤波理论已得到了很大的发展。工程上常用的非线性滤波方法当属扩展卡尔曼滤波(Extended Kalman Filter,EKF)和无迹卡尔曼滤波(Unscented Kalman Filter,UKF)。但是,EKF存在两个比较大的缺点,致使其应用受到一定的限制。首先,当***的非线性度比较强时,线性化后导致的滤波误差会比较大,甚至会导致滤波发散。其次,在进行线性化时需要求雅克比矩阵,使得计算量增大。而且,当***不连续时,也无法应用EKF。
UKF滤波的核心是无迹(Unscented,UT)变换,它通过采取2n+1个确定性点(即Sigma点,2n个点的权值为1/2(n+κ),1个点的权值为κ/n+κ),经非线性映射后逼近高斯正态变量的均值和协方差。这种非线性变换方法的精度能够达到非线性***泰勒展开式的三阶项以上。
自由调节参数κ对于滤波的精度和稳定性会有很大的影响。但是如何设置κ却没有一个严格的理论进行说明。通常认为,如果状态变量服从高斯正态分布时,应该满足n+κ=3。这样,非线性变换能够捕获***大部分的四阶距信息。随着容积卡尔曼滤波(Cubature Kalman Filter,CKF)滤波的产生,滤波参数的选取更是成为了人们关注的焦点。因为从滤波方法上来讲,CKF滤波就是UKF滤波在κ取0时的一种应用。通过理论分析和实验仿真也表明当***为3维及以下时,应选取UKF滤波;当***为3维以上时应选取CKF滤波。但是,对于不同的非线性函数g(x),由于非线性度的不同,可能会导致在进行泰勒级数展开时一些高阶矩信息对UT变换的影响会大于大部分的4阶矩信息。这样,有可能一些高阶矩信息对于变换后统计均值和方差的求取是十分有利的。因此,κ的选取和***模型本身具有很大的相关性的,而且κ在每一次滤波时都应该有一个最优值。
发明内容
本发明提供了一种基于自调节的UKF失准角初始对准方法,用于解决现有技术中UKF滤波鲁棒性差的问题。
一方面,提供了一种基于自调节的UKF大失准角初始对准新方法,包括:
步骤一、根据惯导的误差特性,建立滤波的非线性误差模型,即状态方程和量测方程。
步骤二、设置滤波初值。令和P0|-1=P0。其中,x0为状态变量的初值,P0为状态变量的初始协方差。
令k=k+1,算法继续从步骤三进行;具体算法见图2。
UKF滤波的基础是Unscented变换(UT变换),这种变换是不同维数线性空间的非线性映射。它通过采取一些固定性的点进行逼近变换后的均值与方差,变换过程见图1。由于在实际的大***中,很难满足***的状态变量严格服从高斯正态分布。而且对于滤波模型的建立也很难做到准确无误,总会存在着一定的误差。因此,很难做到事先确定好固定的滤波调节参数κ的值。据此通过κ对滤波量测量的影响能够在线的确定出最优值,从而更加准确的估计出状态变量的均值和协方差。将此发明应用到SINS大失准角初始对准中,能够有效提高滤波的精度、减小估计的失准角误差。
通过上述方法,本发明具有以下优点:
一、本发明增加了UKF滤波的鲁棒性,提高了抗干扰能力;
二、本发明可以使UKF滤波调节参数的选取不在依赖于***的模型与状态的分布特性;
三、本发明可以有效的保证UKF滤波应用在捷联惯导***(Strapdown Intertial Navigation System,SINS)大失准角初始对准中的滤波精度和稳定性。
附图说明
图1是UT变换原理图;
图2是自调节UKF滤波的算法示意图;
图3是方位失准角的估计精度示意图;
图4是方位失准角的稳态精度示意图。
具体实施方式
下面结合附图,对本发明的实现方式进行详细说明。
本发明实施例提供了一种基于自调节的UKF大失准角初始对准新方法,包括:
步骤一、根据惯导的误差特性,建立滤波的非线性误差模型,非线性误差模型包括:状态方程和量测方程。
优选的,所述步骤一中,在惯导初始对准中一般是通过***的速度误差量去估计失准角的大小,而在粗对准后通常水平失准角是小失准角,方位失准角是大失准角,因此,有必要建立起***的非线性误差模型。其静基座初始对准的状态方程如下:
其中,φx、φy和φz为三个方向的失准角(状态量);δvx和δvy为东向和北向速度误差(状态量);为当地纬度;ωie为地球自转角速度;和为三个轴向的陀螺漂移;和为三个轴向的加速度零偏;fx、fy和fz为加速度输出比例在计算地理系上的值;Cij是载体系到地理系的方向矩阵中的元素;C'ij是载体系到计算地理系矩阵中的元素。
初始对准的量测方程如下:
Z=HX+V
其中,量测量Z为惯导东向和北向速度误差δvx,δvy;H取H=[I2×2 02×3](I2×2为2维的单位矩阵;O2×3为2行3列的零矩阵);V为量测噪声。
步骤一包括:在SINS***的粗对准阶段,可以通过引入惯导的航向姿态信息迅速将数学平台对准到导航坐标系下以减小初始的失准角。而在精对准阶段可以通过多普勒提供速度信息作为外观测量,利用自调节的无迹卡尔曼滤波完成精对准过程。
所述步骤二中,由于惯导的非线性滤波模型是可观测且可控的,因此滤波初值的选取不会影响滤波的精度和稳定性。在没有任何先验信息的情况下,可以令初始状态的均值为零,协方差为无穷大。
在步骤三中,根据量测量的信息去估计最优的κ值。一般来讲并不能准确的知道量测量的分布特性。但是,作为一种近似高斯正态分布的假定是合理的,它符合自然界一种能量集中的原则。而且分布的方差越小,说明各个量测量越接近真值。因此,这里在利用量测量确定最优的κ值时,只要保证量测量的方差最小即可。
步骤三包括以下步骤:
步骤三一:确定UKF滤波的误差特性。
vec(A)定义如下:
其中,A是一个p×q的矩阵,ai是它的第i列。依据泰勒级数展开式,则UT变换误差如下:
为了清晰的表达数学变量,记
由于 且
则可以得到误差的表达式,如下:
根据对称性,各奇阶矩均为零。UT变换的误差可以表达如下:
步骤三二:误差特性的定性分析与κ的取值原则。
根据UT变换的误差表达式,UKF滤波的误差取决于***的模型与状态变量的分布。按照传统的UT变换中Sigma点的选取与权值的规定,非线性变换后能够精确的捕获到***的2阶距信息。UT变换的算法框图如图2所示。此时,不同的κ值捕获高阶距信息的能力是不同的。而且,在每一次滤波中κ都应该有一个最优的值,自调节算法正是根据每一次量测后,通过量测信息给出一个最优的值。
对于κ的取值,一般情况下,κmin取为0,κmax不超过9nx。这样能够保证不忽略非中心权值粒子的影响。
步骤四:在最优的κ下滤波一步预测均值和方差的求取。
估计k时刻的状态预测值:
估计k时刻的状态误差协方差预测值:
步骤五:在最优的κ下滤波均值和方差的求取。
k时刻状态估计值:
k时刻状态误差协方差估计值:
其中,Pzz,k|k-1为量测的误差协方差阵。
为比较传统UKF和自调节UKF的滤波效果,这里进行了一般性的仿真验证。初始条件设定如下:初始经度λ0=126.67°,纬度初始姿态角:ψ0=10°,θ0=0°,γ0=0°;初始失准角:φx0=φy0=0.5°,φz=10°;初始加速度a=0m/s,速度v=0m/s2;
由于水平失准角为小角度,方位失准角是大角度的,仿真结果给出方位失准角的估 计精度,如图3所示。稳态精度如图4所示。
通过比较,自调节UKF对方位失准角的估计精度明显由于传统UKF的估计精度。
上述实施例仅是本发明的优选实施例,并不用于限定本发明的保护范围,在不偏离本发明的实质的情况下,本领域技术人员能够做出各种变型,这些变型也应在本发明的保护范围之内。
Claims (4)
1.一种基于自调节的无迹卡尔曼滤波UKF失准角初始对准方法,其特征在于,包括:
步骤一、根据惯导的误差特性,建立滤波的非线性误差模型;
步骤二、设置滤波初值x0和P0,其中,x0为状态变量的初值,P0为状态变量的初始协方差;
步骤三、根据公式确定调节参数的最优值其中,表示对量测量的一步预测概率密度取最大;zk表示k时刻的量测量,zk-1为k时刻以前的所有量测量之和,κ为自由调节参数;K为κ的取值范围,且K={κ:κmin∈R,κmax∈R,κmin≤κ≤κmax},R为实数域,κmin为κ取值的下限,κmax为κ取值上限;
2.根据权利要求1所述的方法,其特征在于,所述步骤一中,在惯导初始对准中,根据***的速度误差量估计失准角的大小,在粗对准后如果确定水平失准角是小失准角,方位失准角是大失准角,则建立起***的非线性误差模型。
3.根据权利要求2所述的方法,其特征在于,所述小失准角是小于第一预定角度的失准角,所述大失准角是大于第二预定角度的失准角。
4.根据权利要求1所述的方法,其特征在于,所属步骤二包括:设置x0为零,P0为无穷大。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310563483.1A CN103575298A (zh) | 2013-11-14 | 2013-11-14 | 基于自调节的ukf失准角初始对准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310563483.1A CN103575298A (zh) | 2013-11-14 | 2013-11-14 | 基于自调节的ukf失准角初始对准方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN103575298A true CN103575298A (zh) | 2014-02-12 |
Family
ID=50047587
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201310563483.1A Pending CN103575298A (zh) | 2013-11-14 | 2013-11-14 | 基于自调节的ukf失准角初始对准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103575298A (zh) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104020480A (zh) * | 2014-06-17 | 2014-09-03 | 北京理工大学 | 一种带自适应因子的交互式多模型ukf的卫星导航方法 |
CN104374405A (zh) * | 2014-11-06 | 2015-02-25 | 哈尔滨工程大学 | 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法 |
CN104374401A (zh) * | 2014-10-15 | 2015-02-25 | 哈尔滨工程大学 | 一种捷联惯导初始对准中重力扰动的补偿方法 |
CN106840194A (zh) * | 2016-09-20 | 2017-06-13 | 南京喂啊游通信科技有限公司 | 一种大方位失准角线性对准方法 |
CN106840211A (zh) * | 2017-03-24 | 2017-06-13 | 东南大学 | 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法 |
CN107479076A (zh) * | 2017-08-08 | 2017-12-15 | 北京大学 | 一种动基座下联合滤波初始对准方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103278813A (zh) * | 2013-05-02 | 2013-09-04 | 哈尔滨工程大学 | 一种基于高阶无迹卡尔曼滤波的状态估计方法 |
CN103344260A (zh) * | 2013-07-18 | 2013-10-09 | 哈尔滨工程大学 | 基于rbckf的捷联惯导***大方位失准角初始对准方法 |
-
2013
- 2013-11-14 CN CN201310563483.1A patent/CN103575298A/zh active Pending
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103278813A (zh) * | 2013-05-02 | 2013-09-04 | 哈尔滨工程大学 | 一种基于高阶无迹卡尔曼滤波的状态估计方法 |
CN103344260A (zh) * | 2013-07-18 | 2013-10-09 | 哈尔滨工程大学 | 基于rbckf的捷联惯导***大方位失准角初始对准方法 |
Non-Patent Citations (4)
Title |
---|
JINDRICH DUNIK ET AL.: "Unscented Kalman Filter:Aspects and Adaptive Setting of Scaling Parameter", 《IEEE TRANSACTIONS ON AUTOMATIC CONTROL》 * |
王新龙: "《捷联式惯导***动、静基座初始对准》", 30 September 2013 * |
苏宛新: "自适应UKF滤波在SINS初始对准中的应用", 《中国惯性技术学报》 * |
金瑶等: "基于差分演化算法的自适应无迹卡尔曼滤波", 《电子与信息学报》 * |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104020480A (zh) * | 2014-06-17 | 2014-09-03 | 北京理工大学 | 一种带自适应因子的交互式多模型ukf的卫星导航方法 |
CN104374401A (zh) * | 2014-10-15 | 2015-02-25 | 哈尔滨工程大学 | 一种捷联惯导初始对准中重力扰动的补偿方法 |
CN104374405A (zh) * | 2014-11-06 | 2015-02-25 | 哈尔滨工程大学 | 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法 |
CN106840194A (zh) * | 2016-09-20 | 2017-06-13 | 南京喂啊游通信科技有限公司 | 一种大方位失准角线性对准方法 |
CN106840194B (zh) * | 2016-09-20 | 2019-09-27 | 南京喂啊游通信科技有限公司 | 一种大方位失准角线性对准方法 |
CN106840211A (zh) * | 2017-03-24 | 2017-06-13 | 东南大学 | 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法 |
CN107479076A (zh) * | 2017-08-08 | 2017-12-15 | 北京大学 | 一种动基座下联合滤波初始对准方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Teixeira et al. | Robust particle filter formulations with application to terrain‐aided navigation | |
CN103644903B (zh) | 基于分布式边缘无味粒子滤波的同步定位与地图构建方法 | |
CN106885569A (zh) | 一种强机动条件下的弹载深组合arckf滤波方法 | |
CN103344260B (zh) | 基于rbckf的捷联惯导***大方位失准角初始对准方法 | |
CN103575298A (zh) | 基于自调节的ukf失准角初始对准方法 | |
CN102353378B (zh) | 一种矢量形式信息分配系数的组合导航***自适应联邦滤波方法 | |
CN103278813B (zh) | 一种基于高阶无迹卡尔曼滤波的状态估计方法 | |
CN110514203B (zh) | 一种基于isr-ukf的水下组合导航方法 | |
CN104655131A (zh) | 基于istssrckf的惯性导航初始对准方法 | |
CN105091907B (zh) | Sins/dvl组合中dvl方位安装误差估计方法 | |
CN103940433B (zh) | 一种基于改进的自适应平方根ukf算法的卫星姿态确定方法 | |
CN107742026A (zh) | 一种船舶动力定位***非线性状态估计方法 | |
CN101975575A (zh) | 基于粒子滤波的被动传感器多目标跟踪方法 | |
CN105424036A (zh) | 一种低成本水下潜器地形辅助惯性组合导航定位方法 | |
CN104061932A (zh) | 一种利用引力矢量和梯度张量进行导航定位的方法 | |
CN104374405A (zh) | 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法 | |
CN111783307A (zh) | 一种高超声速飞行器状态估计方法 | |
CN103424127B (zh) | 一种速度加比力匹配传递对准方法 | |
CN103218482B (zh) | 一种动力学***中不确定参数的估计方法 | |
CN114777812B (zh) | 一种水下组合导航***行进间对准与姿态估计方法 | |
CN103792562A (zh) | 一种基于变换采样点的强跟踪ukf的滤波方法 | |
CN104613966B (zh) | 一种地籍测量事后数据处理方法 | |
CN104075711A (zh) | 一种基于CKF的IMU/Wi-Fi信号紧组合室内导航方法 | |
CN103776449A (zh) | 一种提高鲁棒性的动基座初始对准方法 | |
CN105785358A (zh) | 一种方向余弦坐标系下带多普勒量测的雷达目标跟踪方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20140212 |
|
RJ01 | Rejection of invention patent application after publication |