CN103969672B - A kind of multi-satellite system and strapdown inertial navigation system tight integration air navigation aid - Google Patents

Abstract

The invention discloses a kind of multi-satellite system and strapdown inertial navigation system tight integration air navigation aid, first IMU output data strapdown is resolved and obtain SINS current pose, speed, positional information.Satellite ephemeris message is carried out ephemeris resolving and obtains each satellite position, speed, each satellite system space-time uniformity, carry out satellite and select star.Secondly, SINS position, velocity information that satellite position, velocity information and the strapdown resolving that utilization is chosen obtains are calculated SINS pseudorange, pseudorange rates.Again, utilize the raw measurement data that GNSS exports, i.e. survey code pseudorange and Doppler frequency shift, compare with SINS pseudorange, pseudorange rates, using difference as the observed quantity of wave filter, the optimal estimation of device after filtering, provide the compensation dosage of correction SINS, closed-loop corrected, obtain SINS attitude, speed, position optimal solution.The present invention solves the very few problem affecting tight integration navigation accuracy of number of satellite under the complex environments such as inertial navigation is blocked at high building, trees are covered with single combinations of satellites navigation.

Description

Tightly-combined navigation method of multi-satellite system and strapdown inertial navigation system

Technical Field

The invention belongs to the technical field of navigation, and particularly relates to a close combination navigation method of a multi-satellite system and a strapdown inertial navigation system, which is particularly suitable for combination resolving under the condition of too few satellites in complex environments such as high-rise shelter, tree covering and the like.

Background

The inertial navigation is based on the Newton's classic motion mechanics law, and is an autonomous reckoning navigation method which utilizes inertial measurement elements to measure angular motion and linear motion parameters of a carrier and determines the attitude, the speed and the position of a carrier by recursion on the basis of an initial state. At present, the inertial navigation technology is developed more mature, but the requirement cannot be completely met during high-precision navigation such as remote navigation, long-time navigation, weapon launching and the like, and the main defect is that navigation positioning errors are accumulated along with time. The navigation error is mainly determined by the initial alignment precision, the error of the inertial sensor and the dynamic characteristic of the motion track of the carrier, and the error can be quickly dispersed after long-time independent work. The emergence of the integrated navigation technology provides an effective way for solving the problem, the satellite navigation can measure the speed and the position of the carrier in real time, and the integrated navigation technology is combined with an inertial navigation system, so that the precision of the navigation system can be further improved through the information fusion technology. Currently, the integrated navigation technology has become one of the important directions for the development of the navigation technology. However, under the condition that the number of satellites is too small in complex environments such as high-rise shielding, number covering and the like, the integrated navigation resolving precision of a single satellite and the strapdown inertial navigation system is reduced.

Disclosure of Invention

The invention provides a method for tightly combining a multi-satellite system and an inertial navigation system for navigation, aiming at the problem that the tight combination navigation precision is influenced by too few satellites in complex environments such as high-rise sheltering, tree covering and the like of inertial navigation and single-satellite combined navigation.

A multi-satellite system and strapdown inertial navigation system tightly combined navigation method comprises the following steps:

(1) obtaining the current attitude, speed and position of strapdown inertial navigation by strapdown resolving the data output by an Inertial Measurement Unit (IMU), and carrying out satellite data processing on ephemeris messages output by a satellite system receiver to obtain the speed and position information of a satellite;

(2) calculating SINS pseudo range and pseudo range rate by using satellite speed and position and SINS position and speed information;

(3) obtaining a code measurement pseudo range and Doppler frequency shift from a pseudo range message output by a satellite system receiver, comparing the code measurement pseudo range and the Doppler frequency shift with an SINS pseudo range and a pseudo range rate, establishing a tight combination state model and a measurement model by taking a difference value as an observed quantity of a filter, and outputting an error correction value of the system through optimal estimation and filtering; and correcting the system error, and outputting the optimal combined solution of the SINS attitude, speed and position.

The strapdown calculation algorithm is to utilize the IMU to measure angular motion and linear motion parameters of a carrier, and carry out recursion on the basis of an initial state to determine autonomous inferred navigation of the attitude, the speed and the position of a carrier provided with the SINS.

The satellite data processing comprises satellite selection, satellite ephemeris resolving and satellite system data fusion.

The calculation process of the SINS pseudo range and the pseudo range rate is to calculate the pseudo range corresponding to the position of the SINS according to the position of the SINS and the position of a satellite; the change rate of the pseudo range of the SINS relative to the satellite relative motion, namely the SINS, is obtained according to the velocity of the SINS and the velocity of the satellite.

The optimal estimation and filtering adopt a Kalman filter.

The method for establishing the tight combination state model and the measurement model comprises the following steps:

(1) multi-system tightly-combined state equation establishment

Converting strapdown error equation and satellite system modeling into state equation description

$\left[\begin{array}{c}{\stackrel{\·}{X}}_I\left(t\right)\\ {\stackrel{\·}{X}}_{\mathrm{GPS}}\left(t\right)\\ {\stackrel{\·}{X}}_{\mathrm{GLO}}\left(t\right)\\ {\stackrel{\·}{X}}_{\mathrm{Gal}}\left(t\right)\\ {\stackrel{\·}{X}}_{\mathrm{BD}}\left(t\right)\end{array}\right]=\left[\begin{array}{ccccc}{F}_I\left(t\right)& 0& 0& 0& 0\\ 0& F_{\mathrm{GPS}}\left(t\right)& 0& 0& 0\\ 0& 0& F_{\mathrm{GLO}}\left(t\right)& 0& 0\\ 0& 0& 0& F_{\mathrm{Gal}}\left(t\right)& 0\\ 0& 0& 0& 0& F_{\mathrm{BD}}\left(t\right)\end{array}\right]\left[\begin{array}{c}{X}_I\left(t\right)\\ X_{\mathrm{GPS}}\left(t\right)\\ X_{\mathrm{GLO}}\left(t\right)\\ X_{\mathrm{Gal}}\left(t\right)\\ X_{\mathrm{BD}}\left(t\right)\end{array}\right]+\left[\begin{array}{ccccc}{G}_I\left(t\right)& 0& 0& 0& 0\\ 0& G_{\mathrm{GPS}}\left(t\right)& 0& 0& 0\\ 0& 0& G_{\mathrm{GLO}}\left(t\right)& 0& 0\\ 0& 0& 0& G_{\mathrm{Gal}}\left(t\right)& 0\\ 0& 0& 0& 0& F_{\mathrm{BD}}\left(t\right)\end{array}\right]\left[\begin{array}{c}{W}_I\left(t\right)\\ W_{\mathrm{GPS}}\left(t\right)\\ W_{\mathrm{GLO}}\left(t\right)\\ W_{\mathrm{Gal}}\left(t\right)\\ W_{\mathrm{BD}}\left(t\right)\end{array}\right]$

Wherein, X ∈ R²³The state variables are composed of 15 state variables of an inertial navigation system (SINS), 2 state variables of a United states Global Positioning System (GPS), a Glonass satellite system (GLONASS), a Galileo satellite system (Galileo) and a Beidou satellite system (BD) respectively;

$\begin{array}{ccccccccccccccccccccccc}X=& & & & & & & & & & & & & & & & & & & & & & \\ [{\mathrm{\δV}}_E& {\mathrm{\δV}}_N& {\mathrm{\δV}}_U& {\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\▿}_{\mathrm{bx}}& {\▿}_{\mathrm{by}}& {\▿}_{\mathrm{bz}}& {\mathrm{\ϵ}}_{\mathrm{bx}}& {\mathrm{\ϵ}}_{\mathrm{by}}& {\mathrm{\ϵ}}_{\mathrm{bz}}& {\mathrm{\δt}}_{\mathrm{GPS}}& {\mathrm{\δt}}_{\mathrm{GLO}}& {\mathrm{\δt}}_{\mathrm{Gal}}& {\mathrm{\δt}}_{\mathrm{BD}}& {\mathrm{\δt}}_{\mathrm{rGPS}}& {\mathrm{\δt}}_{\mathrm{rGLO}}& {\mathrm{\δt}}_{\mathrm{rGal}}& {\mathrm{\δt}}_{\mathrm{rBD}}]\end{array}$

in the formula, V_E,V_N,V_UAre the speed errors in the three directions on the northeast,three misalignment angles (platform attitude angles) of the strapdown, L, lambda and h are three position errors of the strapdown and are described by a terrestrial coordinate system,the offset errors in the three axial directions are added,_bx,_by,_bzis the three axial drifts of the gyro, t_GPS、t_GLO、t_Gal、t_BDThe equivalent distances of the clock errors of the GPS, GLONASS, Galileo and BD satellites, t_rGPS、t_rGLO、t_rGal、t_rBDThe satellite clock frequency error equivalent distance change rates of the GPS, the GLONASS, the Galileo and the BD are 23-dimensional in total;

F_I(t) is the state matrix of the inertial navigation system error equation of state, and F_GPS(t)、F_GLO(t)、F_Gal(t)、F_BD(t) is a state matrix obtained by modeling two state quantities related to GPS, GLONASS, Galileo and BD satellites respectively, G_I(t) and G_GPS(t)、G_GLO(t)、G_Gal(t)、G_BD(t) is the input matrix for the noise. t is t_*Representing its excitation as white noise after modeling, t_r*Modeling as a first order Markov process, representing the respective System (GPS, GLONASS, Galileo, BD)

$F_{*}=\left[\begin{array}{cc}0& 1\\ 0& -{\mathrm{\β}}_{\mathrm{tr}*}\end{array}\right],G_{*}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],W_{*}\left(t\right)=\left[\begin{array}{c}{\mathrm{\ω}}_{t*}\\ {\mathrm{\ω}}_{\mathrm{tr}*}\end{array}\right];$

(2) Multi-system tight combination measurement equation establishment

