CN105021198A - Position estimation method based on integrated navigation of multiple sensors - Google Patents

Abstract

The invention discloses a position estimation method based on integrated navigation of multiple sensors. The method comprises the following steps: 1, parameters output by a DR, a GPS, a DME, a VOR and an ADC are acquired; 2, a state equation is established; 3, a position and velocity measurement equation of the GPS, a slope distance measurement equation of the DME and an azimuth measurement equation of the VOR are calculated; 4, a comprehensive measurement equation is established according to each measurement equation; 5, by combining the state equation and the comprehensive measurement equation, position error amount and velocity error amount are estimated; and 6, according to the estimated position error amount and velocity error amount, output correction is carried out on the DR system, and position estimation is finished. According to the invention, there is no need to use an inertial navigation system. The method provided by the invention has high positioning accuracy, high reliability and good real-time performance, is economic, effective and easy for engineering realization, and can meet high-precision and high-reliability positioning requirements of civil aircrafts.

Description

A kind of location estimation method based on MULTISENSOR INTEGRATION navigation

Technical field

The present invention relates to a kind of MULTISENSOR INTEGRATION air navigation aid, particularly a kind of location estimation method based on DR/GPS/DME/VOR integrated navigation, is a kind of method being highly suitable for civil aircraft high precision, high reliability location requirement.

Background technology

For civil aircraft, security is primary forever.In order to ensure the flight safety of civilian flight, Large Civil Aircraft is generally equipped with multiple navigational system.As Air Passenger 320 is equipped with multiple navigator such as inertial navigation system, GPS (GPS), stadimeter (DME), VHF Omnidirection range (VOR), dead reckoning (DR) to be used for location.How effectively fully utilizing multiple navigator information and carry out position calculation, thus guarantee positioning precision and the reliability of civil aircraft, is gordian technique urgently to be resolved hurrily in civil aircraft design process.

Although inertial navigation system can independently provide comprehensive navigation information, its error constantly can accumulate along with the time, so independently can not carry out precision navigation for a long time.GPS can provide airborne position and velocity information round-the-clock, in real time, and error does not accumulate in time, but GPS Existence dependency external information, is subject to the deficiencies such as radio interference, data output rate be low; The positioning precision of the radio positioning systems such as VOR, DME is by affecting service time, but working range is by the restriction of earth station overlay area, and is subject to the spatial noise impacts such as field, ground environment, electromagnetic interference (EMI).DR system is the Camera calibration system utilizing the sensing datas such as airborne existing course, speed, attitude to carry out dead reckoning, DR system has that independence is good, strong interference immunity, export that locator data, short time precision are higher, low cost and other advantages continuously, but systematic error accumulates in time, can not use for a long time individually; Above-mentioned various single navigation sources respectively has relative merits, the feature of the various navigational system of comprehensive performance, makes up for each other's deficiencies and learn from each other, and is got up by the informix of navigational system, to precision and the reliability of navigator fix be improved, meet the requirement of civil aircraft to navigational system better.

At present, traditional integrated navigation locator meams is usually using inertial navigation system as principle navigation system, use other navigational system such as GPS, DME and VOR to assist to combine, Kalman filtering is utilized to realize the estimation of aircraft position, but this mode not only needs to be equipped with expensive inertial navigation system, and model and combinational algorithm complexity.In Project Realization, when adopting traditional kalman filter method to carry out integrated navigation, owing to affecting by the round-off error in model error and computation process, easily there is state covariance matrix and lose symmetry and orthotropicity, thus cause wave filter to lose efficacy.For guaranteeing the reliability of navigating, usually adopt the method for federated filter to realize the navigation of multisensor comprehensively, but this method calculated amount is large, poor real.

Summary of the invention

In order to solve, existing civil aircraft integrated navigation localization method is complicated, wave filter easily lost efficacy, calculated amount is large in the present invention, poor real, and the problems such as the inertial navigation system needing configuration expensive, propose a kind of location estimation method based on MULTISENSOR INTEGRATION navigation, utilize the method can provide for civil aircraft the position data that high precision, reliability are high.

Goal of the invention of the present invention is achieved through the following technical solutions:

Based on a location estimation method for MULTISENSOR INTEGRATION navigation, comprise following steps:

Step 1, gather the parameter that airborne each navigator exports, described parameter comprises: (1) DR system outgoing position, velocity information; (2) gps system export position, velocity information; (3) the oblique distance information of DME system output; (4) azimuth information of VOR system output; (5) elevation information of ADC system output;

Step 2, the elevation information exported according to DR SYSTEM ERROR MODEL and ADC system set up state equation;

Step 3, the position of DR system, velocity information and the position of GPS, the difference of speed is utilized to try to achieve position, the velocity measurement equation of GPS; The oblique distance information of the positional information of DR system and DME system is utilized to try to achieve the oblique distance measurement equation of DME; The azimuth information of the positional information of DR system and VOR system is utilized to try to achieve the position angle measurement equation of VOR;

Step 4, position according to GPS, velocity measurement equation, the oblique distance measurement equation of DME and the position angle measurement equation of VOR set up comprehensive measurement equation;

Step 5, bonding state equation and comprehensive measurement equation, Estimated Position Error amount, velocity error amount;

Step 6, according to site error amount, the velocity error amount estimated, output calibration is carried out to DR system, complete location estimation.

The method, without the need to using inertial navigation system, has that positioning precision is high, reliability is high, real-time is good, an economical and effective and be easy to the feature of Project Realization.

According to above-mentioned feature, described state equation is:

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

Wherein: state variable is:

X＝[δL δλ δV _NδV _E] ^T

System battle array is:

$F=\left[\begin{array}{cccc}0& 0& \frac{1}{R_M+h}& 0\\ \frac{V_E\tan L}{(R_N+h)\cos L}& 0& 0& \frac{1}{(R_N+h)\cos L}\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]$

