CN104931995A - Vector tracking-based GNSS/SINS deep integrated navigation method - Google Patents

Abstract

The invention discloses a vector tracking-based GNSS/SINS deep integrated navigation method. The method includes the following steps that: after in-phase orthogonal signals outputted by a correlator are calculated by a phase discrimination function, an obtained phase discrimination result is adopted as measurement information of a pre-filter, so that a pre-filter model can be constructed to estimate tracking error information, and therefore, the pseudo-range and pseudo-range rate of a GNSS tracking channel can be obtained; an integrated navigation main filter performs processing according to the pseudo-ranges and pseudo-range rates outputted by the GNSS tracking channel and an SINS, so that pseudo-range deviation and range rate deviation can be obtained and are adopted as measurement variables quantity, and the measurement quantity is used for updating navigation error state variables, and updated navigation error parameters are fed back to the SINS, so that the navigation parameters of the SINS can be calibrated; and an integrated navigation system infers the signal tracking parameters of a GNSS according to the calibrated SINS navigation parameters and ephemeris information so as to control the local pseudo codes of a receiver and a carrier digital-controlled oscillator, and therefore, tracking for input signals can be maintained. The method of the invention has excellent anti-jamming performance and dynamic tracking ability, and has a bright application prospect.

Description

A kind of GNSS/SINS deep integrated navigation method based on vector tracking

Technical field

The present invention relates to satellite navigation, integrated navigation field, particularly a kind of GNSS/SINS deep integrated navigation method based on vector tracking.

Background technology

GNSS system has round-the-clock, fulltime advantage, but be subject to electromagnetic interference (EMI), high dynamically under may occur losing star losing lock situation; SINS system is a kind of autonomic navigation system not relying on any external information, also outside emittance, has that volume is little, data updating rate is high, a high and not advantage such as climate condition restriction of precision in short-term, but navigation accuracy reduces in time.Therefore normal by both combine to form SINS/GNSS integrated navigation system.While carrier improves constantly the demand that the combination property of navigational system proposes, from simple pine combination and tight integration, dark combination technique is turned to the research of GNSS/SINS integrated navigation technology aspect.

At present both at home and abroad main research is based on the SINS assisted GNSS hypercompact combination navigation system of traditional receiver tracking loop circuit and directly the fusion baseband I/Q information of GNSS receiver and the centralized deep integrated navigation system of Inertia information.But traditional receiver tracking loop circuit adopts scalar tracing mode, this pattern easily produces signal losing lock under high dynamic environment, and is subject to electromagnetic interference (EMI) under weak signal environment; And though centralized dark combination employs vector tracking algorithm, because observed quantity in model and quantity of state are nonlinearity relation, so be difficult to be committed to engineering practice.

Summary of the invention

The object of the present invention is to provide a kind of GNSS/SINS deep integrated navigation method based on vector tracking that precision is high, reliability is strong, with improve GNSS signal follow the tracks of performance and satellite receiver to the adaptability of high dynamic motion carrier.

Technical solution of the present invention is: a kind of GNSS/SINS deep integrated navigation method based on vector tracking, comprises the following steps:

Step 1, set up the vector tracking model based on prefilter: the homophase that correlator exports, orthogonal signal are after phase demodulation function calculates, gained identified result sets up prefilter model as the measurement information of prefilter, adopt this prefilter model to estimate tracking error information, obtain GNSS tracking channel pseudorange, pseudorange rates according to tracking error information;

Step 2, set up integrated navigation senior filter model: pseudorange, pseudorange rates that integrated navigation senior filter exports according to GNSS tracking channel and SINS system, process obtains pseudorange biases and pseudorange rates deviation, pseudorange biases and pseudorange rates deviation are upgraded navigation error state variable as measurement amount, and navigation error parameter feedback is returned in SINS system SINS navigational parameter is corrected;

Step 3, inertia additional feedback control GNSS forms closed loop: integrated navigation system infers the signal trace parameter of GNSS according to the SINS navigational parameter after correction and ephemeris information, in order to control local pseudo-code, the carrier number controlled oscillator of receiver, to keep the tracking to input signal.

Compared with prior art, its remarkable advantage is in the present invention: (1) adopts vector tracking method, eliminates independent, parallel scalar tracing mode, takes full advantage of the shared information between each satellite channel, can follow the tracks of all satellites in view simultaneously; (2) adopt the mode of cascade to realize dark combination in two steps: the first step is baseband signal pre-processing filter, complete code/carrier tracking error and estimate; Second step is senior filter (i.e. integrated navigation wave filter), and complete the estimation of the SINS control information thought by means of federated filter, which alleviates the processing load of single integrated navigation wave filter in centralized dark combination; (3) inertia ancillary technique is adopted, revised SINS navigational parameter and ephemeris information is utilized to infer the signal trace parameter such as GNSS pseudo code phase place and Doppler shift, and estimated result is fed back to receiver inside track loop is assisted, realize inertial navigation assisting satellite, improve the performance that GNSS signal is followed the tracks of.

Accompanying drawing explanation

Fig. 1 is the system architecture diagram of the GNSS/SINS deep integrated navigation method that the present invention is based on vector tracking.

Fig. 2 is the process flow diagram of the GNSS/SINS deep integrated navigation method that the present invention is based on vector tracking.

Fig. 3 is the high dynamic carrier path curves figure of integrated navigation system in embodiment 1.

Fig. 4 is that in embodiment 1, integrated navigation system is followed the tracks of and the code phase tracking error correlation curve figure under vector tracking two kinds of patterns at scalar.

