CN100498225C - Earth satellite self-independent astronomical navigation method based on self adaptive expansion kalman filtering - Google Patents

Earth satellite self-independent astronomical navigation method based on self adaptive expansion kalman filtering Download PDF

Info

Publication number
CN100498225C
CN100498225C CN 200610165577 CN200610165577A CN100498225C CN 100498225 C CN100498225 C CN 100498225C CN 200610165577 CN200610165577 CN 200610165577 CN 200610165577 A CN200610165577 A CN 200610165577A CN 100498225 C CN100498225 C CN 100498225C
Authority
CN
Grant status
Grant
Patent type
Prior art keywords
self
based
satellite
filtering
kalman
Prior art date
Application number
CN 200610165577
Other languages
Chinese (zh)
Other versions
CN1987355A (en )
Inventor
房建成
杨照华
宁晓琳
武瑾媛
宋婷婷
Original Assignee
北京航空航天大学
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Grant date

Links

Abstract

一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法,建立基于直角坐标系的地球卫星轨道运动学方程,建立以星光角距为量测量建立量测方程,建立离散型扩展卡尔曼滤波方程,判定离散型扩展卡尔曼滤波是否发散,利用对预测滤波残差判断扩展卡尔曼滤波器是否发散,若满足发散条件则进行噪声统计特性的估计,否则按照标准的离散型扩展卡尔曼滤波程序进行计算。 One kind of earth satellite autonomous celestial navigation method based on adaptive extended Kalman filter, to establish earth orbit satellites based on kinematic equations of the Cartesian coordinate system, the establishment of starlight from the established measurement angle measurement equation is established discrete extended Kalman filter equation, it is determined whether the discrete Kalman filter divergence, using filtering prediction residual divergence is determined whether an extended Kalman filter, if the condition is satisfied divergence estimated statistical characteristics of noise, or according to standard procedures discrete extended Kalman filter Calculation. 本发明解决了因噪声统计特性确定不准确造成的噪声滤波器发散,影响导航精度的问题,具有自主、灵活简单、精度高的特点,更适用于对导航精度要求较高的资源、通讯、侦查、气象等地球应用卫星。 The invention solves the noise due to the statistical characteristics of the noise filter to determine the exact cause is not divergence, issues affecting navigation accuracy, with independent, flexible and simple, high accuracy, is more suitable for high precision navigation resource requirements, communication, investigation Earth satellite and meteorological applications.

Description

一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法 An Adaptive Extended Kalman Filter Earth Satellite autonomous navigation method based astronomical

技术领域 FIELD

本发明涉及一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法,可用于资源、通讯、侦察、气象等地球应用卫星自主导航定位。 The present invention relates to an earth satellite autonomous celestial navigation method based on adaptive extended Kalman filter, autonomous navigation positioning resources, communications, surveillance, weather satellites and other applications can be used for the earth.

背景技术 Background technique

天文导航系统是一种不与外界进行信息传输和交换,不依赖于地面设备的完全自主的导航系统,是一种利用光学敏感器测得的天体信息进行载体位置计算的一种定位导航方法。 A celestial navigation system does not perform external transmission and exchange of information, not dependent on completely autonomous navigation system ground equipment, the use of a celestial information optical sensor is measured by a navigation method for locating the position of the support calculation. 其基本原理是利用天体量测信息结合轨道动力学方程,利用最优估计方法估计空间飞行器的位置速度等导航信息。 The basic principle is to use in conjunction with measurement information track celestial dynamics equations, position and speed estimated spacecraft navigation information using the optimal estimation method or the like. 也就是说天文导航方法可分为三部分,一是轨道动力学的精确建模,二是量测量的选择,三是滤波(估计)方法的设计。 That celestial navigation method can be divided into three parts, one accurate modeling orbital dynamics, the second choice is the measurement of the three filter design (estimation) methods.

基于轨道动力学方程的天文导航包括直接敏感地平和利用星光折射间接敏感地平的两种导航方式。 Based on the dynamic equations of celestial navigation track comprises two kinds of peace directly sensitive navigation using indirect sensitive stellar refraction horizon. 间接敏感地平要求必须一颗恒星发生大气折射,把测得的星光折射角作为量测信息,这种导航方式要求星光折射角与大气密度之间建立较精确的函数关系,而实际上很难得到大气密度的精确模型。 Indirect sensing horizon requires a star must occur refraction, the angle of refraction as measured starlight measurement information, which requires navigation function starlight establish more accurate relationship between the angle of refraction and density of the atmosphere, in fact difficult to obtain accurate model density of the atmosphere.

