CN103487822A - BD/DNS/IMU autonomous integrated navigation system and method thereof - Google Patents

Abstract

The invention discloses a BD/DNS/IMU autonomous integrated navigation system and a method thereof. The system includes an airborne navigation computer, an IMU, a Doppler radar, a BD, an air data system, a data recorder, a monitoring system and an AHRS, wherein the IMU, the Doppler radar, the BD, the air data system, the data recorder, the monitoring system and the AHRS are connected with the airborne navigation computer. The method comprises the steps that a self-adapted Kalman filtering model is established by the adoption of a 'position + speed' matching mode, after pose matrix transformation is performed on the Doppler radar, differential operation is performed on velocity measurement information under the navigation system and velocity information calculated through inertial navigation solutions, and differential operation is performed on the position information outputted by the BD and the position information calculated through inertial navigation solutions; difference values obtained serve as observed quantity to perform Kalman filtering information fusion, and loop iteration is preformed on error variances of all branch systems in a state equation of a filtering estimating system; real-time closed-loop estimation is carried out, and all parametric variables in an inertial navigation system, the Doppler radar and the BD are amended to obtain final integrated navigation results after filtering correction. According to the BD/DNS/IMU autonomous integrated navigation system and the method thereof, long-endurance navigation can be carried out in an airborne and seamless mode in real time, navigation accuracy is high, and cost is low.

Description

The Big Dipper/radar Doppler/inertia autonomous type integrated navigation system and method thereof

One, technical field

The present invention relates to a kind of Aero-Space integrated navigation technology field, the particularly Big Dipper/radar Doppler/inertia autonomous type integrated navigation system and method thereof.

Two, background technology

The practical experience of modern several local wars shows, although the GPS positioning system has the incomparable precision of other positioning system, but its independence is poor, may conductively-closed or add mess code to apply interference wartime, so the medium and long distance that how to realize that autonomous type, low cost, precision are high, has good tactics characteristic navigation is urgent problem of current China military navigation system.Due to combat mission and the handling characteristics of armed helicopter, so military airborne navigational system need to meet following requirement: shorten the setup time before navigational system is taken off, can proceed to wartime state from standby condition rapidly, by ground, prepare to proceed to urgent lift-off; Reduce pilot's degree of participation, reliable high precision navigation information can still be provided under interference environment; Complicated meteorology landing requirement between meeting round the clock, make helicopter can complete the tasks such as field landing, hedgehopping, closely fire support, mountain region search rescue; Possess navigation feature during hovering, the unfriendly target type of being hidden attacked realizing, and when carrying out antisubmarine task across the sea hovering stop throwing in sonar and surveyed.

In the airborne integrated navigation system and method for development and application, there is following problem at present:

1) to have precision high for the GPS/IMU integrated navigation system, the advantage that cost is low, but this system depends critically upon the precision of GPS, adopt Beidou satellite receiver (BD) to combine and can form the integrated navigation system BD/IMU that is similar to GPS/IMU with IMU, belong to the independent combined navigation system, but due to the also unrealized comprehensive networking of current China big-dipper satellite, simultaneously, if helicopter flight is to valley, while being rescued between surrounded by mountains and city building, satellite-signal can be because multipath etc. is former thereby can't normally locate, and the aviation-grade inertial navigation adopted, can produce larger navigation error at short notice and can't provide reliable navigation information for carrier aircraft in the situation that can not receive effective satellite navigation signals,

2) the dead reckoning navigation system that radar Doppler forms with airborne boat appearance system (AHRS), be Doppler navigation system (Doppler Navigation System, DNS), just carried out simply the integration reckoning, error to each subsystem is not estimated in real time and eliminates, so navigation accuracy is relatively low, between 1%～2% (2 σ) of flying distance, error is larger usually;

3) radar Doppler/inertia autonomous type integrated navigation system, aspect precision, large increase has been arranged with respect to pure-inertial guidance and DNS system, usually between 0.15%～0.25% (2 σ) of flying distance, can meet the basic need of carrier aircraft navigational system, but the observed quantity of this integrated mode only has velocity information, the position passage does not have external information to carry out damping, so navigation error is cumulative growth in time still, independent navigation demand in the time of can't meeting airborne long boat, and it needs external position information to carry out initial alignment.

In sum, current airborne navigational system poor stability, precision is low, cost is high, and error cumulative growth in time, the demand of independent navigation in the time of can't meeting airborne long boat.

Three, summary of the invention

The object of the present invention is to provide the Big Dipper/radar Doppler that a kind of stability is strong, precision is high, cost is low/inertia autonomous type integrated navigation system and method thereof, the demand of independent navigation while navigating to meet airborne length.

The technical solution that realizes the object of the invention is: a kind of Big Dipper/radar Doppler/inertia autonomous type integrated navigation system comprises Inertial Measurement Unit, radar Doppler, Beidou receiver, air data system, datalogger, supervisory system, AHRS and airborne navigation computer; Described airborne navigation computer comprises DSP, FPGA, crystal oscillator, JTAG, SDRAM, FLASH and power module, wherein the address bus of DSP, data bus and control bus are connected with the corresponding bus of FPGA respectively, the interrupt output termination of FPGA enters the interrupting input end of DSP, the clock signal input terminal of crystal oscillator access DSP, the clock signal input terminal of the clock signal output terminal access FPGA of DSP, power module provides power supply for airborne computer, and airborne computer provides power supply for whole system; Described radar Doppler, air data system and supervisory system are connected with FPGA by the ARINC429 communication interface respectively, Beidou receiver is connected with FPGA by the RS-232 communication interface respectively with datalogger, and Inertial Measurement Unit is connected with FPGA by the RS-422 communication interface respectively with AHRS; Airborne navigation computer is connected with airborne flight control system by aviation 1553B bus;

The FPGA of described airborne navigation computer receives the measurement data of Beidou receiver by the RS-232 communication interface, receive the measurement data of radar Doppler by the ARINC429 communication interface, receive the measurement data of Inertial Measurement Unit by the RS-422 communication interface, FPGA carries out integrated navigation by the measurement data that receives input DSP and resolves and obtain navigation results, finally navigation results information is sent to supervisory system by the ARINC429 communication interface respectively, and send to datalogger by the RS-232 communication interface, it is crosslinked that airborne navigation computer carries out information by aviation 1553B bus and airborne flight control system.