Fig. 5 is that in embodiment 1, integrated navigation system is followed the tracks of and the Carrier phase tracking error correlation curve figure under vector tracking two kinds of patterns at scalar.

Fig. 6 is that in embodiment 1, integrated navigation system is followed the tracks of and the carrier frequency tracking error correlation curve figure under vector tracking two kinds of patterns at scalar.

Fig. 7 is the site error curve map of integrated navigation system in embodiment 1.

Embodiment

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

As shown in Figure 1, 2, the present invention is based on the GNSS/SINS deep integrated navigation method of vector tracking, be implemented as follows:

When not considering noise, the intermediate-freuqncy signal model S that GNSS receiver radio-frequency front-end exports _iF(t) be:

$S_{IF}\left(t\right)=\sqrt{2A}\·D(t-\mathrm{\τ})\·C(t-\mathrm{\τ})\·cos\[{\mathrm{\ω}}_{IF}t+\mathrm{\φ}\left(t\right)\]---\left(1\right)$

In formula, A is signal intensity, and D (t) is navigation message, and C (t) is C/A code, and τ is the time delay in transmitting procedure, ω _iFfor signal intermediate frequency, φ (t) is original carrier phase place.

Two paths of signals I (t) that local oscillator occurs, Q (t) are respectively:

$I\left(t\right)=\sqrt{2}cos\[({\mathrm{\ω}}_{IF}+\mathrm{\Δ}\mathrm{\ω})t+{\mathrm{\φ}}_0\]---\left(2\right)$

$Q\left(t\right)=\sqrt{2}sin\[({\mathrm{\ω}}_{IF}+\mathrm{\Δ}\mathrm{\ω})t+{\mathrm{\φ}}_0\]---\left(3\right)$

In formula, (ω _iF+ Δ ω) be the carrier frequency that local oscillator produces, Δ ω is the difference of the IF signal frequency of local carrier frequency and input, φ ₀for local signal produces original carrier phase place.

The homophase that the intermediate-freuqncy signal inputted occurs with local oscillator, orthogonal signal are multiplied, after filtering radio-frequency component, and the output S of two branch roads _i(t), S _q(t) be:

$S_I\left(t\right)=\sqrt{A}\·D(t-\mathrm{\τ})\·C(t-\mathrm{\τ})\·cos\[\mathrm{\φ}\left(t\right)-\mathrm{\Δ}\mathrm{\ω}t-{\mathrm{\φ}}_0\]---\left(4\right)$

$S_Q\left(t\right)=\sqrt{A}\·D(t-\mathrm{\τ})\·C(t-\mathrm{\τ})\·sin\[\mathrm{\φ}\left(t\right)-\mathrm{\Δ}\mathrm{\ω}t-{\mathrm{\φ}}_o\]---\left(5\right)$

The output signal of two branch roads is relevant with delayed code (L) to the instantaneous code that local pseudo-code generator generates (P), advanced code (E) respectively, and cumulative summation in post detection integration.Suppose in integration interval, carrier frequency difference and phase differential are all similar to constant, then get the output of the correlator after average to be:

$I_P=\frac{\sqrt{2A}}{2}\·D\·R\left(\mathrm{\δ}\mathrm{\τ}\right)\·\frac{sin\left(\mathrm{\π}T\mathrm{\δ}f\right)}{\mathrm{\π}T\mathrm{\δ}f}\·cos(\mathrm{\π}T\mathrm{\δ}f+\mathrm{\δ}\mathrm{\φ})---\left(6\right)$

$Q_P=\frac{\sqrt{2A}}{2}\·D\·R\left(\mathrm{\δ}\mathrm{\τ}\right)\·\frac{sin\left(\mathrm{\π}T\mathrm{\δ}f\right)}{\mathrm{\π}T\mathrm{\δ}f}\·sin(\mathrm{\π}T\mathrm{\δ}f+\mathrm{\δ}\mathrm{\φ})---\left(7\right)$

$I_E=\frac{\sqrt{2A}}{2}\·D\·R(\mathrm{\δ}\mathrm{\τ}-\mathrm{\δ})\·\frac{\sin \left(\mathrm{\π}T\mathrm{\δ}f\right)}{\mathrm{\π}T\mathrm{\δ}f}\·\cos (\mathrm{\π}T\mathrm{\δ}f+\mathrm{\δ}\mathrm{\φ})---\left(8\right)$

$Q_E=\frac{\sqrt{2A}}{2}\·D\·R(\mathrm{\δ}\mathrm{\τ}-\mathrm{\δ})\·\frac{sin\left(\mathrm{\π}T\mathrm{\δ}f\right)}{\mathrm{\π}T\mathrm{\δ}f}\·sin(\mathrm{\π}T\mathrm{\δ}f+\mathrm{\δ}\mathrm{\φ})---\left(9\right)$

$I_L=\frac{\sqrt{2A}}{2}\·D\·R(\mathrm{\δ}\mathrm{\τ}+\mathrm{\δ})\·\frac{\sin \left(\mathrm{\π}T\mathrm{\δ}f\right)}{\mathrm{\π}T\mathrm{\δ}f}\·\cos (\mathrm{\π}T\mathrm{\δ}f+\mathrm{\δ}\mathrm{\φ})---\left(10\right)$

$Q_L=\frac{\sqrt{2A}}{2}\·D\·R(\mathrm{\δ}\mathrm{\τ}+\mathrm{\δ})\·\frac{sin\left(\mathrm{\π}T\mathrm{\δ}f\right)}{\mathrm{\π}T\mathrm{\δ}f}\·sin(\mathrm{\π}T\mathrm{\δ}f+\mathrm{\δ}\mathrm{\φ})---\left(11\right)$