(a) Pseudo-range measurement equation establishment

t_*Is a station clock error, which represents the equivalent distance error of the station clock error, v_ρ*jRepresenting other common errors and unique errors, such as ionosphere errors, troposphere errors, multipath errors and other measurement errors; in tight composite simulation, v_ρ*jConsidering as an error in the form of white noise, the clock error is uniformly set by a plurality of channels; in the tight combination prototype experiment, many errors such as ionospheric delay and the like must be compensated, and the compensation method is discussed below;

taking satellites and writing them in matrix form

$\left[\begin{array}{c}{\mathrm{\δ\ρ}}_{\mathrm{GPS}}\\ {\mathrm{\δ\ρ}}_{\mathrm{GLO}}\\ {\mathrm{\δ\ρ}}_{\mathrm{Gal}}\\ \mathrm{\δ}{\mathrm{\ρ}}_{\mathrm{BD}}\end{array}\right]=\left[\begin{array}{ccccc}{H}_{\mathrm{GPS}}& 1_{\mathrm{GPS}}& 0& 0& 0\\ H_{\mathrm{GLO}}& 0& 1_{\mathrm{GLO}}& 0& 0\\ H_{\mathrm{Gal}}& 0& 0& 1_{\mathrm{Gal}}& 0\\ H_{\mathrm{BD}}& 0& 0& 0& 1_{\mathrm{BD}}\end{array}\right]\left[\begin{array}{c}\mathrm{\δp}\\ {\mathrm{\δt}}_{\mathrm{GPS}}\\ {\mathrm{\δt}}_{\mathrm{GLO}}\\ {\mathrm{\δt}}_{\mathrm{Gal}}\\ {\mathrm{\δt}}_{\mathrm{BD}}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\υ}}_{\mathrm{\ρGPS}}\\ {\mathrm{\υ}}_{\mathrm{\ρGLO}}\\ {\mathrm{\υ}}_{\mathrm{\ρGal}}\\ {\mathrm{\υ}}_{\mathrm{\ρBD}}\end{array}\right]$

Wherein p ═ x y z]^TIs a user state quantity, H_*Is a geometric observation matrix; the dimension of which is 2 in relation to the number (n) of satellites of the selected system; h_*＝[e₁e₂e₃]_2*3，e_kIs 2 x 1 dimension;

measurement equation of pseudo-range observed quantity

Z_ρ(t)＝H_ρ(t)X(t)+V_ρ(t)

Wherein H_ρ(t)＝[0_N*30_N*3H_ρ10_N*6H_ρ20_N*3]_N*23N is 8, the total number of satellites;

H_ρ1＝(a_ji)_N*3， $H_{\mathrm{\ρ}2}=\left[\begin{array}{c}{H}_{\mathrm{\ρGPS}}\\ H_{\mathrm{\ρGLO}}\\ H_{\mathrm{\ρGal}}\\ H_{\mathrm{\ρBD}}\end{array}\right]$

and is

$H_{\mathrm{\ρGPS}}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right],H_{\mathrm{\ρGLO}}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right],H_{\mathrm{\ρGal}}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right],H_{\mathrm{\ρBD}}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right]$

a_j1＝(R_n+h)[-e_j1sinLcosλ-e_j2sinLsinλ]+[R_n*(1-e²)+h]e_j3cosL

a_j2＝(R_n+h)[e_j2cosLcosλ-e_j1cosLsinλ]

a_j3＝e_j1cosLcosλ+e_j2cosLsinλ+e_j3sinL

(b) Establishment of pseudo-range rate measurement equation

Measurement equation of pseudo-range rate observed quantity

$Z_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)=H_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)X\left(t\right)+V_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)$

In the formula, $H_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)=\left[\begin{array}{ccccc}{H}_{\stackrel{\·}{\mathrm{\ρ}}1}& 0_{N*3}& H_{\stackrel{\·}{\mathrm{\ρ}}2}& 0_{N*9}& H_{\stackrel{\·}{\mathrm{\ρ}}3}\end{array}\right]$

wherein, $H_{\stackrel{\·}{\mathrm{\ρ}}1}={\left(b_{\mathrm{ji}}\right)}_{N*3},H_{\stackrel{\·}{\mathrm{\ρ}}2}={\left(c_{\mathrm{ji}}\right)}_{N*3},H_{\stackrel{\·}{\mathrm{\ρ}}3}=\left[\begin{array}{cccc}{1}_{\mathrm{GPS}}& & & \\ & 1_{\mathrm{GLO}}& & \\ & & 1_{\mathrm{Gal}}& \\ & & & 1_{\mathrm{BD}}\end{array}\right]$

b_j1＝-e_*j1sinλ+e_*j2cosλ

b_j2＝-e_*j2sinLcosλ-e_*j1sinLsinλ+e_*j3cosL

b_j3＝e_*j1cosLcosλ+e_*j2cosLsinλ+e_*j3sinL

$c_{j1}=(R_n+h)[-{\stackrel{\·}{e}}_{*j1}\sin L\cos \mathrm{\λ}-{\stackrel{\·}{e}}_{*j2}\sin L\sin \mathrm{\λ}]+[R_n*(1-e^2)+h]{\stackrel{\·}{e}}_{*j3}\cos L$

$c_{j2}=(R_n+h)[{\stackrel{\·}{e}}_{*j2}\cos L\cos \mathrm{\λ}-{\stackrel{\·}{e}}_{*j1}\cos L\sin \mathrm{\λ}]$

$c_{j3}={\stackrel{\·}{e}}_{*j1}\cos L\cos \mathrm{\λ}+{\stackrel{\·}{e}}_{*j2}\cos L\sin \mathrm{\λ}+{\stackrel{\·}{e}}_{*j3}\sin L$

the comprehensive measurement equations (a) and (b) are as follows:

$Z\left(t\right)=\left[\begin{array}{c}{H}_{\mathrm{\ρ}}\\ H_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_{\mathrm{\ρ}}\left(t\right)\\ V_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)\end{array}\right]$

the satellite selection is that a proper satellite is selected for navigation resolving, and the specific process is as follows: firstly, judging the number of satellites of each satellite system; if the number of satellites of a certain satellite system is less than two, the system is excluded; secondly, if the single satellite and SINS combined navigation is adopted, 4-satellite combination is selected; if a double-constellation system and an SINS are combined, the unknown number of synchronous errors of the satellite system is increased, and at least 5 satellites are needed for navigation calculation, namely a 3+2 mode; if three satellite systems are combined with the SINS, 6 satellites are needed to perform positioning calculation, namely a 2+2+2 mode; if four satellite systems are combined, 8 satellites are needed to perform positioning calculation, namely a 2+2+2+2 mode; and finally, selecting the satellites according to a satellite selection algorithm of a maximum vector polyhedron volume method or a least square method on the premise that the total number of the selected satellites and the number of each satellite system are met.

The satellite ephemeris resolving algorithm is used for judging the format of ephemeris messages output by a satellite system and analyzing ephemeris according to the formats of the ephemeris messages of different satellite systems; the satellite system ephemeris is divided into two types according to the format: a broadcast ephemeris parameter format similar to GPS and a broadcast ephemeris parameter format similar to GLONASS; the message analysis of the broadcast ephemeris parameter format of the GPS-like adopts a satellite position algorithm; message analysis of broadcast ephemeris parameter format similar to GLONASS adopts a derivation motion equation integral operation method.

The satellite system data fusion is specifically to select a main satellite system, and other auxiliary satellite systems perform time and coordinate conversion according to the reference time and the coordinate system of the main satellite system.

When the 3+2 mode is that the double constellations are combined with the SINS, one satellite system selects 3 satellites and the other system selects 2 satellites; the 2+2+2 mode refers to that two satellites are respectively selected by three satellite systems; the 2+2+2+2 mode is that two satellites are respectively selected for four satellite systems.

Compared with the prior art, the invention has the following advantages:

(1) the work of the prior single satellite system and strapdown inertial navigation integrated navigation is expanded, the effect of various satellite systems on the integrated navigation system is increased, the accuracy of the integrated navigation in the complex environment with few satellites is ensured, and the reliability of the integrated navigation system is improved.

(2) The invention mainly introduces the research that 4 global satellites are tightly combined with SINS, and the research result of the research can be conveniently transplanted to the combined research of any n global satellite systems and strapdown inertial systems in the long run, thereby having great significance for the research of the application of the satellite/inertial navigation system combination.

Drawings

FIG. 1 is a schematic block diagram of the present invention;

FIG. 2 is a diagram of satellite data processing content in accordance with the present invention;

FIG. 3 is a flowchart of the integrated navigation main program of the present invention;

FIG. 4 is a flow chart of ephemeris resolution of the present invention;

FIG. 5 is a flow chart of the star selection algorithm of the present invention.

Detailed Description

The invention will be further elucidated with reference to the drawing.

As shown in fig. 1, the invention adopts a tight combination method to complete combined navigation, and in order to improve the accuracy of the system in a complex environment, the invention adopts a method of data processing of a multi-satellite system and combined navigation of the multi-satellite system and an SINS, and the method comprises the following concrete implementation steps:

(1) and obtaining the current attitude, speed and position of the strapdown inertial navigation by performing strapdown calculation on the output data of an Inertial Measurement Unit (IMU).

Calculating the SINS attitude angle:

and (3) calculating an attitude matrix by adopting a quaternion method, wherein the position of the moving coordinate system relative to the reference coordinate system is equivalent to the fact that the moving coordinate system rotates by an angle theta around an equivalent rotating shaft according to the Euler's theorem, and if u represents a unit vector of the direction of the equivalent rotating shaft, the position of the moving coordinate system is completely determined by two parameters of u and theta. A quaternion can be constructed with u and θ:

$Q=\cos \frac{\mathrm{\θ}}{2}+u\sin \frac{\mathrm{\θ}}{2}$