A kind of Big Dipper/radar Doppler/inertia autonomous type Combinated navigation method comprises the following steps:

The 1st step, at first carry out initialization after system powers on, then carry out System self-test: if self check is normal, to flight control system, report self-detection result, then proceed to next step; Otherwise system is carried out localization of fault, supervisory system shows failure message and reports to the police;

The 2nd step, carry out initial alignment: the corresponding parameter that Beidou receiver is assigned to Inertial Measurement Unit using the carrier aircraft position that records and velocity information is as the initial value that resolves of Inertial Measurement Unit, then according to accelerometer and the gyrostatic measured value appearance of being navigated, resolves the attitude information that obtains initial alignment;

The 3rd step, carry out pre-filtering to the measurement data of radar Doppler, and single-point type and patch type outliers in measurement data rejected;

The 4th step, carry out inertial navigation to the measurement data of Inertial Measurement Unit and resolve, and obtains the carrier aircraft inertial navigation information under day navigation coordinate system northeastward;

The 5th step, airborne navigation computer receives the positional information of Beidou receiver, the information that tests the speed of radar Doppler, inertial navigation information, adopt the matching way of " position+speed " to set up the adaptive Kalman filter model, to Beidou receiver, Inertial Measurement Unit and radar Doppler subsystem carry out information fusion, estimate each margin of error in Inertial Measurement Unit and the navigation information of Inertial Measurement Unit is carried out to feedback compensation, the range rate error of estimating Doppler radar and scale coefficient error also carry out feedback compensation to the navigation information of radar Doppler, obtain final revised navigation results,

The 6th step, be sent to supervisory system by the integrated navigation result.

The present invention compared with prior art, its remarkable advantage is: (1) Big Dipper navigation positioning system, radar Doppler and inertial navigation are self contained navigational aids, the three has complementary advantages, and can complete fast the autonomous type initial alignment, and provide accurate navigation information for carrier aircraft for a long time; (2) carry out the estimation of state error by adaptive Kalman filter, and adopt the mode of close-loop feedback correction to carry out feedback compensation to error, adaptive capacity to environment is strong, and the integrated navigation precision is high; (3) use the inertial navigation of aviation-grade optical fiber, not only volume is little but also have that cost is low, the integrated level advantages of higher; (4) adopt 1553B and the ARINC429 bus interface of standard, easily crosslinked with the carrier aircraft fire control system, compatible strong.

Four, accompanying drawing explanation

The structural representation that Fig. 1 is the Big Dipper/radar Doppler of the present invention/inertia autonomous type integrated navigation system.

Fig. 2 concerns schematic diagram between the Big Dipper of the present invention, radar Doppler, inertial navigation coordinate system.

The theory diagram that Fig. 3 is the Big Dipper/radar Doppler of the present invention/inertia autonomous type Combinated navigation method.

The principle schematic that Fig. 4 is adaptive Kalman filter of the present invention.

The navigation flowcharts that Fig. 5 is the Big Dipper/radar Doppler of the present invention/inertia autonomous type Combinated navigation method.

The ground preventing test geometric locus figure that Fig. 6 is integrated navigation system in embodiment.

Fig. 7 is integrated navigation site error time history plot in embodiment.

Fig. 8 is integrated navigation range rate error time history plot in embodiment.

Fig. 9 is radar Doppler measurement data time history plot in embodiment.

Figure 10 is attitude angle time history plot in embodiment.

Five, embodiment

Below in conjunction with the drawings and specific embodiments, the present invention is described in further detail.

In conjunction with Fig. 1, the Big Dipper/radar Doppler of the present invention/inertia autonomous type integrated navigation system, comprise Inertial Measurement Unit, radar Doppler, Beidou receiver, air data system, datalogger, supervisory system, AHRS(boat appearance frame of reference) and airborne navigation computer; Described airborne navigation computer comprises DSP, FPGA, crystal oscillator, JTAG, SDRAM, FLASH and power module, wherein the address bus of DSP, data bus and control bus are connected with the corresponding bus of FPGA respectively, the interrupt output termination of FPGA enters the interrupting input end of DSP, the clock signal incoming end of crystal oscillator access DSP, the clock signal input terminal of the clock signal output terminal access FPGA of DSP, power module provides power supply for airborne computer, and airborne computer provides power supply for whole system; Described radar Doppler, air data system and supervisory system are connected with FPGA by the ARINC429 communication interface respectively, Beidou receiver is connected with FPGA by the RS-232 communication interface respectively with datalogger, and Inertial Measurement Unit is connected with FPGA by the RS-422 communication interface respectively with AHRS; Airborne navigation computer is connected with airborne flight control system by aviation 1553B bus;

The FPGA of described airborne navigation computer receives the measurement data of Beidou receiver by the RS-232 communication interface, receive the measurement data of radar Doppler by the ARINC429 communication interface, receive the measurement data of Inertial Measurement Unit by the RS-422 communication interface, FPGA carries out integrated navigation by the measurement data that receives input DSP and resolves and obtain navigation results, finally the integrated navigation result is sent to supervisory system by the ARINC429 communication interface respectively, and send to datalogger by the RS-232 communication interface, it is crosslinked that airborne navigation computer carries out information by aviation 1553B bus and airborne flight control system.

In integrated navigation system of the present invention, the concrete structure of unit and function are as follows:

(1.1) described Inertial Measurement Unit comprises the gyroscope of three axle quadratures installations, accelerometer, A/D modular converter and the temperature compensation system that three axle quadratures are installed, the data output end of gyroscope and accelerometer is through A/D modular converter access temperature compensation system, the output terminal of temperature compensation system is through RS422 communication interface access airborne navigation computer, wherein gyroscope is the aviation-grade fibre optic gyroscope, and accelerometer is silicon micro accerometer.

Described gyroscope provides three axis angular rate measured values, accelerometer provides the 3-axis acceleration measured value, all simulating signals change digital signal into by 16 A/D modular converters, then after the temperature drift compensation of temperature compensation system, by the RS422 communication interface by the data transfer of Inertial Measurement Unit to airborne navigation computer.

(1.2) described Beidou receiver can compatiblely receive an existing Big Dipper generation and the Big Dipper two generations satellite-signal, the output current location information, and providing temporal information synchronous for the information between Inertial Measurement Unit, radar Doppler and Beidou receiver, Beidou receiver carries out communication by RS232 communication interface and airborne navigation computer.