In formula, δ is the interval of local C/A code lead-lag, and T is post detection integration, and δ τ is PRN phase error, δ f and be respectively the carrier frequency difference between integration interval initial time local reference signal and input signal and carrier phase difference, the related function that R (τ) is C/A code.

Step 1, set up the vector tracking model based on prefilter: the homophase that correlator exports, orthogonal signal are after phase demodulation function calculates, gained identified result sets up prefilter model as the measurement information of prefilter, adopt this prefilter model to estimate tracking error information, obtain GNSS tracking channel pseudorange, pseudorange rates according to tracking error information;

Six tunnel correlation integral results are calculated as follows by normalized lead-lag envelope code Discr. and two quadrant arc tangent carrier wave phase detector:

$\left\{\begin{array}{c}\mathrm{\δ\τ}=\frac{\sqrt{I_E^2+Q_E^2}-\sqrt{I_L^2+Q_L^2}}{\sqrt{I_E^2+Q_E^2}+\sqrt{I_L^2+Q_L^2}}\\ \mathrm{\δ\φ}=\mathrm{ATAN}(Q_P/I_P)\end{array}\right.---\left(12\right)$

Using the measurement information of the identified result in formula (12) as prefilter, build prefilter model as follows:

The state equation of (a) prefilter model:

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\mathrm{\τ}}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\φ}}\\ \mathrm{\δ}\stackrel{\·}{f}\\ \mathrm{\δ}\stackrel{\·\·}{f}\end{array}\right]=\left[\begin{array}{cccc}0& 0& \mathrm{\α}& 0\\ 0& 0& 2\mathrm{\π}& 0\\ 0& 0& 0& 2\mathrm{\π}\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\δ\τ}\\ \mathrm{\δ\φ}\\ \mathrm{\δf}\\ \mathrm{\δ}\stackrel{\·}{f}\end{array}\right]+\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{w}_{\mathrm{\τ}}\\ w_{\mathrm{\φ}}\\ w_f\\ w_{\stackrel{\·}{f}}\end{array}\right]---\left(13\right)$

In formula, δ τ is code phase error, and δ φ is carrier phase error, and δ f is carrier frequency error, for carrier frequency variation rate tracking error; α is the Conversion of measurement unit value of carrier phase and code phase, α=λ _carr/ λ _cA, λ _carrfor the wavelength of carrier wave L1, λ _cAfor C/A code chip lengths; w _τfor code phase error noise, w _φfor carrier phase error noise, w _ffor carrier frequency error noise, for carrier frequency variation rate error noise;

B the observation equation of () prefilter model is:

$\left[\begin{array}{c}\frac{\sqrt{I_E^2+Q_E^2}-\sqrt{I_L^2+Q_L^2}}{\sqrt{I_E^2+Q_E^2}+\sqrt{I_L^2+Q_L^2}}\\ ATAN(Q_P/I_P)\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}\mathrm{\τ}\\ \mathrm{\δ}\mathrm{\φ}\\ \mathrm{\δ}f\\ \mathrm{\δ}\stackrel{\·}{f}\end{array}\right]+\left[\begin{array}{c}{v}_1\\ v_2\end{array}\right]---\left(14\right)$

In formula, I _efor output valve in the correlation integral cycle of I passage advanced branch road, Q _efor output valve in the correlation integral cycle of Q passage advanced branch road, I _pfor output valve in the correlation integral cycle of I passage instant branch road, Q _pfor output valve in the correlation integral cycle of Q passage instant branch road, I _lfor output valve in the correlation integral cycle of I passage delayed branch road, Q _lfor output valve in the correlation integral cycle of Q passage delayed branch road; v ₁for code Discr. noise, v ₂for carrier wave Discr. noise.

Reference information in the pseudo-code phase estimated according to prefilter and the tracking error information such as carrier frequency and carrier wave, pseudo-code NCO calculates the pseudorange ρ that GNSS tracking channel exports _g, pseudorange rates as measurement information, be input in integrated navigation wave filter:

${\mathrm{\ρ}}_G=(C_0+\mathrm{\δ}\mathrm{\τ})\·\frac{c}{f_s}---\left(15\right)$

${\stackrel{\·}{\mathrm{\ρ}}}_G=-(f_0+\mathrm{\δ}f)\·\frac{c}{f_{L_1}}---\left(16\right)$

In formula, C ₀, f ₀be respectively the reference value of pseudo-code phase in local signal generator and carrier frequency, c is the light velocity, f _sfor sample frequency, the frequency of carrier wave L1.

Step 2, set up integrated navigation senior filter model: pseudorange, pseudorange rates that integrated navigation senior filter exports according to GNSS tracking channel and SINS system, process obtains pseudorange biases and pseudorange rates deviation, and integrated navigation wave filter receives the pseudorange ρ that GNSS tracking channel exports _g, pseudorange rates the pseudorange ρ exported with SINS _i, pseudorange rates make poor pseudorange biases δ ρ and pseudorange rate variance inclined by pseudorange biases δ ρ and pseudorange rates deviation as measurement amount, navigation error state variable is upgraded, and navigation error parameter feedback is returned in SINS system SINS navigational parameter is corrected;

Build senior filter error model, specific as follows:

(a) system state equation:

$\stackrel{\·}{X}=FX+GW---\left(17\right)$

In formula, X is system state vector, represent the derivative of system state vector, F is systematic state transfer matrix, and G is that system noise drives matrix, and W is system noise vector, specific as follows:

$X={\left[\begin{array}{ccccccccccccccccc}{\mathrm{\ψ}}_e& {\mathrm{\ψ}}_n& {\mathrm{\ψ}}_u& {\mathrm{\δV}}_e& {\mathrm{\δV}}_n& {\mathrm{\δV}}_n& \mathrm{\δ}L& \mathrm{\δ}\mathrm{\λ}& \mathrm{\δ}h& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\▿}_x& {\▿}_y& {\▿}_z& {\mathrm{\δt}}_u& {\mathrm{\δt}}_{ru}\end{array}\right]}^T$

$F_{17\×17}=\left[\begin{array}{ccc}{F}_N& F_S& 0_{9\×2}\\ 0_{6\×9}& 0_{6\×6}& 0_{6\×2}\\ 0_{2\×9}& 0_{2\×6}& F_G\end{array}\right],F_S=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{3\×3}& 0_{3\×3}\end{array}\right],F_G=\left[\begin{array}{cc}0& 1\\ 0& -{\mathrm{\β\δt}}_{ru}\end{array}\right],$

$G=\left[\begin{array}{ccc}{C}_b^n& 0_{3\×3}& 0_{3\×2}\\ 0_{3\×3}& C_b^n& 0_{3\×2}\\ 0_{9\×3}& 0_{9\×3}& 0_{9\×2}\\ 0_{2\×3}& 0_{2\×3}& I_{2\×2}\end{array}\right],W={\left[\begin{array}{cccccccc}{W}_{{\mathrm{\ϵ}}_x}& W_{{\mathrm{\ϵ}}_y}& W_{{\mathrm{\ϵ}}_z}& W_{{\▿}_x}& W_{{\▿}_y}& W_{{\▿}_z}& W_{{\mathrm{\δt}}_u}& W_{{\mathrm{\δt}}_u}\end{array}\right]}^T$

In formula, ψ _e, ψ _n, ψ _ube respectively the misaligned angle of the platform error in east, north, direction, sky; δ V _e, δ V _n, δ V _ube respectively the velocity error in east, north, direction, sky; δ L, δ λ, δ h are respectively latitude, longitude and height error; ε _x, ε _y, ε _zbe respectively the component of gyroscope constant value drift on x, y, z axle; be respectively the component of accelerometer bias on x, y, z axle; δ t _ufor the equivalent distances error that clock correction causes; δ t _rufor clock floats the distance rate error caused; F _nfor the system battle array of corresponding 9 basic navigation parameters; for attitude matrix; τ is correlation time; be respectively the measurement zero mean Gaussian white noise of gyro in x, y, z three axis; be respectively the measurement zero mean Gaussian white noise of accelerometer in x, y, z three axis; be respectively clock correction and clock drift zero mean Gaussian white noise;

(b) system measurements equation:

Z＝HX+V (18)

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

$Z={\left[\begin{array}{cccccccc}{\mathrm{\δ\ρ}}^1& {\mathrm{\δ\ρ}}^2& \mathrm{...}& {\mathrm{\δ\ρ}}^n& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^1& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^2& \mathrm{...}& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^n\end{array}\right]}^T$

In formula, δ ρ is pseudorange biases, for pseudorange rates deviation;

$H=\left[\begin{array}{cccc}{0}_{n\×6}& H_{\mathrm{\ρ}1}& 0_{n\×6}& H_{\mathrm{\ρ}2}\\ 0_{n\×3}& H_{\stackrel{\·}{\mathrm{\ρ}}1}& 0_{n\×9}& H_{\stackrel{\·}{\mathrm{\ρ}}2}\end{array}\right],H_{\mathrm{\ρ}1}=E\·D_{LLA}^{XYZ}={\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ .& .& .\\ .& .& .\\ .& .& .\\ a_{n1}& a_{n2}& a_{n3}\end{array}\right]}_{n\×3},H_{\mathrm{\ρ}2}={\left[\begin{array}{cc}1& 0\\ .& .\\ .& .\\ .& .\\ 1& 0\end{array}\right]}_{{}_{n\×2}},$

In above formula, E is nautical star Direct cosine matrix, for Department of Geography's upper/lower positions error is to the transformational relation matrix of ECEF system:

$E={\[e_{ij}\]}_{n\×3}=\left[\begin{array}{ccc}{e}_{11}& e_{12}& e_{13}\\ e_{21}& e_{22}& e_{23}\\ .& .& .\\ .& .& .\\ .& .& .\\ e_{n1}& e_{n2}& e_{n3}\end{array}\right](i=1,\mathrm{2...}n,j=1,2,3)$

$D_{LLA}^{XYZ}=\left[\begin{array}{ccc}-(R_n+h)\sin L\cos \mathrm{\λ}& -(R_n+h)\cos L\sin \mathrm{\λ}& \cos L\cos \mathrm{\λ}\\ -(R_n+h)\sin L\sin \mathrm{\λ}& (R_n+h)\cos L\cos \mathrm{\λ}& \cos L\sin \mathrm{\λ}\\ \[R_n(1-e^2)+h\]\cos L& 0& \sin L\end{array}\right]$

In formula, e _ijfor SINS resolves the direction cosine of position to i-th nautical star, L, λ, h are respectively the true latitude of carrier, longitude and height, datum ellipsoid body major radius: R _e=6378137.0m, datum ellipsoid body pole ellipticity: f=1/298.257223563, radius of curvature in prime vertical: R _n=R _e(1+fsin ²l), datum ellipsoid excentricity: $e=\sqrt{f(2-f)};$