System noise vector is:

$W={\left[\begin{array}{cccc}{w}_L& w_{\mathrm{\λ}}& w_{V_N}& w_{V_E}\end{array}\right]}^T$

δ L, δ λ are respectively latitude error, longitude error; δ V _n, δ V _ebe respectively north orientation velocity error, east orientation velocity error; V _efor the east orientation speed of DR system; L is the latitude of DR system; H is the height of ADC system; R _n=R _e(1+fsin ²l), R _m=R _e(1-2f+3fsin ²l), f is compression of the earth, R _efor earth radius; w _l, w _λbe respectively DR system latitude white noise, longitude white noise; be respectively DR system north orientation speed white noise, east orientation speed white noise; T is transpose of a matrix; T is the time.

According to above-mentioned feature, in described step 3, the position of GPS, velocity measurement equation are:

$Z_{GPS}\left(t\right)=\left[\begin{array}{c}L-L_{GPS}\\ \mathrm{\λ}-{\mathrm{\λ}}_{GPS}\\ V_N-V_{N_{GPS}}\\ V_E-V_{E_{GPS}}\end{array}\right]=H_{GPS}\left(t\right)X\left(t\right)+V_{GPS}\left(t\right)$

Be wherein L, λ, V _n, V _ebe respectively the latitude of DR system, longitude, north orientation speed, east orientation speed, L _gPS, λ _gPS, be respectively the latitude of gps system, longitude, north orientation speed, east orientation speed, H _gPS=[H ¹ _gPSh ² _gPSh ³ _gPSh ⁴ _gPS] ^t=diag [1 11 1], V _gPSfor the white Gaussian noise of zero-mean, and uncorrelated mutually;

The oblique distance measurement equation of described DME is:

Z _DME(t)＝D _DR-D _DME＝H _DME(t)X(t)+V _DME(t)

In formula $D_{DR}=\sqrt{{(x_{DR}-x_{DME})}^2+{(y_{DR}-y_{DME})}^2+{(z_{DR}-z_{DME})}^2},$ H _dME=[a ₁a ₂0 0], wherein

a ₁＝(R _N+h)[-e ₁sin L cosλ-e ₂sin L sinλ]+[R _N(1-f) ²+h]e ₃cos L

a ₂＝(R _N+h)[e ₂cos L cosλ-e ₁cos L sinλ]

$e_1=\frac{x_{DR}-x_{DME}}{D_{DR}},e_2=\frac{y_{DR}-y_{DME}}{D_{DR}},e_3=\frac{z_{DR}-z_{DME}}{D_{DR}},$ X _dR, y _dR, z _dRfor the position quantity in x, y, z direction under earth fixed coordinate system, DR position, x _dME, y _dME, z _dMEfor the position quantity in x, y, z direction under earth fixed coordinate system, DME guidance station position, D _dMEfor the oblique distance information of DME system, V _dMEfor the white Gaussian noise of zero-mean, earth fixed coordinate system initial point is in the earth's core, and z-axis is along earth rotation direction of principal axis, and x-axis is under the line in plane, and crossing with zero degree meridian, y-axis and x-axis, z-axis form right hand rectangular coordinate system;

The position angle measurement equation of described VOR is:

Z _VOR(t)＝θ _DR-θ _VOR＝H _VOR(t)X(t)+V _VOR(t)

In formula h _vOR=[b ₁b ₂0 0], wherein

b ₁＝c ₁[(R _N+h)sin L sinλcosλ-(R _N+h)sin L sinλcosλ]

+c ₂[(R _N+h)sin ²L cos ²λ+(R _N+h)sin ²L sin ²λ+(R _N(1-f) ²+h)cos ²L]

b ₂＝c ₁[(R _N+h)cos L sin ²λ+(R _N+h)cos L cos ²λ]

+c ₂[(R _N+h)sin L cos L sinλcosλ-(R _N+h)sin L cos L sinλcosλ]

$c_1=\frac{{y^n}_{DR}-{y^n}_{VOR}}{s},c_2=-\frac{{x^n}_{DR}-{x^n}_{VOR}}{s},$ s＝(x ⁿ _DR-x ⁿ _VOR) ²+(y ⁿ _DR-y ⁿ _VOR) ²

X ⁿ _dR, y ⁿ _dRfor the position quantity in x, y direction under geographic coordinate system, DR position, x ⁿ _vOR, y ⁿ _vORfor the position quantity in x, y direction under geographic coordinate system, VOR guidance station position, θ _vORfor the position angle of VOR system, V _vORfor the white Gaussian noise of zero-mean, geographic coordinate system take current location as initial point, and x-axis points to due east, and y-axis points to positive north, and z-axis is pointed to sky and formed right hand rectangular coordinate system to x-axis, y-axis.

Preferably, in order to avoid irrational measurement information is incorporated in filtering system, improve stability and the reliability of system, described step 4 first carries out measurement information reasonalbeness check to position, velocity measurement information, oblique distance measurement information, position angle measurement information before being also included in and setting up comprehensive measurement equation, if there is rational measurement information, include the measurement information of throughput measurement information reasonalbeness check in comprehensive measurement equation, and continue to perform step 5; If when each measurement information is all unreasonable, then do not set up comprehensive measurement equation, by the estimation adopting the method for predicted estimate to carry out site error amount and velocity error amount, and perform step 6.

According to above-mentioned feature, the method for described measurement information reasonalbeness check is:

According to the state estimation in k-1 moment the recursion value of k moment system state is obtained by state equation obtain the predicted value of k moment i-th measurement amount thus and then obtain i-th actual amount measured value with the predicted value of measurement amount between difference namely