(1.3) described radar Doppler is that strapdown " X " font configures four wave beam radar Dopplers, by frequency discriminator, oscillator tracking, frequency mixer, integrator etc., formed, there is the ground self-checking function, radar Doppler is for measuring three axial velocity component of carrier aircraft under carrier system and exporting the current state information of radar Doppler, after pre-filtering, by ARINC429 bus and airborne navigation computer, carry out communication.

(1.4) described air data system is pressure altimeter, can measure the following barometer altitude information of 20000m, with airborne navigation computer, by the ARINC429 bus, communicates, as the standby measuring equipment of carrier aircraft height.

(1.5) described AHRS is aviation with the magnetic instrument that navigates, and by the ARINC429 bus, with airborne navigation computer, is connected, as the stand-by equipment of carrier aircraft attitude measurement.

(1.6) elevation information that described airborne navigation computer provides by ARINC429 communication interface reception air data system is as the amount of redundancy of altitude channel, DSP is the core that the navigational computer data are processed, and is mainly used in realizing that filtering, the error compensation of inertial navigation device, the inertial navigation of radar Doppler measurement data resolves, the functions such as real-Time Compensation of Integrated Navigation Algorithm and navigation error; FPGA mainly complete logic control that CPU extends out memory device, external data collection, with the functions such as the communicating by letter of peripheral hardware, timing synchronization; Described power module is for providing the 28V power supply to airborne navigation computer.

(1.7) described supervisory system is for realizing the functions such as the output of integrated navigation system integrated navigation result parameter and demonstration, working state of system supervision.

(1.8) described datalogger is for carrier aircraft record to the omnidistance navigation data of integrated navigation system when flying.

Fig. 2 is the relation between radar Doppler, Beidou receiver and Inertial Measurement Unit three coordinate system, establishes wind speed and is , the speed of the relative air of aircraft is

the absolute velocity of aircraft

radar Doppler can record the absolute velocity of aircraft,

$V^2={\mathrm{<figure-callout\; id="2"\; label="V\; bx"\; filenames="HDA0000388651070000022.png"\; state="\{\{state\}\}">V</figure-callout>}}_{\mathrm{bx}}^{}+V_{\mathrm{by}}^2+V_{\mathrm{bz}}^2---\left(1\right)$

In formula, V _bx, V _by, V _bzfor three axial velocity component of aircraft along body system; In conjunction with Fig. 2, wherein

point to the right side along the body transverse axis,

before pointing to along the body longitudinal axis,

perpendicular to OX _by _bplane, and point to along the vertical pivot of aircraft;

at ground level coordinate system O _px _py _pbeing projected as on surface level

claim

for ground velocity, ground velocity