$H_{\stackrel{\·}{\mathrm{\ρ}}1}={\left[\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ .& .& .\\ .& .& .\\ .& .& .\\ b_{n1}& b_{n2}& b_{n3}\end{array}\right]}_{n\×3},H_{\stackrel{\·}{\mathrm{\ρ}}2}={\left[\begin{array}{cc}0& 1\\ .& .\\ .& .\\ .& .\\ 0& 1\end{array}\right]}_{n\×2},$

In above formula, b _ij(i=1,2...n, j=1,2,3) are specifically unfolded as follows:

$\left\{\begin{array}{c}{b}_{i1}=-e_{i1}sin\mathrm{\λ}+e_{i2}cos\mathrm{\λ}\\ b_{i2}=-e_{i1}\sin Lcos\mathrm{\λ}-e_{i2}simLsin\mathrm{\λ}+e_{i3}\cos L\\ b_{i3}=e_{i1}\cos Lcos\mathrm{\λ}+e_{i2}\cos L\sin \mathrm{\λ}+e_{i3}\sin L\end{array}\right.$

$V={\left[\begin{array}{cccccccc}{v}_{\mathrm{\ρ}}^1& v_{\mathrm{\ρ}}^2& \mathrm{...}& v_{\mathrm{\ρ}}^n& v_{\stackrel{\·}{\mathrm{\ρ}}}^1& v_{\stackrel{\·}{\mathrm{\ρ}}}^2& \mathrm{...}& v_{\stackrel{\·}{\mathrm{\ρ}}}^n\end{array}\right]}^T$

In formula, v _ρfor pseudorange observation white Gaussian noise, for pseudorange rates observation white Gaussian noise.

Step 3, inertia additional feedback control GNSS forms closed loop: integrated navigation system infers the signal trace parameter of GNSS according to the SINS navigational parameter after correction and ephemeris information, in order to control local pseudo-code, the carrier number controlled oscillator of receiver, to keep the tracking to input signal.

Carrier wave pseudorange carrier wave pseudorange rates with code pseudorange computing formula as follows:

$\left\{\begin{array}{c}{\widehat{\mathrm{\ρ}}}_{carrier}=e^T\left({\stackrel{\→}{R}}_G\right(t_1)-{\stackrel{\→}{R}}_I\left(t_2\right))+c({\mathrm{\Δt}}_I-{\mathrm{\Δt}}_G)-\mathrm{\δ}d\\ {\widehat{\stackrel{\·}{\mathrm{\ρ}}}}_{carrier}=e^T\left(\stackrel{\→}{V_G}\right(t_1)-\stackrel{\→}{V_I}\left(t_2\right))+c(\mathrm{\Δ}{\stackrel{\·}{t}}_I-\mathrm{\Δ}{\stackrel{\·}{t}}_G)-\mathrm{\δ}\stackrel{\·}{d}\\ {\widehat{\mathrm{\ρ}}}_{code}=e^T\left({\stackrel{\→}{R}}_G\right(t_1)-{\stackrel{\→}{R}}_I\left(t_2\right))+c({\mathrm{\Δt}}_I-{\mathrm{\Δt}}_G)+\mathrm{\δ}d\end{array}\right.---\left(19\right)$

In formula, for the unit line of sight between satellite to receiver, for satellite position vectors, for the position vector that SINS exports, c is the light velocity, Δ t _ifor receiver clock-offsets, Δ t _gfor satellite clock correction, for the drift of receiver clock, for satellite clock drift, δ d is ionosphere time delay, for ionosphere time delay rate, t ₁for satellite-signal launch time, t ₂for corresponding to satellite-signal t launch time ₁receiver clock;

Utilize above formula to calculate the NCO controlled quentity controlled variable of local pseudo-code and carrier wave, comprise local carrier-phase and estimate φ _carrier, estimating carrier frequencies f _carrierwith phase estimator τ _code, computing formula is respectively:

$\left\{\begin{array}{c}{\mathrm{\φ}}_{carrier}={\mathrm{\φ}}_{SV}-\mathrm{mod}(\frac{2\mathrm{\π}{\widehat{\mathrm{\ρ}}}_{carrier}}{{\mathrm{\λ}}_{carrier}},2\mathrm{\π})\\ f_{carrier}=f_{IF}-\frac{2\mathrm{\π}{\widehat{\stackrel{\·}{\mathrm{\ρ}}}}_{carrier}}{{\mathrm{\λ}}_{carrier}}\\ {\mathrm{\τ}}_{code}={\mathrm{\τ}}_{SV}-\mathrm{mod}(\frac{{\widehat{\mathrm{\ρ}}}_{code}}{{\mathrm{\λ}}_{code}},L_{code})\end{array}\right.---\left(20\right)$

In formula, φ _sVand τ _sVbe respectively carrier phase and the code phase of previous moment, λ _carrierfor carrier wavelength, f _iFfor digital intermediate frequency, λ _codefor code length, L _codefor the number of chips of one-period, mod is remainder operation.

Below in conjunction with specific embodiment, the present invention is described in further detail.

Embodiment 1

In order to be described algorithm of the present invention, fully show this algorithm have improve GNSS signal follow the tracks of performance and satellite receiver to the adaptability of high dynamic motion carrier, complete high dynamic experiment as follows:

(1) starting condition and optimum configurations is tested

Adopt satellite navigation signal simulator simulation satellite signal; Gather digital medium-frequency signal by intermediate-freuqncy signal collector, sampling rate is 16.369MHz, and intermediate frequency is 3.996MHz; GNSS software receiver adopts scalar to follow the tracks of respectively and vector tracking two kinds of patterns carry out carrier track, and the loop integral time is 1ms.

Simulating high Dynamic Ballistic track arranges as follows: starting point: north latitude 38.7580 °, east longitude 105.6100 °, elevation 1431.90m; Terminal: north latitude 38.8370 °, east longitude 105.6195 °, elevation 11371.9911m.Rest 20s, then flies to terminal from starting point, and flight duration is 20s, and simulated time is 40s altogether, and maximal rate 1000m/s in flight course, peak acceleration 20g, maximum acceleration 40g/s, carrier movement track as shown in Figure 3.

(2) interpretation

For all satellites that follow-up experiment arrives, for No. 1 satellite, com-parison and analysis scalar is followed the tracks of and code phase, carrier phase and the carrier frequency tracking error under vector tracking two kinds of patterns.Figure 4 shows that code phase tracking error, as seen from the figure, code phase error 3 σ that scalar is followed the tracks of is about 0.2 chip, and code phase error 3 σ of vector tracking is about 0.1 chip, and code phase tracking error reduces; Figure 5 shows that Carrier phase tracking error, as seen from the figure, carrier phase error 3 σ that scalar is followed the tracks of is about 0.2rad, and carrier phase error 3 σ of vector tracking is about 0.1rad, and Carrier phase tracking error reduces; Figure 6 shows that carrier frequency tracking error, as seen from the figure, vector tracking reduces about 5Hz than the carrier frequency error that scalar is followed the tracks of, and precision significantly improves.According to Fig. 7, the GNSS/SINS deep integrated navigation system based on vector tracking dynamically can stablize navigator fix down at height, and combined horizontal site error is less than 5m, and vertical error is less than 10m.

By the comparison to tracking error, relative to traditional scalar track algorithm, vector tracking algorithm based on prefilter in this paper can export less track loop error, improve the tracking accuracy of receiver, reliability and the navigation accuracy of combined system not only effectively can be improved under high current intelligence, and good pseudo-code phase and carrier frequency tracking performance can be maintained in the environment that carrier-to-noise ratio is lower, have broad application prospects.

Claims (4)

1., based on a GNSS/SINS deep integrated navigation method for vector tracking, it is characterized in that, comprise the following steps:

Step 1, set up the vector tracking model based on prefilter: the homophase that correlator exports, orthogonal signal are after phase demodulation function calculates, gained identified result sets up prefilter model as the measurement information of prefilter, adopt this prefilter model to estimate tracking error information, obtain GNSS tracking channel pseudorange, pseudorange rates according to tracking error information;

Step 2, set up integrated navigation senior filter model: pseudorange, pseudorange rates that integrated navigation senior filter exports according to GNSS tracking channel and SINS system, process obtains pseudorange biases and pseudorange rates deviation, pseudorange biases and pseudorange rates deviation are upgraded navigation error state variable as measurement amount, and navigation error parameter feedback is returned in SINS system SINS navigational parameter is corrected;

Step 3, inertia additional feedback control GNSS forms closed loop: integrated navigation system infers the signal trace parameter of GNSS according to the SINS navigational parameter after correction and ephemeris information, in order to control local pseudo-code, the carrier number controlled oscillator of receiver, to keep the tracking to input signal.

2. the GNSS/SINS deep integrated navigation method based on vector tracking according to claim 1, is characterized in that, set up the vector tracking model based on prefilter described in step 1, specific as follows:

A the state equation of () prefilter model is as follows:

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\mathrm{\τ}}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\φ}}\\ \mathrm{\δ}\stackrel{\·}{f}\\ \mathrm{\δ}\stackrel{\·\·}{f}\end{array}\right]=\left[\begin{array}{cccc}0& 0& \mathrm{\α}& 0\\ 0& 0& 2\mathrm{\π}& 0\\ 0& 0& 0& 2\mathrm{\π}\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}\mathrm{\τ}\\ \mathrm{\δ}\mathrm{\φ}\\ \mathrm{\δ}f\\ \mathrm{\δ}\stackrel{\·}{f}\end{array}\right]+\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{w}_{\mathrm{\τ}}\\ w_{\mathrm{\φ}}\\ w_f\\ w_{\stackrel{\·}{f}}\end{array}\right]$