直接敏感地平是基于轨道动力学的另一种自主天文导航方式,它的原理是利用红外地球仪敏感地球边缘的切线方向,进而得到地球卫星与地心的连线方向,在利用星敏感器测得导航恒星的星光矢量方向,地心方向矢量与恒星的矢量构成星光角距作为量测量,如图2所示。 Direct sensing horizon is based on another autonomous celestial navigation orbit dynamics, it is the use of a tangential direction of the infrared sensitive edge of the Earth globe, thereby to obtain the connection direction of the satellite and the center of the earth, as measured using star sensors star navigation star vector direction, and the sun direction vector geocentric vector constituting starlight as the angular distance measurement, as shown in FIG. 这种导航方式具有结构简单、成本低廉、运行可靠、技术成熟且易于实现等特点。 This navigation is simple in structure, low cost, reliable, and easy to implement mature technology and other characteristics.

在卫星导航估计中,目前常采用的最优估计方法是卡尔曼滤波方法,这种方法是假设在一种理想条件下进行的,即要求系统模型为线性或弱非线性,系统噪声和量测噪声统计特性为零均值的高斯白噪声,而实际的卫星导航系统不仅系统模型具有强非线性,系统噪声和量测噪声也很难满足要求,显然在实际工程应用中卡尔曼滤波方法的假设前提是十分苛刻的,存在自身无法克服的缺陷,这些都是导致滤波器的发散的主要原因,滤波器发散将导致导航精度下降,严重的将无法输出正确的导航结果,因此说传统的基于卡尔曼滤波方法存在自身难以克服的因噪声估计不准确而造成滤波器发散,导航精度下降的缺陷。 Satellite navigation estimation, the current method often used optimal estimation Kalman filter is a method, which is performed in a assuming ideal conditions, i.e., require the system model is a linear or weakly nonlinear system noise and measurement statistics of noise is zero mean Gaussian white noise, and the actual satellite navigation system not only has strong nonlinear system model, the system noise and measurement noise is difficult to meet the requirements, apparently the assumptions in practical applications of Kalman filtering method is very demanding, there can not overcome its own shortcomings, these are the main cause of the divergence of the filter, the filter will result in divergence navigation accuracy decline, serious will not output the correct navigation results, so said the conventional Kalman the presence of defects due to inaccuracies caused by noise estimation filter divergence, accuracy is deteriorated navigation itself insurmountable filtering method.

发明内容 SUMMARY

本发明解决的技术问题是:克服传统的扩展卡尔曼滤波方法中假设系统噪声和量测噪声为高斯白噪声,造成导航系统精度低的不足,提出一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法,通过直接敏感地平的方式实现地球卫星自主天文导航,根据自适应扩展卡尔曼滤波每次计算出的更新信息,推导出实际模型的系统噪声和量测嗓声统计特性,克服了因噪声统计特性确定不准确造成的噪声滤波器发散问题,大大提高地球导航系统精度和适用范围。 The present invention solves the technical problem: to overcome the traditional extended Kalman filter approach assumes that the system noise and measurement noise is Gaussian white noise, resulting in low precision navigation system deficiency is proposed earth satellite-based Adaptive Extended Kalman Filter astronomical autonomous navigation method, by direct sensing horizon manner earth satellite autonomous celestial navigation, the extended Kalman filter update information according to each calculated adaptive derived system noise measurement and statistical characteristics of voice sound actual model, to overcome the noise filter noise due to the statistical characteristics to determine the exact cause of the divergence is not greatly improve the accuracy of Earth navigation system and scope.

本发明的技术方案为:一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法,其特征以星光角距为量测信息,结合地球卫星的轨道动力学方程,利用自适应扩展卡尔曼滤波方法得到高精度的位置、速度估计,首先通过扩展卡尔曼滤波方法计算滤波残差的大小来判断滤波器是否发散,进而启动自适应扩展卡尔曼滤波程序,利用极大后验估计器推算出系统噪声和量测噪声的统计特性,极大程度克服了传统的卡尔曼滤波器在处理系统噪声和量测噪声为非高斯白噪声时导致滤波器发散的问题,进一步提高地球卫星的导航精度。 Aspect of the present invention is: an adaptive extended Kalman filter Earth Satellite autonomous navigation method based astronomy, characterized in Starlight angular distance measurement information, binding kinetics equations earth satellite orbit, using adaptive extended Kalman the method of filtering a highly accurate position, velocity estimation, determines whether the filter divergence, and further program start adaptive extended Kalman filter using maximum a posteriori estimator calculate the size of the filtered residual is calculated by the first method EKF statistical properties of system noise and measurement noise, to a great extent overcomes the problems of the conventional Kalman filter results in a non-Gaussian white noise filter divergence processing system noise and measurement noise, to further improve the accuracy of earth satellite navigation.

具体步驟如下: Specific steps are as follows:

(1)建立基于直角坐标系的地球卫星轨道运动学方程,即状态方程,(2)建立以星光角距为量测量的量测方程; (1) establishing earth orbit satellites based on the kinematic equations of the Cartesian coordinate system, i.e. the state equation (2) to establish a starlight angular distance measured quantity measurement equation;