projection V with aircraft longitudinal axis on surface level _yp(along Y' _baxle) angle between is called drift angle μ, now, and course angle

(be Y' _bwith Y _nangle) and flight-path angle α between pass be:

o _px _ey _nz _ufor sky, northeast navigation coordinate system.

In conjunction with Fig. 3, for the schematic diagram of the Big Dipper/radar Doppler of the present invention/inertia autonomous type Combinated navigation method, mainly comprise that inertial navigation resolves module, radar Doppler pie slice and modular converter, Beidou satellite navigation and resolves that module, barometer altitude information are resolved module, adaptive Kalman filter resolves module, control and combines aobvious module; The function of modules and logical relation each other are as follows:

(2.1) described inertial navigation resolves module for the three axial angle speed that receive Inertial Measurement Unit and optical fiber inertial navigation output and compares force information, by smothing filtering and integration navigation calculation, obtain position, speed and the attitude angle information of carrier aircraft under navigation system, and each parameter information of the pure-inertial guidance state being provided to the adaptive Kalman filter computing module, for radar Doppler and triones navigation system, carrying out information fusion.

(2.2) described Doppler (Doppler) radar speed filtering and modular converter carry out low-pass digital filter for three axial velocity component under the body system that radar Doppler is recorded, after removing wild value and most measurement noise, the three axial velocity information that the attitude information that adopts inertial navigation to resolve obtains Doppler radar measurement are transformed under navigation coordinate system, the velocity information of resolving with inertial navigation forms observed quantity and carries out information fusion, as the speed observation information of integrated navigation Filtering Model.

(2.3) it is poor that described Beidou satellite navigation resolves corresponding latitude, longitude and elevation information that module resolves with inertial navigation for latitude, longitude and elevation information by Beidou satellite receiver output, the position detection information of structure integrated navigation Filtering Model.

(2.4) described barometer altitude information is resolved module and is measured by pressure altimeter the current flight elevation information that atmospheric pressure obtains carrier aircraft, and pass through the ARINC429 bus transfer in airborne navigation computer, in the situation that big-dipper satellite can't normally be located, with pure-inertial guidance, to resolve the elevation information obtained poor, builds the height observation information of integrated navigation Filtering Model.

(2.5) described adaptive Kalman filter resolves module and adopts the matching way of " position+speed " to set up the adaptive Kalman filter model, the velocity information that velocity information after the attitude transition matrix conversion that the information that tests the speed after the radar Doppler pre-filtering is tied to navigation coordinate system through inertial coordinate under the navigation coordinate system of gained and inertial navigation resolve is poor, it is poor that the positional information that Beidou receiver is exported and inertial navigation resolve the positional information obtained, " position+speed " observed quantity of constructing system is also carried out information fusion, the error variance of each subsystem in loop iteration filtering estimating system state equation, the site error of inertial navigation system is estimated and revised to real-time closed-loop, velocity error, the misaligned angle of the platform, gyroscope constant value drift, Modelling of Random Drift of Gyroscopes, the range rate error of accelerometer bias and radar Doppler and scale coefficient error, when if big-dipper satellite can't normally be located, the elevation information that the elevation information that utilizes pressure altimeter to resolve and inertial navigation resolve is poor, thereby built the information fusion pattern of " highly+speed ", the navigation error of filter correction Inertial Measurement Unit and radar Doppler and device error.

(2.6) described control and to combine aobvious module be supervisory system, for functions such as the System self-test, system initialization, working state control, navigation command control inputs, navigational parameter output and the demonstration that realize integrated navigation system, working state of system supervision.

In conjunction with Fig. 4, the adaptive Kalman that fades (Kalman) wave filter in the present invention and conventional Kalman wave filter difference, for adopting the memory span of forgetting factor restriction Kalman filtering, to take full advantage of up-to-date data, are being calculated one-step prediction variance estimation P _{k, k-1}shi Duocheng adaptive forgetting factor λ _k, adopt adaptive forgetting factor λ _kgo to limit the memory span of Kalman wave filter, take full advantage of current measurement data, increase the effect of current data in system state estimation, with the sudden change of avoiding the measurement noise characteristic, cause dispersing of wave filter.For calculating adaptive forgetting factor λ _k, need to solve intermediary matrix M _kand N _k:

$M_k=H_k{\mathrm{\&Phi;}}_{k,k-1}{P}_{k,k-1}{\mathrm{\&Phi;}}_{k,k-1}^T{H}_k^T,N_k=H_k{P}_{k,k-1}{H}_k^T-H_k{Q}_{k-1}{H}_k^T$

Parameter declaration related in Fig. 4 formula is as follows:

for the state estimation value of initial time, P ₀for initial time

The estimation of error covariance matrix;

for k state estimation value constantly;

for the predicted value of the k-1 moment to k moment state; K _kfor k filter gain battle array constantly; P _kfor k estimation of error covariance matrix constantly; P _{k, k-1}the prediction estimation error covariance battle array constantly to k constantly for k-1; H _kfor k moment system measurements matrix; Q _k-1for k-1 system noise variance battle array constantly; Z _kfor measuring vector; I is unit matrix; Φ _{k, k-1}be carved into k system state transition matrix constantly during for k-1; Γ _k-1for k-1 system noise constantly drives matrix; R _kfor observation noise variance battle array.

The system of setting up departments meets Q _k, R _k, P _kbe the condition of positive definite symmetric matrices, and the system measurements matrix H _kfull rank, introduce and adjust factor alpha (α>1), best forgetting factor λ _kfor:

λ _k＝max{1,α*tr(N _k)/tr(M _k)} (2)

In conjunction with Fig. 5, the Big Dipper/radar Doppler of the present invention/inertia autonomous type Combinated navigation method, step is as follows:

The 1st step: at first carry out initialization after system powers on, then carry out System self-test.If self check is normal, to fire control system, report self-detection result, then proceed to next step; Otherwise system is carried out localization of fault, supervisory system shows failure message and reports to the police;

Described initial work comprises the initialization of the hardware such as Dui Ge road serial ports, ARINC429 bus, 1553B bus communication system and timer; Output data message to equipment such as Beidou satellite receiver, radar Doppler and Inertial Measurement Units is gathered; To the Kalman filter initialization, i.e. the initialization of system initial state X, system state covariance matrix P, measuring noise square difference battle array R, systematic procedure noise variance matrix Q etc.; Last airborne navigation computer receives as remote terminal the System self-test instruction that carrier aircraft sends through the 1553B bus, carries out System self-test;

Described System self-test carries out the validity judgement to received raw data, and whether the correctness of duty, the memory read-write of duty, the Beidou satellite receiver of validity, the pressure altimeter of the duty of duty, the radar Doppler of normal, optical fibre gyro, accelerometer and signal and program can normal load etc. to comprise detection system voltage.

The 2nd step: carry out initial alignment: the corresponding parameter that Beidou receiver is assigned to Inertial Measurement Unit using the carrier aircraft position that records and velocity information is as the initial value that resolves of Inertial Measurement Unit, then according to accelerometer and the gyrostatic measured value appearance of being navigated, resolves the attitude information that obtains initial alignment;

The 3rd step: the measurement data to radar Doppler carries out pre-filtering, to improve the rate accuracy of radar Doppler, and can be rejected the single-point type and the patch type outliers that occur in measurement data.

The measurement data pre-filtering of radar Doppler adopts the LMS adaptive filter algorithm,, by adjusting filter factor, the mean square value ε of output error sequence is minimized, and, according to ε power of amendment coefficient, the mean square value ε of error sequence is:

ε＝E[e ²(n)]＝E{[d(n)-y(n)] ²} (3)

Wherein, d (n) is ideal signal, and e (n) is the output error sequence, the output that y (n) is wave filter.

Suppose input signal vector be x (n)=[x (n), x (n-1) ..., x (n-m+1)] ^t, weight vector is w (n)=[w _n1, w _n2..., w _nm] ^t, the output y (n) of wave filter is:

$y\left(n\right)=\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{w}_{\mathrm{ni}}\left(n\right)x(n-i+1)=w^T\left(n\right)x\left(n\right)---\left(4\right)$

Wherein, m means the exponent number of wave filter, and n means to input the number of sample at every turn.

The output y (n) of wave filter and the error sequence e (n) between ideal signal are:

e(n)＝d(n)-y(n)＝d(n)-w ^T(n)x(n) (5)

According to Minimum Mean Square Error (MSE) criterion, the weight vector of optimum filter should make the mean square deviation minimum, adopts the method for recursion convergence to find best wave filter weight vector to be:

w(n+1)＝w(n)+μe(n)x(n) (6)

Wherein, μ is step-length, and the condition of convergence is: 0<μ≤1/ λ _max, λ wherein _maxbe the eigenvalue of maximum of input signal autocorrelation matrix, in the present invention, step size mu is taken as 0.00001.

The 4th step: the measurement data of Inertial Measurement Unit is carried out to inertial navigation and resolve, obtain the carrier aircraft lower inertial navigation information of day navigation coordinate system northeastward, the strapdown attitude during inertial navigation resolves is resolved part employing Shuangzi sample algorithm;

Described inertial navigation information comprises: angular velocity information, than force information, velocity information, positional information and attitude information.Wherein angular velocity information comprises: the east orientation angular velocity omega of inertial navigation system _e, inertial navigation system the north orientation angular velocity omega _n, inertial navigation system the sky to angular velocity omega _u, specific force information comprises: the east orientation specific force f of inertial navigation system _e, inertial navigation system north orientation specific force f _n, inertial navigation system the sky to specific force f _u, velocity information comprises: the east orientation speed V of inertial navigation system _e, inertial navigation system north orientation speed V _n, inertial navigation system the sky to speed V _u, positional information comprises longitude λ, the latitude L of inertial navigation system, the height h of inertial navigation system of inertial navigation system, attitude information comprises: pitching angle theta, roll angle γ and course angle ψ, its corresponding body is tied to the attitude matrix of navigation system

for:

$C_b^n=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}\\ \cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}\\ -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]$