In formula, δ τ is code phase error, and δ φ is carrier phase error, and δ f is carrier frequency error, for carrier frequency variation rate tracking error; α is the Conversion of measurement unit value of carrier phase and code phase, α=λ _carr/ λ _cA, λ _carrfor the wavelength of carrier wave L1, λ _cAfor C/A code chip lengths; w _τfor code phase error noise, w _φfor carrier phase error noise, w _ffor carrier frequency error noise, for carrier frequency variation rate error noise;

B the observation equation of () prefilter model is:

$\left[\begin{array}{c}\frac{\sqrt{I_E^2+Q_E^2}-\sqrt{I_L^2+Q_L^2}}{\sqrt{I_E^2+Q_E^2}+\sqrt{I_L^2+Q_L^2}}\\ ATAN(Q_P/I_P)\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}\mathrm{\τ}\\ \mathrm{\δ}\mathrm{\φ}\\ \mathrm{\δ}f\\ \mathrm{\δ}\stackrel{\·}{f}\end{array}\right]+\left[\begin{array}{c}{v}_1\\ v_2\end{array}\right]$

In formula, I _efor output valve in the correlation integral cycle of I passage advanced branch road, Q _efor output valve in the correlation integral cycle of Q passage advanced branch road, I _pfor output valve in the correlation integral cycle of I passage instant branch road, Q _pfor output valve in the correlation integral cycle of Q passage instant branch road, I _lfor output valve in the correlation integral cycle of I passage delayed branch road, Q _lfor output valve in the correlation integral cycle of Q passage delayed branch road; v ₁for code Discr. noise, v ₂for carrier wave Discr. noise.