(3)建立离散型扩展卡尔曼滤波方程,将状态方程和量测方程离散化后再线性化,根据最小方差估计原则推导出离散型扩展广卡尔曼滤波的递推方程; (3) establishing a discrete Kalman filter equations, then discretized state equation and linear measurement equation, the recursive estimation equation derived principles discrete widely extended Kalman filter based on the minimum variance;

(4)利用G/DrxZKX)判定离散型扩展卡尔曼滤波是否发散,即利用匕/、。 (4) using the G / DrxZKX) determining whether the discrete Kalman filter divergence, i.e. using a dagger / ,. 力⑷)对预测滤波残差判断扩展卡尔曼滤波器是否发散,若满足发散条件则进行噪声统计特性的估计,否则按照标准的离散型扩展卡尔曼滤波程序进行计算; ⑷ force) filtering prediction residual divergence is determined whether an extended Kalman filter, if the condition is satisfied divergence estimated statistical properties of the noise, calculated according to otherwise standard procedures discrete extended Kalman filter;

(5)对地球卫星导航系统中噪声统计特性的估计,包括地球卫星导航系统状态模型的系统噪声和量测方程的量测噪声统计特性估计,采用离散型扩展卡尔曼滤波根据每次计算出的更新信息,推算出实际系统中系统噪声和量测嗓声统计特性,即利用滤波器估计值和预报值近似代替自适应卡尔曼滤波平滑估计值,可得到次优的极大后验估计值,并使离散型扩展卡尔曼滤波成为最优; (5) the Earth satellite navigation system estimates the statistical properties of the noise, including the amount of system noise and measurement equation Earth navigation satellite system state model noise statistics measured characteristic estimation using discrete Kalman filter is calculated according to each update information, and calculate the measurement system noise voice sound statistical properties of the actual system, i.e. using a filter value estimates and forecasts smoothed approximation instead of adaptive Kalman filter estimate is obtained suboptimal maximum a posteriori estimation value, and discrete extended Kalman filter to be the best;

(6)按照上述步骤(1) ~ (5),输出为地球卫星状态矢量估计值X及其方差矩阵P,其中状态估计值义包括地球卫星位置和速度矢量[W,vx,vy,vz]T,状态估计方差矩阵P包括地球卫星位置和速度估计方差 (6) The above steps (1) to (5), the estimated value of the output vector X and its covariance matrix P for the Earth satellite states, wherein the state estimation value includes a sense earth satellite position and velocity vector [W, vx, vy, vz] T, state estimation covariance matrix P including the earth satellite position and velocity estimation variance