The 5th step: airborne navigation computer receives the positional information of Beidou receiver, the information that tests the speed of radar Doppler, inertial navigation information, adopt the matching way of " position+speed " to set up the adaptive Kalman filter model, Beidou receiver, Inertial Measurement Unit and radar Doppler subsystem are carried out to information fusion, estimate each margin of error in Inertial Measurement Unit and the navigation information of Inertial Measurement Unit is carried out to feedback compensation; The range rate error of estimating Doppler radar and scale coefficient error also carry out feedback compensation to the navigation information of radar Doppler, obtain final revised navigation results;

Velocity information after attitude matrix conversion by the information that tests the speed after radar Doppler pre-filtering in the 3rd step through resolving in the 4th step in the lower velocity information of the navigation system of gained and the 4th step gained inertial navigation information is poor, positional information and the positional information in the 4th step gained inertial navigation information of Beidou receiver output is poor, carry out the Kalman filtering information fusion using the gained difference as observed quantity, the error variance of each subsystem in loop iteration filtering estimating system state equation, the site error of inertial navigation system is estimated and revised to real-time closed-loop, velocity error, the misaligned angle of the platform, gyroscope constant value drift, Modelling of Random Drift of Gyroscopes, the range rate error of accelerometer bias and radar Doppler, scale coefficient error.The filtering cycle of adaptive Kalman filter is taken as 1s, airborne navigation computer is every the original output data of 5ms once sampling optical fibre gyro and accelerometer, the navigation information of every 100ms once sampling Beidou satellite receiver, the inertial navigation that every 10ms carries out an Inertial Measurement Unit resolves, and every 1s carries out an integrated navigation filtering and error real time calibration.

The matching way of described employing " position+speed " is set up the adaptive Kalman filter model, specific as follows:

(a) system state equation:

$\stackrel{\&CenterDot;}{X}=\mathrm{FX}+\mathrm{GW}---\left(7\right)$

Wherein, X is system state vector,

mean the derivative of system state vector, F is the system state transition matrix, and G is that system noise drives matrix, and W is the system noise vector, specific as follows:

Wherein

be respectively carrier aircraft east orientation, north orientation, day to attitude error, δ v _e, δ v _n, δ v _ube respectively carrier aircraft east orientation, north orientation, day to velocity error, δ L, δ λ, δ h are respectively latitude error, longitude error, the height error of carrier aircraft, ε _rx, ε _ry, ε _rzit is the component on three axles that the single order Markov that is respectively gyro drifts in carrier, ▽ _ax, ▽ _ay, ▽ _azit is the component on three axles that the normal value that is respectively accelerometer is biased in carrier, ε _x, ε _y, ε _zthe constant value drift that is respectively gyro is the component on three axles at carrier, δ v _tx, δ v _ty, δ v _tzthe range rate error that is respectively radar Doppler is the component on three axles at carrier, δ K _dx, δ K _dy, δ K _dzthe scale coefficient error that tests the speed that is respectively radar Doppler is the component on three axles at carrier;

Concrete, δ v _tx, δ v _ty, δ v _tzbecause landform changes the range rate error caused, because landform changes rarer sudden change relatively, therefore adopt single order Markov process approximate description, τ for radar Doppler _tx, τ _ty, τ _tzbe respectively δ v _tx, δ v _ty, δ v _tzsingle order Markov process model in correlation time, δ K _dx, δ K _dy, δ K _dzdepend primarily on the alignment error of fixed antenna, because the relative aircraft of established angle of dual-mode antenna is changeless, so the scale coefficient error caused by the alignment error angle is also changeless, be generally stochastic variable, adopt the arbitrary constant model to carry out modeling to it, and δ v _ti(i=x, y, z) is a process become slowly, can adopt the single order Markov model to be described, δ K _dithe model of (i=x, y, z) is suc as formula (8), δ v _tithe model of (i=x, y, z) is suc as formula shown in (9):

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{K}}_{\mathrm{dx}}=0\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{K}}_{\mathrm{dy}}=0\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{K}}_{\mathrm{dz}}=0\end{array}\right.---\left(8\right)$

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}_{\mathrm{tx}}=-\frac{1}{{\mathrm{\&tau;}}_{\mathrm{tx}}}{\mathrm{\&delta;v}}_{\mathrm{tx}}+w_{\mathrm{tx}}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}_{\mathrm{ty}}=-\frac{1}{{\mathrm{\&tau;}}_{\mathrm{ty}}}{\mathrm{\&delta;v}}_{\mathrm{ty}}+w_{\mathrm{ty}}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}_{\mathrm{tz}}=-\frac{1}{{\mathrm{\&tau;}}_{\mathrm{tz}}}{\mathrm{\&delta;v}}_{\mathrm{tz}}+w_{\mathrm{tz}}\end{array}\right.---\left(9\right)$

Wherein, w _tx, w _ty, w _tzbe respectively δ v _tx, δ v _ty, δ v _tzthe driving white noise.

So,

$F=\left[\begin{array}{ccc}{\left(F_{\mathrm{INS}}\right)}_{9\&times;9}& {\left(F_S\right)}_{9\&times;9}& 0_{3\&times;6}\\ 0_{9\&times;9}& {\left(F_{\mathrm{IMU}}\right)}_{9\&times;9}& 0_{3\&times;6}\\ 0_{6\&times;9}& 0_{6\&times;9}& {\left(F_{\mathrm{DVS}}\right)}_{6\&times;6}\end{array}\right]$

