CN103630139A - Underwater vehicle all-attitude determination method based on magnetic gradient tensor measurement - Google Patents

Abstract

The invention belongs to the field of underwater geomagnetic aided navigation, and particularly relates to an underwater vehicle all-attitude determination method based on magnetic gradient tensor measurement. The method comprises the steps of: under a carrier system, establishing a three-dimensional rectangular coordinate system to obtain five components of magnetic gradient tensor; extracting five isolated components of the magnetic gradient tensor under a geographical system from the prestored magnetic gradient tensor database; substituting the results into a non-linear equation set about q0, q1, q2 and q3; substituting quaternion q0, q1, q2 and q3 to a transformation matrix between a substitution carrier system and the geographical system to obtain the estimated value of a pitch angle theta. The underwater vehicle locating technology based on magnetic anomaly inversion has the characteristics of being low in cost, high in sensitivity, high in anti-interference capability, free from accumulative error and the like; the problem that a single magnetometer can not independently determine the attitude by depending on the geomagnetic field can be solved; an independent and concealed way is provided for underwater vehicle attitude estimation.

Description

A kind of full attitude of underwater carrier of measuring based on earth magnetism gradient tensor is determined method

Technical field

The invention belongs to geomagnetic auxiliary navigation field under water, be specifically related to a kind of full attitude of underwater carrier of measuring based on earth magnetism gradient tensor and determine method.

Background technology

Underwater vehicle can keep higher positioning precision according to inertia/geomagnetic matching integrated navigation system, but long-time underwater navigation meeting makes submarine attitude angle have several years error, bring larger error can to the Underwater Battery transmitting that depends on navigational system attitude, seriously restrict the fighting efficiency of submerge device.Therefore, to determine appearance technology be one of core content of underwater vehicle navigational system to underwater carrier.GPS determines height independence and the concealed feature that appearance technology has lost underwater navigation system, restricted by underwater complex landform and cannot normally accept gps signal.The degree of accuracy of optical profile type attitude determination system and resolution decline with the increase of target and optical sensor distance, and optical noise, parasitic light, shade block etc. all may to cause determines appearance failure, so the attitude determination system based on optical measurement is difficult to be applied under water.Be considered at present the star sensor attitude measure method that precision is the highest and be mainly used in the spacecrafts such as airship, satellite, the method provides be carrier with respect to the attitude of inertial space, can not independently determine appearance, need to be combined with inertial navigation system and expensive.Attitude measurement method based on geomagnetic field intensity is due to the restriction in its principle, easily be subject to the impact of various disturbing magnetic fields around, and the magnetic field that on carrier, soft iron magnetic material produces is relevant with carrier movement state, be difficult to accurate correction, particularly to the magnetic field under large angle of inclination.On the other hand, single geomagnetic field intensity can not provide enough attitude measurement informations.The present invention proposes a kind of attitude based on magnetic field gradient Tensor measuring and determines method, by the extraction of magnetic field gradient information, eliminate the impact of disturbing magnetic field, solve the problem that single magnetometer cannot rely on terrestrial magnetic field autonomous attitude determination, simultaneously for underwater carrier attitude estimates to provide a kind of autonomous, hidden approach.

Summary of the invention

The object of the present invention is to provide that a kind of cost is low, highly sensitive, antijamming capability is strong and determines method without the full attitude of the underwater carrier based on magnetic field gradient Tensor measuring of accumulated error.

The object of the present invention is achieved like this:

(1) under carrier system, set up three-dimensional cartesian coordinate system x _b, y _b, z _b, the magnetometer of two three axle quadratures is placed on respectively x _ba, B 2 points of axle symmetry centered by initial point, measure respectively magnetic-field component (h ₄, h ₅, h ₆) and (h ₁, h ₂, h ₃); Another is placed in respectively y to two axle orthogonal magnetometers _bc, D 2 points of axle symmetry centered by initial point, measure respectively magnetic-field component (h ₉, h ₁₀) and (h ₇, h ₈), the sensitive axes direction of all magnetometers is consistent, base area magnetic gradient tensor G ^b5 components

and

with magnetic-field component h ₁～h ₁₀relational expression