[P, Pf P, ^ dT。 [P, Pf P, ^ dT.

本发明的基本原理是:利用星敏感器观测导航恒星得到该星光在星敏感测量坐标系的方向,通过星敏感器安装矩阵的转换,可算得星光在地球卫星本体坐标系中的方向;再利用红外地球敏感器直接测得地球卫星至地球边缘的切线方向或法线方向,得到地心矢量在地球卫星本体坐标系中的方向;继而得到天文量测信息如星光角距,如图2所示等,再结合轨道动力学方程和先进的滤波技术即可估计出地球卫星的位置信息。 The basic principles of the present invention are: the use of star sensor observations navigation stars give the direction of the stars in the star sensor measurement coordinate system, the installation transformation matrix by star sensor, it can be considered as the direction of a star in the Earth satellite body coordinate system; reuse infrared earth sensor to measure directly to earth satellite earth edge normal direction or a tangential direction, to give a direction vector in the center of the earth Earth satellite body coordinate system; then obtain astronomical measurement information like a star angular distance, shown in Figure 2 etc., combined with orbit dynamics equations and advanced filtering techniques to estimate the earth satellite position information. 自适应卡尔曼滤波方法原理是在离散型扩展卡尔曼滤波的基础上,即根据每次离散型量测噪声统计特性,解决了传统扩展卡尔曼滤波中直接假设系统噪声和量测噪声为高斯白噪声的缺陷,提高自适应扩展卡尔曼滤波器收敛性以及地球卫星导航系统导航定位精度。 Principle adaptive Kalman filtering method is based on the discrete Kalman filter, i.e. according to the statistical properties of each discrete measurement noise, to solve the traditional extended Kalman filter directly assumption that the system noise and measurement noise is Gaussian white noise defects and improve the convergence of the adaptive extended Kalman filter and the earth GNSS navigation positioning accuracy.

本发明与现有技术相比的优点在于:解决了传统卡尔曼滤波应用于非线性较强的地球卫星导航系统中由于假设系统噪声和量侧噪声为非高斯白噪声所造成的缺陷,设计了自适应扩展卡尔曼滤波器。 It advantages over the prior art that the present invention is: address the deficiencies of the conventional Kalman filter is applied to nonlinear strong earth satellite navigation system, since it is assumed side system noise and noise caused by a non-Gaussian white noise, the design of adaptive extended Kalman filter. 利用滤波器中前/C步的更新信息推算系统噪声和量测噪声的真实统计特性的特点,解决了传统扩展卡尔曼滤波器因噪声统计特性估计不准确造成的滤波器发散,影响滤波精度的问题,提高了地球卫星的导航精度及其适用范围,使其更加适用于对导航定位要求较高的资源、通讯、侦察、气象等地球卫星。 Using a filter in front / update information C features real step calculated the statistical characteristics of the system noise and measurement noise, to solve the traditional extended Kalman filter due to the statistical properties of the noise caused by inaccurate estimation filter divergence, the impact of filtering accuracy problems, improve navigation accuracy and scope of Earth satellites, making it more suitable for earth satellite navigation and positioning requirements of high resource, communications, reconnaissance, weather and so on.

附图说明 BRIEF DESCRIPTION

图1为本发明的流程图; Figure 1 is a flowchart of the present invention;

图2为本发明中的量测信息一星光角距示意图。 A measurement information starlight corner in FIG. 2 of the present invention from FIG.

具体实施方式 detailed description

本发明具体实施的流程如图1所示,先由星敏感器测得的星光矢量和地平仪测量的地心方向矢量构成的星光角距作为量测量,结合地球卫星的状态方程,利用离散型扩展卡尔曼滤波方法估计预测方差是否满足滤波器发散的要求,就利用更新信息推算系统噪声和量测噪声的统计特性,即利用滤波估计值和预测值近似代替自适应卡尔曼滤波的平滑估值,得到次极大后验估计值,再进行离散型扩展卡尔曼滤波,估计地球卫星的位置、速度信息;否则滤波器不发散就直接估计出地球卫星的位置、速度信息. Equation of state of the particular embodiment of the process of the present invention shown in Figure 1, angular Star Star vector and the direction vector geocentric horizon measured first by the star sensor as measured from the configuration of a measurement, combined earth satellite, using Discrete extended Kalman filter estimates whether the prediction variance divergence filter satisfies the requirements, it is estimated using the updated statistical properties of the information system noise and measurement noise, i.e., a smoothing filter estimates the estimated value and the predicted value approximately instead of the adaptive Kalman filter to give secondary maximum a posteriori estimation value, then the discrete extended Kalman filter estimates the position of the earth satellite, the speed information; otherwise, the filter does not diverge directly estimate the position of the earth satellite, velocity information.

具体的实施步骤如下。 Specific steps are as follows embodiment.

1、产生标称的轨道数据 1, a nominal track data

①坐标系:J2000.0地心赤道惯性坐标系 ① coordinates: J2000.0 equatorial geocentric inertial coordinate system

②标称轨道参数设定,目的是产生标准的轨道。 ② nominal orbital parameters set, the purpose is to produce a standard track. 偏心率:e= 1.809XΙΟ-3 轨道倾角:i=65。 Eccentricity: e = 1.809XΙΟ-3 orbital inclination angle: i = 65.

升交点赤经:Ω=30.00。 RAAN: Ω = 30.00.

近升角距:ω= 30.00° Near angular distance L: ω = 30.00 °

2、建立基于直角坐标系的地球卫星轨道运动学方程在研究地球卫星的运动时,选取历元(J2000.0)地心赤道坐标系。 2, based on the establishment of the Earth orbit satellite motion equation rectangular coordinate system in the study of the movement of earth satellites, select epoch (J2000.0) geocentric equatorial coordinate system. 此时,通常选用的卫星导航系统状态模型(轨道动力学模型)为 In this case, the state model satellite navigation system (orbital dynamics model) is usually used

式中' r = ^jx2+y2+z2,简写为如) = /(幻) + H;⑴ (7) Wherein 'r = ^ jx2 + y2 + z2, such as abbreviated) = / (phantom) + H; ⑴ (7)

式中,状态矢量久=[:少z V1 V,vz]T,文,少,z, V,, V v2分别为卫星在X、Y、Z三个方向的位置和速度;P是地心引力常数;Γ是卫星位置参数矢量;Λ为地球引力系数;AFz为地球非球形摄动的高阶摄动项和日、月摄动以及太阳光压摄动和大气摄动等摄动力的影响,在简化模型中这些摄动力的影响通常用系统噪声WO表示。 Wherein, for a long time the state vector = [: Less z V1 V, vz] T, text, less, z, V ,, V v2 satellite position and velocity respectively in X, Y, Z three directions; P geocentric gravitational constant; Gamma] is the satellite position parameter vector; Lambda is the coefficient of the Earth's gravity; AFz earth higher order non-spherical perturbation perturbation terms and date, month and perturbation sunlight and atmospheric pressure perturbation perturbation dynamic impact force and the like perturbation in a simplified model of the effect of these disturbing force usually expressed WO system noise.

上述方程为连续系统状态方程,将其离散化表示为 The above equation is a state equation of a continuous system, which is expressed as a discrete

Xk+l =巾M'kXk +^k ^ = 1,2,3... ( 8 ) Xk + l = towel M'kXk + ^ k ^ = 1,2,3 ... (8)