derivation and simplification of the above equation can yield a quaternion differential equation:

$Q\left(\stackrel{\·}{q}\right)=\frac{1}{2}{M}^{*}\left({\mathrm{\ω}}_b\right)Q\left(q\right)$

in the formula $M^{*}\left({\mathrm{\ω}}_b\right)=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0\end{array}\right]$

Solving a quaternion differential equation according to a Picard approximation method to obtain:

$q\left(t\right)=\{\cos \frac{\mathrm{\Δ}{\mathrm{\θ}}_0}{2}I+\frac{\sin \frac{\mathrm{\Δ}{\mathrm{\θ}}_0}{2}}{\mathrm{\Δ}{\mathrm{\θ}}_0}[\mathrm{\Δ\θ}\left]\right\}q\left(0\right)$

in the formula

$\mathrm{\Δ}{\mathrm{\θ}}_0=\sqrt{\mathrm{\Δ}{\mathrm{\θ}}_x^2+\mathrm{\Δ}{\mathrm{\θ}}_y^2+\mathrm{\Δ}{\mathrm{\θ}}_z^2}$

$\left[\mathrm{\Δ\θ}\right]={\∫}_{t_1}^{t_1+h}{M}^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)\mathrm{dt}=\left[\begin{array}{cccc}0& -\mathrm{\Δ}{\mathrm{\θ}}_x& -\mathrm{\Δ}{\mathrm{\θ}}_y& -\mathrm{\Δ}{\mathrm{\θ}}_z\\ \mathrm{\Δ}{\mathrm{\θ}}_x& 0& \mathrm{\Δ}{\mathrm{\θ}}_z& -\mathrm{\Δ}{\mathrm{\θ}}_y\\ \mathrm{\Δ}{\mathrm{\θ}}_y& -\mathrm{\Δ}{\mathrm{\θ}}_z& 0& \mathrm{\Δ}{\mathrm{\θ}}_x\\ \mathrm{\Δ}{\mathrm{\θ}}_z& \mathrm{\Δ}{\mathrm{\θ}}_y& -\mathrm{\Δ}{\mathrm{\θ}}_x& 0\end{array}\right]$

In the formula

$\mathrm{\Δ}{\mathrm{\θ}}_i={\∫}_t^{t+h}{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bi}}\mathrm{dt},i=x,y,z.$

Let earth coordinate systemThe rotation angular velocity relative to the inertial frame is omega_ie(the value is 15.04088 DEG/h), L represents the local latitude, and lambda represents the local longitude, then

ω_ie ⁿ: the vector of the earth coordinate system relative to the rotation angular velocity of the inertial coordinate system in the geographic coordinate system is as follows:

${\mathrm{\ω}}_{\mathrm{ie}}^n={\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]}^T$

ω_ie ^b: the vector of the earth coordinate system relative to the rotation angular velocity of the inertial coordinate system in the carrier coordinate system is as follows:

${{\mathrm{\ω}}_{\mathrm{ie}}}^b=C_n^b{{\mathrm{\ω}}_{\mathrm{ie}}}^n$

the attitude matrix in the formula is determined by an initial angle when the carrier is static; when the carrier rotates relative to the geographic coordinate system, the attitude matrix changes accordingly, and is obtained by correcting the quaternion in real time (the same below).

ω_en ⁿThe vector of the rotation angular speed of the geographic coordinate relative to the terrestrial coordinate system in the geographic coordinate system is as follows:

${\mathrm{\ω}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}-V_N/R_N& V_E/R_E& V_E\tan L/R_E\end{array}\right]}^T$

V_E、V_Neast and north speeds of carrier motion, respectively;

R_Nfor reference to the radius of curvature in the meridian plane of the ellipsoid, R_N＝R_e(1-2e+3esin²L)；

R_ERadius of curvature in a plane normal to the meridian plane, R_E＝R_e(1+esin²L)；

Wherein R is_eIs the major axis radius of the reference ellipsoid; e is the ellipticity of the ellipsoid.

And because of that, $\stackrel{\·}{L}=V_N/R_N,\stackrel{\·}{\mathrm{\λ}}=V_E/\left(R_E\cos L\right)$ then

${\mathrm{\ω}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}-\stackrel{\·}{L}& \stackrel{\·}{\mathrm{\λ}}\cos L& \stackrel{\·}{\mathrm{\λ}}\sin L\end{array}\right]}^T$

ω_en ^bThe vector of the rotation angular speed of the geographic coordinate relative to the terrestrial coordinate system in the carrier coordinate system is as follows:

${{\mathrm{\ω}}_{\mathrm{en}}}^b=C_n^b{{\mathrm{\ω}}_{\mathrm{en}}}^n$

ω_ib ^b: gyro output angular velocity, noted

${\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bx}}& {\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{by}}& {\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bz}}\end{array}\right]}^T$

ω_nb ^b: the vector of the rotation angular velocity of the carrier coordinate system relative to the geographic coordinate system in the carrier coordinate system is recorded as

${\mathrm{\ω}}_{\mathrm{nb}}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\end{array}\right]}^T$

Then it can be obtained

ω_nb ^b＝ω_ib ^b-ω_ie ^b-ω_en ^b

After the quaternion is corrected in real time, the attitude matrix can be updated in real time by the elements of the quaternion according to the following formula

$C_n^b=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

The real-time attitude angle can be extracted from the attitude matrix

Calculating SINS speed:

the specific force vector in the carrier coordinate system is f^bThen, in the geographic coordinate system:

$f^n=C_b^n{f}^b$

directional cosine matrix in formulaWhen the carrier is static, the initial angle is determined; direction cosine matrix when carrier rotates relative to geographical coordinate systemAnd then, the result is obtained by correcting the quaternion in real time.

The basic equation of inertial navigation is as follows:

${\stackrel{\·}{V}}^n=f^n-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×V^n+g^n$

the writing component is in the form of:

$\left[\begin{array}{c}{\stackrel{\·}{V}}_E\\ {\stackrel{\·}{V}}_N\\ {\stackrel{\·}{V}}_U\end{array}\right]=\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]+\left[\begin{array}{ccc}0& (\stackrel{\·}{\mathrm{\λ}}+2{\mathrm{\ω}}_{\mathrm{ie}})\sin L& -(\stackrel{\·}{\mathrm{\λ}}+2{\mathrm{\ω}}_{\mathrm{ie}})\cos L\\ -(\stackrel{\·}{\mathrm{\λ}}+2{\mathrm{\ω}}_{\mathrm{ie}})\sin L& 0& -\stackrel{\·}{L}\\ (\stackrel{\·}{\mathrm{\λ}}+2{\mathrm{\ω}}_{\mathrm{ie}})\cos L& \stackrel{\·}{L}& 0\end{array}\right]\left[\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]$

in the formula: f. ofⁿFor the projection of the acceleration of the carrier on the navigation coordinate system, fⁿ＝[f_Ef_Nf_U]^T；VⁿRepresenting the velocity vector, V, of the hull in a navigational coordinate systemⁿ＝[V_EV_NV_U]^T；gⁿIs the gravity acceleration vector, gⁿ＝[0 0 -g]^T。

By integrating, the individual velocity components V of the vehicle on the navigation coordinate system are determined_E、V_N、V_U。

Calculating the SINS position:

the differential equation for longitude and latitude can be expressed as follows:

$\left\{\begin{array}{c}\stackrel{\·}{L}=\frac{V_N}{R_N+h}\\ \stackrel{\·}{\mathrm{\λ}}=\frac{V_E}{(R_E+h)\cos L}\end{array}\right.$

wherein h is the height.

The longitude and latitude can be obtained by integrating the update formula of the longitude and latitude of the upper formula:

$\left\{\begin{array}{c}L=\∫\stackrel{\·}{L}\mathrm{dt}+L\left(0\right)\\ \mathrm{\λ}=\∫\stackrel{\·}{\mathrm{\λ}}\mathrm{dt}+\mathrm{\λ}\left(0\right)\end{array}\right.$

(2) satellite data processing is carried out on ephemeris messages output by a satellite system receiver to obtain speed and position information of a satellite;

according to the ephemeris format classification of table 1, it can be seen that the satellite system ephemeris is divided into two types according to the format: a GPS-like ephemeris parameter format and a GLONASS-like ephemeris parameter format.

TABLE 1 ephemeris Format Classification

And (3) analyzing ephemeris in the broadcast ephemeris parameter format of the GPS-like system:

(a) order to

t_k＝t-t_oe

Wherein t is_oeA reference time for satellite ephemeris data; t is t_kIs a normalized relative time.

(b) The average angular velocity n of the satellite is

n＝n₀+Δn

WhereinIs the theoretical average angular velocity of the satellite, Δ n is the correction term of the average angular velocity of the satellite, μ 3.986005 × 10¹⁴Is the gravitational constant.

(c) The mean-near point angle M at the observation time is

M＝M₀+n*t_k

Wherein M is₀Is the mean anomaly angle of the reference time instant.

(d) The approximate point angle of the observation time can be calculated by the mean approximate point angle

E＝M+e_s*sinE

For the above equation, the eccentricity e of the satellite orbit_sGenerally less than 0.02, if the solution is solved by an iterative method, the overall yield must be obtainedConvergence, let E be the initial value E₀After ten iterations, typically, Δ E/E < 10^-10The accuracy of (2).

(e) Calculating the earth center radial r of the satellite according to the deflection point angle

r＝a*(1-e_s*cosE)

WhereinIs the square root of the half-length of the satellite's elliptical orbit.

(f) Calculating the true near point angle f of the satellite according to the partial near point angle

$\tan (f/2)=(\sqrt{1+e_s}/\sqrt{1-e_s}*\tan (E/2))$

The value of the above equation in the calculation process may be large in the vicinity of E ═ pi (180 °), and a simple method of setting a threshold value for E or tan (E/2) may be employed. Or calculated using the following formula

$\left\{\begin{array}{c}\sin f=\frac{\sqrt{1-e_s^2}\&CenterDot;\sin E}{1-e_s\cos E}\\ \cos f=\frac{\cos E-e_s}{1-e_s\cos E}\end{array}\right.$

(g) Calculating the lift intersection angular distance phi according to the true approach point angle

φ＝f+ω

Where ω is the perigee angular separation of the satellite orbits.

(h) Correcting satellite orbit parameters according to the perturbation correction term in ephemeris and the angular distance of a lifting intersection point to calculate the perturbation correction term

Lifting intersection angle distance correction value mu

μ＝C_uc*cos(2φ)+C_us*sin(2φ)

Correction value r of earth radial direction

r＝C_rc*cos(2φ)+C_rs*sin(2φ)

Track inclination correction value i

i＝C_ic*cos(2φ)+C_is*sin(2φ)

Wherein C is_uc,C_usA harmonic correction term for the lifting intersection angular distance; c_rc,C_rsA harmonic correction term for the satellite earth centripetal radial direction; c_ic,C_isA harmonic correction term for satellite orbit inclination.

(i) Updating orbit parameters of satellite at current observation time

Angular distance phi between the lifting points_kφ_k＝φ+μ

Radial direction r of earth center_kr_k＝r+r

Track inclination angle i_ki_k＝i₀+idot*t_k+i

Wherein i₀The inclination angle of the satellite orbit plane relative to the earth equatorial plane at the reference moment; idot is the rate of change of the satellite orbital plane inclination.

(j) Calculating at normalized time t_kThe right ascension angle omega of_k

${\mathrm{\&Omega;}}_k={\mathrm{\&Omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&Omega;}}*t_k-{\mathrm{\&omega;}}_{\mathrm{ie}}*t$

Wherein, ω is_ieIs the angular rate of rotation of the earth, omega_ie＝7.2921151467×10^-5(rad/s)；Ω_eThe right ascension of the satellite orbit;the rate of change of the ascension at the ascending crossing point;

(k) satellite position coordinates

In the plane of the satellite orbit, a coordinate system with the origin as the geocenter, the north direction of the Z axis as the positive and the long axis of the elliptic orbit of the X axis is established to obtain the position coordinate of the satellite as

$P=\left[\begin{array}{c}{r}_k*\cos \left({\mathrm{\&phi;}}_k\right)\\ r_k*\sin \left({\mathrm{\&phi;}}_k\right)\\ 0\end{array}\right]$

(l) The process of satellite velocity calculation is similar to that of coordinate calculation, and it should be noted that the satellite velocity should be corrected using the derivative term of the satellite clock error and the derivative term of the generalized relativistic effect.

According to the steps, the position coordinates and the speed of the satellite in the coordinate system of the satellite can be calculated.

Resolving the broadcast ephemeris parameter format ephemeris of the GLONASS-like:

(a) establishing equations of motion for satellites

Assuming that an inertial coordinate system (0-xyz) exists, the coordinate origin is coincident with the satellite coordinate system, and the coordinate axes point to t_eThe coordinate axes of the satellite coordinate system point the same at the moment. If the positions (x (t), y (t), and z (t)) of the satellite in the earth-fixed coordinate system at any time t are (x (t), y (t), and z (t)) in the inertial system 0-xyz, the relationship between them is:

$\left[\begin{array}{c}X\left(t\right)\\ Y\left(t\right)\\ Z\left(t\right)\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\&omega;}(t-t_e)& \sin \mathrm{\&omega;}(t-t_e)& 0\\ -\sin \mathrm{\&omega;}(t-t_e)& \cos \mathrm{\&omega;}(t-t_e)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\left(t\right)\\ y\left(t\right)\\ z\left(t\right)\end{array}\right]=R\left(t\right)\left[\begin{array}{c}x\left(t\right)\\ y\left(t\right)\\ z\left(t\right)\end{array}\right]$

the first and second order differential relations are:

$\left[\begin{array}{c}\stackrel{\&CenterDot;}{X}\left(t\right)\\ \stackrel{\&CenterDot;}{Y}\left(t\right)\\ \stackrel{\&CenterDot;}{Z}\left(t\right)\end{array}\right]=R\left(t\right)\left[\begin{array}{c}\stackrel{\&CenterDot;}{x}\left(t\right)\\ \stackrel{\&CenterDot;}{y}\left(t\right)\\ \stackrel{\&CenterDot;}{z}\left(t\right)\end{array}\right]+\stackrel{\&CenterDot;}{R}\left(t\right)\left[\begin{array}{c}x\left(t\right)\\ y\left(t\right)\\ z\left(t\right)\end{array}\right]$

$\left[\begin{array}{c}\stackrel{\&CenterDot;\&CenterDot;}{X}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{Y}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{Z}\left(t\right)\end{array}\right]=R\left(t\right)\left[\begin{array}{c}\stackrel{\&CenterDot;\&CenterDot;}{x}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{y}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{z}\left(t\right)\end{array}\right]+2\stackrel{\&CenterDot;}{R}\left(t\right)\left[\begin{array}{c}\stackrel{\&CenterDot;}{x}\left(t\right)\\ \stackrel{\&CenterDot;}{y}\left(t\right)\\ \stackrel{\&CenterDot;}{z}\left(t\right)\end{array}\right]+\stackrel{\&CenterDot;\&CenterDot;}{R}\left(t\right)\left[\begin{array}{c}x\left(t\right)\\ y\left(t\right)\\ z\left(t\right)\end{array}\right]$

it is known that $R\left(t\right)=\left[\begin{array}{ccc}\cos \mathrm{\&omega;}(t-t_e)& \sin \mathrm{\&omega;}(t-t_e)& 0\\ -\sin \mathrm{\&omega;}(t-t_e)& \cos \mathrm{\&omega;}(t-t_e)& 0\\ 0& 0& 1\end{array}\right],$ Then

$\stackrel{\&CenterDot;}{R}\left(t\right)=\left[\begin{array}{ccc}-\mathrm{\&omega;}\sin \mathrm{\&omega;}(t-t_e)& \mathrm{\&omega;}\cos \mathrm{\&omega;}(t-t_e)& 0\\ -\mathrm{\&omega;}\cos \mathrm{\&omega;}(t-t_e)& \mathrm{\&omega;}\sin \mathrm{\&omega;}(t-t_e)& 0\\ 0& 0& 1\end{array}\right]=\mathrm{\&omega;}\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]R\left(t\right)$

In the same way

$\stackrel{\&CenterDot;\&CenterDot;}{R}\left(t\right)={\mathrm{\&omega;}}^2\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\end{array}\right]R\left(t\right)$

In summary, the following results can be obtained:

$\left[\begin{array}{c}\stackrel{\&CenterDot;\&CenterDot;}{X}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{Y}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{Z}\left(t\right)\end{array}\right]=R\left(t\right)\left[\begin{array}{c}\stackrel{\&CenterDot;\&CenterDot;}{x}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{y}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{z}\left(t\right)\end{array}\right]+2\mathrm{\&omega;}\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}\stackrel{\&CenterDot;}{X}\left(t\right)\\ \stackrel{\&CenterDot;}{Y}\left(t\right)\\ \stackrel{\&CenterDot;}{Z}\left(t\right)\end{array}\right]+{\mathrm{\&omega;}}^2\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}X\left(t\right)\\ Y\left(t\right)\\ Z\left(t\right)\end{array}\right]$

is that

$\left[\begin{array}{c}\stackrel{\&CenterDot;\&CenterDot;}{X}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{Y}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{Z}\left(t\right)\end{array}\right]=R\left(t\right)\left[\begin{array}{c}\stackrel{\&CenterDot;\&CenterDot;}{x}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{y}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{z}\left(t\right)\end{array}\right]+\left[\begin{array}{c}{\mathrm{\&omega;}}^{2}X\left(t\right)+2\mathrm{\&omega;}\stackrel{\&CenterDot;}{Y}\left(t\right)\\ {\mathrm{\&omega;}}^{2}Y\left(t\right)-2\mathrm{\&omega;}\stackrel{\&CenterDot;}{X}\left(t\right)\\ 0\end{array}\right]$

The first term on the right of the above equation is the representation of the satellite motion acceleration in the earth-fixed coordinate system. Theoretically, the acceleration of the satellite due to all forces (gravity and all perturbation forces) must be calculated. Due to the short integration time, the precision requirement can be met only by calculating the non-spherical gravity perturbation of the earth (generally, only the influence of the C term is considered) and the gravity perturbation of the sun and the moon.

In the earth-fixed coordinate system, the mass of the earth is set as M_e，GM_e＝3.9860044×10¹⁴m³/s²The gravity constant of the earth, the acceleration caused by the gravity of the center of the earth is:

$a_e=-\frac{{\mathrm{GM}}_e}{r^3}\left[\begin{array}{c}X\left(t\right)\\ Y\left(t\right)\\ Z\left(t\right)\end{array}\right]$

$r=\sqrt{X^2\left(t\right)+Y^2\left(t\right)+Z^2\left(t\right)}$

the acceleration caused by the gravity of the earth's non-spherical stars is:

$a_n=1.5\frac{{\mathrm{GM}}_e}{r^5}{R}_e^2{C}_2^0\left[\begin{array}{c}X\left(t\right)(r^2-5Z{\left(t\right)}^2)/r^2\\ Y\left(t\right)(r^2-5Z{\left(t\right)}^2)/r^2\\ Z\left(t\right)(3r^2-5Z{\left(t\right)}^2/r^2)\end{array}\right]$

wherein R is_eMajor radius of reference ellipsoid (R)_e＝6378136m)，Is a non-normalized gravitational field coefficient and is a normalized coefficientRiding device $\sqrt{5}(C_2^0=\sqrt{5}{\stackrel{-}{C}}_2^0=1082625.7\&times;{10}^{-9}).$