${\hat{Z}}_k^i=Z_k^i-{\hat{Z}}_{k/k-1}^i=Z_k^i-H_k^i{\hat{X}}_{k/k-1}$

The variance of the theoretical prediction error of i-th measurement amount is if then i-th measurement amount meets requirement of reasonableness, otherwise measurement information is unreasonable;

Wherein whether reasonably γ is decision content measurement information threshold value, and i=1,2,3,4,5,6 represents successively to GPS latitude, longitude, north orientation speed, east orientation velocity information, and DME oblique distance information, VOR azimuth information carries out reasonalbeness check.

According to above-mentioned feature, in described step 4, comprehensive measurement equation is:

When the position of GPS, velocity measurement information all meet requirement of reasonableness, position and velocity measurement information Z _gPSincluding system quantities in measures in Z; When DME oblique distance measurement information meets requirement of reasonableness, oblique distance measurement information Z _dMEincluding system quantities in measures in Z; When VOR position angle measurement information meets requirement of reasonableness, position angle measurement information Z _vORincluding system quantities in measures in Z; Build comprehensive measurement equation:

$Z\left(t\right)={\left[\begin{array}{c}{Z}_{GPS}\left(t\right)\\ Z_{DME}\left(t\right)\\ Z_{VOR}\left(t\right)\end{array}\right]}_{m\×1}$

System quantities is measured as m dimensional vector, and m changes according to the rationality of GPS location and velocity measurement information, DME oblique distance measurement information, VOR position angle measurement information:

The m=6 when position, velocity measurement information, oblique distance measurement information, position angle measurement information are all reasonable;

When position, velocity measurement information are reasonable, oblique distance measurement information, position angle measurement information only have one reasonable time m=5;

When position, velocity measurement information are reasonable, m=4 when oblique distance measurement information, position angle measurement information are all unreasonable;

When oblique distance measurement information, position angle measurement information are all reasonable, m=2 when position, velocity measurement information are unreasonable;

When oblique distance measurement information, position angle measurement information only have one rationally, m=1 when position, velocity measurement information are unreasonable;

M dimension measures separate, and measuring noise square difference battle array is diagonal matrix

Preferably, in order to efficiently solve the continuity problem of filtering in all unreasonable situation of measurement information in recursive process, the step that described prediction estimation method carries out site error amount, velocity error amount is estimated is adopted to be: to estimate the position margin of error, velocity error amount according to the state estimation of previous moment and state equation:

${\hat{X}}_k={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}.$