式中,Α+u为欠时刻至/C+1时刻的一步转移矩阵;%为系统噪声矩阵。 Wherein, Α + u is less time to / C + 1 time step transfer matrix;% for the system noise matrix.

3、以星光角距为量测量建立量测方程星光角距是天文导航中经常使用的一种量测量,星光角距Cr指从卫星上观测到的导航恒星星光的矢量方向与地心矢量方向之间的夹角,如图2所示。 3, Starlight established angular distance measurement from the angle measurement equation starlight amount of one celestial navigation is often used in measurement, and Cr is the angular distance starlight observation vector direction from the satellite to the center of the Earth navigation star starlight vector direction angle between, as shown in FIG. 从图中所示的几何关系,可得到星光角距《的表达式和相应的量测方程分别为 Geometric relationship shown in FIG., The angle can be obtained from Star "and corresponding expressions are measurement equation

式中,r是卫星在地心惯性球坐标系中的位置矢量,由地平敏感器获得;s是导航星星光方向的单位矢量,由星敏感器识别。 Where, r is the position vector of the satellite geocentric inertia ball in a coordinate system, obtained by the horizon sensor; S is a guide star light direction unit vector, identified by a star sensor.

对于量测噪声为零均值白噪声有 For the measurement noise is zero mean white noise with a

4、建立离散型扩展卡尔曼滤波方程 4, the establishment of discrete extended Kalman filter equations

离散型扩展的卡尔曼滤波是先将随机非线性系统模型中的非线性向量函数《P和λ围绕滤波值线性化,得到系统线性化模型,然后应用卡尔曼滤波基本方程,解决非线性滤波问题的。 Discrete extended Kalman filter is first nonlinear vector function random nonlinear system model "P and the linear filter value around λ to obtain a linear system model and the basic Kalman filter equations, solving nonlinear filtering problem of. 将离散随机非线性系统(8)的状态方程和量测方程(11 )中的非线性向量函数P和A围绕滤波值夕(Mfc)展开成泰勒级数,并略去二次以上项,分别得 The discrete stochastic nonlinear systems (8) of the state equation and the measurement equation (11) is a nonlinear vector function P and A Taylor series expanded about the filtered values ​​Xi (the Mfc), and the above second term is omitted, respectively, get

离散型扩展卡尔曼滤波的递推方程为: Discrete extended Kalman filter recursive equation is:

Α:(^ + 1|Α: + ΐ) = ^[χ(Α:μ)^] + ^(λ: + ΐ){ζ(^ + ΐ)-Λ[χ(λ + ΐμ)^ + ΐ]} ( 15) Α: (^ + 1 | Α: + ΐ) = ^ [χ (Α: μ) ^] + ^ (λ: + ΐ) {ζ (^ + ΐ) -Λ [χ (λ + ΐμ) ^ + ΐ ]} (15)

初始值为 The initial value

T为状态矢量AT估计方差矩阵。 AT T is the state vector estimation variance matrix.

5、利用FA//R/t QxTHX)判定离散型扩展卡尔曼滤波是否发散,即利用VklkrVknc < tX7>(\)对预测滤波残差判断扩展卡尔曼滤波器是否发散,若满足发散条件则进行嗓声统计特性的估计,否则按照标准的扩展卡尔曼滤波程序进行计算。 5, using the FA // R / t QxTHX) determining whether the discrete Kalman filter divergence, i.e. using VklkrVknc <tX7> (\) for filtering the prediction residual divergence is determined whether an extended Kalman filter, if conditions are met for the divergence estimate the statistical characteristics of my voice, or calculated in accordance with the standard extended Kalman filter program.

计算滤波残差Vklk = Zk-HkXkKk·以及其方差\ = HkPklik^H7k+Rk,判断滤波器是否发散条件为I Computing residuals filtering Vklk = Zk-HkXkKk · and variance \ = HkPklik ^ H7k + Rk, it determines whether the filter condition is divergent I

Vk/kTVklk<txTr(Sk) (21) Vk / kTVklk <txTr (Sk) (21)

式中,r为可调系数(Ql),若不满足条件,则判定扩展卡尔曼滤波发散,下一步进行地球卫星导航系统中噪声统计特性的估计,即估计系统噪声和量测嗓声统计特性。 Where, r is the adjustable coefficient (Ql,), conditions are not satisfied, it is determined that divergence extended Kalman filter, the next step which estimates a statistical property of an earth satellite navigation system noise, i.e. noise estimation system and the statistical characteristics of voice sound measurement .

6、对地球卫星导航系统中噪声统计特性的估计,包括地球卫星导航系统状态模型的系统噪声和量测方程的量测噪声统计特性估计,采用离散型扩展卡尔曼滤波根据每次计算出的更新信息,推算出实际系统中系统嗓声和量测嗓声统计特性,即利用滤波器估计值和预报值近似代替自适应卡尔曼滤波的平滑估值,可得到次优的极大后验估计值,并使离散型扩展卡尔①系统嗓声的统计特性为: 6, the earth satellite navigation system estimates the statistical properties of the noise, including measurement statistics of noise and system noise measurement equation Earth navigation satellite system state model estimation using discrete extended Kalman filter according to each calculated update information, and calculate the measurement system voice sound voice sound statistical properties of the real system, i.e. the estimated value using a filter instead of smoothing and prediction value approximate estimate of the adaptive Kalman filter, available suboptimal maximum a posteriori estimation value , and discrete extensions Carl ① voice sound statistical properties of the system are:

式中,文广尤-尤^广夂-由〜义^为预测残差,之,么为系统噪声误差均值和协方差。 Wherein, in particular SMG - Fan Guang particular ^ - ^ a ~ Yi prediction residuals, the system noise is what error mean and covariance.

②量测噪声的统计特性为: ② measurement noise statistical properties as follows:

式中,尤=々尤为量测残差,&,夂为量测噪声误差均值和协方差。 In the formula, especially the amount of prediction residual = 々 particularly, &, Fan error of measurement noise mean and covariance.

7、利用步骤1产生的标称轨迹,利用步骤2求解状态方程,利用步骤3建立量测方程,步骤2与步骤3之间的相互关系代入步骤4中的离散型扩展卡尔曼滤波方程中,估计出地球卫星位置和速度矢量[:<:,>%2;^?^>1^]1',而且在估计过程要通过步骤5计算滤波残差,据此判定离散型扩展卡尔曼滤波器是否发散,如果滤波器发散通过步骤6估计模型噪声的统计特性,将估计出来的统计特性再代入步骤4中,估计出地球卫星状态矢量估计值文及其方差矩阵P,其中状态估计值文包括地球卫星位置和速度矢量[x,3;^,vx,vy,vz]T,状态估计方差矩阵P包括地球卫星位置和速度估计方差 7, step 1 using the nominal trajectory is generated by solving the state equations in step 2, using 3 measurement equation establishing step, discrete extended Kalman filter equations 4 substituting the relationship between Steps 2 and 3 into the step, Earth estimated satellite position and velocity vector [: <:,> 2%; ^ ^> ^ 1?] 1 ', and the estimation process for filtering a residual calculating step 5, whereby it is determined discrete extended Kalman filter whether the divergence, if the divergence of the filter by estimating the statistical properties of the model noise step 6, the statistical properties of the estimated re-substituted into step 4, the satellite earth estimated state vector and its covariance matrix estimate packet P, wherein the status message includes an estimated value Earth satellite position and velocity vector [x, 3; ^, vx, vy, vz] T, state estimation covariance matrix P including the earth satellite position and velocity estimation variance