T is provided in GLONASS-like broadcast ephemeris parameter format ephemeris navigation message_eThe acceleration caused by the perturbation of the day and the month at the reference moment has a value in a terrestrial coordinate systemWhich is seen as constant over a short period of time.

In summary, the equation of motion of the satellite in the earth-fixed coordinate system is:

$\left[\begin{array}{c}\stackrel{\&CenterDot;\&CenterDot;}{X}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{Y}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{Z}\left(t\right)\end{array}\right]=-\frac{{\mathrm{GM}}_e}{r^3}\left[\begin{array}{c}\stackrel{\&CenterDot;\&CenterDot;}{x}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{y}\left(t\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{z}\left(t\right)\end{array}\right]+1.5\frac{{\mathrm{GM}}_e}{r^5}{R}_e^2{C}_2^0\left[\begin{array}{c}X\left(t\right)(r^2-5Z{\left(t\right)}^2)/r^2\\ Y\left(t\right)(r^2-5Z{\left(t\right)}^2)/r^2\\ Z\left(t\right)(3r^2-5Z{\left(t\right)}^2)/r^2\end{array}\right]+\left[\begin{array}{c}\stackrel{\&CenterDot;\&CenterDot;}{X}\left(t_e\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{Y}\left(t_e\right)\\ \stackrel{\&CenterDot;\&CenterDot;}{Z}\left(t_e\right)\end{array}\right]+\left[\begin{array}{c}{\mathrm{\&omega;}}^{2}X\left(t\right)+2\mathrm{\&omega;}\stackrel{\&CenterDot;}{Y}\left(t\right)\\ {\mathrm{\&omega;}}^{2}Y\left(t\right)-2\mathrm{\&omega;}\stackrel{\&CenterDot;}{X}\left(t\right)\\ 0\end{array}\right]$

(b) calculating satellite position and velocity at any time t

According to the motion equation of the satellite in the earth-fixed coordinate system, t can be expressed in a functional form_iThe moment acceleration function is formulated as follows:

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;\&CenterDot;}{X}}_i=f_1(t_i,X_i,Y_i,Z_i,{\stackrel{\&CenterDot;}{X}}_i,{\stackrel{\&CenterDot;}{Y}}_i,{\stackrel{\&CenterDot;}{Z}}_i,\stackrel{\&CenterDot;\&CenterDot;}{X}\left(t_e\right))\\ {\stackrel{\&CenterDot;\&CenterDot;}{Y}}_i=f_2(t_i,X_i,Y_i,Z_i,{\stackrel{\&CenterDot;}{X}}_i,{\stackrel{\&CenterDot;}{Y}}_i,{\stackrel{\&CenterDot;}{Z}}_i,\stackrel{\&CenterDot;\&CenterDot;}{Y}\left(t_e\right))\\ {\stackrel{\&CenterDot;\&CenterDot;}{Z}}_i=f_3(t_i,X_i,Y_i,Z_i,{\stackrel{\&CenterDot;}{X}}_i,{\stackrel{\&CenterDot;}{Y}}_i,{\stackrel{\&CenterDot;}{Z}}_i,\stackrel{\&CenterDot;\&CenterDot;}{Z}\left(t_e\right))\end{array}\right.$

with t₀The time is taken as the initial state, then t_iThe integral equation for the satellite position at time is:

$\stackrel{\&CenterDot;}{r}\left(t_i\right)={\stackrel{\&CenterDot;}{r}}_0+{\&Integral;}_{t_0}^{t_i}\stackrel{\&CenterDot;\&CenterDot;}{r}\mathrm{dt}$

$r\left(t_i\right)=r\left(t_0\right)+{\&Integral;}_{t_0}^{t_i}\stackrel{\&CenterDot;}{r}\mathrm{dt}$

r(t₀)，calculating t by twice integral solution for satellite position coordinate vector and speed of reference time_iThe satellite position at the moment is integrated by adopting a fixed step length fourth-order Longbeige-Kutta to the orbit, and the primary integration formula is as follows:

$r_{n+1}=r_n+\frac{1}{6}(k_1+2k_2+2k_3+k_4)$

integrating acceleration, calculating a step speed integration result, performing speed integration according to the speed integration result, calculating a coordinate integration result, performing next step integration according to the speed and position obtained by the integration calculation of the step, and performing cycle calculation sequentially until t_iThe time is over. Number of integrations and acquisitionAnd the integration time-dependent calculation formula:and finally, obtaining the satellite coordinates and the speed at the moment t.

(3) Calculating SINS pseudo range and pseudo range rate by using satellite speed and position and SINS position and speed information;

SINS pseudorange rho_IjAnd (3) calculating:

let the coordinate of the carrier position under ECEF coordinate system be (x)_I,y_I,z_I) Given by strapdown; the position of the jth satellite in the reference frame is (x)_sj,y_sj,z_sj) And obtaining the satellite ephemeris. The pseudo range from the carrier position given by the inertial navigation to the jth satellite is

${\mathrm{\&rho;}}_{\mathrm{Ij}}=\sqrt{{(x_I-x_{\mathrm{sj}})}^2+{(y_I-y_{\mathrm{sj}})}^2+{(z_I-z_{\mathrm{sj}})}^2}$

SINS pseudorange rateAnd (3) calculating:

ρ_Ijderivative of (2)Comprises the following steps:

$\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_{\mathrm{Ij}}=d\left(\sqrt{{(x_I-x_{\mathrm{sj}})}^2+{(y_I-y_{\mathrm{sj}})}^2+{(z_I-z_{\mathrm{sj}})}^2}\right)/\mathrm{dt}\\ =\frac{(x_I-x_{\mathrm{sj}})({\stackrel{\&CenterDot;}{x}}_I-{\stackrel{\&CenterDot;}{x}}_{\mathrm{sj}})+(y_I-y_{\mathrm{sj}})({\stackrel{\&CenterDot;}{y}}_I-{\stackrel{\&CenterDot;}{y}}_{\mathrm{sj}})+(z_I-z_{\mathrm{sj}})({\stackrel{\&CenterDot;}{z}}_I-{\stackrel{\&CenterDot;}{z}}_{\mathrm{sj}})}{{\mathrm{\&rho;}}_{\mathrm{Ij}}}\\ =e_{j1}({\stackrel{\&CenterDot;}{x}}_I-{\stackrel{\&CenterDot;}{x}}_{\mathrm{sj}})+e_{j2}\left({{\stackrel{\&CenterDot;}{y}}_I-\stackrel{\&CenterDot;}{y}}_{\mathrm{sj}}\right)+e_{j3}({\stackrel{\&CenterDot;}{z}}_I-{\stackrel{\&CenterDot;}{z}}_{\mathrm{sj}})\end{array}$

wherein,is the velocity of SINS in ECEF coordinate system, (x)_sj,y_sj,z_sj) The satellite velocity of the jth satellite in the reference satellite coordinate system is shown. And is $\begin{array}{c}(x_I-x_{\mathrm{sj}})/{\mathrm{\&rho;}}_{\mathrm{Ij}}=e_{j1}\\ (y_I-y_{\mathrm{sj}})/{\mathrm{\&rho;}}_{\mathrm{Ij}}=e_{j2}\\ (z_I-z_{\mathrm{sj}})/{\mathrm{\&rho;}}_{\mathrm{Ij}}=e_{j3}\end{array}.$

(4) And obtaining a code measurement pseudo range and Doppler frequency shift from a pseudo range message output by a satellite system receiver, comparing the code measurement pseudo range and the Doppler frequency shift with an SINS pseudo range and a pseudo range rate, and establishing a tight combination state model and a measurement model by taking a difference value as an observed quantity of a filter. And outputting the error correction quantity of the system through optimal estimation and filtering. And correcting the system error, and outputting the optimal combined solution of the SINS attitude, speed and position.

Establishing an error equation and a model of the strapdown inertial navigation system:

(a) equation of speed error

The velocity error equation is generally of the form

By expanding the formula

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&delta;}}{V}_E=f_N{\mathrm{\&phi;}}_U-f_U{\mathrm{\&phi;}}_N+(\frac{V_N}{R_E}\tan L-\frac{V_U}{R_E})\mathrm{\&delta;}{V}_E+(2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E}\tan L)\mathrm{\&delta;}{V}_N-(2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E})\mathrm{\&delta;}{V}_U\\ +(2{\mathrm{\&omega;}}_{\mathrm{ie}}(V_N\cos L+V_U\sin L)+\frac{V_E{V}_N}{R_E}{\sec }^{2}L)\mathrm{\&delta;L}+\frac{V_E{V}_U-V_E{V}_N\tan L}{{R_E}^2}\mathrm{\&delta;h}+{\&dtri;}_E\\ \stackrel{}{\stackrel{\&CenterDot;}{\mathrm{\&delta;}}{V}_N=f_U{\mathrm{\&phi;}}_E-f_E{\mathrm{\&phi;}}_U-2}({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E}\tan L)\mathrm{\&delta;}{V}_E-\frac{V_U}{R_N}\mathrm{\&delta;}{V}_N-\frac{V_N}{R_N}\mathrm{\&delta;}{V}_U\\ -({2\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E}{\sec }^{2}L)V_E\mathrm{\&delta;L}+(\frac{V_N{V}_U}{{R_N}^2}+\frac{V_E^2\tan L}{{R_E}^2})\mathrm{\&delta;h}+{\&dtri;}_N\\ \stackrel{\&CenterDot;}{\mathrm{\&delta;}}{V}_U=f_E{\mathrm{\&phi;}}_N-f_N{\mathrm{\&phi;}}_E+2({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E}){\mathrm{\&delta;V}}_E+\frac{{2V}_N}{R_N}{\mathrm{\&delta;V}}_N-2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin {\mathrm{LV}}_E\mathrm{\&delta;L}\\ -(\frac{{V_N}^2}{{R_N}^2}+\frac{V_E^2}{{R_E}^2})\mathrm{\&delta;h}+{\&dtri;}_U\end{array}\right.$