3. the GNSS/SINS deep integrated navigation method based on vector tracking according to claim 1, is characterized in that, set up integrated navigation senior filter model described in step 2, specific as follows:

(a) system state equation:

$\stackrel{\·}{X}=FX+GW$

In formula, X is system state vector, represent the derivative of system state vector, F is systematic state transfer matrix, and G is that system noise drives matrix, and W is system noise vector, specific as follows:

$X={\left[\begin{array}{ccccccccccccccccc}{\mathrm{\ψ}}_e& {\mathrm{\ψ}}_n& {\mathrm{\ψ}}_u& {\mathrm{\δV}}_e& {\mathrm{\δV}}_n& {\mathrm{\δV}}_u& \mathrm{\δ}L& \mathrm{\δ}\mathrm{\λ}& \mathrm{\δ}h& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\▿}_x& {\▿}_y& {\▿}_z& {\mathrm{\δt}}_u& {\mathrm{\δt}}_{ru}\end{array}\right]}^T$

$F_{17\×17}=\left[\begin{array}{ccc}{F}_N& F_S& 0_{9\×2}\\ 0_{6\×9}& 0_{6\×6}& 0_{6\×2}\\ 0_{2\×9}& 0_{2\×6}& F_G\end{array}\right],$ $F_S=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{3\×3}& 0_{3\×3}\end{array}\right],$ $F_G=\left[\begin{array}{cc}0& 1\\ 0& -{\mathrm{\β}}_{{\mathrm{\δt}}_{ru}}\end{array}\right],$

$G=\left[\begin{array}{ccc}{C}_b^n& 0_{3\×3}& 0_{3\×2}\\ 0_{3\×3}& C_b^n& 0_{3\×2}\\ 0_{9\×3}& 0_{9\×3}& 0_{9\×2}\\ 0_{2\×3}& 0_{2\×3}& I_{2\×2}\end{array}\right],$ $W={\left[\begin{array}{cccccccc}{W}_{{\mathrm{\ϵ}}_x}& W_{{\mathrm{\ϵ}}_y}& W_{{\mathrm{\ϵ}}_z}& W_{{\▿}_x}& W_{{\▿}_y}& W_{{\▿}_z}& W_{{\mathrm{\δt}}_u}& W_{{\mathrm{\δt}}_{ru}}\end{array}\right]}^T$

In formula, ψ _e, ψ _n, ψ _ube respectively the misaligned angle of the platform error in east, north, direction, sky; δ V _e, δ V _n, δ V _ube respectively the velocity error in east, north, direction, sky; δ L, δ λ, δ h are respectively latitude, longitude and height error; ε _x, ε _y, ε _zbe respectively the component of gyroscope constant value drift on x, y, z axle; be respectively the component of accelerometer bias on x, y, z axle; δ t _ufor the equivalent distances error that clock correction causes; δ t _rufor clock floats the distance rate error caused; F _nfor the system battle array of corresponding 9 basic navigation parameters; for attitude matrix; τ is correlation time; be respectively the measurement zero mean Gaussian white noise of gyro in x, y, z three axis; be respectively the measurement zero mean Gaussian white noise of accelerometer in x, y, z three axis; be respectively clock correction and clock drift zero mean Gaussian white noise;

(b) system measurements equation:

Z＝HX+V

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

$Z={\left[\begin{array}{cccccccc}{\mathrm{\δ\ρ}}^1& {\mathrm{\δ\ρ}}^2& ...& {\mathrm{\δ\ρ}}^n& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^1& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^2& ...& \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^n\end{array}\right]}^T$

In formula, δ ρ is pseudorange biases, for pseudorange rates deviation;

$H=\left[\begin{array}{cccc}{0}_{n\×6}& H_{\mathrm{\ρ}1}& 0_{n\×6}& H_{\mathrm{\ρ}2}\\ 0_{n\×3}& H_{\stackrel{\·}{\mathrm{\ρ}}1}& 0_{n\×9}& H_{\stackrel{\·}{\mathrm{\ρ}}2}\end{array}\right],$ $H_{\mathrm{\ρ}1}=E\·D_{LLA}^{\mathrm{XYZ}}={\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ .& .& .\\ .& .& .\\ .& .& .\\ a_{n1}& a_{n2}& a_{n3}\end{array}\right]}_{n\×3},$ $H_{\mathrm{\ρ}2}={\left[\begin{array}{cc}1& 0\\ .& .\\ .& .\\ .& .\\ 1& 0\end{array}\right]}_{n\×2},$

In above formula, E is nautical star Direct cosine matrix, for Department of Geography's upper/lower positions error is to the transformational relation matrix of ECEF system:

$E={\[e_{ij}\]}_{n\×3}=\left[\begin{array}{ccc}{e}_{11}& e_{12}& e_{13}\\ e_{21}& e_{22}& e_{23}\\ .& .& .\\ .& .& .\\ .& .& .\\ e_{n1}& e_{n2}& e_{n3}\end{array}\right],(i=1,2...n,j=1,2,3)$

$D_{LLA}^{XYZ}=\left[\begin{array}{ccc}-(R_n+h)\sin L\cos \mathrm{\λ}& -(R_n+h)\cos L\sin \mathrm{\λ}& \cos L\cos \mathrm{\λ}\\ -(R_n+h)\sin L\sin \mathrm{\λ}& (R_n+h)\cos L\cos \mathrm{\λ}& \cos L\sin \mathrm{\λ}\\ \[\left(R_n\right(1-e^2)+h)\]\cos L& 0& \sin L\end{array}\right]$

