CN102359786B - Initial alignment method on the basis of hypersphere sampling - Google Patents

Abstract

The invention discloses an initial alignment method on the basis of hypersphere sampling, which comprises the following steps of: step 1: establishing a nonlinearity state equation of the initial alignment of a SINS stationary base; step 2: establishing a strong tracking unscented filter method on the basis of hypersphere sampling to obtain a state variable estimated value; and step 3: collecting the inertia output information of a track generator by a navigational computer and completing an alignment process. By adopting a hypersphere sampling strategy, the initial alignment method has the advantages of reducing the amount of sampling points and the calculated amount as well as the sampling time; and according to the initial alignment method, the strong tracking filter and the UKF (unscented Kalman filter) are combined and applied to the initial alignment of the strapdown inertial navigation stationary base, thereby solving the problem of reduction of filtering accuracy caused by system and uncertainty noise.

Description

A kind of initial alignment method based on the suprasphere sampling

Technical field

The present invention relates to a kind of initial alignment method, belong to field of navigation technology based on the suprasphere sampling.

Background technology

Initial alignment is to realize the high-precision gordian technique of inertial navigation, the precision of the accuracy affects inertial navigation system of initial alignment, and the aligning time also is the important tactics index of reflection armament systems quick-reaction capability (QRC).Traditional initial alignment is based on linear initial alignment error equation, introduces extraneous metrical information, utilizes Kalman filtering algorithm that the attitude error angle is estimated.Because being an actual problem, initial alignment determined the difficulty in practical operation.As:

(1) actual initial alignment process is a non-linear process, corresponding error equation also should be nonlinear model, its linear model is under the prerequisite of error angle for a small amount of nonlinear model to be carried out linearization to obtain, when the attitude misalignment becomes big, to cause linear model no longer suitable, cause the Filtering Estimation precise decreasing.

(2) in actual applications when under the noise statistics situation of out of true or mistake, the stability decreases of Kalman filtering, speed of convergence is slack-off, even causes filtering divergence, at this moment just needs the stronger filtering algorithm of design tracking power at nonlinear model.

(3) nonlinear filtering need transmit nonlinear funtcional relationship by sampled point, traditional UT (no mark) conversion needs the individual sampled point of 2n+1 (n is the state variable number), in order to reduce calculated amount, under the condition that guarantees filtering accuracy, utilize the suprasphere sampling policy, reduce sampled point, reduced the calculated amount of sampling section.

UKF (no mark filtering) calculated amount in the quiet pedestal Initial Alignment of Large Azimuth Misalignment On of SINS (strapdown inertial navigation system) is big, and under the noise statistics situation of out of true or mistake, speed of convergence is slack-off, and estimated accuracy descends, even filtering divergence.

Summary of the invention

The objective of the invention is in order to address the above problem, propose a kind ofly to be used for the quiet pedestal initial alignment of SINS based on suprasphere sampling and STFUKF (the no mark filtering of strong tracking) method, this initial alignment method adopts the suprasphere sampling to combine with strong no mark filtering (STFUKF) method of following the tracks of, utilize SINS nonlinearity erron model, by the Filtering Estimation information that does well, thereby obtain initial attitude matrix.

A kind of initial alignment method based on the suprasphere sampling of the present invention comprises following step:

Step 1 is set up the nonlinear state equation of the quiet pedestal initial alignment of SINS;

Step 2 is set up based on the strong tracking of suprasphere sampling and is not had the mark filtering method, obtains the state variable estimated value;

Step 3, by navigational computer, the inertia device output information of acquisition trajectories generator, and finish alignment procedures.

The invention has the advantages that:

(1) utilizes the sampling policy of suprasphere, reduced the quantity of sampled point, reduced calculated amount, reduced the time of sampling section;

(2) strong tracking filter is combined with UKF be applied in the quiet pedestal initial alignment of inertial navigation, the problem that the filtering accuracy that resolution system and noise uncertainty are brought descends;

(3) nonlinear model that is adopted is not subjected to the restriction of course misalignment size, and usable range is more extensive;

(4) suprasphere sampling is combined with strong tracking no mark filtering (STFUKF) method, have precision height, anti-interference is good, tracking power is strong characteristics.

Description of drawings

Fig. 1 is a method flow diagram of the present invention;

Fig. 2 is to be 300s at simulation time, the Φ of three kinds of methods _xThe error angle line of writing music.

Fig. 3 is to be 300s at simulation time, the Φ of three kinds of methods _yThe error angle line of writing music.

Fig. 4 is to be 300s at simulation time, the Φ of three kinds of methods _zThe error angle line of writing music.

Fig. 5 is to be 1000s at simulation time, the Φ of three kinds of methods _xThe error angle line of writing music.

Fig. 6 is to be 1000s at simulation time, the Φ of three kinds of methods _yThe error angle line of writing music.

Fig. 7 is to be 1000s at simulation time, the Φ of three kinds of methods _zThe error angle line of writing music.

Fig. 8 is to be 1000s at simulation time, adds the Φ after improving one's methods _xThe error angle line of writing music.

Fig. 9 is to be 1000s at simulation time, adds the Φ after improving one's methods _yThe error angle line of writing music.

Figure 10 is to be 1000s at simulation time, adds the Φ after improving one's methods _zThe error angle line of writing music.

Embodiment

The present invention is described in further detail below in conjunction with accompanying drawing.

A kind of initial alignment method of the present invention based on the suprasphere sampling, flow process comprises following step as shown in Figure 1:

Step 1 is set up the nonlinear state equation of the quiet pedestal initial alignment of SINS;

Among the present invention, geocentric inertial coordinate system (i represents) is x _iy _iz _iNavigation coordinate system elects Department of Geography's (t represents) as, and Department of Geography's (t represents) is x _ty _tz _tCalculating Department of Geography's (c represents) is x _cy _cz _c

Suppose that the attitude error angle between calculating Department of Geography and the actual Department of Geography is respectively the lateral error angle Φ of horizontal both direction _x, Φ _yWith on the vertical direction azimuthal error angle Φ _z, then geography is tied to and calculates Department of Geography and realize by following rotating manner:

T system is around z _tAxle rotates Φ _z, obtain t ₁(the t of system ₁Coordinate system); t ₁System around

Axle rotates Φ _x, obtain t ₂(the t of system ₂Coordinate system); t ₂System around

Axle rotates Φ _y, obtain c system (c coordinate system).Because lateral error angle Φ _x, Φ _yAll in 1 °, can regard it as in a small amount, then sin Φ _x≈ Φ _x, sin Φ _y≈ Φ _y, cos Ф _x=cos Φ _y≈ 1, and the relation between t system and the c system is with following matrix representation:

$C_t^c=C_{t_2}^c{C}_{t_1}^{t_2}{\mathrm{<figure-callout\; id="1"\; label="C\; t\; t"\; filenames="HDA0000076767120000021.png,HDA0000076767120000051.png"\; state="\{\{state\}\}">C</figure-callout>}}_{t{t}_{}}^{{}_1}$

$=\left[\begin{array}{ccc}\cos {\mathrm{\&Phi;}}_y& 0& -\sin {\mathrm{\&Phi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA0000076767120000012.png,HDA0000076767120000021.png"\; state="\{\{state\}\}">y</figure-callout>}}\\ 0& 1& 0\\ \sin {\mathrm{\&Phi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA0000076767120000012.png,HDA0000076767120000021.png"\; state="\{\{state\}\}">y</figure-callout>}}& 0& \cos {\mathrm{\&Phi;}}_{\mathrm{<figure-callout\; id="1"\; label="y"\; filenames="HDA0000076767120000021.png,HDA0000076767120000051.png"\; state="\{\{state\}\}">y</figure-callout>}}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\&Phi;}}_x& \sin {\mathrm{\&Phi;}}_x\\ 0& -\sin {\mathrm{\&Phi;}}_x& \cos {\mathrm{\&Phi;}}_x\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\&Phi;}}_z& \sin {\mathrm{\&Phi;}}_z& 0\\ -\sin {\mathrm{\&Phi;}}_z& \cos {\mathrm{\&Phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA0000076767120000012.png,HDA0000076767120000021.png"\; state="\{\{state\}\}">z</figure-callout>}}& 0\\ 0& 0& 1\end{array}\right]$

Wherein:

Expression is tied to the transition matrix that c is by t,

Expression is by t ₂Be tied to the transition matrix of c system,

Expression is by t ₁Be tied to t ₂The transition matrix of system,

Expression is tied to t by t ₁The transition matrix of system is because sin Φ _x≈ Φ _x, sin Φ _y≈ Φ _y, cos Ф _x=cos Φ _y≈ 1,

$C_t^c=\left[\begin{array}{ccc}\cos {\mathrm{\&Phi;}}_z& \sin {\mathrm{\&Phi;}}_z& -{\mathrm{\&Phi;}}_y\\ -{\sin \mathrm{\&Phi;}}_z& \cos {\mathrm{\&Phi;}}_z& {\mathrm{\&Phi;}}_x\\ \cos {\mathrm{\&Phi;}}_z+{\mathrm{\&Phi;}}_x\sin {\mathrm{\&Phi;}}_z& {\mathrm{\&Phi;}}_y\sin {\mathrm{\&Phi;}}_z-{\mathrm{\&Phi;}}_x\cos {\mathrm{\&Phi;}}_{\mathrm{<figure-callout\; id="1"\; label="z"\; filenames="HDA0000076767120000021.png,HDA0000076767120000051.png"\; state="\{\{state\}\}">z</figure-callout>}}& 1\end{array}\right]$

Attitude error angle Φ _t=(Φ _x, Φ _y, Φ _z) ^T, get by the attitude differential equation:

${\stackrel{\&CenterDot;}{C}}_t^c=-{\mathrm{\&Omega;}}_{\mathrm{tc}}\&CenterDot;C_t^c$

Wherein, Ω _Tc=Ω _Ic-Ω _It, Ω _ItBe the angular velocity omega of coordinate system t with respect to coordinate system i _ItAntisymmetric matrix, Ω _IcBe the angular velocity omega of coordinate system c with respect to coordinate system i _ItAntisymmetric matrix, Ω _TcBe the angular velocity omega of coordinate system i with respect to coordinate system t _ItAntisymmetric matrix, obtain:

${\stackrel{\&CenterDot;}{C}}_t^c=-{\mathrm{\&Omega;}}_{\mathrm{ic}}\&CenterDot;C_t^c+C_t^c\&CenterDot;{\mathrm{\&Omega;}}_{\mathrm{it}}$

Wherein: ${\mathrm{\&Omega;}}_{\mathrm{it}}=\left[\begin{array}{ccc}0& -{\mathrm{\&omega;}}_{\mathrm{iz}}& {\mathrm{\&omega;}}_{\mathrm{iy}}\\ {\mathrm{\&omega;}}_{\mathrm{iz}}& 0& -{\mathrm{\&omega;}}_{\mathrm{ix}}\\ -{\mathrm{\&omega;}}_{\mathrm{iy}}& {\mathrm{\&omega;}}_{\mathrm{<figure-callout\; id="0"\; label="ix"\; filenames="HDA0000076767120000012.png,HDA0000076767120000021.png"\; state="\{\{state\}\}">ix</figure-callout>}}& 0\end{array}\right],$ ω _IxThe expression angular velocity omega _ItAt the component of x axle, ω _IyThe expression angular velocity omega _ItAt the component of y axle, ω _IzThe expression angular velocity omega _ItComponent at the z axle.

${\mathrm{\&omega;}}_{\mathrm{it}}={({\mathrm{\&omega;}}_{\mathrm{ix}},{\mathrm{\&omega;}}_{\mathrm{iy}},{\mathrm{\&omega;}}_{\mathrm{iz}})}^T={(-\stackrel{\&CenterDot;}{L},({\mathrm{\&omega;}}_{\mathrm{ie}}+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\cos L,({\mathrm{\&omega;}}_{\mathrm{ie}}+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\sin L)}^T$

In the formula, λ is local longitude, and L is local latitude, ω _IeIt is the earth rotation angular speed.

Work as Φ _x, Φ _yIn the time of all in 1 °, obtain:

${\stackrel{\&CenterDot;}{\mathrm{\&Phi;}}}_t={\mathrm{\&omega;}}_{\mathrm{tc}}={({\mathrm{\&omega;}}_{\mathrm{tx}},{\mathrm{\&omega;}}_{\mathrm{ty}},{\mathrm{\&omega;}}_{\mathrm{tz}})}^T$

Wherein: ω _TcDenotation coordination is the angular velocity of c with respect to coordinate system t, ω _TxThe expression angular velocity omega _TcAt the component of x axle, ω _TyThe expression angular velocity omega _TcAt the component of y axle, ω _TzThe expression angular velocity omega _TcAt the component of z axle, again because: According to the corresponding relation of vector and antisymmetric matrix thereof, following formula can be write as:

${\mathrm{\&omega;}}_{\mathrm{tc}}={\mathrm{\&omega;}}_{\mathrm{ic}}-C_t^c{\mathrm{\&omega;}}_{\mathrm{it}}$

Wherein, ω _Ic=ω _It+ δ ω _It+ ε, ε are the gyrostatic projections that drifts in Department of Geography, δ ω _ItBe in SINS, to calculate ω _ItThe time error.In sum, obtain nonlinear aligning equation under the quiet pedestal of SINS.

${\stackrel{\&CenterDot;}{\mathrm{\&Phi;}}}_t={\mathrm{\&omega;}}_{\mathrm{tc}}={\mathrm{\&omega;}}_{\mathrm{it}}+\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{it}}+\mathrm{\&epsiv;}-C_t^c{\mathrm{\&omega;}}_{\mathrm{it}}=(I-C_t^c){\mathrm{\&omega;}}_{\mathrm{it}}+\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{it}}+\mathrm{\&epsiv;}$

Wherein: I represents 3 rank unit matrixs;

Under quiet pedestal, ω _It=(0, ω _IeCosL, ω _IeSinL) ^T, R is an earth radius.Can obtain:

${\stackrel{\&CenterDot;}{\mathrm{\&Phi;}}}_x=-\sin {\mathrm{\&Phi;}}_z{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+{\mathrm{\&Phi;}}_y{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L-\mathrm{\&Delta;}{V}_y/R+{\mathrm{\&epsiv;}}_x$

${\stackrel{\&CenterDot;}{\mathrm{\&Phi;}}}_y=(1-\cos {\mathrm{\&Phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L-{\mathrm{\&Phi;}}_x{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\mathrm{\&Delta;}{V}_x/R+{\mathrm{\&epsiv;}}_y$

${\stackrel{\&CenterDot;}{\mathrm{\&Phi;}}}_z=(-{\mathrm{\&Phi;}}_y\sin {\mathrm{\&Phi;}}_z+{\mathrm{\&Phi;}}_x\cos {\mathrm{\&Phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\mathrm{\&Delta;}{V}_x\mathrm{tgL}/R+{\mathrm{\&epsiv;}}_z$

Wherein:, ε _x, ε _y, ε _zBe respectively the projection components of ε, Δ V at three change in coordinate axis direction _x, Δ V _y, Δ V _zBe the projection of velocity error in Department of Geography.

By the specific force equation:

${\stackrel{\&CenterDot;}{V}}_t=a_t-\mathrm{\&Delta;}{n}_t+g_t$

Wherein, V _tThe speed of-carrier is at the projection of t system, a _t-specific force is at the projection of t system, Δ n _t-harmful acceleration is at the projection of t system, g _t=[0 0-g] ^TBe acceleration of gravity.

The rate equation that is calculated by inertial navigation is:

${\stackrel{\stackrel{\&CenterDot;}{\&OverBar;}}{V}}_t={\stackrel{\&OverBar;}{a}}_t-\mathrm{\&Delta;}{\stackrel{\&OverBar;}{n}}_t+g_t$

And the reading of accelerometer is: ${\stackrel{\&OverBar;}{a}}_t=a_c+{\&dtri;}_c=C_t^c{a}_t+{\&dtri;}_c.$

With the following formula premultiplication Be out of shape: $a_t=C_c^t{\stackrel{\&OverBar;}{a}}_t-{\&dtri;}_t$

Wherein, a _cBe the projection of specific force in c system,

It is the projection that the acceleration calculation system deviation is fastened in corresponding coordinate.

By on can get: $\mathrm{\&Delta;}{\stackrel{\&CenterDot;}{V}}_t={\stackrel{\stackrel{\&CenterDot;}{\&OverBar;}}{V}}_t-{\stackrel{\&CenterDot;}{V}}_t={\stackrel{\&OverBar;}{a}}_t-C_c^t{\stackrel{\&OverBar;}{a}}_t+{\&dtri;}_t-(\mathrm{\&Delta;}{\stackrel{\&OverBar;}{n}}_t-\mathrm{\&Delta;}{n}_t)$

Wherein, $\mathrm{\&Delta;}{\stackrel{\&OverBar;}{n}}_t-\mathrm{\&Delta;}{n}_t=(2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L\&CenterDot;\mathrm{\&Delta;}{V}_z-2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L\&CenterDot;\mathrm{\&Delta;}{V}_y,2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L\&CenterDot;\mathrm{\&Delta;}{V}_x,2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L\&CenterDot;\mathrm{\&Delta;}{V}_x)$

L is local latitude, ω _IeBe rotational-angular velocity of the earth, Δ V _x, Δ V _y, Δ V _zBe the projection of velocity error in Department of Geography.

Orthogonality according to transition matrix has: $\mathrm{\&Delta;}{C}_c^t={\left(\mathrm{\&Delta;}{C}_t^c\right)}^T={(C_c^t-I)}^T$

So, when as do not consider Δ V _z, the velocity error equation is:

$\mathrm{\&Delta;}{\stackrel{\&CenterDot;}{V}}_x=-(\cos {\mathrm{\&Phi;}}_z-1){\stackrel{\&OverBar;}{n}}_x+\sin {\mathrm{\&Phi;}}_z{\stackrel{\&OverBar;}{n}}_y-g({\mathrm{\&Phi;}}_y\cos {\mathrm{\&Phi;}}_z+{\mathrm{\&Phi;}}_x\sin {\mathrm{\&Phi;}}_z)+2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin \mathrm{L\&Delta;}{V}_y+{\&dtri;}_x$

$\mathrm{\&Delta;}{\stackrel{\&CenterDot;}{V}}_y=-{\sin \mathrm{\&Phi;}}_z{\stackrel{\&OverBar;}{n}}_x-(\cos {\mathrm{\&Phi;}}_z-1){\stackrel{\&OverBar;}{n}}_y+g(-{\mathrm{\&Phi;}}_y\cos {\mathrm{\&Phi;}}_z+{\mathrm{\&Phi;}}_x\sin {\mathrm{\&Phi;}}_z)-2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin \mathrm{L\&Delta;}{V}_x+{\&dtri;}_y$

As azimuthal error angle Φ _zDuring greater than 1 °, the nonlinear state equation of the quiet pedestal initial alignment of SINS is:

$\stackrel{\&CenterDot;}{X}=f\left(X\right)+\mathrm{BW}$

$f\left(X\right)=\left[\begin{array}{c}-(\cos {\mathrm{\&Phi;}}_z-1){\stackrel{\&OverBar;}{n}}_x+\sin {\mathrm{\&Phi;}}_z{\stackrel{\&OverBar;}{n}}_y-g({\mathrm{\&Phi;}}_y\cos {\mathrm{\&Phi;}}_z+{\mathrm{\&Phi;}}_x\sin {\mathrm{\&Phi;}}_z)+2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin \mathrm{L\&Delta;}{V}_y+\&dtri;x\\ -\sin {\mathrm{\&Phi;}}_z{\stackrel{\&OverBar;}{n}}_x-(\cos {\mathrm{\&Phi;}}_z-1){\stackrel{\&OverBar;}{n}}_y+g(-{\mathrm{\&Phi;}}_y\cos {\mathrm{\&Phi;}}_z+{\mathrm{\&Phi;}}_x\sin {\mathrm{\&Phi;}}_z)-2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin \mathrm{L\&Delta;}{V}_x+{\&dtri;}_y\\ -\sin {\mathrm{\&Phi;}}_z{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+{\mathrm{\&Phi;}}_y{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L-\mathrm{\&Delta;}{V}_y/R+{\mathrm{\&epsiv;}}_x\\ (1-\cos {\mathrm{\&Phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L-{\mathrm{\&Phi;}}_x{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\mathrm{\&Delta;}{V}_x/R+{\mathrm{\&epsiv;}}_y\\ (-{\mathrm{\&Phi;}}_y\sin {\mathrm{\&Phi;}}_z+{\mathrm{\&Phi;}}_x\cos {\mathrm{\&Phi;}}_z){\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\mathrm{\&Delta;}{V}_x\mathrm{tgL}/R+{\mathrm{\&epsiv;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA0000076767120000012.png,HDA0000076767120000021.png"\; state="\{\{state\}\}">z</figure-callout>}}\\ 0_{5\&times;1}\end{array}\right]$

$B=\left[\begin{array}{c}{I}_{5\&times;5}\\ 0_{5\&times;5}\end{array}\right]$

Wherein, state variable $X{=\left[\begin{array}{cccccccccc}\mathrm{\&Delta;}{V}_x& \mathrm{\&Delta;}{V}_y& {\mathrm{\&Phi;}}_x& {\mathrm{\&Phi;}}_y& {\mathrm{\&Phi;}}_z& {\&dtri;}_x& {\&dtri;}_y& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z\end{array}\right]}^T,$

W=[Δ α _xΔ α _yΔ ω _xΔ ω _yΔ ω _z] ^TBe 5 * 1 process noise matrixes,

Be accelerometer drift, ε _x, ε _y, ε _zBe gyrostatic drift, Δ α _x, Δ α _yBe the white noise of accelerometer error, Δ ω _x, Δ ω _y, Δ ω _zBe the white noise of gyroscopic drift, ω _IeBe rotational-angular velocity of the earth, L is local geographic latitude, and g is a local gravitational acceleration.

In quiet pedestal initial alignment, get the output of accelerometer on horizontal both direction as observed quantity Z, can obtain measurement equation:

Z＝HX+η

Wherein, Z is observed quantity, $H=\left[\begin{array}{cccc}{0}_{2\&times;2}& \begin{array}{ccc}0& -g& 0\\ \mathrm{<figure-callout\; id="0"\; label="g"\; filenames="HDA0000076767120000012.png,HDA0000076767120000021.png"\; state="\{\{state\}\}">g</figure-callout>}& 0& 0\end{array}& I_{2\&times;2}& 0_{2\&times;3}\end{array}\right],$ η is 2 * 1 observation noise matrixes.

Step 2 is set up based on the strong tracking of suprasphere sampling and is not had the mark filtering method, obtains the state variable estimated value;

Based on suprasphere sampling and STFUKF algorithm, the present invention proposes suprasphere sampling STFUKF filtering algorithm, to solve standard UKF calculated amount greatly and in the filtering problem of unstable appears.

Strong tracking based on the suprasphere sampling does not have the mark filtering method, specifically comprises following step:

1) state variable initialization;

State variable in the nonlinear state equation of the quiet pedestal initial alignment of SINS that step 1 is obtained is carried out initialization, and Δ V is set _x, Δ V _y, Φ _x, Φ _y, Φ _z,

ε _x, ε _y, ε _zInitial value, and set the average of X With mean square deviation P ₀, average

Be the column vector of 10 row, 1 row, mean square deviation P ₀Be the diagonal matrix of 10 row, 10 row, be specially:

${\hat{x}}_0=E\left(x_0\right)$

${\mathrm{<figure-callout\; id="0"\; label="P"\; filenames="HDA0000076767120000012.png,HDA0000076767120000021.png"\; state="\{\{state\}\}">P</figure-callout>}}_0=E\left((x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right)$

Wherein: x ₀The initial value of expression state variable X.

2) obtain strong tracking and do not have the sampled point that mark filtering needs;

Adopt the suprasphere method of sampling to obtain the weights W of n+2 Sigma sampled point and Sigma sampled point, n represents the dimension of state variable, i.e. totally 12 Sigma sampled points are specially:

(1) determines constant W at random ₀, 0≤W ₀≤ 1.

(2) obtain the weights of 12 Sigma sampled points, the weights W of each Sigma sampled point is:

$W=\frac{1-W_0}{n+1}$

The 12nd Sigma sampled point when (3) obtaining 10 dimension state variables is specially:

At first obtain state variable dimension j=1, during i=j+2=3, three Sigma sampled points:

${\mathrm{\&chi;}}_3^1=\left[\begin{array}{ccc}0& -\frac{1}{\sqrt{2\mathrm{<figure-callout\; id="1"\; label="W"\; filenames="HDA0000076767120000021.png,HDA0000076767120000051.png"\; state="\{\{state\}\}">W</figure-callout>}}}& \frac{1}{\sqrt{2W}}\end{array}\right]$

Wherein:

The 0th Sigma sampled point during expression 1 dimension,

The 1st Sigma sampled point during expression 1 dimension,

The 2nd Sigma sampled point during expression 1 dimension.

By, 1 dimension state variable is carried out recursion calculating, when state variable dimension j=2,3 ..., 10 o'clock, j=2,3 ..., 10, i=j+2, the Sigma sampled point is specially:

${\mathrm{\&chi;}}_i^j=\left[\begin{array}{ccccc}{\mathrm{\&chi;}}_0^{j-1}& {\mathrm{\&chi;}}_1^{j-1}& \&CenterDot;\&CenterDot;\&CenterDot;& {\mathrm{\&chi;}}_j^{j-1}& 0\\ 0& -\frac{1}{\sqrt{j(j-1)W}}& \&CenterDot;\&CenterDot;\&CenterDot;& -\frac{1}{\sqrt{j(j-1)W}}& \frac{j-1}{\sqrt{j(j-1)W}}\end{array}\right]$

Wherein:

The expression state is an i Sigma sampled point of j dimension,

It is the matrix of the capable i row of j.

Be exemplified below now: when dimension j=1, sampled point is: ${\mathrm{\&chi;}}_3^1=\left[\begin{array}{ccc}0& -\frac{1}{\sqrt{2\mathrm{<figure-callout\; id="1"\; label="W"\; filenames="HDA0000076767120000021.png,HDA0000076767120000051.png"\; state="\{\{state\}\}">W</figure-callout>}}}& \frac{1}{\sqrt{2W}}\end{array}\right],$ Utilizing above-mentioned recursion to calculate the state variable dimension is that 2 o'clock sampled point is: ${\mathrm{\&chi;}}_4^2=\left[\begin{array}{cccc}0& -\frac{1}{\sqrt{2\mathrm{<figure-callout\; id="1"\; label="W"\; filenames="HDA0000076767120000021.png,HDA0000076767120000051.png"\; state="\{\{state\}\}">W</figure-callout>}}}& \frac{1}{\sqrt{2\mathrm{<figure-callout\; id="0"\; label="W"\; filenames="HDA0000076767120000012.png,HDA0000076767120000021.png"\; state="\{\{state\}\}">W</figure-callout>}}}& 0\\ 0& -\frac{1}{\sqrt{2W}}& -\frac{1}{\sqrt{2\mathrm{<figure-callout\; id="1"\; label="W"\; filenames="HDA0000076767120000021.png,HDA0000076767120000051.png"\; state="\{\{state\}\}">W</figure-callout>}}}& \frac{1}{\sqrt{2W}}\end{array}\right],$ It by the state variable dimension 2 o'clock sampled point The sampled point that can calculate the state variable dimension and be 3 o'clock is: ${\mathrm{\&chi;}}_5^3=\left[\begin{array}{ccccc}0& -\frac{1}{\sqrt{2\mathrm{<figure-callout\; id="1"\; label="W"\; filenames="HDA0000076767120000021.png,HDA0000076767120000051.png"\; state="\{\{state\}\}">W</figure-callout>}}}& \frac{1}{\sqrt{2\mathrm{<figure-callout\; id="0"\; label="W"\; filenames="HDA0000076767120000012.png,HDA0000076767120000021.png"\; state="\{\{state\}\}">W</figure-callout>}}}& 0& 0\\ 0& -\frac{1}{\sqrt{2W}}& -\frac{1}{\sqrt{2\mathrm{<figure-callout\; id="1"\; label="W"\; filenames="HDA0000076767120000021.png,HDA0000076767120000051.png"\; state="\{\{state\}\}">W</figure-callout>}}}& \frac{1}{\sqrt{2\mathrm{<figure-callout\; id="0"\; label="W"\; filenames="HDA0000076767120000012.png,HDA0000076767120000021.png"\; state="\{\{state\}\}">W</figure-callout>}}}& 0\\ 0& -\frac{1}{\sqrt{6W}}& -\frac{1}{\sqrt{6W}}& -\frac{1}{\sqrt{6W}}& \frac{2}{\sqrt{6W}}\end{array}\right],$ By that analogy.

At last, can calculate the state variable dimension by recursion is 10 sampled point

It is the matrix of one 10 row 12 row.

(4) by the state variable dimension be 10 sampled point Obtain 12 sampled point χ of state variable X ₁, χ ₂χ ₁₂

At first, utilize sampled point

Calculate the mean square deviation P that adds state variable X ₀After Sigma sampled point χ ¹²:

${\mathrm{\&chi;}}^{12}=\sqrt{{\mathrm{<figure-callout\; id="0"\; label="P"\; filenames="HDA0000076767120000012.png,HDA0000076767120000021.png"\; state="\{\{state\}\}">P</figure-callout>}}_0}\&CenterDot;{\mathrm{\&chi;}}_{12}^{10}$

Wherein: χ ¹²Sigma sampled point after the mean square deviation of expression adding state variable X, χ ¹²It is the matrix of one 10 row 12 row.

Then, take out χ respectively ¹²The 1st row of matrix, the 2nd row ... the 12nd row are for the average of each row with state variable X

Addition, 12 sampled points that obtained state variable X are respectively χ ₁, χ ₂χ ₁₂, each sampled point all is the vector of one 10 row 1 row.

3) 12 sampled points of the state variable X of utilization acquisition are brought in the nonlinear equation, carry out the one-step prediction of state variable X:

12 sampled points for state variable X are brought into respectively in the nonlinear state equation, obtain a step status predication value χ ' of sampled point _kFor:

${\mathrm{\&chi;}}_k^{\&prime;}=f\left({\mathrm{\&chi;}}_k\right),$ k＝1，2...，12

Wherein, k is the numbering of each sampled point of from 1 to 12, and f is the shorthand notation of nonlinear state equation, χ ' _kOne-step prediction value for sampled point.

Utilize the one-step prediction value χ ' of sampled point _k, be weighted summation by weights W with each Sigma sampled point, obtain the one-step prediction value of state variable X

$\hat{X}=\underset{k=1}{\overset{12}{\mathrm{\&Sigma;}}}W\&CenterDot;{\mathrm{\&chi;}}_k^{\&prime;}$

By χ ' _kWith

Obtain the one-step prediction value P ' of state variable X mean square deviation:

$P^{\&prime;}=\mathrm{\&mu;}\&CenterDot;(\underset{k=1}{\overset{12}{\mathrm{\&Sigma;}}}W\&CenterDot;({\mathrm{\&chi;}}_k^{\&prime;}-\hat{X}){({\mathrm{\&chi;}}_k^{\&prime;}-\hat{X})}^T)+Q$

Wherein, μ is strong tracking factor, and Q is the mean square deviation matrix of process noise, is the matrix of 10 row, 10 row.

12 sampled points for state variable X are brought in the measurement equation respectively and calculate, and obtain the step observation predicted value z of sampled point _kFor:

z _k＝h(χ _k)，k＝1，2...，12

Wherein, k is the numbering of each sampled point of from 1 to 12, and h is the shorthand notation of measurement equation.

Utilize the step observation predicted value z of sampled point _k, be weighted summation by weights W with each Sigma sampled point, obtain the one-step prediction value of observed quantity Z

$\hat{Z}=\underset{k=1}{\overset{12}{\mathrm{\&Sigma;}}}W\&CenterDot;z_k$

μ can obtain according to following strong track algorithm:

The one-step prediction value of observed quantity Z With observed quantity Z, try to achieve residual error γ and be

Can calculate residual matrix V by residual error γ, the initial value V (1) of V is: V (1)=γ (1) γ (1) ^T, wherein, the residual error that γ (1) produces when being first step filtering.

When carrying out the second step filtering and further filtering, the value of V can obtain by following recurrence relation:

$V\left(\mathrm{\&lambda;}\right)=\frac{\mathrm{\&rho;}\&CenterDot;V(\mathrm{\&lambda;}-1)+\mathrm{\&gamma;}\left(\mathrm{\&lambda;}\right)\&CenterDot;{\mathrm{\&gamma;}}^T\left(\mathrm{\&lambda;}\right)}{1+\mathrm{\&rho;}}$

Wherein, ρ is a forgetting factor, generally gets 0.95; λ was the 1st step, the 2nd step ... the 100000th step filtering; The residual error that γ (λ) produces when being expressed as λ step filtering; V (λ) is expressed as the residual matrix V that λ step filtering calculates.

Utilize the value of residual matrix V, calculate by following formula, can be in the hope of μ ₀:

M(λ)＝H(λ)·F(λ)·P(λ-1)·F ^T(λ)·H ^T(λ)

N(λ)＝V(λ)-H(λ)·Q·H ^T(λ)-Φ·R

Wherein, F (λ) is the linearization matrix of λ step filtering non-linear hour state equation, the linearization matrix of measurement equation when H (λ) is λ step filtering, the mean square deviation battle array of state variable X when P (λ-1) is λ-1 step filtering, Q, R are process noise and the mean square deviation matrix of measuring noise; Φ is the reduction factor, generally gets Φ 〉=1.

μ ₀＝trN(λ)/trM(λ)

Wherein, the mark of the matrix M (k) that the mark of the matrix N (k) that trN (k) calculates when being λ step filtering, trM (k) calculate when being λ step filtering.Work as μ ₀More than or equal to 1 o'clock, strong tracking factor μ=μ ₀Work as μ ₀Less than 1 o'clock, strong tracking factor μ=1.

By above-mentioned steps, obtain a step status predication value χ ' of sampled point _k, state variable X the one-step prediction value

The one step observation predicted value z of the one-step prediction value P ' of state variable X mean square deviation, sampled point _k, observed quantity Z the one-step prediction value

4) according to χ ' _k, P ', z _k,

With the observed quantity Z that measures, obtain the estimated value of state variable X

Estimation square error matrix with state variable X

At first, utilize χ ' _k,

z _k,

Can be in the hope of the filter gain matrix K:

Utilize matrix P _Zz, ask matrix P _ZzContrary obtaining

The left side is multiplied by matrix P then _Xz, obtain the filter gain matrix K, be shown below:

$K=P_{\mathrm{xz}}\&CenterDot;P_{\mathrm{zz}}^{-1}$

Then, utilize filter gain matrix K that obtains and the observed quantity Z that measures, go to revise the one-step prediction value of state variable X

With the one-step prediction value P ' of state variable X mean square deviation, obtain the estimated value of state variable X

Estimation square error matrix with state variable X

$\tilde{X}=\hat{X}+K\&CenterDot;(Z-\hat{Z})$

$\tilde{P}=P^{\&prime;}-K\&CenterDot;P_{\mathrm{zz}}\&CenterDot;K^T$

From as can be seen above, there is not apodizing filter by utilizing residual sequence calculating regulatory factor in real time based on the strong tracking of suprasphere sampling, adjust the mean square deviation battle array and the corresponding tracking of battle array maintenance that gain of state prediction error in real time to the real system state.

Step has to sum up obtained the estimated value of state variable X

Estimation square error matrix with state variable X

Through further filtering, filtering method of the present invention can well be estimated the attitude error angle, takes out the estimated value of state variable X

In Φ _x, Φ _y, Φ _zThree, be the estimated value at attitude error angle.

Step 3, by navigational computer, the inertia device output information of acquisition trajectories generator, and finish alignment procedures.

Be created in gyroscope when static and the output of accelerometer by path generator, the output valve of utilizing accelerometer is as observed reading, obtain the estimated value at attitude error angle by step 1 and step 2, compensate the attitude error angle by making software, can obtain initial attitude matrix more accurately, the STFUKF that adopts suprasphere to sample is filtered into navigation calculation provides the initial attitude matrix that satisfies accuracy requirement, and then can carry out strap-down inertial and resolve, for the user provides strap-down inertial information.

Embodiment:

Initial attitude: 0 ° of the angle of pitch, 0 ° of roll angle, 30 ° of crab angles.Three attitude errors that add are [20 ' 20 ' 60 '] ^T, the gyroscopic drift error: comprise constant value drift [0.01 °/h of 0.01 °/h of 0.01 °/h] ^TRandom drift: [0.001 °/h of 0.001 °/h of 0.001 °/h] ^T, accelerometer bias stability is [100 μ g, 100 μ g, 100 μ g] ^T, 300s and two groups of emulation of 1000s have been carried out in emulation altogether, and the sampling period of SINS is 0.1s, and emulated data is got the mean value of 20 emulation, and the filtering result is shown in Fig. 2-8.

At first the duration that carries out is the emulation of 300s, as Fig. 2～4, as can be seen, Kalman filtering (Kalman), no mark filtering (UKF), the strong tracking based on the suprasphere sampling of the present invention do not have three kinds of methods of mark filtering (SSTFUKF) and are more or less the same on precision, as table 1.

Table 1 simulation time is 300s, three kinds of Algorithm Error angle contrasts

Type	Kalman	UKF	SSTFUKF
				Φ x	-0.06147’	-0.04147’	-0.04074’
Φ y	-0.000549’	0.005656’	0.014186’
				Φ z	-33.3038’	-0.24814’	-0.2167’

Yet,, cause the filter effect variation because the statistical property of noise and nonlinear state equation is indeterminate along with simulation time increases (1000s), but in traditional UKF, add after the strong tracking filter of suprasphere, can well the tracking mode value, filter effect is better than UKF, sees Fig. 5～7.The precision such as the table 2 of three kinds of methods.

Table 2 simulation time is 1000s, three kinds of Algorithm Error angle contrasts

Type

Kalman

UKF

SSTFUKF

Φ x	-0.07704’	-0.04269’	-0.04263’
				Φ y	-0.07097’	-0.07921’	-0.0027079’
Φ z	-228.643’	-2.8254’	-1.7563’

The STFUKF filtering of suprasphere sampling has good tracking accuracy for coloured noise or under the uncertain situation of noise statistics to state, but what meanwhile bring is the increase of computing time, analogous diagram by observing 1000s as can be seen, the moment before 200s, Kalman's precision meets the demands, but after the filter effect variation.Preceding in actual applications 200s Kalman filtering, 200s-1000s uses based on the strong tracking of suprasphere sampling does not have mark filtering, so both can reduce the part calculated amount, shortens the aligning time, can guarantee last tracking accuracy again.The present invention also improves one's methods to this and has made emulation, as Fig. 8～10.

From Fig. 8～10 as can be seen, dotted line is the simulation curve of improvement project, and fundamental sum SSTFUKF method overlaps on the precision, but littler than SSTFUKF on the time that calculating is consumed, and sees Table 3.

Table 3 simulation time is 1000s, and adding improves the error angle contrast behind the algorithm

Wherein: relative time is meant, with time of Kalman filtering be a benchmark, the time that other filtering mode consumed and the ratio of this benchmark are as time that filtering consumed.

Claims (2)

1. the initial alignment method based on the suprasphere sampling is characterized in that, comprises following step:

Step 1 is set up the nonlinear state equation of the quiet pedestal initial alignment of SINS;

Geocentric inertial coordinate system i is x _iy _iz _iNavigation coordinate system elects Department of Geography as, and the t of Department of Geography is x _ty _tz _tCalculating the c of Department of Geography is x _cy _cz _c

Suppose that the attitude error angle between calculating Department of Geography and the actual Department of Geography is respectively the lateral error angle Φ of horizontal both direction _x, Φ _yWith on the vertical direction azimuthal error angle Φ _z, then geography is tied to and calculates Department of Geography and realize by following rotating manner:

T system is around z _tAxle rotates Φ _z, obtain t ₁System; t ₁System around

Axle rotates Φ _x, obtain t ₂System; t ₂System around

Axle rotates Φ _y, obtain c system; Lateral error angle Φ _x, Φ _yIn 1 °, sin Φ then _x≈ Φ _x, sin Φ _y≈ Φ _y, cos Φ _x=cos Φ _y≈ 1, and the relation between t system and the c system is with following matrix representation:

Wherein:

Expression is tied to the transition matrix that c is by t,

Expression is by t ₂Be tied to the transition matrix of c system,

Expression is by t ₁Be tied to t ₂The transition matrix of system, Expression is tied to t by t ₁The transition matrix of system is because sin Φ _x≈ Φ _x, sin Φ _y≈ Φ _y, cos Φ _x=cos Φ _y≈ 1,

Attitude error angle Φ _t=(Φ _x, Φ _y, Φ _z) ^T, get by the attitude differential equation:

Wherein, Ω _Tc=Ω _Ic-Ω _It, Ω _ItBe the angular velocity omega of coordinate system t with respect to coordinate system i _ItAntisymmetric matrix, Ω _IcBe the angular velocity omega of coordinate system c with respect to coordinate system i _IcAntisymmetric matrix, Ω _TcBe the angular velocity omega of coordinate system c with respect to coordinate system t _TcAntisymmetric matrix, obtain:

Wherein:

ω _IxThe expression angular velocity omega _ItAt the component of x axle, ω _IyThe expression angular velocity omega _ItAt the component of y axle, ω _IzThe expression angular velocity omega _ItComponent at the z axle;

In the formula, λ is local longitude, and L is local latitude, ω _IeIt is the earth rotation angular speed;

Work as Φ _x, Φ _yIn the time of all in 1 °, obtain:

Wherein: ω _TcDenotation coordination is the angular velocity of c with respect to coordinate system t, ω _TxThe expression angular velocity omega _TcAt the component of x axle, ω _TyThe expression angular velocity omega _TcAt the component of y axle, ω _TzThe expression angular velocity omega _TcAt the component of z axle, again because:

According to the corresponding relation of vector and antisymmetric matrix thereof, following formula is write as:

Wherein, ω _Ic=ω _It+ δ ω _It+ ε, ε are the gyrostatic projections that drifts in Department of Geography, δ ω _ItBe in SINS, to calculate ω _ItThe time error; In sum, obtain nonlinear aligning equation under the quiet pedestal of SINS;

Wherein: I represents 3 rank unit matrixs;

Under quiet pedestal, ω _It=(0, ω _IeCosL, ω _IeSinL) ^T,

R is an earth radius; Obtain:

Wherein: ε _x, ε _y, ε _zBe respectively the projection components of ε, Δ V at three change in coordinate axis direction _x, Δ V _y, Δ V _zBe the projection of velocity error in Department of Geography;

By the specific force equation:

Wherein, V _tThe speed of-carrier is at the projection of t system, a _t-specific force is at the projection of t system, Δ n _t-harmful acceleration is at the projection of t system, g _t=[0 0-g] ^TBe acceleration of gravity;

The rate equation that is calculated by inertial navigation is:

And the reading of accelerometer is:

With the following formula premultiplication Be out of shape:

Wherein, a _cBe the projection of specific force in c system,

It is the projection that the acceleration calculation system deviation is fastened in corresponding coordinate;

By on can get:

Wherein,

L is local latitude, ω _IeBe rotational-angular velocity of the earth, Δ V _x, Δ V _y, Δ V _zBe the projection of velocity error in Department of Geography;

Orthogonality according to transition matrix has:

So, when as do not consider Δ V _z, the velocity error equation is:

Be the x component of harmful acceleration,

Y component for harmful acceleration;

As azimuthal error angle Φ _zDuring greater than 1 °, the nonlinear state equation of the quiet pedestal initial alignment of SINS is:

Wherein, state variable

W=[Δ α _xΔ α _yΔ ω _xΔ ω _yΔ ω _z] ^TBe 5 * 1 process noise matrixes,

Be accelerometer drift, ε _x, ε _y, ε _zBe gyrostatic drift, Δ α _x, Δ α _yBe the white noise of accelerometer error, Δ ω _x, Δ ω _y, Δ ω _zBe the white noise of gyroscopic drift, ω _IeBe rotational-angular velocity of the earth, L is local geographic latitude, and g is a local gravitational acceleration;

In quiet pedestal initial alignment, get the output of accelerometer on horizontal both direction as observed quantity Z, obtain measurement equation:

Z＝HX+η

Wherein, Z is observed quantity,

η is 2 * 1 observation noise matrixes;

Step 2 is set up based on the strong tracking of suprasphere sampling and is not had the mark filtering method, obtains the state variable estimated value;

Specifically comprise following step:

1) state variable initialization;

State variable in the nonlinear state equation of the quiet pedestal initial alignment of SINS that step 1 is obtained is carried out initialization, and Δ V is set _x, Δ V _y, Φ _x, Φ _y, Φ _z,

ε _x, ε _y, ε _zInitial value, and set the average of X

With mean square deviation P ₀, average

Be the column vector of 10 row, 1 row, mean square deviation P ₀Be the diagonal matrix of 10 row, 10 row, be specially:

Wherein: x ₀The initial value of expression state variable X;

2) obtain strong tracking and do not have the sampled point that mark filtering needs;

Adopt the suprasphere method of sampling to obtain the weights W of n+2 Sigma sampled point and Sigma sampled point, n represents the dimension of state variable, is specially:

(1) determines constant W at random ₀, 0≤W ₀≤ 1;

(2) obtain the weights of 12 Sigma sampled points, the weights W of each Sigma sampled point is:

The 12nd Sigma sampled point when (3) obtaining 10 dimension state variables is specially:

At first obtain state variable dimension j=1, during i=j+2=3, three Sigma sampled points:

Wherein:

The 0th Sigma sampled point during expression 1 dimension,

The 1st Sigma sampled point during expression 1 dimension,

The 2nd Sigma sampled point during expression 1 dimension;

By, 1 dimension state variable is carried out recursion calculating, when state variable dimension j=2,3 ..., 10 o'clock, j=2,3 ..., 10, i=j+2, the Sigma sampled point is specially:

Wherein:

The expression state is an i Sigma sampled point of j dimension, It is the matrix of the capable i row of j;

At last, obtaining the state variable dimension is 10 sampled point

It is the matrix of one 10 row 12 row;

(4) by the state variable dimension be 10 sampled point Obtain 12 sampled point χ of state variable X ₁, χ ₂χ ₁₂

At first, utilize sampled point

Calculate the mean square deviation P that adds state variable X ₀After Sigma sampled point χ ¹²:

Wherein: χ ¹²Sigma sampled point after the mean square deviation of expression adding state variable X, χ ¹²It is the matrix of one 10 row 12 row;

Then, take out χ respectively ¹²The 1st row of matrix, the 2nd row ... the 12nd row are for the average of each row with state variable X

Addition, 12 sampled points that obtained state variable X are respectively χ ₁, χ ₂χ ₁₂, each sampled point all is the vector of one 10 row 1 row;

3) 12 sampled points of the state variable X of utilization acquisition are brought in the nonlinear equation, carry out the one-step prediction of state variable X:

12 sampled points for state variable X are brought into respectively in the nonlinear state equation, obtain a step status predication value χ ' of sampled point _kFor:

χ' _k＝f(χ _k)，k＝1,2...,12

Wherein, k is the numbering of each sampled point of from 1 to 12, and f is the shorthand notation of nonlinear state equation, χ ' _kOne-step prediction value for sampled point;

Utilize the one-step prediction value χ ' of sampled point _k, be weighted summation by weights W with each Sigma sampled point, obtain the one-step prediction value of state variable X

By χ ' _kWith

Obtain the one-step prediction value P' of state variable X mean square deviation:

Wherein, μ is strong tracking factor, and Q is the mean square deviation matrix of process noise, is the matrix of 10 row, 10 row;

12 sampled points for state variable X are brought in the measurement equation respectively and calculate, and obtain the step observation predicted value z of sampled point _kFor:

z _k＝h(χ _k)，k＝1,2...,12

Wherein, k is the numbering of each sampled point of from 1 to 12, and h is the shorthand notation of measurement equation;

Utilize the step observation predicted value z of sampled point _k, be weighted summation by weights W with each Sigma sampled point, obtain the one-step prediction value of observed quantity Z

By above-mentioned steps, obtain a step status predication value χ ' of sampled point _k, state variable X the one-step prediction value

The one step observation predicted value z of the one-step prediction value P' of state variable X mean square deviation, sampled point _k, observed quantity Z the one-step prediction value

4) according to χ ' _k,

P', z _k,

With the observed quantity Z that measures, obtain the estimated value of state variable X Estimation square error matrix with state variable X

At first, utilize χ ' _k,

z _k,

Try to achieve the filter gain matrix K:

Utilize matrix P _Zz, ask matrix P _ZzContrary obtaining

The left side is multiplied by matrix P then _Xz, obtain the filter gain matrix K, be shown below:

Then, utilize filter gain matrix K that obtains and the observed quantity Z that measures, go to revise the one-step prediction value of state variable X

With the one-step prediction value P' of state variable X mean square deviation, obtain the estimated value of state variable X Estimation square error matrix with state variable X

Step has to sum up obtained the estimated value of state variable X

Estimation square error matrix with state variable X

Obtain the estimated value of state variable X

In the estimated value Φ at attitude error angle _x, Φ _y, Φ _z

Step 3, by navigational computer, the inertia device output information of acquisition trajectories generator, and finish alignment procedures;

Be created in gyroscope when static and the output of accelerometer by path generator, the output valve of utilizing accelerometer is as observed reading, obtains the estimated value at attitude error angle by step 1 and step 2, and the attitude error angle is compensated, obtain initial attitude matrix accurately, finish aligning.

2. a kind of initial alignment method based on suprasphere sampling according to claim 1 is characterized in that described step 2 3) in μ obtain according to following strong track algorithm:

The one-step prediction value of observed quantity Z

With observed quantity Z, try to achieve residual error γ and be

Calculate residual matrix V by residual error γ, the initial value V (1) of V is: V (1)=γ (1) γ (1) ^T, wherein, the residual error that γ (1) produces when being first step filtering;

When carrying out the second step filtering and further filtering, the value of V obtains by following recurrence relation:

Wherein, ρ is a forgetting factor, generally gets 0.95; λ was the 1st step, the 2nd step ... the 100000th step filtering; The residual error that γ (λ) produces when being expressed as λ step filtering; V (λ) is expressed as the residual matrix V that λ step filtering calculates;

Utilize the value of residual matrix V, calculate, try to achieve μ by following formula ₀:

M（λ)=H（λ）·F（λ）·P（λ-1)·F ^T（λ）·H ^T（λ）

N(λ)=V(λ)-H(λ）·Q·H ^T(λ)-Φ·R

Wherein, F (λ) is the linearization matrix of λ step filtering non-linear hour state equation, the linearization matrix of measurement equation when H (λ) is λ step filtering, the mean square deviation battle array of state variable X when P (λ-1) is λ-1 step filtering, Q, R are process noise and the mean square deviation matrix of measuring noise; Φ is the reduction factor, generally gets Φ 〉=1;

μ ₀=trN(λ)/trM(λ)

Wherein, the mark of the matrix M (k) that the mark of the matrix N (k) that trN (k) calculates when being λ step filtering, trM (k) calculate when being λ step filtering; Work as μ ₀More than or equal to 1 o'clock, strong tracking factor μ=μ ₀Work as μ ₀Less than 1 o'clock, strong tracking factor μ=1.

Images