(b) Equation of attitude error

The attitude error equation of the strapdown inertial navigation is

Will be unfolded to

(c) Equation of position error

By $\stackrel{\&CenterDot;}{L}=\frac{V_N}{R_N},\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}=\frac{V_E}{R_E\cos L}$ Can obtain the product

$\left\{\begin{array}{c}\mathrm{\&delta;}\stackrel{\&CenterDot;}{L}=\frac{{\mathrm{\&delta;V}}_N}{R_N}\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}=\frac{\mathrm{\&delta;}{V}_E}{R_E}\sec L+\frac{V_E}{R_E}\sec L\tan \mathrm{L\&delta;L}\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{h}=\mathrm{\&delta;}{V}_U\end{array}\right.$

Modeling a satellite system:

the error state of a satellite, in a combined pseudorange and pseudorange rate system, typically takes two time-dependent errors: one is the distance error t equivalent to the clock error_uThe other is a distance error t equivalent to the clock frequency error_ru。

(a) GPS system modeling

Pseudo range and pseudo range rate are selected from the observation quantity of the SINS/GPS tight combination, and two related state quantities, namely clock error equivalent distance error and clock frequency error equivalent rate error of a station clock, are selected according to the mathematical model GPS part of the pseudo range and the pseudo range rate. The model is described as

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{t}}_{\mathrm{GPS}}=\mathrm{\&delta;}{t}_{\mathrm{rGPS}}+{\mathrm{\&omega;}}_{\mathrm{tGPS}}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{t}}_{\mathrm{rGPS}}=-{\mathrm{\&beta;}}_{\mathrm{trGPS}}\mathrm{\&delta;}{t}_{\mathrm{rGPS}}+{\mathrm{\&omega;}}_{\mathrm{trGPS}}\end{array}\right.$

The above formula shows that the GPS clockThe deviation is a white noise excited first order differential equation and the clock frequency deviation is a first order Markov process β_trGPSIs the inverse correlation time.

(b) GLONASS system modeling

According to the mathematical model GLONASS part of the pseudo range and the pseudo range rate, two related state quantities, namely a clock difference equivalent distance error and a clock frequency difference equivalent rate error of a station clock, need to be selected. The model is described as

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{t}}_{\mathrm{GLO}}=\mathrm{\&delta;}{t}_{\mathrm{rGLO}}+{\mathrm{\&omega;}}_{\mathrm{tGLO}}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{t}}_{\mathrm{rGLO}}=-{\mathrm{\&beta;}}_{\mathrm{trGLO}}\mathrm{\&delta;}{t}_{\mathrm{rGLO}}+{\mathrm{\&omega;}}_{\mathrm{trGLO}}\end{array}\right.$

(c) Galileo system modeling

According to the mathematical model Galileo part of the pseudo range and the pseudo range rate, two related state quantities, namely a clock difference equivalent distance error and a clock frequency difference equivalent rate error of a station clock, need to be selected. The model is described as

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{t}}_{\mathrm{Gal}}=\mathrm{\&delta;}{t}_{\mathrm{rGal}}+{\mathrm{\&omega;}}_{\mathrm{tGal}}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{t}}_{\mathrm{rGal}}=-{\mathrm{\&beta;}}_{\mathrm{trGal}}\mathrm{\&delta;}{t}_{\mathrm{rGal}}+{\mathrm{\&omega;}}_{\mathrm{trGal}}\end{array}\right.$

(d) BD system modeling

According to the pseudo range and the mathematical model BD part of the pseudo range rate, two related state quantities, namely a clock difference equivalent distance error and a clock frequency difference equivalent rate error of a station clock, need to be selected. The model is described as

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{t}}_{\mathrm{BD}}=\mathrm{\&delta;}{t}_{\mathrm{rBD}}+{\mathrm{\&omega;}}_{\mathrm{tBD}}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{t}}_{\mathrm{rBD}}=-{\mathrm{\&beta;}}_{\mathrm{trBD}}\mathrm{\&delta;}{t}_{\mathrm{rBD}1}+{\mathrm{\&omega;}}_{\mathrm{trBD}}\end{array}\right.$

Establishing a multi-system tight combination state equation:

converting strapdown error equation and satellite system modeling into state equation description

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{X}}_I\left(t\right)\\ {\stackrel{\&CenterDot;}{X}}_{\mathrm{GPS}}\left(t\right)\\ {\stackrel{\&CenterDot;}{X}}_{\mathrm{GLO}}\left(t\right)\\ {\stackrel{\&CenterDot;}{X}}_{\mathrm{Gal}}\left(t\right)\\ {\stackrel{\&CenterDot;}{X}}_{\mathrm{BD}}\left(t\right)\end{array}\right]=\left[\begin{array}{ccccc}{F}_I\left(t\right)& 0& 0& 0& 0\\ 0& F_{\mathrm{GPS}}\left(t\right)& 0& 0& 0\\ 0& 0& F_{\mathrm{GLO}}\left(t\right)& 0& 0\\ 0& 0& 0& F_{\mathrm{Gal}}\left(t\right)& 0\\ 0& 0& 0& 0& F_{\mathrm{BD}}\left(t\right)\end{array}\right]\left[\begin{array}{c}{X}_I\left(t\right)\\ X_{\mathrm{GPS}}\left(t\right)\\ X_{\mathrm{GLO}}\left(t\right)\\ X_{\mathrm{Gal}}\left(t\right)\\ X_{\mathrm{BD}}\left(t\right)\end{array}\right]+\left[\begin{array}{ccccc}{G}_I\left(t\right)& 0& 0& 0& 0\\ 0& G_{\mathrm{GPS}}\left(t\right)& 0& 0& 0\\ 0& 0& G_{\mathrm{GLO}}\left(t\right)& 0& 0\\ 0& 0& 0& G_{\mathrm{Gal}}\left(t\right)& 0\\ 0& 0& 0& 0& F_{\mathrm{BD}}\left(t\right)\end{array}\right]\left[\begin{array}{c}{W}_I\left(t\right)\\ W_{\mathrm{GPS}}\left(t\right)\\ W_{\mathrm{GLO}}\left(t\right)\\ W_{\mathrm{Gal}}\left(t\right)\\ W_{\mathrm{BD}}\left(t\right)\end{array}\right]$

Wherein, X ∈ R²³The state variables are composed of state quantities of 15 inertial navigations, state quantities of 2 GPS, state quantities of 2 GLONASS, state quantities of 2 Galileo and state quantities of 2 BD.

$\begin{array}{ccccccccccccccccccccccc}X=& & & & & & & & & & & & & & & & & & & & & & \\ [{\mathrm{\&delta;V}}_E& {\mathrm{\&delta;V}}_N& {\mathrm{\&delta;V}}_U& {\mathrm{\&phi;}}_E& {\mathrm{\&phi;}}_N& {\mathrm{\&phi;}}_U& \mathrm{\&delta;L}& \mathrm{\&delta;\&lambda;}& \mathrm{\&delta;h}& {\&dtri;}_{\mathrm{bx}}& {\&dtri;}_{\mathrm{by}}& {\&dtri;}_{\mathrm{bz}}& {\mathrm{\&epsiv;}}_{\mathrm{bx}}& {\mathrm{\&epsiv;}}_{\mathrm{by}}& {\mathrm{\&epsiv;}}_{\mathrm{bz}}& {\mathrm{\&delta;t}}_{\mathrm{GPS}}& {\mathrm{\&delta;t}}_{\mathrm{GLO}}& {\mathrm{\&delta;t}}_{\mathrm{Gal}}& {\mathrm{\&delta;t}}_{\mathrm{BD}}& {\mathrm{\&delta;t}}_{\mathrm{rGPS}}& {\mathrm{\&delta;t}}_{\mathrm{rGLO}}& {\mathrm{\&delta;t}}_{\mathrm{rGal}}& {\mathrm{\&delta;t}}_{\mathrm{rBD}}]\end{array}$ The state quantities from 1 st to 21 st dimensions are:

V_E,V_N,V_Uare the speed errors in the three directions on the northeast,three misalignment angles (platform attitude angles) of the strapdown, L, lambda and h are three position errors of the strapdown and are described by a terrestrial coordinate system,the offset errors in the three axial directions are added,_bx,_by,_bzis the three axial drifts of the gyro, t_GPS、t_GLO、t_Gal、t_BDThe equivalent distances of the clock errors of the GPS, GLONASS, Galileo and BD satellites, t_rGPS、t_rGLO、t_rGal、t_rBDThe satellite clock frequency error equivalent distance change rates of the GPS, the GLONASS, the Galileo and the BD respectively have 23 dimensions.

In the formula, F_I(t) is the state matrix of the inertial navigation system error equation of state, and F_GPS(t)、F_GLO(t)、F_Gal(t)、F_BD(t) is a state matrix obtained by modeling two state quantities related to GPS, GLONASS, Galileo and BD satellites respectively, G_I(t) and G_GPS(t)、G_GLO(t)、G_Gal(t)、G_BD(t) is the input matrix for the noise. t is t_*Representing its excitation as white noise after modeling, t_r*Modeling as a first order markov process, representing the systems (GPS, GLONASS, Galileo, BD). As shown below

$F_{*}=\left[\begin{array}{cc}0& 1\\ 0& -{\mathrm{\&beta;}}_{\mathrm{tr}*}\end{array}\right],G_{*}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],W_{*}\left(t\right)=\left[\begin{array}{c}{\mathrm{\&omega;}}_{t*}\\ {\mathrm{\&omega;}}_{\mathrm{tr}*}\end{array}\right];$

Establishing a multi-system tight combination measurement equation:

(a) pseudo-range measurement equation establishment