In formula, e _ijfor SINS resolves the direction cosine of position to i-th nautical star, L, λ, h are respectively the true latitude of carrier, longitude and height, datum ellipsoid body major radius: R _e=6378137.0m, datum ellipsoid body pole ellipticity: f=1/298.257223563, radius of curvature in prime vertical: R _n=R _e(1+fsin ²l), datum ellipsoid excentricity: $e=\sqrt{f(2-f)};$

$H_{\stackrel{\·}{\mathrm{\ρ}}1}={\left[\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ .& .& .\\ .& .& .\\ .& .& .\\ b_{n1}& b_{n2}& b_{n3}\end{array}\right]}_{n\×3},$ $H_{\stackrel{\·}{\mathrm{\ρ}}2}={\left[\begin{array}{cc}0& 1\\ .& .\\ .& .\\ .& .\\ 0& 1\end{array}\right]}_{n\×2},$

In above formula, b _ij(i=1,2...n, j=1,2,3) are specifically unfolded as follows:

$\left\{\begin{array}{c}{b}_{i1}=-e_{i1}\sin \mathrm{\λ}+e_{i2}\cos \mathrm{\λ}\\ b_{i2}=-e_{i1}\sin L\cos \mathrm{\λ}-e_{i2}\sin L\sin \mathrm{\λ}+e_{i3}\cos L\\ b_{i3}=e_{i1}\cos L\cos \mathrm{\λ}+e_{i2}\cos L\sin \mathrm{\λ}+e_{i3}\sin L\end{array}\right.$

$V={\left[\begin{array}{cccccccc}{v}_{\mathrm{\ρ}}^1& v_{\mathrm{\ρ}}^2& ...& {\mathrm{\ν}}_{\mathrm{\ρ}}^n& v_{\stackrel{\·}{\mathrm{\ρ}}}^1& v_{\stackrel{\·}{\mathrm{\ρ}}}^2& ...& v_{\stackrel{\·}{\mathrm{\ρ}}}^n\end{array}\right]}^T$

In formula, v _ρfor pseudorange observation white Gaussian noise, for pseudorange rates observation white Gaussian noise.

4. the GNSS/SINS deep integrated navigation method based on vector tracking according to claim 1, is characterized in that, the additional feedback of inertia described in step 3 control GNSS forms closed loop, specific as follows:

Carrier wave pseudorange carrier wave pseudorange rates with code pseudorange computing formula as follows:

$\left\{\begin{array}{c}{\widehat{\mathrm{\ρ}}}_{carrier}=e^T\left({\stackrel{\→}{R}}_G\right(t_1)-{\stackrel{\→}{R}}_1(t_2\left)\right)+c(\mathrm{\Δ}{t}_I-\mathrm{\Δ}{t}_G)-\mathrm{\δ}d\\ {\stackrel{\widehat{\·}}{\mathrm{\ρ}}}_{carrier}=e^T\left({\stackrel{\→}{V}}_G\right(t_1)-{\stackrel{\→}{V}}_1(t_2\left)\right)+c(\mathrm{\Δ}{\stackrel{\·}{t}}_I-\mathrm{\Δ}{\stackrel{\·}{t}}_G)-\mathrm{\δ}\stackrel{\·}{d}\\ {\widehat{\mathrm{\ρ}}}_{code}=e^T\left({\stackrel{\→}{R}}_G\right(t_1)-{\stackrel{\→}{R}}_1(t_2\left)\right)+c(\mathrm{\Δ}{t}_I-\mathrm{\Δ}{t}_G)+\mathrm{\δ}d\end{array}\right.$

In formula, for the unit line of sight between satellite to receiver, for satellite position vectors, for the position vector that SINS exports, c is the light velocity, Δ t _ifor receiver clock-offsets, Δ t _gfor satellite clock correction, for the drift of receiver clock, for satellite clock drift, δ d is ionosphere time delay, for ionosphere time delay rate, t ₁for satellite-signal launch time, t ₂for corresponding to satellite-signal t launch time ₁receiver clock;

Utilize above formula to calculate the NCO controlled quentity controlled variable of local pseudo-code and carrier wave, comprise local carrier-phase and estimate φ _carrier, estimating carrier frequencies f _carrierwith phase estimator τ _code, computing formula is respectively:

$\left\{\begin{array}{c}{\mathrm{\φ}}_{carrier}={\mathrm{\φ}}_{SV}-\mathrm{mod}(\frac{2\mathrm{\π}{\widehat{\mathrm{\ρ}}}_{carrier}}{{\mathrm{\λ}}_{carrier}},2\mathrm{\π})\\ f_{carrier}=f_{IF}-\frac{2\mathrm{\π}{\stackrel{\widehat{\·}}{\mathrm{\ρ}}}_{carrier}}{{\mathrm{\λ}}_{carrier}}\\ {\mathrm{\τ}}_{code}={\mathrm{\τ}}_{SV}-\mathrm{mod}(\frac{{\widehat{\mathrm{\ρ}}}_{code}}{{\mathrm{\λ}}_{code}},L_{code})\end{array}\right.$

In formula, φ _sVand τ _sVbe respectively carrier phase and the code phase of previous moment, λ _carrierfor carrier wavelength, f _iFfor digital intermediate frequency, λ _codefor code length, L _codefor the number of chips of one-period, mod is remainder operation.