Wherein $F_S=\left[\begin{array}{ccc}{\mathrm{<figure-callout\; id="0"\; label="C\; b\; n"\; filenames="HDA0000388651070000042.png,HDA0000388651070000051.png"\; state="\{\{state\}\}">C</figure-callout>}}_{bn}^{}& 0_{3\&times;3}& {\mathrm{<figure-callout\; id="0"\; label="C\; b\; n"\; filenames="HDA0000388651070000042.png,HDA0000388651070000051.png"\; state="\{\{state\}\}">C</figure-callout>}}_{bn}^{}\\ 0_{3\&times;3}& {\mathrm{<figure-callout\; id="0"\; label="C\; b\; n"\; filenames="HDA0000388651070000042.png,HDA0000388651070000051.png"\; state="\{\{state\}\}">C</figure-callout>}}_{bn}^{}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right],F_{\mathrm{DVS}}=\left[\begin{array}{cccc}-1/{\mathrm{\&tau;}}_{\mathrm{<figure-callout\; id="0"\; label="tx"\; filenames="HDA0000388651070000042.png,HDA0000388651070000051.png"\; state="\{\{state\}\}">tx</figure-callout>}}& 0& 0& 0_{1\&times;3}\\ 0& -1/{\mathrm{\&tau;}}_{\mathrm{<figure-callout\; id="0"\; label="ty"\; filenames="HDA0000388651070000042.png,HDA0000388651070000051.png"\; state="\{\{state\}\}">ty</figure-callout>}}& 0& 0_{1\&times;3}\\ 0& 0& -1/{\mathrm{\&tau;}}_{\mathrm{<figure-callout\; id="0"\; label="tz"\; filenames="HDA0000388651070000042.png,HDA0000388651070000051.png"\; state="\{\{state\}\}">tz</figure-callout>}}& 0_{1\&times;3}\\ 0_{3\&times;1}& 0_{3\&times;1}& 0_{3\&times;1}& 0_{3\&times;3}\end{array}\right]$

F _iNSfor corresponding 9 basic navigation parameters δ v _e, δ v _n, δ v _u, δ L, δ λ, δ h system matrix, F _iMUfor corresponding 9 inertial device error ε _rx, ε _ry, ε _rz, ▽ _ax, ▽ _ay, ▽ _az, ε _x, ε _y, ε _zsystem matrix.

System noise drives matrix G as follows:

$G=\left[\begin{array}{cccc}{\mathrm{<figure-callout\; id="0"\; label="C\; b\; n"\; filenames="HDA0000388651070000042.png,HDA0000388651070000051.png"\; state="\{\{state\}\}">C</figure-callout>}}_{bn}^{}& 0_{3\&times;3}& {\mathrm{<figure-callout\; id="0"\; label="C\; b\; n"\; filenames="HDA0000388651070000042.png,HDA0000388651070000051.png"\; state="\{\{state\}\}">C</figure-callout>}}_{bn}^{}& 0_{3\&times;3}\\ 0_{3\&times;3}& {\mathrm{<figure-callout\; id="0"\; label="C\; b\; n"\; filenames="HDA0000388651070000042.png,HDA0000388651070000051.png"\; state="\{\{state\}\}">C</figure-callout>}}_{bn}^{}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{12\&times;3}& 0_{12\&times;3}& 0_{12\&times;3}& 0_{12\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& I_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]$

W is as follows for the system noise vector:

W＝[ω _rxω _ryω _rzω _axω _ayω _azω _xω _yω _zω _txω _tyω _tz] ^T

Wherein, ω _rx, ω _ry, ω _rzbe respectively the driving white noise of the single order Markov drift model of Modelling of Random Drift of Gyroscopes, ω _ax, ω _ay, ω _azbe respectively the white Gaussian noise of accelerometer bias, ω _x, ω _y, ω _zbe respectively the white Gaussian noise of gyroscope constant value drift, ω _tx, ω _ty, ω _tzbe respectively the range rate error δ v of radar Doppler _tx, δ v _ty, δ v _tzthe driving white noise of single order Markov drift model.

(b) system measurements equation:

Z＝HX+V

Wherein, Z is measurement vector, and H is observing matrix, and V is the observation noise matrix, specific as follows:

Z＝[δLδλδhδV _EδV _NδV _U] ^T，H＝[H _PH _V] ^T

$H_P=\left[\begin{array}{ccc}{0}_{3\&times;6}& \mathrm{diag}\left[\begin{array}{ccc}{R}_m& R_{\mathrm{<figure-callout\; id="1"\; label="n\; cos\; L"\; filenames="HDA0000388651070000022.png"\; state="\{\{state\}\}">n</figure-callout>}}\cos L& 1\end{array}\right]& 0_{3\&times;15}\end{array}\right]$

$H_V=\left[\begin{array}{ccccccccc}& & & C_{11}& C_{12}& C_{13}& C_{11}{v}_x& C_{12}{v}_y& C_{13}{\mathrm{<figure-callout\; id="0"\; label="v\; z"\; filenames="HDA0000388651070000042.png,HDA0000388651070000051.png"\; state="\{\{state\}\}">v</figure-callout>}}_z\\ 0_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;12}& C_{21}& C_{22}& C_{23}& C_{21}{v}_x& C_{22}{v}_y& C_{23}{v}_z\\ & & & C_{31}& C_{32}& C_{33}& C_{31}{v}_x& C_{32}{v}_y& C_{33}{v}_z\end{array}\right]$

$V={\left[\begin{array}{cccccc}{V}_L& V_{\mathrm{\&lambda;}}& V_h& V_{V_E}& V_{V_N}& V_{V_U}\end{array}\right]}^T$

R wherein _mfor the radius-of-curvature of each point on the ellipsoid meridian circle, R _nfor the radius-of-curvature of the upper each point of prime vertical (plane at its place is vertical with meridian ellipse), L, λ, h mean respectively latitude, longitude and height, V _e, V _n, V _ube respectively the speed of east, north, day direction, v _x, v _y, v _zdextrad, forward direction and component vertically upward under the carrier aircraft body system recorded for radar Doppler, C _ijmean the attitude transition matrix

the component of the capable j row of corresponding i, i=1,2,3; J=1,2,3, V _l, V _λ, V _hthe latitude, longitude and the height observational error that mean respectively Beidou receiver,

mean that respectively radar Doppler is through attitude matrix

be transformed into the speed observational error that east, north, old name for the Arabian countries in the Middle East under navigation system make progress.

The 6th step: the navigation results parameter information is sent to supervisory system.

The navigation results parameter information comprises position, speed, the attitude information under navigation system, the test the speed information such as available star number, PDOP, HDOP value of the out-of-lock condition of information, radar Doppler, current big-dipper satellite of radar Doppler under carrier aircraft body system, send to supervisory system to facilitate the pilot intuitively the navigation information of carrier aircraft is grasped and judged.

Embodiment 1

The Big Dipper/radar Doppler of the present invention/inertia autonomous type integrated navigation system and method thereof, be applied in the helicopter independent integrated navigation system, carried out double axle table test and unidirectional near linear expressway preventing test:

(1) the double axle table test swings under environment (flight environment of vehicle for helicopter is mainly pitching and course passage) in order to test at body, whether the attitude angle of the Big Dipper/radar Doppler/inertia autonomous type integrated navigation system disperses, adopt the high-precision dual-axis turntable to carry out the turntable test to designed integrated navigation system principle prototype, the attitude error extreme value under each swinging condition is as shown in table 1.

Attitude error extreme value under each swinging condition of table 1

(2) but the flight path of the unidirectional preventing test helicopter simulating in expressway is further verified the navigation accuracy of integrated navigation under dynamic environment.The geometric locus that Fig. 6 is sport car, Fig. 7～8 are respectively the navigation results correlation curve of position, speed and the GPS of integrated navigation system while adopting the long boat of autonomous type of the present invention, the result curve that tests the speed that Fig. 9 is Doppler radar in the sport car process, Figure 10 is the attitude of carrier change curve in the sport car process.From test findings, find out, the location of autonomous type integrated navigation system under current intelligence during designed long boat, test the speed and to survey the appearance precision higher, in the situation that the sport car highway section is near linear and existing Beidou satellite navigation precision, there is the position navigation accuracy of 15m (1 σ) and the speed navigation accuracy of 0.2m/s (1 σ), can meet the requirement of airborne independent combined navigation System Dependent index.

In sum, the present invention takes full advantage of the complementarity of big-dipper satellite, radar Doppler and Inertial Measurement Unit, adopt the information fusion algorithm of optimizing, seamlessly export in real time accurate navigation information, navigation feature while having realized the long boat of a kind of airborne autonomous type cheaply, solved the problem that existing autonomous navigation system error is large, cost is high effectively.

Claims (8)

1. the Big Dipper/radar Doppler/inertia autonomous type integrated navigation system, is characterized in that, comprises Inertial Measurement Unit, radar Doppler, Beidou receiver, air data system, datalogger, supervisory system, AHRS and airborne navigation computer; Described airborne navigation computer comprises DSP, FPGA, crystal oscillator, JTAG, SDRAM, FLASH and power module, wherein the address bus of DSP, data bus and control bus are connected with the corresponding bus of FPGA respectively, the interrupt output termination of FPGA enters the interrupting input end of DSP, the clock signal input terminal of crystal oscillator access DSP, the clock signal input terminal of the clock signal output terminal access FPGA of DSP, power module provides power supply for airborne computer, and airborne computer provides power supply for whole system; Described radar Doppler, air data system and supervisory system are connected with FPGA by the ARINC429 communication interface respectively, Beidou receiver is connected with FPGA by the RS-232 communication interface respectively with datalogger, and Inertial Measurement Unit is connected with FPGA by the RS-422 communication interface respectively with AHRS; Airborne navigation computer is connected with airborne flight control system by aviation 1553B bus;

The FPGA of described airborne navigation computer receives the measurement data of Beidou receiver by the RS-232 communication interface, receive the measurement data of radar Doppler by the ARINC429 communication interface, receive the measurement data of Inertial Measurement Unit by the RS-422 communication interface, FPGA carries out integrated navigation by the measurement data input DSP received and resolves, obtain navigation results, finally the integrated navigation result is sent to supervisory system by the ARINC429 communication interface respectively, and send to datalogger by the RS-232 communication interface, it is crosslinked that airborne navigation computer carries out information by aviation 1553B bus and airborne flight control system.

2. the Big Dipper/radar Doppler according to claim 1/inertia autonomous type integrated navigation system, it is characterized in that, described Inertial Measurement Unit comprises the gyroscope of three axle quadratures installations, accelerometer, A/D modular converter and the temperature compensation system that three axle quadratures are installed, the data output end of gyroscope and accelerometer is through A/D modular converter access temperature compensation system, the output terminal of temperature compensation system is through RS422 communication interface access airborne navigation computer, wherein gyroscope is the aviation-grade fibre optic gyroscope, and accelerometer is silicon micro accerometer.

3. the Big Dipper/radar Doppler according to claim 1/inertia autonomous type integrated navigation system, it is characterized in that, described Beidou receiver is compatible receives a Big Dipper generation and the Big Dipper two generations satellite-signal, and carries out communication by RS232 communication interface and embedded navigation computer.

4. the Big Dipper/radar Doppler according to claim 1/inertia autonomous type integrated navigation system, is characterized in that, described radar Doppler is that strapdown X font configures four wave beam radar Dopplers.

5. the Big Dipper/radar Doppler/inertia autonomous type Combinated navigation method, is characterized in that, comprises the following steps:

The 1st step, at first carry out initialization after system powers on, then carry out System self-test: if self check is normal, to flight control system, report self-detection result, then proceed to next step; Otherwise system is carried out localization of fault, supervisory system shows failure message and reports to the police;

The 2nd step, carry out initial alignment: the corresponding parameter that Beidou receiver is assigned to Inertial Measurement Unit using the carrier aircraft position that records and velocity information is as the initial value that resolves of Inertial Measurement Unit, then according to accelerometer and the gyrostatic measured value appearance of being navigated, resolves the attitude information that obtains initial alignment;

The 3rd step, carry out pre-filtering to the measurement data of radar Doppler, and single-point type and patch type outliers in measurement data rejected;

The 4th step, carry out strap-down inertial to the measurement data of Inertial Measurement Unit and resolve, and obtains the carrier aircraft inertial navigation information under day navigation coordinate system northeastward;

The 5th step, airborne navigation computer receives the positional information of Beidou receiver, the information that tests the speed of radar Doppler, inertial navigation information, adopt the matching way of " position+speed " to set up the adaptive Kalman filter model, Beidou receiver, Inertial Measurement Unit and radar Doppler subsystem are carried out to information fusion, estimate each margin of error in Inertial Measurement Unit and the navigation information of Inertial Measurement Unit is carried out to feedback compensation; The range rate error of estimating Doppler radar and scale coefficient error also carry out feedback compensation to the navigation information of radar Doppler, obtain final revised navigation results;

The 6th step, be sent to supervisory system by the integrated navigation result.

6. the Big Dipper/radar Doppler according to claim 5/inertia autonomous type Combinated navigation method, it is characterized in that, the measurement data pre-filtering of the described radar Doppler of the 3rd step adopts the LMS adaptive filter algorithm, by adjusting filter factor, the mean square value ε of output error sequence is minimized, and, according to ε power of amendment coefficient, the mean square value ε of error sequence is:

ε＝E[e ²(n)]＝E{[d(n)-y(n)] ²}

Wherein, d (n) is ideal signal, and e (n) is the output error sequence, the output that y (n) is wave filter;

Suppose input signal vector be x (n)=[x (n), x (n-1) ..., x (n-m+1)] ^t, weight vector is w (n)=[w _n1, w _n2..., w _nm] ^t, the output y (n) of wave filter is:

Wherein, m means the exponent number of wave filter, and n means to input the number of sample at every turn;

The output y (n) of wave filter and the error sequence e (n) between ideal signal d (n) are:

e(n)＝d(n)-y(n)＝d(n)-w ^T(n)x(n)

According to least-mean-square-error criterion, adopt the recursion convergence method to determine that best wave filter weight vector is:

w(n+1)＝w(n)+μe(n)x(n)

Wherein, μ is step-length, and the condition of convergence is: 0<μ≤1/ λ _max, λ wherein _maxit is the eigenvalue of maximum of input signal autocorrelation matrix.

7. the Big Dipper/radar Doppler according to claim 5/inertia autonomous type Combinated navigation method, it is characterized in that, the described carrier aircraft of the 4th step inertial navigation information under day navigation coordinate system northeastward comprises: angular velocity information, than force information, velocity information, positional information and attitude information, wherein angular velocity information comprises: the east orientation angular velocity omega of inertial navigation system _e, inertial navigation system the north orientation angular velocity omega _n, inertial navigation system the sky to angular velocity omega _u, specific force information comprises: the east orientation specific force f of inertial navigation system _e, inertial navigation system north orientation specific force f _n, inertial navigation system the sky to specific force f _u, velocity information comprises: the east orientation speed V of inertial navigation system _e, inertial navigation system north orientation speed V _n, inertial navigation system the sky to speed V _u, positional information comprises longitude λ, the latitude L of inertial navigation system, the height h of inertial navigation system of inertial navigation system, attitude information comprises: pitching angle theta, roll angle γ and course angle ψ, its corresponding body is tied to the attitude matrix of navigation system for:

。

8. the Big Dipper/radar Doppler according to claim 5/inertia autonomous type Combinated navigation method, is characterized in that, the matching way of the described employing of the 5th step " position+speed " is set up the adaptive Kalman filter model, specific as follows:

(a) system state equation:

Wherein, X is system state vector,

mean the derivative of system state vector, F is the system state transition matrix, and G is that system noise drives matrix, and W is the system noise vector, specific as follows:

Wherein

be respectively carrier aircraft east orientation, north orientation, day to attitude error, δ v _e, δ v _n, δ v _ube respectively carrier aircraft east orientation, north orientation, day to velocity error, δ L, δ λ, δ h are respectively latitude error, longitude error, the height error of carrier aircraft, ε _rx, ε _ry, ε _rzit is the component on three axles that the single order Markov that is respectively gyro drifts in carrier, ▽ _ax, ▽ _ay, ▽ _azit is the component on three axles that the normal value that is respectively accelerometer is biased in carrier, ε _x, ε _y, ε _zthe constant value drift that is respectively gyro is the component on three axles at carrier, δ v _tx, δ v _ty, δ v _tzthe range rate error that is respectively radar Doppler is the component on three axles at carrier, δ K _dx, δ K _dy, δ K _dzthe scale coefficient error that tests the speed that is respectively radar Doppler is the component on three axles at carrier;

δ K wherein _dx, δ K _dy, δ K _dzadopt the arbitrary constant model to carry out modeling, δ K _dithe model of (i=x, y, z) as shown in the formula:

δ v _ti(i=x, y, z) adopts the single order Markov model to be described, δ v _tithe model of (i=x, y, z) is shown below:

Wherein, τ _txfor δ v _txthe single order Markov model in correlation time, τ _tyfor δ v _tythe single order Markov model in correlation time, τ _tzfor δ v _tzthe single order Markov model in correlation time, w _txfor δ v _txdriving white noise, w _tyfor δ v _tydriving white noise, w _tzfor δ v _tzthe driving white noise;

So,

Wherein

F _iNSfor corresponding 9 basic navigation parameters

δ v _e, δ v _n, δ v _u, δ L, δ λ, δ h system matrix, F _iMUfor corresponding 9 inertial device error ε _rx, ε _ry, ε _rz, ▽ _ax, ▽ _ay, ▽ _az, ε _x, ε _y, ε _zsystem matrix;

System noise drives matrix G as follows:

W is as follows for the system noise vector:

W＝[ω _rxω _ryω _rzω _axω _ayω _azω _xω _yω _zω _txω _tyω _tz] ^T

Wherein, ω _rx, ω _ry, ω _rzbe respectively the driving white noise of the single order Markov drift model of Modelling of Random Drift of Gyroscopes, ω _ax, ω _ay, ω _azbe respectively the white Gaussian noise of accelerometer bias, ω _x, ω _y, ω _zbe respectively the white Gaussian noise of gyroscope constant value drift, ω _tx, ω _ty, ω _tzbe respectively the range rate error δ v of radar Doppler _tx, δ v _ty, δ v _tzthe driving white noise of single order Markov drift model;

(b) system measurements equation:

Z＝HX+V

Wherein, Z is measurement vector, and H is observing matrix, and V is the observation noise matrix, specific as follows:

Z＝[δL δλ δh δV _E δV _N δV _U] ^T，H＝[H _P H _V] ^T

R wherein _mfor the radius-of-curvature of each point on earth meridian circle, R _nfor the radius-of-curvature of each point on earth prime vertical, L, λ, h mean respectively latitude, longitude and height, V _e, V _n, V _ube respectively the speed of east, north, day direction, v _x, v _y, v _zdextrad under the carrier aircraft body system recorded for radar Doppler, forward direction and the component on direction vertically upward, V _l, V _λ, V _hthe latitude, longitude and the height observational error that mean respectively Beidou receiver, Ci _jmean the attitude transition matrix

component and the i=1 of the capable j row of corresponding i, 2,3; J=1,2,3,

mean that respectively radar Doppler is through attitude matrix

be transformed into the speed observational error that east, north, old name for the Arabian countries in the Middle East under navigation system make progress.

Images