$\left\{\begin{array}{c}{g}_{\mathrm{xx}}^b=\frac{h_4-h_1}{L_x},g_{\mathrm{yy}}^b=\frac{h_9-h_7}{L_y},g_{\mathrm{yx}}^b=\frac{h_5-h_2}{L_x},\\ g_{\mathrm{zy}}^b=\frac{h_{10}-h_8}{L_y},g_{\mathrm{zx}}^b=\frac{h_6-h_3}{L_x}\end{array}\right.$ Obtain 5 components of magnetic gradient tensor, L _xand L _ybe respectively x _band y _bgradiometry base length in direction;

(2), according to the indicating positions of inertia/earth magnetism integrated navigation system, from pre-stored earth magnetism gradient tensor database, extract earth magnetism gradient tensor G under Department of Geography ⁿ5 isolated components

and

(3) the lower earth magnetism gradient tensor component of carrier system step 1,2 being obtained with earth magnetism gradient tensor component under Department of Geography and

substitution is about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations:

$\begin{array}{c}{g}_{\mathrm{xx}}^b=4g_{\mathrm{xz}}^n({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_1{q}_3+q_0{q}_2)+8g_{\mathrm{zy}}^n(q_1{q}_2-q_0{q}_3)(q_1{q}_3+q_0{q}_2)+4g_{\mathrm{yy}}^n{(q_1{q}_2-q_0{q}_3)}^2\\ +g_{\mathrm{xx}}^n{({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)}^2+4g_{\mathrm{xy}}^n({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_1{q}_2-q_0{q}_3)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n){(q_1{q}_3+q_0{q}_2)}^2\end{array}$

$\begin{array}{c}{g}_{\mathrm{yy}}^b=4g_{\mathrm{xx}}^n{(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)}^2+4g_{\mathrm{yx}}^n(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)+8g_{\mathrm{zx}}^n(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)(q_2{q}_3-q_0{q}_1)\\ +g_{\mathrm{yy}}^n{({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)}^2+4g_{\mathrm{zy}}^n({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_2{q}_3-q_0{q}_1)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)\end{array}$

$\begin{array}{c}{g}_{\mathrm{zy}}^b=4g_{\mathrm{xx}}^n(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[2(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)(q_2{q}_3+q_0{q}_1)+({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)]\\ +2g_{\mathrm{zx}}^n[(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2)+2(q_2{q}_3-q_0{q}_1)(q_1{q}_3-q_0{q}_2)]+2g_{\mathrm{yy}}^n({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)\\ {+g}_{\mathrm{zy}}^n[({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2)+4(q_2{q}_3-q_0{q}_1)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2)\end{array}$

$\begin{array}{c}{g}_{\mathrm{zx}}^b=2g_{\mathrm{xx}}^n({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)+2(q_1{q}_2-q_0{q}_3)(q_1{q}_3-q_0{q}_2)]\\ +g_{\mathrm{zx}}^n[({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2)+4(q_1{q}_3+q_0{q}_2)(q_1{q}_3-q_0{q}_2)]+4g_{\mathrm{yy}}^n(q_1{q}_2-q_0{q}_3)(q_2{q}_3+q_0{q}_1)\\ {+2g}_{\mathrm{zy}}^n[(q_1{q}_2-q_0{q}_3)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2)+2(q_1{q}_3+q_0{q}_2)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_1{q}_3+q_0{q}_2)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2)\end{array}$

About q ₀, q ₁, q ₂and q ₃nonlinear System of Equations can be by G ⁿand G ^brelational expression

obtain, T representing matrix transposition wherein,

the transformation matrix that geography is tied to carrier system, and

the transformation matrix of carrier Xi Yu Department of Geography, and in conjunction with q ₀, q ₁, q ₂and q ₃restriction relation utilize newton's down-hill method to solve about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations;

(4) by the hypercomplex number that meets iterated conditional obtaining in step (3)

with

the transformation matrix of substitution carrier Xi Yu Department of Geography,

$C_b^n=\left[\begin{array}{ccc}{\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_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{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)& {\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2& 2\left(q_2{q}_3{-q}_0{q}_1\right)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& {\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2\end{array}\right],$

Obtain matrix element

(i, j=1,2,3),

Obtain the estimated value of pitching angle theta;

Course angle

estimated value in the following several ways: when ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}>0$ Time, $\widehat{\mathrm{\&psi;}}={90}^0;$ When ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}<0$ Time, $\widehat{\mathrm{\&psi;}}=-{90}^0;$ When ${\hat{c}}_{22}>0,{\hat{c}}_{12}>0$ Time,

when ${\hat{c}}_{22}>0,{\hat{c}}_{12}<0$ Time,

when ${\hat{c}}_{22}<0,{\hat{c}}_{12}>0$ Time,

when ${\hat{c}}_{22}<0,{\hat{c}}_{12}<0$ Time,

Roll angle

estimated value divide 3 kinds of situations: work as c ₃₃during > 0,

work as c ₃₃< 0,

time,

work as c ₃₃< 0,

time,

Beneficial effect of the present invention is: the underwater carrier location technology based on geomagnetic anomaly inverting that the present invention proposes has that cost is low, highly sensitive, antijamming capability strong and without characteristics such as accumulated errors, solve the problem that single magnetometer cannot rely on terrestrial magnetic field autonomous attitude determination, for underwater carrier attitude estimates to provide a kind of autonomous, hidden approach.

Accompanying drawing explanation

Fig. 1 is method flow diagram.

Fig. 2 is that earth magnetism gradient tensor is measured arrangement plan.

Fig. 3 is attitude measurement principle schematic.

Fig. 4 is the algorithm convergence situation of different initial solution conditions.

Fig. 5 is the attitude angle of estimating in different initial solution situations.

Fig. 6 is the algorithm convergence situation under different noise levels.

Fig. 7 is the pose estimation value under different noise levels.

Embodiment

The invention provides a kind of carrier method for determining posture being applied under water, the method can be finally inversed by the attitude angle information of carrier comparatively accurately; Solve the problem that single magnetometer cannot rely on terrestrial magnetic field autonomous attitude determination, for underwater carrier attitude estimates to provide a kind of autonomous, hidden approach.

A kind of full attitude under water of measuring based on earth magnetism gradient tensor of the present invention is determined method, and strapdown forms earth magnetism gradient tensor measurement mechanism in the vector magnetometer group of underwater carrier, measures 5 isolated components of the lower geomagnetic gradient tensor of carrier system.The reference position of exporting according to inertia/earth magnetism integrated navigation system from earth magnetism gradient tensor database, extract 5 isolated components of geomagnetic gradient tensor under local Department of Geography, form the Nonlinear System of Equations about 3 attitude angle of carrier with earth magnetism gradient tensor measured value under carrier system, the inertial navigation system of usining is exported hypercomplex number corresponding to attitude angle as initial solution, use the optimized algorithms such as newton's down-hill method to estimate attitude of carrier, Fig. 1 has provided the process flow diagram of the method, and its concrete steps are as follows:

Step 1, for geodetic magnetic gradient tensor more accurately adopts ten single shaft magnetometer configuration modes, under carrier system, set up three-dimensional cartesian coordinate system x _by _bz _b, Fig. 2 has provided the configuration mode of magnetometer group: the magnetometer of two three axle quadratures is placed on respectively x _bthe A of axle, B 2 points, measure respectively magnetic-field component (h ₄, h ₅, h ₆) and (h ₁, h ₂, h ₃); Another is placed in respectively y to two axle orthogonal magnetometers _bthe C of axle, D 2 points, measure respectively magnetic-field component (h ₉, h ₁₀) and (h ₇, h ₈), arrow represents the sensitive axes direction of each magnetometer.5 components of base area magnetic gradient tensor Gb

and

with magnetic-field component h ₁～h ₁₀relational expression

$\left\{\begin{array}{c}{g}_{\mathrm{xx}}^b=\frac{h_4-h_1}{L_x},g_{\mathrm{yy}}^b=\frac{h_9-h_7}{L_y},g_{\mathrm{yx}}^b=\frac{h_5-h_2}{L_x},\\ g_{\mathrm{zy}}^b=\frac{h_{10}-h_8}{L_y},g_{\mathrm{zx}}^b=\frac{h_6-h_3}{L_x}\end{array}\right.---\left(1\right)$

Can calculate 5 components of magnetic gradient tensor, L in equation (1) _xand L _ybe respectively x _band y _bgradiometry base length in direction;

Step 2, according to the indicating positions of inertia/earth magnetism integrated navigation system, from pre-stored earth magnetism gradient tensor database, extract earth magnetism gradient tensor G under Department of Geography ⁿ5 isolated components and

Step 3, the lower earth magnetism gradient tensor component of the carrier system that step 1,2 is obtained

with earth magnetism gradient tensor component under Department of Geography

substitution is about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations:

$\begin{array}{c}{g}_{\mathrm{xx}}^b=4g_{\mathrm{xz}}^n({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_1{q}_3+q_0{q}_2)+8g_{\mathrm{zy}}^n(q_1{q}_2-q_0{q}_3)(q_1{q}_3+q_0{q}_2)+4g_{\mathrm{yy}}^n{(q_1{q}_2-q_0{q}_3)}^2\\ +g_{\mathrm{xx}}^n{({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)}^2+4g_{\mathrm{xy}}^n({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_1{q}_2-q_0{q}_3)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n){(q_1{q}_3+q_0{q}_2)}^2\end{array}---\left(2\right)$

$\begin{array}{c}{g}_{\mathrm{yy}}^b=4g_{\mathrm{xx}}^n{(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)}^2+4g_{\mathrm{yx}}^n(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)+8g_{\mathrm{zx}}^n(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)(q_2{q}_3-q_0{q}_1)\\ +g_{\mathrm{yy}}^n{({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)}^2+4g_{\mathrm{zy}}^n({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_2{q}_3-q_0{q}_1)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)\end{array}---\left(3\right)$

$\begin{array}{c}{g}_{\mathrm{yx}}^b=2g_{\mathrm{xx}}^n({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)+g_{\mathrm{yx}}^n[({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)+4\left(q_1{q}_2{-q}_0{q}_3\right)(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)]\\ +2g_{\mathrm{zx}}^n[({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_2{q}_3-q_0{q}_1)+2(q_1{q}_3+q_0{q}_2)(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)]+2g_{\mathrm{yy}}^n(q_1{q}_2-q_0{q}_3)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)\\ +2g_{\mathrm{zy}}^n[2(q_1{q}_2-q_0{q}_3)(q_2{q}_3-q_0{q}_1)+(q_1{q}_3+q_0{q}_2)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-{\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+{\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-{\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2)]-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_1{q}_3+q_0{q}_2)(q_2{q}_3-q_0{q}_1)\end{array}---\left(4\right)$

$\begin{array}{c}{g}_{\mathrm{zy}}^b=4g_{\mathrm{xx}}^n(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[2(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)(q_2{q}_3+q_0{q}_1)+({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)]\\ +2g_{\mathrm{zx}}^n[(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2)+2(q_2{q}_3-q_0{q}_1)(q_1{q}_3-q_0{q}_2)]+2g_{\mathrm{yy}}^n({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)\\ {+g}_{\mathrm{zy}}^n[({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2)+4(q_2{q}_3-q_0{q}_1)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2)\end{array}---\left(5\right)$

$\begin{array}{c}{g}_{\mathrm{zx}}^b=2g_{\mathrm{xx}}^n({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)+2(q_1{q}_2-q_0{q}_3)(q_1{q}_3-q_0{q}_2)]\\ +g_{\mathrm{zx}}^n[({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2)+4(q_1{q}_3+q_0{q}_2)(q_1{q}_3-q_0{q}_2)]+4g_{\mathrm{yy}}^n(q_1{q}_2-q_0{q}_3)(q_2{q}_3+q_0{q}_1)\\ {+2g}_{\mathrm{zy}}^n[(q_1{q}_2-q_0{q}_3)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2)+2(q_1{q}_3+q_0{q}_2)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_1{q}_3+q_0{q}_2)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2)\end{array}---\left(6\right)$

About q ₀, q ₁, q ₂and q ₃nonlinear System of Equations can be by G ⁿand G ^brelational expression

obtain, T representing matrix transposition wherein,

the transformation matrix that geography is tied to carrier system, and

the transformation matrix of carrier Xi Yu Department of Geography, and in conjunction with q ₀, q ₁, q ₂and q ₃restriction relation

utilize newton's down-hill method to solve about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations;

Step 4: by the hypercomplex number that meets iterated conditional obtaining in step 3 with the transformation matrix of substitution carrier Xi Yu Department of Geography

$C_b^n=\left[\begin{array}{ccc}{\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_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{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)& {\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2& 2\left(q_2{q}_3{-q}_0{q}_1\right)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& {\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2\end{array}\right]---\left(7\right)$

Obtain matrix element

(i, j=1,2,3).According to following formula:

Obtain the estimated value of pitching angle theta.

Course angle

estimated value in the following several ways: when ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}>0$ Time, $\widehat{\mathrm{\&psi;}}={90}^0;$ When ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}<0$ Time, $\widehat{\mathrm{\&psi;}}=-{90}^0;$ When ${\hat{c}}_{22}>0,{\hat{c}}_{12}>0$ Time, when ${\hat{c}}_{22}>0,{\hat{c}}_{12}<0$ Time,

when ${\hat{c}}_{22}<0,{\hat{c}}_{12}>0$ Time,

when ${\hat{c}}_{22}<0,{\hat{c}}_{12}<0$ Time,

Roll angle

estimated value divide 3 kinds of situations: work as c ₃₃during > 0, work as c ₃₃< 0, time, work as c ₃₃< 0, time,

Below in conjunction with accompanying drawing, embodiments of the present invention are described in detail:

Step 1, the single shaft magnetometer group of measuring earth magnetism gradient tensor is configured by Fig. 2 mode for geodetic magnetic gradient tensor more accurately.Arrow represents the sensitive axes direction of each magnetometer.At A, B place, there are a pair of three axle orthogonal magnetometers, measure respectively magnetic-field component (h ₄, h ₅, h ₆) and (h ₁, h ₂, h ₃); Another at C, D place, measures respectively magnetic-field component (h to two axle magnetometers ₉, h ₁₀) and (h ₇, h ₈).X _band y _bin direction, gradiometry base length is respectively L _xand L _y, earth magnetism gradient tensor G ^bfive isolated components by following formula, calculated:

$\left\{\begin{array}{c}{g}_{\mathrm{xx}}^b=\frac{h_4-h_1}{L_x},g_{\mathrm{yy}}^b=\frac{h_9-h_7}{L_y},g_{\mathrm{yx}}^b=\frac{h_5-h_2}{L_x},\\ g_{\mathrm{zy}}^b=\frac{h_{10}-h_8}{L_y},g_{\mathrm{zx}}^b=\frac{h_6-h_3}{L_x}\end{array}\right.---\left(1\right)$

Step 2, according to the indicating positions of inertia/earth magnetism integrated navigation system, from pre-stored earth magnetism gradient tensor database, extract earth magnetism gradient tensor G under Department of Geography ⁿ5 isolated components

with

Step 3, the course angle of establishing carrier are Ψ (traditionally with north by east for just), and the angle of pitch is θ, and roll angle is γ, gets three coordinate axis x of geographic coordinate system _n, y _n, z _nsensing be followed successively by sky, northeast, as shown in Figure 3.The relationship description of body axis system b and geographic coordinate system n is, the 1st step: Ox _ny _nz _naround-z _naxle rotation Ψ obtains Ox ₁y ₁z ₁, the 2nd step: Ox ₁y ₁z ₁around x ₁axle rotation θ obtains Ox ₂y ₂z ₂, the 3rd step: Ox ₂y ₂z ₂around y ₂axle rotation γ obtains Ox _by _bz _b, the transformation matrix between two coordinate systems

for

$C_n^b=\left[\begin{array}{ccc}\begin{array}{c}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\\ \sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}\end{array}& \begin{array}{c}-\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\\ \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}\end{array}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}\\ \sin \mathrm{\&psi;}\cos \mathrm{\&theta;}& \cos \mathrm{\&psi;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\\ \begin{array}{c}\sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\\ \mathrm{sos\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}\end{array}& \begin{array}{c}-\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\\ \cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}\end{array}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]---\left(2\right)$

If G ⁿand G ^bbe respectively the expression of earth magnetism gradient tensor under n system and b system, have

$G^b=C_n^b{G}^n{\left(C_n^b\right)}^T---\left(3\right)$

In formula, T representing matrix transposition.G ^bby earth magnetism gradient measuring device, measured G ⁿcan measure in advance and be stored in navigational computer.By formula (3), obtain five independent equations about attitude of carrier angle (Ψ, θ, γ).For avoiding the trigonometric function operation in optimum estimation algorithm, introduce hypercomplex number Q=q ₀+ q ₁i ₀+ q ₂j ₀+ q ₃k ₀determine that b is tied to the transformation matrix of n system,

$C_b^n=\left[\begin{array}{ccc}{\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_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{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)& {\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2& 2\left(q_2{q}_3{-q}_0{q}_1\right)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& {\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2\end{array}\right]---\left(4\right)$

By formula (3) and formula (4), obtained about q ₀, q ₁, q ₂and q ₃5 independent equations

$\begin{array}{c}{g}_{\mathrm{xx}}^b=4g_{\mathrm{xz}}^n({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_1{q}_3+q_0{q}_2)+8g_{\mathrm{zy}}^n(q_1{q}_2-q_0{q}_3)(q_1{q}_3+q_0{q}_2)+4g_{\mathrm{yy}}^n{(q_1{q}_2-q_0{q}_3)}^2\\ +g_{\mathrm{xx}}^n{({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)}^2+4g_{\mathrm{xy}}^n({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_1{q}_2-q_0{q}_3)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n){(q_1{q}_3+q_0{q}_2)}^2\end{array}---\left(5\right)$

$\begin{array}{c}{g}_{\mathrm{yy}}^b=4g_{\mathrm{xx}}^n{(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)}^2+4g_{\mathrm{yx}}^n(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)+8g_{\mathrm{zx}}^n(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)(q_2{q}_3-q_0{q}_1)\\ +g_{\mathrm{yy}}^n{({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)}^2+4g_{\mathrm{zy}}^n({\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2)(q_2{q}_3-q_0{q}_1)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)\end{array}---\left(6\right)$

$\begin{array}{c}{g}_{\mathrm{yx}}^b=2g_{\mathrm{xx}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_2+q_0{q}_3)+g_{\mathrm{yx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_0^2-q_1^2+q_2^2-q_3^2)+4\left(q_1{q}_2{-q}_0{q}_3\right)(q_1{q}_2+q_0{q}_3)]\\ +2g_{\mathrm{zx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_2{q}_3-q_0{q}_1)+2(q_1{q}_3+q_0{q}_2)(q_1{q}_2+q_0{q}_3)]+2g_{\mathrm{yy}}^n(q_1{q}_2-q_0{q}_3)(q_0^2-q_1^2+q_2^2-q_3^2)\\ +2g_{\mathrm{zy}}^n[2(q_1{q}_2-q_0{q}_3)(q_2{q}_3-q_0{q}_1)+(q_1{q}_3+q_0{q}_2)(q_0^2-q_0^2+q_0^2-q_0^2)]-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_1{q}_3+q_0{q}_2)(q_2{q}_3-q_0{q}_1)\end{array}---\left(7\right)$

$\begin{array}{c}{g}_{\mathrm{zy}}^b=4g_{\mathrm{xx}}^n(q_1{q}_2+q_0{q}_3)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[2(q_1{q}_2+q_0{q}_3)(q_2{q}_3+q_0{q}_1)+(q_0^2-q_1^2+q_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)]\\ +2g_{\mathrm{zx}}^n[(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)(q_1{q}_3-q_0{q}_2)]+2g_{\mathrm{yy}}^n(q_0^2-q_1^2+q_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)\\ {+g}_{\mathrm{zy}}^n[(q_0^2-q_1^2+q_2^2-q_3^2)(q_0^2-q_1^2-q_2^2+q_3^2)+4(q_2{q}_3-q_0{q}_1)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)(q_0^2-q_1^2-q_2^2+q_3^2)\end{array}---\left(8\right)$

$\begin{array}{c}{g}_{\mathrm{zx}}^b=2g_{\mathrm{xx}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)+2(q_1{q}_2-q_0{q}_3)(q_1{q}_3-q_0{q}_2)]\\ +g_{\mathrm{zx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_0^2-q_1^2-q_2^2+q_3^2)+4(q_1{q}_3+q_0{q}_2)(q_1{q}_3-q_0{q}_2)]+4g_{\mathrm{yy}}^n(q_1{q}_2-q_0{q}_3)(q_2{q}_3+q_0{q}_1)\\ {+2g}_{\mathrm{zy}}^n[(q_1{q}_2-q_0{q}_3)(q_0^2-q_1^2-q_2^2+q_3^2)+2(q_1{q}_3+q_0{q}_2)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_1{q}_3+q_0{q}_2)(q_0^2-q_1^2-q_2^2+q_3^2)\end{array}---\left(9\right)$

The lower earth magnetism gradient tensor component of carrier system that step 1,2 is obtained with earth magnetism gradient tensor component under Department of Geography

and

substitution is about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations:

In conjunction with q ₀, q ₁, q ₂and q ₃restriction relation

utilize newton's down-hill method to solve about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations.

The iterative formula of newton's down-hill method is:

x ⁿ⁺¹＝x ⁿ-ω(F′(x ⁿ)) ^-1F(x ⁿ) (10)

The span of ω is 0 < ω≤1, for guaranteeing convergence, generally needs the value of ω to meet

||F(x ⁿ⁺¹)||＜||F(x ⁿ)|| (11)

The value of ω adopts successively halving method, for reducing calculated amount, also partial derivative in Jacobian matrix is replaced by the formal approximation of difference coefficient; Inertial navigation system output attitude angle and the actual attitude angle of submerge device are comparatively approaching, inertial navigation system can be exported to hypercomplex number corresponding to attitude angle as newton's down-hill method iteration initial value.

Under Matlab simulated conditions, the method is carried out to emulation experiment:

Utilize magnetic field gradient that single magnetic dipole produces magnetic anomaly field gradient in analog, magnetic dipole magnetic moment component is respectively m _x=10 * 10 ⁸am ², m _y=2 * 10 ⁸am ²and m _z=1 * 10 ⁸am ², earth magnetism gradient measuring device is respectively x=100m with respect to the location components of magnetic dipole, y=50m and z=20m, Magnetic Gradient Measurement base length Δ x=Δ y=1m, algorithm iteration end condition be error ε=|| x _k+1-x _k||≤10 ^-6.

First probe into the impact of initial solution on Algorithm for Solving precision, the noise of supposing each magnetometer is separate Gaussian process, and its average is 0, and mean square deviation is σ=2nT.What Fig. 4 represented is in different initial solution situations, adopt the convergence situation of newton's down-hill method, just because of relaxing of the iterative algorithm condition of convergence being caused to the abnormal of convergence curve, but finally all can restrain after a few step iteration, in Multi simulation running experiment, attitude angle deviation generally can not surpass 20 times lower than iterative steps in the situation of 20 °, and visible, algorithm the convergence speed is very fast.Algorithm initial solution is relatively truly separated deviation ± 20 ° and can be estimated well attitude angle with interior as can be seen from Figure 5, the estimation error of the angle of pitch is slightly large, between ± 0.7 °, course angle estimation error is between ± 0.15 °, and roll angle estimation error is between ± 0.12 °.Visible algorithm is low to initial solution accuracy requirement, can estimate preferably true attitude angle departing from actual value ± 20 ° with interior, can utilize inertial navigation system output attitude information as algorithm initial solution completely, installs practical.

In the situation that initial solution is constant, consider the calculation accuracy of algorithm under different magnetometer noise levels, other simulated conditions are constant.As can be seen from Figure 6 the impact of noise on algorithm the convergence speed, at noise level lower than 10nT in the situation that, algorithm the convergence speed difference is not clearly, and after noise level is higher than 10nT, speed of convergence is obviously slack-off, lower than greatest iteration step number under 10nT noise level, is only 18 steps.The noise level of three axis fluxgate magnetometers can be less than 10nT, and therefore available a plurality of fluxgate magnetometer is measured earth magnetism gradient tensor, forms cheap earth magnetism gradient tensor measurement mechanism.Pose estimation value under the corresponding different magnetometer noise levels of Fig. 7, in the situation that magnetometer noise mean square deviation is less than 10nT, angle of pitch estimation error is substantially all in ± 0.5 °, course angle estimation error is in ± 0.13 °, roll angle estimation error is also no more than ± and 0.1 °, algorithm still can calculate attitude angle comparatively exactly.

Step 4, the hypercomplex number of iterated conditional will be met

with

the transformation matrix of substitution carrier Xi Yu Department of Geography

$C_b^n=\left[\begin{array}{ccc}{\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2+q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_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{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_0{q}_3)& {\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2& 2\left(q_2{q}_3{-q}_0{q}_1\right)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& {\mathrm{<figure-callout\; id="0"\; label="q"\; filenames="HDA0000439297110000013.png,HDA0000439297110000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_0^2-q_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000439297110000014.png,HDA0000439297110000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2\end{array}\right]---\left(12\right)$

Obtain matrix element

(i, j=1,2,3).Comparison expression (2) and formula (3) obtain:

for the estimated value of the angle of pitch, according to

matrix element

and formula (13) obtains three attitude angle.Course angle

estimated value in the following several ways: when ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}>0$ Time, $\widehat{\mathrm{\&psi;}}={90}^0;$ When ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}<0$ Time, $\widehat{\mathrm{\&psi;}}=-{90}^0;$ When ${\hat{c}}_{22}>0,{\hat{c}}_{12}>0$ Time,

when ${\hat{c}}_{22}>0,{\hat{c}}_{12}<0$ Time,

when ${\hat{c}}_{22}<0,{\hat{c}}_{12}>0$ Time,

when ${\hat{c}}_{22}<0,{\hat{c}}_{12}<0$ Time,

Roll angle estimated value divide 3 kinds of situations: work as c ₃₃during > 0, work as c ₃₃< 0, time,

work as c ₃₃< 0,

time,

Beneficial effect of the present invention is described as follows:

Utilize magnetic field gradient that single magnetic dipole produces magnetic anomaly field gradient in analog, Multi simulation running experimental result shows: this algorithm is low to initial solution accuracy requirement, at magnetometer noise level 10nT and initial value, depart from true value ± 20 ° and can estimate preferably true attitude angle with interior, can utilize fluxgate magnetometer as earth magnetism gradient tensor measuring unit completely, inertial navigation system output attitude information, as algorithm initial solution, installs cheap, practical.

Claims (1)

1. the full attitude of underwater carrier of measuring based on earth magnetism gradient tensor is determined a method, it is characterized in that:

(1) under carrier system, set up three-dimensional cartesian coordinate system x _b, y _b, z _b, the magnetometer of two three axle quadratures is placed on respectively x _ba, B 2 points of axle symmetry centered by initial point, measure respectively magnetic-field component (h ₄, h ₅, h ₆) and (h ₁, h ₂, h ₃); Another is placed in respectively y to two axle orthogonal magnetometers _bc, D 2 points of axle symmetry centered by initial point, measure respectively magnetic-field component (h ₉, h ₁₀) and (h ₇, h ₈), the sensitive axes direction of all magnetometers is consistent, base area magnetic gradient tensor G ^b5 components

and

with magnetic-field component h ₁～h ₁₀relational expression,

$\left\{\begin{array}{c}{g}_{\mathrm{xx}}^b=\frac{h_4-h_1}{L_x},g_{\mathrm{yy}}^b=\frac{h_9-h_7}{L_y},g_{\mathrm{yx}}^b=\frac{h_5-h_2}{L_x},\\ g_{\mathrm{zy}}^b=\frac{h_{10}-h_8}{L_y},g_{\mathrm{zx}}^b=\frac{h_6-h_3}{L_x}\end{array}\right.$

Obtain 5 components of magnetic gradient tensor, L _xand L _ybe respectively x _band y _bgradiometry base length in direction;

(2), according to the indicating positions of inertia/earth magnetism integrated navigation system, from pre-stored earth magnetism gradient tensor database, extract earth magnetism gradient tensor G under Department of Geography ⁿ5 isolated components

and

(3) the lower earth magnetism gradient tensor component of carrier system step 1,2 being obtained with earth magnetism gradient tensor component under Department of Geography and

substitution is about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations:

$\begin{array}{c}{g}_{\mathrm{xx}}^b=4g_{\mathrm{xz}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_3+q_0{q}_2)+8g_{\mathrm{zy}}^n(q_1{q}_2-q_0{q}_3)(q_1{q}_3+q_0{q}_2)+4g_{\mathrm{yy}}^n{(q_1{q}_2-q_0{q}_3)}^2\\ +g_{\mathrm{xx}}^n{(q_0^2+q_1^2-q_2^2-q_3^2)}^2+4g_{\mathrm{xy}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_2-q_0{q}_3)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n){(q_1{q}_3+q_0{q}_2)}^2\end{array}$

$\begin{array}{c}{g}_{\mathrm{yy}}^b=4g_{\mathrm{xx}}^n{(q_1{q}_2+q_0{q}_3)}^2+4g_{\mathrm{yx}}^n(q_1{q}_2+q_0{q}_3)(q_0^2-q_1^2+q_2^2-q_3^2)+8g_{\mathrm{zx}}^n(q_1{q}_2+q_0{q}_3)(q_2{q}_3-q_0{q}_1)\\ +g_{\mathrm{yy}}^n{(q_0^2-q_1^2+q_2^2-q_3^2)}^2+4g_{\mathrm{zy}}^n(q_0^2-q_1^2+q_2^2-q_3^2)(q_2{q}_3-q_0{q}_1)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)\end{array}$

$\begin{array}{c}{g}_{\mathrm{zy}}^b=4g_{\mathrm{xx}}^n(q_1{q}_2+q_0{q}_3)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[2(q_1{q}_2+q_0{q}_3)(q_2{q}_3+q_0{q}_1)+(q_0^2-q_1^2+q_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)]\\ +2g_{\mathrm{zx}}^n[(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)(q_1{q}_3-q_0{q}_2)]+2g_{\mathrm{yy}}^n(q_0^2-q_1^2+q_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)\\ {+g}_{\mathrm{zy}}^n[(q_0^2-q_1^2+q_2^2-q_3^2)(q_0^2-q_1^2-q_2^2+q_3^2)+4(q_2{q}_3-q_0{q}_1)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)(q_0^2-q_1^2-q_2^2+q_3^2)\end{array}$

$\begin{array}{c}{g}_{\mathrm{zx}}^b=2g_{\mathrm{xx}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)+2(q_1{q}_2-q_0{q}_3)(q_1{q}_3-q_0{q}_2)]\\ +g_{\mathrm{zx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_0^2-q_1^2-q_2^2+q_3^2)+4(q_1{q}_3+q_0{q}_2)(q_1{q}_3-q_0{q}_2)]+4g_{\mathrm{yy}}^n(q_1{q}_2-q_0{q}_3)(q_2{q}_3+q_0{q}_1)\\ {+2g}_{\mathrm{zy}}^n[(q_1{q}_2-q_0{q}_3)(q_0^2-q_1^2-q_2^2+q_3^2)+2(q_1{q}_3+q_0{q}_2)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_1{q}_3+q_0{q}_2)(q_0^2-q_1^2-q_2^2+q_3^2)\end{array}$

About q ₀, q ₁, q ₂and q ₃nonlinear System of Equations can be by G ⁿand G ^brelational expression

obtain, T representing matrix transposition wherein,

the transformation matrix that geography is tied to carrier system, and

the transformation matrix of carrier Xi Yu Department of Geography, and

in conjunction with q ₀, q ₁, q ₂and q ₃restriction relation

utilize newton's down-hill method to solve about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations;

(4) by the hypercomplex number that meets iterated conditional obtaining in step (3)

with

the transformation matrix of substitution carrier Xi Yu Department of Geography,

$C_b^n=\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\left(q_2{q}_3{-q}_0{q}_1\right)\\ 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],$

Obtain

matrix element

(i, j=1,2,3),

Obtain the estimated value of pitching angle theta;

Course angle

estimated value in the following several ways: when ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}>0$ Time, $\widehat{\mathrm{\&psi;}}={90}^0;$ When ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}<0$ Time, $\widehat{\mathrm{\&psi;}}=-{90}^0;$ When ${\hat{c}}_{22}>0,{\hat{c}}_{12}>0$ Time, when ${\hat{c}}_{22}>0,{\hat{c}}_{12}<0$ Time,

when ${\hat{c}}_{22}<0,{\hat{c}}_{12}>0$ Time,

when ${\hat{c}}_{22}<0,{\hat{c}}_{12}<0$ Time,

Roll angle

estimated value divide 3 kinds of situations: work as c ₃₃during > 0,

work as c ₃₃< 0,

time, when time,

Images