$P_k={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

Preferably, in order to efficiently solve the problem that recursive process median filter is dispersed, improve filtering accuracy, and be easy to Project Realization, adopt the square root filtering method based on Sequential processing to carry out the estimation of site error amount and velocity error amount when there is rational measurement information, described employing based on the step of Estimated Position Error amount, velocity error amount in the square root filtering method of Sequential processing is:

(5.1) by state equation and comprehensive measurement equation discretize, obtain:

X _k＝Φ _k,k-1X _k-1+W _k-1

Z _k＝H _kX _k+V _k

Wherein, X _kfor state vector, Φ _{k, k-1}for state-transition matrix, Z _kfor measuring vector, H _kfor measurement matrix, W _k-1for system noise vector, V _kfor measurement noise vector, W _k-1and V _kbe incoherent zero mean Gaussian white noise, the variance matrix of system noise and measurement noise is respectively Q _kand R _k;

(5.2) square root filtering method Estimated Position Error amount, the velocity error amount based on Sequential processing is adopted:

Note Δ in formula _kand Δ _k/k-1it is all lower triangular matrix;

If $P_0=\left[\begin{array}{cc}{\mathrm{\αI}}_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& {\mathrm{\βI}}_{2\×2}\end{array}\right],$ α, β are the variance initial value of position estimation error, speed estimation error;

Time update equation is:

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

With Qiao Laisiji decomposition method to P _k/k-1Δ is obtained as square root factorization _k/k-1;

Measure and upgrade:

If the Sequential processing result according to the k-1 moment obtains and Δ _k/k-1, then the measurement renewal Sequential processing in k moment performs in the steps below:

Get ${\hat{X}}_k^0={\hat{X}}_{k/k-1},{\mathrm{\Δ}}_k^0={\mathrm{\Δ}}_{k/k-1}$

For j=1,2 ..., the following equation of m iterative computation:

$a_k^j={\left(H_k^j{\mathrm{\Δ}}_k^{j-1}\right)}^T$

$b_k^j={(a_k^{jT}{a}_k^j+R_k^j)}^{-1}$

${\mathrm{\γ}}_k^j={(1+\sqrt{b_k^j{R}_k^j})}^{-1}$

$K_k^j=b_k^j{\mathrm{\Δ}}_k^{j-1}{a}_k^j$

${\hat{X}}_k^j={\hat{X}}_k^{j-1}+K_k^j(Z_k^j-H_k^j{\hat{X}}_k^{j-1})$

${\mathrm{\Δ}}_k^j={\mathrm{\Δ}}_k^{j-1}-{\mathrm{\γ}}_k^j{K}_k^j{a}_k^{jT}$

As j=m, the measurement namely obtaining the k moment upgrades result:

${\hat{X}}_k={\hat{X}}_k^m$

${\mathrm{\Δ}}_k={\mathrm{\Δ}}_k^m$

Wherein for state estimation, Δ _kfor estimating square error P _ksquare root,

Advantage of the present invention and remarkable result:

1, the inventive method proposes and does not use expensive inertial navigation system, the output parameter of abundant use existing DR, GPS, DME, VOR and ADC system, adopt the method for integrated navigation to realize the estimation of aircraft position, there is high, the cost-effective feature of positioning precision;

2, the inventive method proposes the method to measurement information reasonalbeness check, realizes the inspection to unreasonable message and isolation, avoids irrational measurement information to be incorporated in filtering system, improves stability and the reliability of system;

3, the inventive method proposes the different filtering method of to measurement information in reasonable and unreasonable two kinds of situations two kinds, effectively solves to resolve problem in the unreasonable situation of measurement information, ensure that the continuity of location estimation data;

4, the method for the inventive method employing square root filtering efficiently solves the problem that recursive process median filter is dispersed, and improves filtering accuracy, and is easy to Project Realization;

5, the inventive method adopts the method for Sequential processing to upgrade multiple measurement information, significantly reduces the calculated amount that filtering solves, improves the real-time of location.

Accompanying drawing explanation

Fig. 1 is the structure principle chart of the inventive method;

Fig. 2 is the program flow diagram of the inventive method.

Specific implementation

Embodiment one:

Referring to Fig. 1, the present invention, under the prerequisite not using inertial navigation system, makes full use of the output parameter of existing DR, GPS, DME, VOR and ADC system, realizes the estimation of positional information, comprises following steps:

Step 1, gather the parameter that airborne each navigator exports, described parameter comprises: (1) DR system outgoing position, velocity information; (2) gps system export position, velocity information; (3) the oblique distance information of DME system output; (4) azimuth information of VOR system output; (5) elevation information of ADC system output;

Step 2, the elevation information exported according to DR SYSTEM ERROR MODEL and ADC system set up the state equation of integrated navigation system;

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

State variable is:

X＝[δL δλ δV _NδV _E] ^T

System battle array is:

$F=\left[\begin{array}{cccc}0& 0& \frac{1}{R_M+h}& 0\\ \frac{V_E\tan L}{(R_N+h)\cos L}& 0& 0& \frac{1}{(R_N+h)\cos L}\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]$

System noise vector is:

$W={\left[\begin{array}{cccc}{w}_L& w_{\mathrm{\λ}}& w_{V_N}& w_{V_E}\end{array}\right]}^T$

Wherein, δ L, δ λ are respectively latitude error, longitude error; δ V _n, δ V _ebe respectively north orientation velocity error, east orientation velocity error; V _efor the east orientation speed of DR system; L is the latitude of DR system; H is the height of ADC system; R _n=R _e(1+fsin ²l), R _m=R _e(1-2f+3fsin ²l), f=1/298.257, R _e=6378137 meters; w _l, w _λbe respectively DR system latitude white noise, longitude white noise; be respectively DR system north orientation speed white noise, east orientation speed white noise; T is transpose of a matrix; T is the time;

Step 3, the position of DR system, velocity information and the position of GPS, the difference of speed is utilized to try to achieve position, the velocity measurement equation of GPS; The oblique distance information of the positional information of DR system and DME system is utilized to try to achieve the oblique distance measurement equation of DME; The azimuth information of the positional information of DR system and VOR system is utilized to try to achieve the position angle measurement equation of VOR;

Position, the velocity measurement equation of GPS are specially:

By the difference of the position that the position of DR system and GPS are measured $\left[\begin{array}{c}L-L_{GPS}\\ \mathrm{\λ}-{\mathrm{\λ}}_{GPS}\end{array}\right]$ Measure, by the difference of the speed that the speed of DR system and GPS are measured as position quantity $\left[\begin{array}{c}{V}_N-V_{N_{GPS}}\\ V_E-V_{E_{GPS}}\end{array}\right]$ As velocity measurement amount, position quantity is measured and merges as the total measurement amount Z of GPS with velocity measurement amount _gPS, set up the measurement equation of GPS:

$Z_{GPS}\left(t\right)=\left[\begin{array}{c}L-L_{GPS}\\ \mathrm{\λ}-{\mathrm{\λ}}_{GPS}\\ V_N-V_{N_{GPS}}\\ V_E-V_{E_{GPS}}\end{array}\right]=H_{GPS}\left(t\right)X\left(t\right)+V_{GPS}\left(t\right)$

Be wherein L, λ, V _n, V _ebe respectively the latitude of DR system, longitude, north orientation speed, east orientation speed, L _gPS, λ _gPS, be respectively the latitude of gps system, longitude, north orientation speed, east orientation speed, H _gPS=[H ¹ _gPSh ² _gPSh ³ _gPSh ⁴ _gPS] ^t=diag [1 11 1], V _gPSfor the white Gaussian noise of zero-mean, and uncorrelated mutually.

The oblique distance measurement equation of DME is specially:

According to the oblique distance D of the positional information calculation of DR _dRthe oblique distance D exported with DME _dMEdifference D _dR-D _dMEas oblique distance measuring value Z _dME, set up the measurement equation of DME:

Z _DME(t)＝D _DR-D _DME＝H _DME(t)X(t)+V _DME(t)

In formula $D_{DR}=\sqrt{{(x_{DR}-x_{DME})}^2+{(y_{DR}-y_{DME})}^2+{(z_{DR}-z_{DME})}^2},$ H _dME=[a ₁a ₂0 0], wherein

a ₁＝(R _N+h)[-e ₁sin L cosλ-e ₂sin L sinλ]+[R _N(1-f) ²+h]e ₃cos L

a ₂＝(R _N+h)[e ₂cos L cosλ-e ₁cos L sinλ]

$e_1=\frac{x_{DR}-x_{DME}}{D_{DR}},e_2=\frac{y_{DR}-y_{DME}}{D_{DR}},e_3=\frac{z_{DR}-z_{DME}}{D_{DR}},$ X _dR, y _dR, z _dRfor the position quantity in x, y, z direction under earth fixed coordinate system, DR position, x _dME, y _dME, z _dMEfor the position quantity in x, y, z direction under earth fixed coordinate system, DME guidance station position, V _dMEfor the white Gaussian noise of zero-mean, earth fixed coordinate system initial point is in the earth's core, and z-axis is along earth rotation direction of principal axis, and x-axis is under the line in plane, and crossing with zero degree meridian, y-axis and x-axis, z-axis form right hand rectangular coordinate system.

The position angle measurement equation of VOR is specially:

According to the azimuth angle theta of the positional information calculation of DR _dRthe azimuth angle theta exported with VOR _vORdifference as position angle measuring value Z _vOR, set up the measurement equation of VOR:

Z _VOR(t)＝θ _DR-θ _VOR＝H _VOR(t)X(t)+V _VOR(t)

In formula h _vOR=[b ₁b ₂0 0], wherein

b ₁＝c ₁[(R _N+h)sin L sinλcosλ-(R _N+h)sin L sinλcosλ]

+c ₂[(R _N+h)sin ²L cos ²λ+(R _N+h)sin ²L sin ²λ+(R _N(1-f) ²+h)cos ²L]

b ₂＝c ₁[(R _N+h)cos L sin ²λ+(R _N+h)cos L cos ²λ]

+c ₂[(R _N+h)sin L cos L sinλcosλ-(R _N+h)sin L cos L sinλcosλ]

$c_1=\frac{{y^n}_{DR}-{y^n}_{VOR}}{s},c_2=-\frac{{x^n}_{DR}-{x^n}_{VOR}}{s},$ s＝(x ⁿ _DR-x ⁿ _VOR) ²+(y ⁿ _DR-y ⁿ _VOR) ²

X ⁿ _dR, y ⁿ _dRfor the position quantity in x, y direction under geographic coordinate system, DR position, x ⁿ _vOR, y ⁿ _vORfor the position quantity in x, y direction under geographic coordinate system, VOR guidance station position, V _vORfor the white Gaussian noise of zero-mean, geographic coordinate system take current location as initial point, and x-axis points to due east, and y-axis points to positive north, and z-axis is pointed to sky and formed right hand rectangular coordinate system to x-axis, y-axis.

Step 4, set up the measurement equation of integrated navigation system;

$Z\left(t\right)=\left[\begin{array}{c}{Z}_{GPS}\left(t\right)\\ Z_{DME}\left(t\right)\\ Z_{VOR}\left(t\right)\end{array}\right]$

Step 5, in conjunction with the state equation of integrated navigation system and measurement equation, realize the estimation of site error amount, velocity error amount.

In this step, in order to effectively solve the problem that recursive process median filter is dispersed, improve filtering accuracy, and the calculated amount that significantly reduction filtering solves is easy to Project Realization, this step preferably adopts the square root filtering method based on Sequential processing to come Estimated Position Error amount, velocity error amount, and other method also can be adopted for a person skilled in the art to come Estimated Position Error amount, velocity error amount.

Square root filtering method concrete steps based on Sequential processing are:

(a) state equation and measurement equation discretize

By state equation and measurement equation discretize, obtain:

X _k＝Φ _k,k-1X _k-1+W _k-1

Z _k＝H _kX _k+V _k

Wherein, X _kfor state vector, Φ _{k, k-1}for state-transition matrix, Z _kfor measuring vector, H _kfor measurement matrix, W _k-1for system noise vector, V _kfor measurement noise vector, W _k-1and V _kbe incoherent zero mean Gaussian white noise, the variance matrix of system noise and measurement noise is respectively Q _kand R _k.

B () is based on the square root filtering method of Sequential processing

Note $P_k={\mathrm{\Δ}}_k{\mathrm{\Δ}}_k^T,P_{k/k-1}={\mathrm{\Δ}}_{k/k-1}{\mathrm{\Δ}}_{k/k-1}^t,$ Δ in formula _kand Δ _k/k-1it is all lower triangular matrix.If ${\hat{X}}_0=0_{4\×1},$ $P_0=\left[\begin{array}{cc}{\mathrm{\αI}}_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& {\mathrm{\βI}}_{2\×2}\end{array}\right],$ α, β are the variance initial value of position estimation error, speed estimation error.

Time update equation is:

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

With Qiao Laisiji decomposition method to P _k/k-1Δ is obtained as square root factorization _k/k-1.

Measure and upgrade:

If the Sequential processing result according to the k-1 moment obtains and Δ _k/k-1, then the measurement renewal Sequential processing in k moment can perform in the steps below:

Get ${\hat{X}}_k^0={\hat{X}}_{k/k-1},{\mathrm{\Δ}}_k^0={\mathrm{\Δ}}_{k/k-1}$

For j=1,2 ..., the following equation of m iterative computation

$a_k^j={\left(H_k^j{\mathrm{\Δ}}_k^{j-1}\right)}^T$

$b_k^j={(a_k^{jT}{a}_k^j+R_k^j)}^{-1}$

${\mathrm{\γ}}_k^j={(1+\sqrt{b_k^j{R}_k^j})}^{-1}$

$K_k^j=b_k^j{\mathrm{\Δ}}_k^{j-1}{a}_k^j$

${\hat{X}}_k^j={\hat{X}}_k^{j-1}+K_k^j(Z_k^j-H_k^j{\hat{X}}_k^{j-1})$

${\mathrm{\Δ}}_k^j={\mathrm{\Δ}}_k^{j-1}-{\mathrm{\γ}}_k^j{K}_k^j{a}_k^{jT}$

As j=m, the measurement namely obtaining the k moment upgrades result

${\hat{X}}_k={\hat{X}}_k^m$

${\mathrm{\Δ}}_k={\mathrm{\Δ}}_k^m$

Wherein for state estimation, Δ _kfor estimating square error P _ksquare root,

Step 6, according to site error amount, the velocity error amount estimated, output calibration is carried out to DR system, complete the location estimation based on MULTISENSOR INTEGRATION navigation thus.

Therefore, accurately estimating in real time of aircraft position can be realized by step 1 to step 6.

Embodiment two

As shown in Figure 2, the invention allows for one and improved plan is carried out to embodiment one, to measurement information reasonalbeness check in embodiment two, realize the inspection to unreasonable message and isolation, avoid irrational measurement information to be incorporated in filtering system, improve stability and the reliability of system; And the filtering method rationally different with the employing in unreasonable two kinds of situations to measurement information, effectively solve and resolve problem in the unreasonable situation of measurement information, ensure that the continuity of location estimation data, the implementation step of embodiment two is as follows:

Step 1-3 is with embodiment one.

Step 4, employing measurement information reasonalbeness check method carry out reasonalbeness check to position, velocity measurement information (being called for short: GPS measurement information), oblique distance measurement information (being called for short: DME measurement information), position angle measurement information (being called for short VOR measurement information) successively, if there is rational measurement information, the measurement information of throughput measurement information reasonalbeness check is set up comprehensive measurement equation, if there is no rational measurement information, then comprehensive measurement equation does not exist.

Described measurement information reasonalbeness check method is:

According to the state estimation in k-1 moment the recursion value of k moment system state can be obtained by system state equation the predicted value of k moment i-th measurement amount can be obtained thus and then obtain i-th actual amount measured value with the predicted value of measurement amount between difference namely

${\hat{Z}}_k^i=Z_k^i-{\hat{Z}}_{k/k-1}^i=Z_k^i-H_k^i{\hat{X}}_{k/k-1}$

The variance of the theoretical prediction error of i-th measurement amount is if then i-th measurement amount meets requirement of reasonableness, otherwise measurement information is unreasonable.Wherein whether reasonably γ is decision content measurement information threshold value, and i=1,2,3,4,5,6 represents successively to GPS latitude, longitude, north orientation speed, east orientation velocity information, and DME oblique distance information, VOR azimuth information carries out reasonalbeness check.

When the position of GPS, velocity measurement information all meet requirement of reasonableness, position and velocity measurement information Z _gPSincluding system quantities in measures in Z; When DME oblique distance measurement information meets requirement of reasonableness, oblique distance measurement information Z _dMEincluding system quantities in measures in Z; When VOR position angle measurement information meets requirement of reasonableness, position angle measurement information Z _vORincluding system quantities in measures in Z; Build comprehensive measurement equation:

$Z\left(t\right)={\left[\begin{array}{c}{Z}_{GPS}\left(t\right)\\ Z_{DME}\left(t\right)\\ Z_{VOR}\left(t\right)\end{array}\right]}_{m\×1}$

System quantities is measured as m dimensional vector, and m changes according to the rationality of GPS, DME, VOR measurement information:

The m=6 when GPS, DME, VOR measurement information is all reasonable;

When GPS measurement information rationally, DME and VOR measurement information only have one reasonable time m=5;

The m=4 when GPS measurement information is reasonable, DME, VOR measurement information is all unreasonable;

When DME, VOR measurement information is all reasonable, m=2 when GPS measurement information is unreasonable;

When DME and VOR measurement information only has one rationally, m=1 when GPS measurement information is unreasonable;

M dimension measures separate, and measuring noise square difference battle array is diagonal matrix $R_k=diag\[\begin{array}{cccc}{R}_k^1& R_k^1& \mathrm{...}& R_k^m\end{array}\].$

Step 5, adopt diverse ways to carry out the estimation of site error amount and velocity error amount according to measurement information rationality, be specially:

If adopt the square root filtering method based on Sequential processing to carry out the estimation of site error amount and velocity error amount when there is rational measurement information, the described square root filtering method based on Sequential processing is with embodiment one;

If there is no reasonably measurement information time adopt the method for predicted estimate to carry out the estimation of site error amount and velocity error amount, realize the estimation of site error amount, velocity error amount according to the state estimation of previous moment and system state equation:

$\begin{array}{c}{\hat{X}}_k={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_k={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}\end{array}.$

Step 6, according to whether there is site error amount that rational measurement information two kinds of situations obtain in step 5, velocity error amount carries out output calibration to DR system, completes the location estimation based on MULTISENSOR INTEGRATION navigation thus.

Be understandable that, for those of ordinary skills, can be equal to according to technical scheme of the present invention and inventive concept thereof and replace or change, and all these change or replace the protection domain that all should belong to the claim appended by the present invention.

Claims (9)

1., based on a location estimation method for MULTISENSOR INTEGRATION navigation, comprise following steps:

Step 1, gather the parameter that airborne each navigator exports, described parameter comprises: (1) DR system outgoing position, velocity information; (2) gps system export position, velocity information; (3) the oblique distance information of DME system output; (4) azimuth information of VOR system output; (5) elevation information of ADC system output;

Step 2, the elevation information exported according to DR SYSTEM ERROR MODEL and ADC system set up state equation;

Step 3, the position of DR system, velocity information and the position of GPS, the difference of speed is utilized to try to achieve position, the velocity measurement equation of GPS; The oblique distance information of the positional information of DR system and DME system is utilized to try to achieve the oblique distance measurement equation of DME; The azimuth information of the positional information of DR system and VOR system is utilized to try to achieve the position angle measurement equation of VOR;

Step 4, position according to GPS, velocity measurement equation, the oblique distance measurement equation of DME and the position angle measurement equation of VOR set up comprehensive measurement equation;

Step 5, bonding state equation and comprehensive measurement equation, Estimated Position Error amount, velocity error amount;

Step 6, according to site error amount, the velocity error amount estimated, output calibration is carried out to DR system, complete location estimation.

2. location estimation method according to claim 1, is characterized in that, in described step 2, state equation is:

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

Wherein: state variable is:

X＝[δL δλ δV _NδV _E] ^T

System battle array is:

$F=\left[\begin{array}{cccc}0& 0& \frac{1}{R_M+h}& 0\\ \frac{V_E\tan L}{(R_N+h)\cos L}& 0& 0& \frac{1}{(R_N+h)\cos L}\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]$

System noise vector is:

$W={\left[\begin{array}{cccc}{w}_L& w_{\mathrm{\λ}}& w_{V_N}& w_{V_E}\end{array}\right]}^T$

δ L, δ λ are respectively latitude error, longitude error; δ V _n, δ V _ebe respectively north orientation velocity error, east orientation velocity error; V _efor the east orientation speed of DR system; L is the latitude of DR system; H is the height of ADC system; R _n=R _e(1+fsin ²l), R _m=R _e(1-2f+3fsin ²l), f is compression of the earth, R _efor earth radius; w _l, w _λbe respectively DR system latitude white noise, longitude white noise; be respectively DR system north orientation speed white noise, east orientation speed white noise; T is transpose of a matrix; T is the time.

3. location estimation method according to claim 1, is characterized in that, in described step 3, the position of GPS, velocity measurement equation are:

$Z_{GPS}\left(t\right)=\left[\begin{array}{c}L-L_{GPS}\\ \mathrm{\λ}-{\mathrm{\λ}}_{GPS}\\ V_N-V_{N_{GPS}}\\ V_E-V_{E_{GPS}}\end{array}\right]=H_{GPS}\left(t\right)X\left(t\right)+V_{GPS}\left(t\right)$

Be wherein L, λ, V _n, V _ebe respectively the latitude of DR system, longitude, north orientation speed, east orientation speed, L _gPS, λ _gPS, V _nGPS, V _eGPSbe respectively the latitude of gps system, longitude, north orientation speed, east orientation speed, H _gPS=[H ¹ _gPSh ² _gPSh ³ _gPSh ⁴ _gPS] ^t=diag [1 11 1], V _gPSfor the white Gaussian noise of zero-mean, and uncorrelated mutually;

In described step 3, the oblique distance measurement equation of DME is:

Z _DME(t)＝D _DR-D _DME＝H _DME(t)X(t)+V _DME(t)

In formula $D_{DR}=\sqrt{{(x_{DR}-x_{DME})}^2+{(y_{DR}-y_{DME})}^2+{(z_{DR}-z_{DME})}^2},$ H _dME=[a ₁a ₂0 0], wherein

a ₁＝(R _N+h)[-e ₁sinLcosλ-e ₂sinLsinλ]+[R _N(1-f) ²+h]e ₃cosL

a ₂＝(R _N+h)[e ₂cosLcosλ-e ₁cosLsinλ]

$e_1=\frac{x_{DR}-x_{DME}}{D_{DR}},e_2=\frac{y_{DR}-y_{DME}}{D_{DR}},e_3=\frac{z_{DR}-z_{DME}}{D_{DR}},$ X _dR, y _dR, z _dRfor the position quantity in x, y, z direction under earth fixed coordinate system, DR position, x _dME, y _dME, z _dMEfor the position quantity in x, y, z direction under earth fixed coordinate system, DME guidance station position, D _dMEfor the oblique distance information of DME system, V _dMEfor the white Gaussian noise of zero-mean, earth fixed coordinate system initial point is in the earth's core, and z-axis is along earth rotation direction of principal axis, and x-axis is under the line in plane, and crossing with zero degree meridian, y-axis and x-axis, z-axis form right hand rectangular coordinate system;

In described step 3, the position angle measurement equation of VOR is:

Z _VOR(t)＝θ _DR-θ _VOR＝H _VOR(t)X(t)+V _VOR(t)

In formula h _vOR=[b ₁b ₂0 0], wherein

b ₁＝c ₁[(R _N+h)sinLsinλcosλ-(R _N+h)sinLsinλcosλ]

+c ₂[(R _N+h)sin ²Lcos ²λ+(R _N+h)sin ²Lsin ²λ+(R _N(1-f) ²+h)cos ²L]

b ₂＝c ₁[(R _N+h)cosLsin ²λ+(R _N+h)cosLcos ²λ]

+c ₂[(R _N+h)sinLcosLsinλcosλ-(R _N+h)sinLcosLsinλcosλ]

$c_1=\frac{{y^n}_{DR}-{y^n}_{VOR}}{s},c_2=-\frac{{x^n}_{DR}-{x^n}_{VOR}}{s},$ s＝(x ⁿ _DR-x ⁿ _VOR) ²+(y ⁿ _DR-y ⁿ _VOR) ²

X ⁿ _dR, y ⁿ _dRfor the position quantity in x, y direction under geographic coordinate system, DR position, x ⁿ _vOR, y ⁿ _vORfor the position quantity in x, y direction under geographic coordinate system, VOR guidance station position, θ _vORfor the position angle of VOR system, V _vORfor the white Gaussian noise of zero-mean, geographic coordinate system take current location as initial point, and x-axis points to due east, and y-axis points to positive north, and z-axis is pointed to sky and formed right hand rectangular coordinate system to x-axis, y-axis.

4. location estimation method according to claim 1, it is characterized in that, described step 4 first carries out measurement information reasonalbeness check to position, velocity measurement information, oblique distance measurement information, position angle measurement information before being included in and setting up comprehensive measurement equation, if there is rational measurement information, include each measurement information of throughput measurement information reasonalbeness check in comprehensive measurement equation, and continue to perform step 5; If when each measurement information is all unreasonable, then do not set up comprehensive measurement equation, by the estimation adopting the method for predicted estimate to carry out site error amount and velocity error amount, and perform step 6.

5. location estimation method according to claim 4, is characterized in that the method for described measurement information reasonalbeness check is:

According to the state estimation in k-1 moment the recursion value of k moment system state is obtained by state equation obtain the predicted value of k moment i-th measurement amount thus and then obtain i-th actual amount measured value with the predicted value of measurement amount between difference namely

${\hat{Z}}_k^i=Z_k^i-{\hat{Z}}_{k/k-1}^i=Z_k^i-H_k^i{\hat{X}}_{k/k-1}$

The variance of the theoretical prediction error of i-th measurement amount is if then i-th measurement amount meets requirement of reasonableness, otherwise measurement information is unreasonable;

Wherein whether reasonably γ is decision content measurement information threshold value, and i=1,2,3,4,5,6 represents successively to GPS latitude, longitude, north orientation speed, east orientation velocity information, and DME oblique distance information, VOR azimuth information carries out reasonalbeness check.

6. location estimation method according to claim 4, is characterized in that, in described step 4, comprehensive measurement equation is:

When the position of GPS, velocity measurement information all meet requirement of reasonableness, position and velocity measurement information Z _gPSincluding system quantities in measures in Z; When DME oblique distance measurement information meets requirement of reasonableness, oblique distance measurement information Z _dMEincluding system quantities in measures in Z; When VOR position angle measurement information meets requirement of reasonableness, position angle measurement information Z _vORincluding system quantities in measures in Z; Build comprehensive measurement equation:

$Z\left(t\right)={\left[\begin{array}{c}{Z}_{GPS}\left(t\right)\\ Z_{DME}\left(t\right)\\ Z_{VOR}\left(t\right)\end{array}\right]}_{m\×1}$

System quantities is measured as m dimensional vector, and m changes according to the rationality of GPS location and velocity measurement information, DME oblique distance measurement information, VOR position angle measurement information:

The m=6 when position, velocity measurement information, oblique distance measurement information, position angle measurement information are all reasonable;

When position, velocity measurement information are reasonable, oblique distance measurement information, position angle measurement information only have one reasonable time m=5;

When position, velocity measurement information are reasonable, m=4 when oblique distance measurement information, position angle measurement information are all unreasonable;

When oblique distance measurement information, position angle measurement information are all reasonable, m=2 when position, velocity measurement information are unreasonable;

When oblique distance measurement information, position angle measurement information only have one rationally, m=1 when position, velocity measurement information are unreasonable;

M dimension measures separate, and measuring noise square difference battle array is diagonal matrix $R_k=\mathrm{diag}\left[\begin{array}{cccc}{R}_k^1& R_k^1& ...& R_k^m\end{array}\right].$

7. location estimation method according to claim 4, it is characterized in that, described prediction estimation method carries out site error amount, step that velocity error amount is estimated is: according to the state estimation of previous moment and state equation, the position margin of error, velocity error amount are estimated:

${\hat{X}}_k={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

$P_k={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

8. location estimation method according to claim 1, is characterized in that, adopts the square root filtering method based on Sequential processing to carry out the estimation of site error amount and velocity error amount in described step 5.

9. location estimation method according to claim 8, is characterized in that, the step that the described square root filtering method based on Sequential processing carries out site error amount, velocity error amount is estimated is:

(5.1) by state equation and comprehensive measurement equation discretize, obtain:

X _k＝Φ _k,k-1X _k-1+W _k-1

Z _k＝H _kX _k+V _k

Wherein, X _kfor state vector, Φ _{k, k-1}for state-transition matrix, Z _kfor measuring vector, H _kfor measurement matrix, W _k-1for system noise vector, V _kfor measurement noise vector, W _k-1and V _kbe incoherent zero mean Gaussian white noise, the variance matrix of system noise and measurement noise is respectively Q _kand R _k;

(5.2) square root filtering method Estimated Position Error amount, the velocity error amount based on Sequential processing is adopted:

Note Δ in formula _kand Δ _k/k-1it is all lower triangular matrix;

If ${\hat{X}}_0=0_{4\×1},P_0=\left[\begin{array}{cc}{\mathrm{\αI}}_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& {\mathrm{\βI}}_{2\×2}\end{array}\right],$ α, β are the variance initial value of position estimation error, speed estimation error;

Time update equation is:

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

With Qiao Laisiji decomposition method to P _k/k-1Δ is obtained as square root factorization _k/k-1;

Measure and upgrade:

If the Sequential processing result according to the k-1 moment obtains and Δ _k/k-1, then the measurement renewal Sequential processing in k moment performs in the steps below:

Get ${\hat{X}}_k^0={\hat{X}}_{k/k-1},{\mathrm{\Δ}}_k^0={\mathrm{\Δ}}_{k/k-1}$

For j=1,2 ..., the following equation of m iterative computation:

$a_k^j={\left(H_k^j{\mathrm{\Δ}}_k^{j-1}\right)}^T$

$b_k^j={(a_k^{jT}{a}_k^j+R_k^j)}^{-1}$

${\mathrm{\γ}}_k^j={(1+\sqrt{b_k^j{R}_k^j})}^{-1}$

$K_k^j=b_k^j{\mathrm{\Δ}}_k^{j-1}{a}_k^j$

${\hat{X}}_k^j-{\hat{X}}_k^{j-1}+K_k^j(Z_k^j-H_k^j{\hat{X}}_k^{j-1})$

${\mathrm{\Δ}}_k^j={\mathrm{\Δ}}_k^{j-1}-{\mathrm{\γ}}_k^j{K}_k^j{a}_k^{jT}$

As j=m, the measurement namely obtaining the k moment upgrades result:

${\hat{X}}_k={\hat{X}}_k^m$

${\mathrm{\Δ}}_k={\mathrm{\Δ}}_k^m$

Wherein for state estimation, Δ _kfor estimating square error P _ksquare root,