[P, P, P, ^ P', [P, P, P, ^ P ',

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。 SUMMARY specification, the present invention not described in detail belong to the prior art the present skilled in the art well known.

Claims (3)

  1. 1、一种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法,其特征在于包括下列步骤: (1)建立基于直角坐标系的地球卫星轨道运动学方程,即状态方程,通过解微分方程计算出地球卫星的位置(Xy5Z)和速度(vx,vy,vz)信息; (2)建立以星光角距为量测量的量测方程; (3)建立离散型扩展卡尔曼滤波方程,将状态方程和量测方程离散化后再线性化,根据最小方差估计原则推导出离散型扩展卡尔曼滤波的递推方程; (4)利用VklkrVklk < t X TriSk)判定离散型扩展卡尔曼滤波是否发散,即利用匕/ド+/ーぃ斤⑷)对预测滤波残差判断离散型扩展卡尔曼滤波器是否发散,若满足发散条件则进行噪声统计特性的估计,否则按照标准的离散型扩展卡尔曼滤波程序进行计算; (5)对地球卫星导航系统中噪声统计特性的估计,包括地球卫星导航系统状态模型的系统噪声和量测方 An earth satellite autonomous celestial navigation method based on adaptive extended Kalman filter, comprising the steps of: (1) establishing Earth orbit satellites based on kinematic equation rectangular coordinate system, i.e., the state equation, the solution of differential equations calculate the position of the satellite earth (Xy5Z) and velocity (vx, vy, vz) information; and (2) establish a starlight angular distance measured quantity measurement equation; (3) establishing a discrete Kalman filter equations, state equation and the measurement equation discretized after linearization principle derive estimated discrete extended Kalman filter according to the minimum variance of the recurrence equation; (4) determining whether the discrete Kalman filter using divergence VklkrVklk <t X TriSk), i.e. using dagger / ド + / kg ぃ ー ⑷) for filtering the prediction residual is determined whether the discrete extended Kalman filter divergence, if a diverging condition is met if the estimated statistical characteristics of noise, otherwise in accordance with standard discrete extended Kalman filter calculation procedures; (5) an estimate of the statistical properties of the Earth satellite navigation system noise, including system noise and earth satellite navigation system side state model test 的量测噪声统计特性估计,采用离散型扩展卡尔曼滤波根据每次计算出的更新信息,推算出实际系统中系统噪声和量测噪声统计特性,即利用滤波器估计值和预报值近似代替自适应卡尔曼滤波平滑估计值,可得到次优的极大后验估计值,并使离散型扩展卡尔曼滤波成为最优; (6)按照上述步骤(1) ~ (5),输出为地球卫星状态矢量估计值X及其方差矩阵P,其中状态估计值文包括地球卫星位置和速度矢量[x,y^,vx,vy,vz]T,状态估计方差矩阵P包括地球卫星位置和速度估计方差[ち芩ぢ冬や幻7。 The measurement statistics of noise estimation, discrete extended Kalman filter type system according to the actual system noise and measurement noise characteristics of each calculation of statistical updates, calculate, i.e. using a filter estimation and prediction value instead of from a value of approximately adapt the Kalman smoothing filter estimate is obtained suboptimal maximum a posteriori estimation value, and the discrete Kalman filter becomes optimum; (6) according to the above steps (1) to (5), the output of earth satellite state vector and covariance matrix estimation value X P, wherein the status message includes an estimated value earth satellite position and velocity vector [x, y ^, vx, vy, vz] T, state estimation covariance matrix P including the earth satellite position and velocity estimation variance [ち skullcap ぢ ya winter magic 7.
  2. 2、根据权利要求1所述的ー种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法,其特征在于:所述步骤(4)中的扩展卡尔曼滤波是否发散的判定为计算渡波残差Vklk= 以及其方差Sk =HkPm一'、Hl +Rk,判断滤波器是否发散条件为: 2, ー according to claim 1 species earth satellite autonomous celestial navigation method based on adaptive extended Kalman filter, wherein: said step of extended Kalman filter (4) is calculated is determined whether or diverging wave transit residues difference Vklk = Sk = HkPm variance and a ', Hl + Rk, divergence is determined whether the filter condition is:
    式中,r为可调系数(Pl),Xk, Zk, Hk, R分別/(时刻的状态量,量测f,量测振阵和量测残差;丨,分别为滤、波残差的オ差,估计均方误差,量测嗓声的协方差,T/•为求&矩阵的迹,若不满足条件,则判定传统卡尔曼滤波发散,启动对地球卫星导航系统中噪声统计特性的估计,分别估计系统锋声与量测噪声统计特性。 Where, r is the adjustable coefficient (Pl), Xk, Zk, Hk, R are / (timing state quantity measuring F, and the amount of vibration measurement residuals matrix; Shu, respectively filter, the residual wave Bio the difference, the mean square error estimation, measurement covariance voice sound, T / • for the sake of trace & matrices, if the condition is satisfied the conventional Kalman filter divergence, start the Earth navigation satellite system statistics of noise is determined the estimates were estimated Feng sound system and measurement noise statistical characteristics.
  3. 3、根据权利要求1所述的ー种基于自适应扩展卡尔曼滤波的地球卫星自主天文导航方法,其特征在于:所述步骤(5)中对系统噪声与量测噪声统计特性的估计,采用根据离散型扩展卡尔曼滤波根据每次计算出的更新信息推算系统嗓声和量测噪声特性,即利用滤波估计值文⑴和预测值)近似代替自适应滤波的平滑估计值,得到次极大后验估计值,该方法的具体步骤为: ①系统嗓声的统计特性为: 3. ー species according to claim 1 autonomous celestial earth satellite navigation method based on adaptive extended Kalman filter, wherein: said step (5) in system noise estimate and statistical properties of the measurement noise, using the discrete Kalman filter estimation system according to the update information and voice sound measurement noise characteristics of each calculated, i.e. the filtering estimated value and the predicted value ⑴ packets) instead of an approximate value of the estimated smoothing adaptive filtering, to obtain a submaximal posteriori estimate, specific steps of the method are as follows: ① statistical characteristics of voice sound system:
    式中,毛==も-…尤为预测残差,qk,么为系统噪声误差均值和协方差,,为し,时刻的一步转移矩阵; ②量测嗓声的统计特性为: Where, hair == mo - ... especially prediction residuals, qk, it is the system noise error of the mean and covariance ,, as shi, time-step transition matrix; ② statistical properties of the sound is measured voice:
CN 200610165577 2006-12-22 2006-12-22 Earth satellite self-independent astronomical navigation method based on self adaptive expansion kalman filtering CN100498225C (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN 200610165577 CN100498225C (en) 2006-12-22 2006-12-22 Earth satellite self-independent astronomical navigation method based on self adaptive expansion kalman filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN 200610165577 CN100498225C (en) 2006-12-22 2006-12-22 Earth satellite self-independent astronomical navigation method based on self adaptive expansion kalman filtering

Publications (2)

Publication Number Publication Date
CN1987355A true CN1987355A (en) 2007-06-27
CN100498225C true CN100498225C (en) 2009-06-10

Family

ID=38184235

Family Applications (1)

Application Number Title Priority Date Filing Date
CN 200610165577 CN100498225C (en) 2006-12-22 2006-12-22 Earth satellite self-independent astronomical navigation method based on self adaptive expansion kalman filtering

Country Status (1)

Country Link
CN (1) CN100498225C (en)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101246011B (en) 2008-03-03 2012-03-21 北京航空航天大学 Multi-target multi-sensor information amalgamation method based on convex optimized algorithm
US8416133B2 (en) * 2009-10-15 2013-04-09 Navcom Technology, Inc. System and method for compensating for faulty measurements
CN101949703B (en) * 2010-09-08 2012-11-14 北京航空航天大学 Strapdown inertial/satellite combined navigation filtering method
JP5818608B2 (en) * 2011-09-27 2015-11-18 インターナショナル・ビジネス・マシーンズ・コーポレーションInternational Business Machines Corporation Processing Kalman filter method, program and system
CN102680989B (en) * 2012-05-17 2014-07-23 北京邮电大学 Positioning result filtering method and device
CN103281054A (en) * 2013-05-10 2013-09-04 哈尔滨工程大学 Self adaption filtering method adopting noise statistic estimator
CN104019817B (en) * 2014-05-30 2017-01-04 哈尔滨工程大学 Fan for satellite attitude estimation Constraint volume of strong tracking Kalman Filter
CN105631063A (en) * 2014-10-30 2016-06-01 北京航天长征飞行器研究所 Intersection moment prediction method based on object features
CN105704071A (en) * 2015-07-07 2016-06-22 大连大学 Information-sequence-based adaptive fading extended kalman particle filter (AFEKPF) doppler frequency shift estimation method
CN106291641A (en) * 2016-09-09 2017-01-04 中国人民解放军国防科学技术大学 Speed adaptive auxiliary positioning method

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
一种基于信息融合的卫星自主天文导航新方法. 宁晓琳,房建成.宇航学报,第24卷第6期. 2003 *
一种深空探测器自主天文导航新方法及其可观测性分析. 宁晓琳,房建成.空间科学学报,第25卷第4期. 2005 *
基于模糊卡尔曼滤波量测噪声自适应校正的方法研究. 马野,王孝通,付建国.中国惯性技术学报,第13卷第2期. 2005 *
基于滤波过程的卡尔曼滤波发散判定方法. 邱凯,黄国荣,陈天如,杨亚莉.系统工程与电子技术,第27卷第2期. 2005 *
航天器天文导航模糊自适应卡尔曼滤波研究. 张瑜,房建成.北京航空航天大学学报,第30卷第8期. 2004 *
航天器自主天文导航系统的可观测性及可观测度分析. 宁晓琳,房建成.北京航空航天大学学报,第31卷第6期. 2005 *

Also Published As

Publication number Publication date Type
CN1987355A (en) 2007-06-27 application

Similar Documents

Publication Publication Date Title
Crassidis et al. Survey of nonlinear attitude estimation methods
Georgy et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation
Jiancheng et al. Study on innovation adaptive EKF for in-flight alignment of airborne POS
Wendel et al. A performance comparison of tightly coupled GPS/INS navigation systems based on extended and sigma point Kalman filters
Rocken et al. Improved mapping of tropospheric delays
CN101038169A (en) Navigation satellite autonomous navigation system and method based on X-ray pulsar
CN102445200A (en) Microminiature personal combined navigation system as well as navigating and positioning method thereof
CN101576384A (en) Indoor movable robot real-time navigation method based on visual information correction
CN101893440A (en) Celestial autonomous navigation method based on star sensors
US20090227266A1 (en) Location measurement method based on predictive filter
CN101082494A (en) Self boundary marking method based on forecast filtering and UPF spacecraft shading device
CN102519450A (en) Integrated navigation device for underwater glider and navigation method therefor
CN101846510A (en) High-precision satellite attitude determination method based on star sensor and gyroscope
CN101788296A (en) SINS/CNS deep integrated navigation system and realization method thereof
Teixeira et al. Spacecraft tracking using sampled-data Kalman filters
US6356815B1 (en) Stellar attitude-control systems and methods with weighted measurement-noise covariance matrices
CN101078936A (en) High precision combined posture-determining method based on optimally genetic REQUEST and GUPF
CN102829777A (en) Integrated navigation system for autonomous underwater robot and method
CN101762273A (en) Autonomous optical navigation method for soft landing for deep space probe
Lammas et al. A 6 DoF navigation algorithm for autonomous underwater vehicles
Raol et al. On the orbit determination problem
CN103090867A (en) Error restraining method for fiber-optic gyroscope strapdown inertial navigation system rotating relative to geocentric inertial system
Wang et al. Huber-based unscented filtering and its application to vision-based relative navigation
CN101344391A (en) Lunar vehicle pose self-confirming method based on full-function sun-compass
CN101701826A (en) Target tracking method of passive multi-sensor based on layered particle filtering

Legal Events

Date Code Title Description
C06 Publication
C10 Request of examination as to substance
C14 Granted