t_*Is a station clock error, which represents the equivalent distance error of the station clock error, v_ρ*jOther common errors and individual errors are represented, such as ionospheric errors, tropospheric errors and multipath errors, etc. measurement errors. In tight composite simulation, v_ρ*jThe error is regarded as a white noise form, and the clock error is uniformly set by a plurality of channels. In the tight-fit prototype experiment, many errors such as ionospheric delay must be compensated, and the compensation method is discussed below.

Taking satellites and writing them in matrix form

$\left[\begin{array}{c}{\mathrm{\&delta;\&rho;}}_{\mathrm{GPS}}\\ {\mathrm{\&delta;\&rho;}}_{\mathrm{GLO}}\\ {\mathrm{\&delta;\&rho;}}_{\mathrm{Gal}}\\ \mathrm{\&delta;}{\mathrm{\&rho;}}_{\mathrm{BD}}\end{array}\right]=\left[\begin{array}{ccccc}{H}_{\mathrm{GPS}}& 1_{\mathrm{GPS}}& 0& 0& 0\\ H_{\mathrm{GLO}}& 0& 1_{\mathrm{GLO}}& 0& 0\\ H_{\mathrm{Gal}}& 0& 0& 1_{\mathrm{Gal}}& 0\\ H_{\mathrm{BD}}& 0& 0& 0& 1_{\mathrm{BD}}\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;p}\\ {\mathrm{\&delta;t}}_{\mathrm{GPS}}\\ {\mathrm{\&delta;t}}_{\mathrm{GLO}}\\ {\mathrm{\&delta;t}}_{\mathrm{Gal}}\\ {\mathrm{\&delta;t}}_{\mathrm{BD}}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\&upsi;}}_{\mathrm{\&rho;GPS}}\\ {\mathrm{\&upsi;}}_{\mathrm{\&rho;GLO}}\\ {\mathrm{\&upsi;}}_{\mathrm{\&rho;Gal}}\\ {\mathrm{\&upsi;}}_{\mathrm{\&rho;BD}}\end{array}\right]$

Wherein p ═ x y z]^TIs a user state quantity, H_*Is a geometric observation matrix. The dimension of which is 2 in relation to the number (n) of satellites of the selected system. H_*＝[e₁e₂e₃]_2*3，e_kIs 2 x 1 dimension.

Measurement equation of pseudo-range observed quantity

Z_ρ(t)＝H_ρ(t)X(t)+V_ρ(t)

Wherein H_ρ(t)＝[0_N*30_N*3H_ρ10_N*6H_ρ20_N*3]_N*23And N is 8, the total number of satellites.

H_ρ1＝(a_ji)_N*3， $H_{\mathrm{\&rho;}2}=\left[\begin{array}{c}{H}_{\mathrm{\&rho;GPS}}\\ H_{\mathrm{\&rho;GLO}}\\ H_{\mathrm{\&rho;Gal}}\\ H_{\mathrm{\&rho;BD}}\end{array}\right]$

And is

$H_{\mathrm{\&rho;GPS}}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right],H_{\mathrm{\&rho;GLO}}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right],H_{\mathrm{\&rho;Gal}}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right],H_{\mathrm{\&rho;BD}}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right]$

a_j1＝(R_n+h)[-e_j1sinLcosλ-e_j2sinLsinλ]+[R_n*(1-e²)+h]e_j3cosL

a_j2＝(R_n+h)[e_j2cosLcosλ-e_j1cosLsinλ]

a_j3＝e_j1cosLcosλ+e_j2cosLsinλ+e_j3sinL

(b) Establishment of pseudo-range rate measurement equation

Measurement equation of pseudo-range rate observed quantity

$Z_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\left(t\right)=H_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\left(t\right)X\left(t\right)+V_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\left(t\right)$

In the formula, $H_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\left(t\right)=\left[\begin{array}{ccccc}{H}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}1}& 0_{N*3}& H_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}2}& 0_{N*9}& H_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}3}\end{array}\right]$

wherein, $H_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}1}={\left(b_{\mathrm{ji}}\right)}_{N*3},H_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}2}={\left(c_{\mathrm{ji}}\right)}_{N*3},H_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}3}=\left[\begin{array}{cccc}{1}_{\mathrm{GPS}}& & & \\ & 1_{\mathrm{GLO}}& & \\ & & 1_{\mathrm{Gal}}& \\ & & & 1_{\mathrm{BD}}\end{array}\right]$

b_j1＝-e_*j1sinλ+e_*j2cosλ

b_j2＝-e_*j2sinLcosλ-e_*j1sinLsinλ+e_*j3cosL

b_j3＝e_*j1cosLcosλ+e_*j2cosLsinλ+e_*j3sinL

$c_{j1}=(R_n+h)[-{\stackrel{\&CenterDot;}{e}}_{*j1}\sin L\cos \mathrm{\&lambda;}-{\stackrel{\&CenterDot;}{e}}_{*j2}\sin L\sin \mathrm{\&lambda;}]+[R_n*(1-e^2)+h]{\stackrel{\&CenterDot;}{e}}_{*j3}\cos L$

$c_{j2}=(R_n+h)[{\stackrel{\&CenterDot;}{e}}_{*j2}\cos L\cos \mathrm{\&lambda;}-{\stackrel{\&CenterDot;}{e}}_{*j1}\cos L\sin \mathrm{\&lambda;}]$

$c_{j3}={\stackrel{\&CenterDot;}{e}}_{*j1}\cos L\cos \mathrm{\&lambda;}+{\stackrel{\&CenterDot;}{e}}_{*j2}\cos L\sin \mathrm{\&lambda;}+{\stackrel{\&CenterDot;}{e}}_{*j3}\sin L$

the comprehensive measurement equations (a) and (b) are as follows:

$Z\left(t\right)=\left[\begin{array}{c}{H}_{\mathrm{\&rho;}}\\ H_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_{\mathrm{\&rho;}}\left(t\right)\\ V_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\left(t\right)\end{array}\right]$

(5) and (5) outputting an optimal combination result after the compensation is finished. And (4) returning to continue to execute the step (1).

Claims (9)

1. A multi-satellite system and strapdown inertial navigation system tightly combined navigation method is characterized by comprising the following steps:

(1) obtaining the current attitude, speed and position of strapdown inertial navigation by strapdown resolving the data output by an Inertial Measurement Unit (IMU), and carrying out satellite data processing on ephemeris messages output by a satellite system receiver to obtain the speed and position information of a satellite;

(2) calculating SINS pseudo range and pseudo range rate by using satellite speed and position and SINS position and speed information;

(3) obtaining a code measurement pseudo range and Doppler frequency shift from a pseudo range message output by a satellite system receiver, comparing the code measurement pseudo range and the Doppler frequency shift with an SINS pseudo range and a pseudo range rate, establishing a tight combination state model and a measurement model by taking a difference value as an observed quantity of a filter, and outputting an error correction value of the system through optimal estimation and filtering; correcting the system error, and outputting the optimal combined solution of the SINS attitude, speed and position; the method for establishing the tight combination state model and the measurement model comprises the following steps:

(3.1) establishment of tightly-combined state equation of multiple systems

Converting strapdown error equation and satellite system modeling into state equation description

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{X}}_I\left(t\right)\\ {\stackrel{\&CenterDot;}{X}}_{GPS}\left(t\right)\\ {\stackrel{\&CenterDot;}{X}}_{GLO}\left(t\right)\\ {\stackrel{\&CenterDot;}{X}}_{Gal}\left(t\right)\\ {\stackrel{\&CenterDot;}{X}}_{BD}\left(t\right)\end{array}\right]=\left[\begin{array}{ccccc}{F}_I\left(t\right)& 0& 0& 0& 0\\ 0& F_{GPS}\left(t\right)& 0& 0& 0\\ 0& 0& F_{GLO}\left(t\right)& 0& 0\\ 0& 0& 0& F_{Gal}\left(t\right)& 0\\ 0& 0& 0& 0& F_{BD}\left(t\right)\end{array}\right]\left[\begin{array}{c}{X}_I\left(t\right)\\ X_{GPS}\left(t\right)\\ X_{GLO}\left(t\right)\\ X_{Gal}\left(t\right)\\ X_{BD}\left(t\right)\end{array}\right]+\left[\begin{array}{ccccc}{G}_I\left(t\right)& 0& 0& 0& 0\\ 0& G_{GPS}\left(t\right)& 0& 0& 0\\ 0& 0& G_{GLO}\left(t\right)& 0& 0\\ 0& 0& 0& G_{Gal}\left(t\right)& 0\\ 0& 0& 0& 0& F_{BD}\left(t\right)\end{array}\right]\left[\begin{array}{c}{W}_I\left(t\right)\\ W_{GPS}\left(t\right)\\ W_{GLO}\left(t\right)\\ W_{Gal}\left(t\right)\\ W_{BD}\left(t\right)\end{array}\right]$

Wherein, X ∈ R²³Is a state variable, which consists of 15 state variables of SINS, 2 state variables of a United states Global Positioning System (GPS), a Glonass satellite system (GLONASS), a Galileo satellite system (Galileo) and a Beidou satellite system (BD) respectively;

$\begin{array}{c}X=\\ \left[\begin{array}{ccccccccccccccccccccccc}{\mathrm{\&delta;V}}_E& {\mathrm{\&delta;V}}_N& {\mathrm{\&delta;V}}_U& {\mathrm{\&phi;}}_E& {\mathrm{\&phi;}}_N& {\mathrm{\&phi;}}_U& \mathrm{\&delta;}L& \mathrm{\&delta;}\mathrm{\&lambda;}& \mathrm{\&delta;}h& {\&dtri;}_{bx}& {\&dtri;}_{by}& {\&dtri;}_{bz}& {\mathrm{\&epsiv;}}_{bx}& {\mathrm{\&epsiv;}}_{by}& {\mathrm{\&epsiv;}}_{bz}& {\mathrm{\&delta;t}}_{GPS}& {\mathrm{\&delta;t}}_{GLO}& {\mathrm{\&delta;t}}_{Gal}& {\mathrm{\&delta;t}}_{BD}& {\mathrm{\&delta;t}}_{rGPS}& {\mathrm{\&delta;t}}_{rGLO}& {\mathrm{\&delta;t}}_{rGal}& {\mathrm{\&delta;t}}_{rBD}\end{array}\right]\end{array}$

in the formula, V_E,V_N,V_UAre the speed errors in the three directions on the northeast,three platform attitude angles of the strapdown, L, lambda and h are three position errors of the strapdown and are described by an earth coordinate system,the offset errors in the three axial directions are added,_bx,_by,_bzis the three axial drifts of the gyro, t_GPS、t_GLO、t_Gal、t_BDThe equivalent distances of the clock errors of the GPS, GLONASS, Galileo and BD satellites, t_rGPS、t_rGLO、t_rGal、t_rBDThe satellite clock frequency error equivalent distance change rates of the GPS, the GLONASS, the Galileo and the BD are 23-dimensional in total; f_I(t) is the state matrix of the inertial navigation system error equation of state, and F_GPS(t)、F_GLO(t)、F_Gal(t)、F_BD(t) is a state matrix obtained by modeling two state quantities related to GPS, GLONASS, Galileo and BD satellites respectively, G_I(t) and G_GPS(t)、G_GLO(t)、G_Gal(t)、G_BD(t) is an input matrix for noise; t is t_*The representation is modeled with its excitation as white noise,modeling as a first order Markov process, representing the respective systems GPS, GLONASS, Galileo, BD

$F_{*}=\left[\begin{array}{cc}0& 1\\ 0& -{\mathrm{\&beta;}}_{tr*}\end{array}\right],G_{*}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],W_{*}\left(t\right)=\left[\begin{array}{c}{\mathrm{\&omega;}}_{t*}\\ {\mathrm{\&omega;}}_{tr*}\end{array}\right];$

(3.2) establishment of multisystem tight combination measurement equation

(a) Pseudo-range measurement equation establishment

t_*Is a station clock error, which represents the equivalent distance error of the station clock error, v_ρ*jRepresenting other common errors and unique errors, such as ionosphere errors, troposphere errors, multipath errors and other measurement errors; in tight composite simulation, v_ρ*jConsidering as an error in the form of white noise, the clock error is uniformly set by a plurality of channels; in the tight combination prototype experiment, many errors such as ionospheric delay and the like must be compensated, and the compensation method is discussed below;

taking satellites and writing them in matrix form

$\left[\begin{array}{c}{\mathrm{\&delta;\&rho;}}_{GPS}\\ {\mathrm{\&delta;\&rho;}}_{GLO}\\ {\mathrm{\&delta;\&rho;}}_{Gal}\\ {\mathrm{\&delta;\&rho;}}_{BD}\end{array}\right]=\left[\begin{array}{ccccc}{H}_{GPS}& 1_{GPS}& 0& 0& 0\\ H_{GLO}& 0& 1_{GLO}& 0& 0\\ H_{Gal}& 0& 0& 1_{Gal}& 0\\ H_{BD}& 0& 0& 0& 1_{BD}\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;}p\\ {\mathrm{\&delta;t}}_{GPS}\\ {\mathrm{\&delta;t}}_{GLO}\\ {\mathrm{\&delta;t}}_{Gal}\\ {\mathrm{\&delta;t}}_{BD}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\&upsi;}}_{\mathrm{\&rho;}GPS}\\ {\mathrm{\&upsi;}}_{\mathrm{\&rho;}GLO}\\ {\mathrm{\&upsi;}}_{\mathrm{\&rho;}Gal}\\ {\mathrm{\&upsi;}}_{\mathrm{\&rho;}BD}\end{array}\right]$

Wherein p ═ x y z]^TIs a user state quantity, H_*Is a geometric observation matrix; the dimension of which is 2 in relation to the number (n) of satellites of the selected system; h_*＝[e₁e₂e₃]_2*3，e_kIs 2 x 1 dimension; measurement equation of pseudo-range observed quantity

Z_ρ(t)＝H_ρ(t)X(t)+V_ρ(t)

Wherein H_ρ(t)＝[0_N*30_N*3H_ρ10_N*6H_ρ20_N*3]_N*23N is 8, the total number of satellites;

H_ρ＝(a_ji)_N*3，

and is

$H_{\mathrm{\&rho;}GPS}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right],H_{\mathrm{\&rho;}GLO}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right],H_{\mathrm{\&rho;}Gal}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right],H_{\mathrm{\&rho;}BD}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right]$

a_j1＝(R_n+h)[-e_j1sinLcosλ-e_j2sinLsinλ]＋[R_n*(1-e²)＋h]e_j3cosL

a_j2＝(R_n＋h)[e_j2cosLcosλ-e_j1cosLsinλ]

a_j3＝e_j1cosLcosλ+e_j2cosLsinλ+e_j3sinL

(b) Establishment of pseudo-range rate measurement equation

Measurement equation of pseudo-range rate observed quantity

$Z_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\left(t\right)=H_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\left(t\right)X\left(t\right)+V_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\left(t\right)$

In the formula,

wherein,

b_j1＝-e_*j1sinλ+e_*j2cosλ

b_j2＝-e_*j2sinLcosλ-e_*j1sinLsinλ+e_*j3cosL

b_j3＝e_*j1cosLcosλ+e_*j2cosLsinλ+e_*j3sinL

$e_{j1}=(R_n+h)\&lsqb;-{\stackrel{\&CenterDot;}{e}}_{*j1}\sin Lcos\mathrm{\&lambda;}-{\stackrel{\&CenterDot;}{e}}_{*j2}\sin Lsin\mathrm{\&lambda;}\&rsqb;+\&lsqb;R_n*(1-e^2)+h\&rsqb;{\stackrel{\&CenterDot;}{e}}_{*j3}\cos L$

$c_{j2}=(R_n+h)\&lsqb;{\stackrel{\&CenterDot;}{e}}_{*j2}\cos Lcos\mathrm{\&lambda;}-{\stackrel{\&CenterDot;}{e}}_{*j1}\cos Lsin\mathrm{\&lambda;}\&rsqb;$

$c_{j3}={\stackrel{\&CenterDot;}{e}}_{*j1}\cos Lcos\mathrm{\&lambda;}+{\stackrel{\&CenterDot;}{e}}_{*j2}\cos Lsin\mathrm{\&lambda;}+{\stackrel{\&CenterDot;}{e}}_{*j3}\sin L$

the comprehensive measurement equations (a) and (b) are as follows:

$Z\left(t\right)=\left[\begin{array}{c}{H}_{\mathrm{\&rho;}}\\ H_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_{\mathrm{\&rho;}}\left(t\right)\\ V_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\left(t\right)\end{array}\right].$

2. the method of claim 1, wherein the method comprises: the strapdown calculation is autonomous inferred navigation which utilizes an IMU to measure angular motion and linear motion parameters of a carrier and determines the attitude, the speed and the position of a carrier provided with the SINS by recursion on the basis of an initial state.

3. The method of claim 1, wherein the method comprises: the satellite data processing comprises satellite selection, satellite ephemeris resolving and satellite system data fusion.

4. The method as claimed in claim 1, wherein the method comprises: the calculation process of the SINS pseudo range and the pseudo range rate is to calculate the pseudo range corresponding to the position of the SINS according to the position of the SINS and the position of a satellite; the change rate of the pseudo range of the SINS relative to the satellite relative motion, namely the SINS, is obtained according to the velocity of the SINS and the velocity of the satellite.

5. The method as claimed in claim 1, wherein the method comprises: the optimal estimation and filtering adopt a Kalman filter.

6. The method of claim 3, wherein the method comprises: the satellite selection is that a proper satellite is selected for navigation resolving, and the specific process is as follows: firstly, judging the number of satellites of each satellite system; if the number of satellites of a certain satellite system is less than two, the system is excluded; secondly, if the single satellite and SINS combined navigation is adopted, 4-satellite combination is selected; if a double-constellation system and an SINS are combined, the unknown number of synchronous errors of the satellite system is increased, and at least 5 satellites are needed for navigation calculation, namely a 3+2 mode; if three satellite systems are combined with the SINS, 6 satellites are needed to perform positioning calculation, namely a 2+2+2 mode; if four satellite systems are combined, 8 satellites are needed to perform positioning calculation, namely a 2+2+2+2 mode; and finally, selecting the satellites according to a satellite selection algorithm of a maximum vector polyhedron volume method or a least square method on the premise that the total number of the selected satellites and the number of each satellite system are met.

7. The method as claimed in claim 3, wherein the method comprises: the satellite ephemeris resolving algorithm is used for judging the format of ephemeris messages output by a satellite system and analyzing ephemeris according to the formats of the ephemeris messages of different satellite systems; the satellite system ephemeris is divided into two types according to the format: a broadcast ephemeris parameter format similar to GPS and a broadcast ephemeris parameter format similar to GLONASS; the message analysis of the broadcast ephemeris parameter format of the GPS-like adopts a satellite position algorithm; message analysis of broadcast ephemeris parameter format similar to GLONASS adopts a derivation motion equation integral operation method.

8. The method as claimed in claim 3, wherein the method comprises: the satellite system data fusion is specifically to select a main satellite system, and other auxiliary satellite systems perform time and coordinate conversion according to the reference time and the coordinate system of the main satellite system.

9. The method as claimed in claim 6, wherein the method comprises: when the 3+2 mode is that the double constellations are combined with the SINS, one satellite system selects 3 satellites and the other system selects 2 satellites; the 2+2+2 mode refers to that two satellites are respectively selected by three satellite systems; the 2+2+2+2 mode is that two satellites are respectively selected for four satellite systems.