CN104165642B - Method for directly correcting and compensating course angle of navigation system - Google Patents

Abstract

The invention discloses a method for directly correcting and compensating a course angle of a navigation system and belongs to the technical field of location of navigation systems. The method for directly correcting and compensating the course angle of the navigation system comprises the following steps: firstly expanding an error model of a magnetometer according to the basic principle of circulating the course angle by virtue of the magnetometer; secondarily obtaining a parameter estimation model according to distribution characteristics of an earth magnetic field; finally estimating parameters of the expanded error model of the magnetometer by using a two-step total least square method so as to directly correct the course angle of the navigation system at a system level. The method can be used for correcting and compensating the error of a course angle of an inertial integrated navigation and location system which comprises an inertia measurement unit (IMU) and the magnetometer (or an electronic compass). The method is capable of directly correcting the course angle of the navigation system, strong in robustness, strong in reliability, low in cost, high in accuracy, simple in calculation process, and capable of achieving high-accuracy location and navigation and achieving good effects.

Description

One kind is used for navigation system course angle direct rectification building-out method

Technical field

The present invention relates to one kind directly corrects and compensation method for navigation system course angle, belong to navigation system positioning skill Art field.

Background technology

In the evolution of navigation system, one most important function of navigation system is exactly to provide the exact posture of carrier And course angle information.For simply using accelerometer and gyroscope composition Inertial Measurement Unit (IMU), due to accelerometer and The error of gyroscope itself and drift make the attitude of IMU output and course parameters precision can not meet wanting of some navigation system Ask.The navigation system being made up of Inertial Measurement Unit (IMU) and magnetometer, can improve the precision of attitude angle and course angle.Magnetic Power meter applies ratio wide abroad, but yet suffers from some problems in terms of field compensation, is devoted to finding a kind of high accuracy The correction of magnetometer and compensation method are magnetometer correction and the difficult point place compensating.

Carry out navigator fix with earth's magnetic field, there is passive, radiationless, anti-interference, round-the-clock, round-the-clock, small volume, energy consumption Low advantage, is therefore used widely in fields such as aircraft, naval vessel and submarines.Navigation carrier passes through Magnetic Sensor measurement space Magnetic Field, these Magnetic Field not only include the geomagnetic field information used by navigator fix, also include the interference of carrier itself Magnetic Field.Need during high-precision earth-magnetism navigation the observation Magnetic Field of Magnetic Sensor to be processed, in real time to load Body magnetic interference compensates, and improves earth-magnetism navigation precision.

At present, about being corrected to the magnetometer in navigation system and compensation method is very universal, Part Methods Certain required precision that is very ripe, can reaching.But, the occasion existing method for high-precision requirement is not met by, Existing method needs for the error model of magnetometer to be converted into ellipsoidal model simultaneously, estimates that the parameter of ellipsoidal model is then converted to Error model, such processing method is not succinct.

Content of the invention

Goal of the invention：Magnetometer cannot be corrected to overcome and compensate in high precision and strong to reach in prior art The situation of robustness, the present invention provides one kind to be used for navigation system course angle direct rectification building-out method, and the method can improve Magnetometer correction and the precision compensating, the method strong robustness simultaneously, processing procedure is succinct.

For achieving the above object, the technical solution used in the present invention is：One kind directly corrects for navigation system course angle Compensation method, comprises the following steps：

Step A, according to the error source and its error source characteristic that affect three axle output vectors of magnetometer in navigation system, builds Vertical magnetometer output error models；

Step B, the initial point with the barycenter of carrier as carrier coordinate system, O_bY_bAxle points to carrier direction of advance, O_bX_bAxle and load Body direction of advance is perpendicularly oriented to the right side, O_bZ_bAxle and O_bY_b, O_bX_bAxle constitutes the right-hand rule and sets up carrier coordinate system O_bX_bY_bZ_b；According to Local geographic coordinate system sets up local horizontal coordinates O₁X₁Y₁Z₁；Using carrier coordinate system O_bX_bY_bZ_bTo local horizontal coordinates O₁X₁Y₁Z₁Transformation matrix, the output vector of three axles of magnetometer is transformed into local horizontal coordinates by carrier coordinate system, that is, The output error models of the magnetometer that step A is drawn are expanded, and obtain expanding magnetometer output error models；

Step C, according to first feature letter of distribution in local horizontal coordinates for the three axle output vectors of magnetometer Change the expansion magnetometer output error models that step B obtains, and then obtain the error model for parameter estimation；Described first Feature refers to three axle output vectors of magnetometer O in local horizontal coordinates under ideal conditions₁Z₁The output valve of axle is one Individual constant amount；Described ideal conditionss refer to that magnetometer does not have error interference, and the Magnetic Field of output is exactly the true of earth's magnetic field Real information.

Step D, in the error model for parameter estimation step C being obtained by two step total least squares methods Parameter is estimated, then calculates navigation system course angle according to the parameter estimating, completes navigation system course angle direct Rectification building-out.

Setting up magnetometer output error models in described step A is：

In upper,For correction after magnetometer three axle output vectors,For total error matrix,For total The inverse matrix of error matrix,For three axle output vectors of magnetometer, b^bFor zero bias error vector.

The expansion magnetometer output error models drawing in described step B：

Wherein,It is vector value in local horizontal coordinates for the three axle output vectors of magnetometer,It is carrier coordinate It is tied to the direction cosine matrix of local horizontal coordinates, 1 represents local horizontal coordinates, and b represents carrier coordinate system,For correction Three axle output vectors of magnetometer afterwards,For total error matrix,For total error inverse of a matrix matrix, For three axle output vectors of magnetometer, b^bFor zero bias error vector.

The error model for parameter estimation obtaining in described step C comprises the following steps：

Step C1, obtains the angle of pitch P and roll angle R of carrier according to the attitude information of navigation system, using the angle of pitch and Roll angle information can obtain carrier coordinate system to the direction cosine matrix of local horizontal coordinatesAnd angle of pitch P and roll Angle R is obtained by the output information of the accelerometer in navigation system, willSubstitute into expression formula (2) can obtain：

Wherein,

b^b=[n₁n₂n₃]^T,

(a′₁₁, a '₁₂, a '₁₃, a '₂₁, a '₂₂, a '₂₃, a '₃₁, a '₃₂, a '₃₃) for matrix M each component, (b₁₁, b₁₂, b₁₃, b₂₁, b₂₂, b₂₃, b₃₁, b₃₂, b₃₃) it is matrixEach component, (n₁, n₂, n₃) it is vector b^bEach component, (x, y, z) is VectorEach component, (x₀, y₀, z₀) it is vectorEach component.

Step C2, according to first feature pair of distribution in local horizontal coordinates for the three axle output vectors of magnetometer Formula (3) in step C1 carries out abbreviation, can be used for the error model of parameter estimation：

c₁a₁₁+c₂a₁₂+c₃a₁₃+c₄a₂₁+c₅a₂₂+c₆a₂₃+c₇a₃₁+c₈a₃₂+c₉a₃₃+c₁₀α+c₁₁β+c₁₂γ=1 (4)

Wherein,

The side that in described step D, two step total least squares methods are estimated to the parameter in parameter estimating error model Method, a step is estimated to obtain parameter (a₁₁, a₁₂, a₁₃, a₂₁, a₂₂, a₂₃, a₃₁, a₃₂, a₃₃, α, beta, gamma), recycle the ginseng that a step is estimated Number is calculated parameter (n₁, n₂, n₃), specifically include following steps：

Step D1, three axle output vectors of magnetometer is substituted into the parameter estimating error model drawing in step C2, obtains：

A σ=b (5)

Wherein,

I represents three axle output arrows of magnetometer I-th group of data of amount, n represents the n-th group of data of three axle output vectors of magnetometer, n group data altogether；

Step D2, by Least Square Method parameter (a₁₁, a₁₂, a₁₃, a₂₁, a₂₂, a₂₃, a₃₁, a₃₂, a₃₃, α, beta, gamma), First, matrix most preferably approached by A, the formula (5) in step D1 is converted into：

A ' σ=(A+E) σ=b (6)

Wherein, A ' be A most preferably approach matrix, A '=A+E, E be A in contain systematic error interference；

Then, remember, Matrix C=[A b], singular value decomposition is carried out to Matrix C, can obtain：U^TCV=∑, wherein, U, V are Orthogonal matrix, ∑ is diagonal matrix, subscript T representing matrix transposition；Note matrix V=[V₁V₂… V₁₂V₁₃], wherein (V₁, V₂..., V₁₂, V₁₃) be respectively matrix V column vector；

Finally, the solution obtaining expression formula (5) is：

This solution is parameter (a₁₁, a₁₂, a₁₃, a₂₁, a₂₂, a₂₃, a₃₁, a₃₂, a₃₃, α, beta, gamma) value,

Step D3, the parameter (a being obtained using step D2₁₁, a₁₂, a₁₃, a₂₁, a₂₂, a₂₃, a₃₁, a₃₂, a₃₃, α, beta, gamma) calculate Parameter (n₁, n₂, n₃), can be obtained by formula (4)：

Formula (7) is one group of system of linear equations, solves above-mentioned system of linear equations and can obtain parameter (n₁, n₂, n₃) value.

One kind that the present invention provides is used for navigation system course angle direct rectification building-out method, compared to existing technology, has Following beneficial effect：

(1) method strong robustness proposed by the invention, reliability are strong, low cost, high precision are it is adaptable to improve navigation system Course angular accuracy, thus improve navigation and stationkeeping ability；

(2) method proposed by the invention is compared with prior art it is not necessary to be converted into ellipsoid magnetometer error model Model, then by estimating ellipsoidal model parameter, and then obtaining magnetometer error model parameters, the present invention is directly to magnetometer error Models treated, thus decrease some calculation errors producing because of model conversation；

(3) prior art considers that magnetometer error model and estimation difference model parameter are all to examine in carrier coordinate system Consider, but needed three axle outputs of magnetometer using the course angle that three axle output informations of magnetometer solve navigation system Information is transformed into local horizontal coordinates.Therefore, three axle output informations of magnetometer are first turned by method proposed by the invention Change to after local horizontal coordinates and carry out parameter estimation again, so can reduce some calculating producing because of coordinate system conversion Error；

(4) prior art solve error model parameters when, by total error matrixRegard symmetrical matrix as, but in reality In, total error matrix is not symmetrical matrix, thus can produce error of approximation.The present invention considers total error matrix generality, because And applied range, the algorithm that simultaneously proposed process succinct, using data volume few it is not necessary to outside any information, from And the method hinge structure being proposed is easier to realize real-time.

Brief description

Fig. 1 for magnetometer correction and compensates schematic diagram.

Specific embodiment

Below in conjunction with the accompanying drawings the present invention is further described.

One kind is used for navigation system course angle direct rectification building-out method, as shown in figure 1, comprising the following steps：

For achieving the above object, the technical solution used in the present invention is：One kind directly corrects for navigation system course angle Compensation method, comprises the following steps：

Step A, according to the error source and its error source characteristic that affect three axle output vectors of magnetometer in navigation system, builds Vertical magnetometer output error models；

Step B, using carrier coordinate system O_bX_bY_bZ_b(barycenter choosing carrier is the initial point of carrier coordinate system, O_bY_bAxle refers to To carrier direction of advance, O_bX_bAxle and carrier direction of advance are perpendicularly oriented to the right side, O_bZ_bAxle and O_bY_b, O_bX_bAxle constitutes the right-hand rule) arrive Local horizontal coordinates O_lX_lY_lZ_l(local horizontal coordinates is similar with local geographic coordinate system (choosing northeast sky coordinate system), will Carrier coordinate system rotation makes the angle of pitch and roll angle level, and angle of not changing course, therefore local horizontal coordinates and local ground Reason coordinate system differ only by a course angle) transformation matrix, the output vector of three axles of magnetometer is changed by carrier coordinate system To local horizontal coordinates, the output error models of the magnetometer step A being drawn are expanded, and obtain expansion magnetometer defeated Go out error model；

Step C, first feature according to distribution in local horizontal coordinates for the three axle output vectors of magnetometer ( (output of magnetometer does not have error, and the Magnetic Field of output is exactly the real information in earth's magnetic field) magnetometer under the conditions of preferably Three axle output vector O in local horizontal coordinates_lZ_lThe output valve of axle is constant, is a constant amount), simplify The expansion magnetometer output error models that step B obtains, and then obtain the error model for parameter estimation；

Step D, in the error model for parameter estimation step C being obtained by two step total least squares methods Parameter is estimated, then calculates navigation system course angle according to the parameter estimating, completes navigation system course angle direct Rectification building-out；

Setting up magnetometer output error models in described step A is：

In upper,For correction after magnetometer three axle output vectors, For total error matrix,Miss for total Difference inverse of a matrix matrix,For three axle output vectors of magnetometer, b^bFor zero bias error vector.

The expansion magnetometer output error models drawing in described step B：

Wherein,It is vector value in local horizontal coordinates for the three axle output vectors of magnetometer,It is carrier coordinate system Arrive the direction cosine matrix of local horizontal coordinates, l represents local horizontal coordinates, and b represents carrier coordinate system,After correction Magnetometer three axle output vectors, For total error matrix,For total error inverse of a matrix matrix,For magnetic force Three axle output vectors of meter, b^bFor zero bias error vector.

Show in described step C that the error model for parameter estimation obtaining through abbreviation comprises the following steps：

Step C1, obtains the angle of pitch P and roll angle R of carrier according to the attitude information of navigation system, using the angle of pitch and Roll angle information can obtain carrier coordinate system to the direction cosine matrix of local horizontal coordinatesAnd angle of pitch P and roll Angle R is obtained by the output information of the accelerometer in navigation system, willSubstitute into expression formula (2) can obtain：

Wherein,

b^b=[n₁n₂n₃]^T,

(a′₁₁, a '₁₂, a '₁₃, a '₂₁, a '₂₂, a '₂₃, a '₃₁, a '₃₂, a '₃₃) for matrix M each component, (b₁₁,_b13, b₁₂, b₂₁, b₂₂, b₂₃, b₃₁, b₃₂, b₃₃) it is matrixEach component, (n₁, n₂, n₃) it is vector b^bEach component, (x, y, z) is VectorEach component, (x₀, y₀, z₀) it is vectorEach component.

Step C2, according to first feature of distribution in local horizontal coordinates for the three axle output vectors of magnetometer ((output of magnetometer does not have error, and the Magnetic Field of output is exactly the real information in earth's magnetic field) magnetic under ideal conditions Power meter three axle output vectors O in local horizontal coordinates_lZ_lThe output valve of axle is constant, is a constant amount) right Formula (3) in step C1 carries out abbreviation, can be used for the error model of parameter estimation：

c₁a₁₁+c₂a₁₂+c₃a₁₃+c₄a₂₁+c₅a₂₂+c₆a₂₃+c₇a₃₁+c₈a₃₂+c₉a₃₃+c₁₀α+c₁₁β+c₁₂γ=1 (4)

Wherein,

The side that in described step D, two step total least squares methods are estimated to the parameter of parameter estimating error model Method, a step is estimated to obtain parameter (a₁₁, a₁₂, a₁₃, a₂₁, a₂₂, a₂₃, a₃₁, a₃₂, a₃₃, α, beta, gamma), recycle the ginseng that a step is estimated Number is calculated parameter (n₁, n₂, n₃), comprise the following steps：

Step D1, three axle output vectors of magnetometer is substituted into the parameter estimating error model drawing in step C2, obtains：

A σ=b (5)

Wherein,

I represents three axle output arrows of magnetometer I-th group of data of amount, n represents the n-th group of data of three axle output vectors of magnetometer, n group data altogether；

Step D2, by Least Square Method parameter (a₁₁, a₁₂, a₁₃, a₂₁, a₂₂, a₂₃, a₃₁, a₃₂, a₃₃, α, beta, gamma), First, matrix most preferably approached by A, the formula (5) in step D1 is converted into：

A ' σ=(A+E) σ=b (6)

Wherein, A ' be A most preferably approach matrix, A '=A+E, E be A in contain systematic error interference；

Then, remember, Matrix C=[A b], singular value decomposition is carried out to Matrix C, can obtain：U^TCV=∑, wherein, U, V are Orthogonal matrix, ∑ is diagonal matrix, subscript T representing matrix transposition；Note matrix V=[V₁V₂… V₁₂V₁₃], wherein (V₁, V₂..., V₁₂, V₁₃) be respectively matrix V column vector；

Finally, the solution obtaining expression formula (5) is：

This solution is parameter (a₁₁, a₁₂, a₁₃, a₂₁, a₂₂, a₂₃, a₃₁, a₃₂, a₃₃, α, beta, gamma) value,

Step D3, the parameter (a being obtained using step D2₁₁, a₁₂, a₁₃, a₂₁, a₂₂, a₂₃, a₃₁, a₃₂, a₃₃, α, beta, gamma) calculate Parameter (n₁, n₂, n₃), can be obtained by formula (4)：

Formula (7) is one group of system of linear equations, solves above-mentioned system of linear equations and can obtain parameter (n₁, n₂, n₃) value.

For the ease of understanding the present invention, now the principle of the present invention is carried out as described below：

One kind is used for navigation system course angle direct rectification building-out method, comprises the following steps：

(1) in order to realize the direct correction to navigation system course angle, the present invention calculates course angle according to magnetometer first Ultimate principle, expanded the error model of magnetometer, and then the distribution characteristicss according to earth's magnetic field, finally utilize two steps overall The parameter of the error model to the magnetometer expanded for the method for least square is estimated, may finally realize system-level to navigation system System course angle is directly corrected.This bearing calibration avoids some bearing calibrations traditional and first error model is converted into ellipsoid mould Type, then to ellipsoidal model parameter estimation, then the ellipsoidal model after estimating is converted into magnetometer error model, finally realizes leading The correction of boat system course angle and compensation.The present invention is directly corrected to navigation system course angle, and method is easy, emulates simultaneously Experimental verification obtains this method than traditional method high precision, and strong robustness, the therefore present invention can effectively be realized leading The correction of boat system course angle and compensation, and then realize hi-Fix and navigation, and obtain good result.

(2) cardinal principle that the output error models of magnetometer are expanded is：Magnetometer calculates the basic of course angle Principle is, by the attitude information of carrier, three axle output vectors of magnetometer are transformed into local horizontal coordinates, then sharp again X in horizontal coordinates, the component of Y-axis carry out the calculating of course angle.Simultaneously in order to improve navigation system precision, existing The disposable bearing calibration of system-level magnetometer it is contemplated that the alignment error of magnetometer, all using realizing to magnetic force on a system level The correction of meter, is so more beneficial for improving the precision of navigation system.The present invention first will be defeated for three axles of magnetometer according to these principles Go out vector to be transformed into local horizontal coordinates and then again magnetometer is corrected, and three axle output vectors of magnetometer are converted Attitude information is needed during to local horizontal coordinates system.Due to being on a system level magnetometer to be corrected, so these appearances State information can (e.g., attitude information by the gyroscope in navigation system and can add by some redundancies of navigation system The output information of velometer obtains) obtain, and the present invention is exactly corrected to magnetometer using these redundancies.By magnetic force After three axle output vectors of meter transform to local horizontal coordinates, further according to distribution in local horizontal coordinates for the earth's magnetic field Feature, realizes the parameter estimation to error model.The present invention does not need any external information beyond IMU.

(3) after the output vector of three axles of magnetometer being transformed into local horizontal coordinates by carrier coordinate system, according to The distribution characteristicss in earth's magnetic field understand, at this moment, (output of magnetometer does not have error, the magnetic field letter of output under ideal conditions Breath is exactly the real information in earth's magnetic field) output valve of three axle output vector axiss of ordinates in local horizontal coordinates of magnetometer It is constant, be a constant amount.In addition, magnetometer three axle output vectors X, Y-axis in local horizontal coordinates Output valve is on a circle, that is, meet round equation (x²+y²=g, wherein g be equation of a circle radius square, be a constant, (x, y) is the output valve of magnetometer X, Y-axis in local horizontal coordinates).The present invention is adopted by carefully analyzing this two features With first feature, reason is, if adopting second feature, then the expansion error model parameters obtaining are relatively more, Calculate complicated, also ratio is larger for amount of calculation, therefore adopt first error characteristics.

(4) obtain an error model using the method in (3), be a linear equation, by the magnetometer collecting three Output valve after individual axle output valve transforms to horizontal coordinates is updated in this equation it is possible to be estimated according to existing parameter Meter method realizes the estimation of error parameter, and the present invention, according to the error distribution characteristicss of this linear equation, estimates in conjunction with existing parameter Meter method, it is proposed that two step total least squares methods, compared for the method and traditional least square by emulation experiment simultaneously Method is compared, and precision is higher, simultaneously higher to the robustness of noise, and why the present invention proposes obtained by this method cause is The coefficient matrix of equation group be containing error, if directly adopting method of least square, then these error components will be directly Impact estimated accuracy.

Specifically include following steps：

(1) magnetometer output error models are：

If not considering Gauss white noise ε₀Impact, then magnetometer can be obtained by the conversion of a series of abbreviation Error ellipsoid model as follows：

In formula,For total error matrix；ForInverse matrix；B^bIt is to work as geomagnetic fieldvector；Three axles for magnetometer Output vector；b^bFor zero bias error vector；ε₀Being measurement error vector it is considered that being Gauss white noise, can ignore；The three axle output vectors for the magnetometer after correction.Subscript b represents value in carrier coordinate system for this vector.Can be to above-mentioned It is as follows that expression formula (9) makees conversion further：

Note：Then, formula (9) can be expressed as：

Above-mentioned expression formula (1) is in carrier coordinate system, the output error of magnetometer to be modeled.The present invention utilizes carrier Coordinate is tied to the transformation matrix of local horizontal coordinates, and the output error models of magnetometer are expanded, and concrete conversion is as follows：

In formulaIt is value in local horizontal coordinates for the three axle output vectors of magnetometer,Be carrier coordinate system arrive work as The direction cosine matrix of ground horizontal coordinates, subscript 1 represents value in local horizontal coordinates for this vector.By expression formula (2) Understand, realize the correction of magnetometer being sought to realize to parameter M and parameter b^bEstimation.

(2) according to above analysis, the expansion error model of magnetometer is derived in detail as follows：First according to navigation system The attitude information of system obtains the angle of pitch P and roll angle R of carrier, can obtain carrier coordinate using the angle of pitch and roll angle information It is tied to the direction cosine matrix of local horizontal coordinatesExpression form as follows：

Parameter P in above-mentioned expression formula (10) and R, can be believed by the output of the gyroscope in navigation system and accelerometer Breath obtains, then can be byRegard known quantity as.Note：

Expression formula (2) is carried out launch and abbreviation using above parameter, can obtain：

First spy according to the distribution to three axle output vectors of magnetometer in local horizontal coordinates described above Zheng Ke get：

(b₃₁a′₁₁+b₃₂a′₂₁+b₃₃a′₃₁)(x-n₁)+(b₃₁a′₁₂+b₃₂a′₂₂+b₃₃a′₃₂)(y-n₂)+(b₃₁a′₁₃+b₃₂a′₂₃ +b₃₃a′₃₃)(z-n₃)=z₀

According to the formula being calculated navigation system course angle from three axle output vectors of magnetometer, matrix M is taken advantage of simultaneously With or simultaneously divided by a constant, do not affect the calculating of navigation system course angle.Then by above formula both sides simultaneously divided by z₀Can obtain：

In formula,Do further abbreviation to expression formula (11) can obtain：

Note：Then, expression formula (12), Can be further simplified as：

Wherein, (c₁, c₂, c₃, c₄, c₅, c₆, c₇, c₈, c₉, c₁₀, c₁₁, c₁₂) it is known quantity, realize the correction to magnetometer Essence seeks to obtain parameter (a₁₁, a₁₂, a₁₃, a₂₁, a₂₂, a₂₃, a₃₁, a₃₂, a₃₃, n₁, n₂, n₃).The present invention adopts Two-step estimation Method, estimates parameter (a one step ahead₁₁, a₁₂, a₁₃, a₂₁, a₂₂, a₂₃, a₃₁, a₃₂, a₃₃, α, beta, gamma), then estimated by a step Parameter (a₁₁, a₁₂, a₁₃, a₂₁, a₂₂, a₂₃, a₃₁, a₃₂, a₃₃, α, beta, gamma) obtain parameter (n₁, n₂, n₃), finally give former error mould Parameter (a of type₁₁, a₁₂, a₁₃, a₂₁, a₂₂, a₂₃, a₃₁, a₃₂, a₃₃, n₁, n₂, n₃).

(3) second according to the distribution to three axle output vectors of magnetometer in local horizontal coordinates described above Feature can obtain：

In formula, g is a constant, x₀And y₀Form as follows：

x₀=(b₁₁a′₁₁+b₁₂a′₂₁+b₁₃a′₃₁)(x-n₁)+(b₁₁a′₁₂+b₁₂a′₂₂+b₁₃a′₃₂)(y-n₂)

+(b₁₁a′₁₃+b₁₂a′₂₃+b₁₃a′₃₃)(z-n₃)

y₀=(b₂₁a′₁₁+b₂₂a′₂₁+b₂₃a′₃₁)(x-n₁)+(b₂₁a′₁₂+b₂₂a′₂₂+b₂₃a′₃₂)(y-n₂)

+(b₂₁a′₁₃+b₂₂a′₂₃+b₂₃a′₃₃)(z-n₃)

By x₀And y₀It is updated in expression formula (13) and carry out the general equation that a series of abbreviation can obtain an ellipsoid Expression formula, then the parameter of the error model of magnetometer can be arrived by variable replacement and ellipsoid fitting.From above-mentioned derivation Although journey can be seen that this method can realize the correction of magnetometer, process is complicated.The present invention proposes this method In order to illustrate that first feature of the distribution in local horizontal coordinates obtains according to three axle output vectors of magnetometer for selection The reason expand error model.

(3) by above-mentioned analysis, the present invention adopts the expansion error model of expression formula (4) form.In order to realize to expression The estimation of formula (4) parameter, the present invention adopts two step total least squares methods.Specific algorithm process is as follows：

Note：

Wherein, i represents i-th group of data of three axle output vectors of magnetometer, and n represents three axle output vectors of magnetometer N-th group of data, n group data altogether.Then, the n group data of three axle output vectors of magnetometer is substituted into equation (4) and n can be obtained Individual equation, these equations are write as matrix form, and its matrix representation forms is as follows：

A σ=b (5)

Then, it is can be found that by above analysis and magnetometer error model parameters are carried out estimating substantially to translate into ask The solution of solution expression formula (5).The method solving expression formula (5) is a lot, and most common method is to utilize method of least square, but due to Method of least square, only considered the inexactness of b, and does not consider the uncertainty of matrix A.For expression formula (5), due to matrix A is to be calculated using three axle output vector data of magnetometer, and three axle output vector data of magnetometer can be subject to The impact such as system noise will produce error.It is thus impossible to directly adopt method of least square to solve expression formula (5) obtain magnetometer by mistake The parameter of differential mode type.In expression formula (5), b does not contain the margin of error, is accurate, and contains the margin of error in A, that is, inaccurately. So need find A most preferably approach A ' so that：

A ' σ=b (14)

Therefore the present invention adopts two step total least square methods, and the method has taken into full account that A's in expression formula (5) is inaccurate Property.Detailed process is as follows：Assume that the A ' that most preferably approaches of A is：A '=A+E, wherein, E is the systematic error interference containing in A, then Expression formula (14) can be converted into following form：

A ' σ=(A+E) σ=b (6)

Note, Matrix C=[A b], singular value decomposition is carried out to Matrix C, can obtain：U^TCV=∑, wherein, U, V are orthogonal Matrix, ∑ is diagonal matrix.Note matrix V=[V₁V₂…V₁₂V₁₃], wherein (V₁, V₂..., V₁₂, V₁₃) be respectively matrix V row to Amount.Then, the solution that can obtain expression formula (6) is：

Parameter (a being obtained using formula (15)₁₁, a₁₂, a₁₃, a₂₂, a₂₃, a₃₃, n₁, n₂, n₃) calculating parameter (n₁, n₂, n₃).Meter Calculation method is as follows, can be obtained by formula (12)：

Formula (7) is one group of system of linear equations, solves above-mentioned system of linear equations and can obtain parameter (n₁, n₂, n₃) value.

In sum, present invention could apply to by the group such as Inertial Measurement Unit (IMU) and magnetometer (or electronic compass) The error correction of course angle and compensation in the inertia combined navigation becoming and alignment system.This bearing calibration avoids some schools traditional Error model is first converted into ellipsoidal model by correction method, then to ellipsoidal model parameter estimation, then ellipsoidal model is converted into magnetic Power meter error model, finally realizes correction and the compensation of navigation system course angle.It is an advantage of the current invention that directly to navigation system System course angle is corrected, strong robustness, and highly reliable, low cost, high precision, calculating process process succinct.The therefore present invention Correction and the compensation of navigation system course angle can effectively be realized, and then realize hi-Fix and navigation, and obtain good Effect.

The above be only the preferred embodiment of the present invention it should be pointed out that：Ordinary skill people for the art For member, under the premise without departing from the principles of the invention, some improvements and modifications can also be made, these improvements and modifications also should It is considered as protection scope of the present invention.

Claims (3)

1. a kind of navigation system course angle direct rectification building-out method that is used for is it is characterised in that comprise the following steps：

Step A, according to the error source and its error source characteristic that affect three axle output vectors of magnetometer in navigation system, sets up magnetic Power meter output error models；

Setting up magnetometer output error models in step A is：

${\hat{B}}^b=H({\hat{B}}^h-b^b)---\left(1\right)$

In upper,For correction after magnetometer three axle output vectors, For total error matrix,For total error square The inverse matrix of battle array,For three axle output vectors of magnetometer, b^bFor zero bias error vector；

Step B, the initial point with the barycenter of carrier as carrier coordinate system, O_bY_bAxle points to carrier direction of advance, O_bX_bBefore axle and carrier Enter direction and be perpendicularly oriented to the right side, O_bZ_bAxle and O_bY_b,O_bX_bAxle constitutes the right-hand rule and sets up carrier coordinate system O_bX_bY_bZ_b；According to locality Geographic coordinate system sets up local horizontal coordinates O_lX_lY_lZ_l；Using carrier coordinate system O_bX_bY_bZ_bTo local horizontal coordinates O_lX_lY_lZ_lTransformation matrix, the output vector of three axles of magnetometer is transformed into local horizontal coordinates by carrier coordinate system, that is, The output error models of the magnetometer that step A is drawn are expanded, and obtain expanding magnetometer output error models；

The expansion magnetometer output error models drawing in step B：

${\hat{B}}^l=C_b^l{\hat{B}}^b=C_b^{l}M({\hat{B}}^h-b^b)---\left(2\right)$

Wherein,It is vector value in local horizontal coordinates for the three axle output vectors of magnetometer,Be carrier coordinate system arrive work as The direction cosine matrix of ground horizontal coordinates, l represents local horizontal coordinates, and b represents carrier coordinate system,For the magnetic after correction Three axle output vectors of power meter, For total error matrix,For total error inverse of a matrix matrix,For magnetometer Three axle output vectors, b^bFor zero bias error vector；

Step C, according to first feature reduction step of distribution in local horizontal coordinates for the three axle output vectors of magnetometer The expansion magnetometer output error models that rapid B obtains, and then obtain the error model for parameter estimation；Described first feature Refer to three axle output vectors of magnetometer O in local horizontal coordinates under ideal conditions_lZ_lThe output valve of axle is a perseverance Fixed amount；

The error model for parameter estimation obtaining in step C comprises the following steps：

Step C1, obtains the angle of pitch P and roll angle R of carrier, using the angle of pitch and roll according to the attitude information of navigation system Angle information can obtain carrier coordinate system to the direction cosine matrix of local horizontal coordinatesAnd angle of pitch P and roll angle R by The output information of the accelerometer in navigation system obtains, willSubstitute into expression formula (2) can obtain：

$\begin{array}{c}{\hat{B}}^l=C_b^{l}M({\hat{B}}^h-b^b)=\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]=\left[\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ b_{21}& b_{22}& b_{23}\\ b_{31}& b_{32}& b_{33}\end{array}\right]\left[\begin{array}{ccc}{a}_{11}^{\′}& a_{12}^{\′}& a_{13}^{\′}\\ a_{21}^{\′}& a_{22}^{\′}& a_{23}^{\′}\\ a_{31}^{\′}& a_{32}^{\′}& a_{33}^{\′}\end{array}\right]\left[\begin{array}{c}x-n_1\\ y-n_2\\ z-n_3\end{array}\right]\\ =\left[\begin{array}{ccc}{b}_{11}{a}_{11}^{\′}+b_{12}{a}_{21}^{\′}+b_{13}{a}_{31}^{\′}& b_{11}{a}_{12}^{\′}+b_{12}{a}_{22}^{\′}+b_{13}{a}_{32}^{\′}& b_{11}{a}_{13}^{\′}+b_{12}{a}_{23}^{\′}+b_{13}{a}_{33}^{\′}\\ b_{21}{a}_{11}^{\′}+b_{22}{a}_{21}^{\′}+b_{23}{a}_{31}^{\′}& b_{21}{a}_{12}^{\′}+b_{22}{a}_{22}^{\′}+b_{23}{a}_{32}^{\′}& b_{21}{a}_{13}^{\′}+b_{22}{a}_{23}^{\′}+b_{23}{a}_{33}^{\′}\\ b_{31}{a}_{11}^{\′}+b_{32}{a}_{21}^{\′}+b_{33}{a}_{31}^{\′}& b_{31}{a}_{12}^{\′}+b_{32}{a}_{22}^{\′}+b_{33}{a}_{32}^{\′}& b_{31}{a}_{13}^{\′}+b_{32}{a}_{23}^{\′}+b_{33}{a}_{33}^{\′}\end{array}\right]\left[\begin{array}{c}x-n_1\\ y-n_2\\ z-n_3\end{array}\right]\\ =\left[\begin{array}{c}(b_{11}{a}_{11}^{\′}+b_{12}{a}_{21}^{\′}+b_{13}{a}_{31}^{\′})(x-n_1)+(b_{11}{a}_{12}^{\′}+b_{12}{a}_{22}^{\′}+b_{13}{a}_{32}^{\′})(y-n_2)+(b_{11}{a}_{13}^{\′}+b_{12}{a}_{23}^{\′}+b_{13}{a}_{33}^{\′})(z-n_3)\\ (b_{21}{a}_{11}^{\′}+b_{22}{a}_{21}^{\′}+b_{23}{a}_{31}^{\′})(x-n_1)+(b_{21}{a}_{12}^{\′}+b_{22}{a}_{22}^{\′}+b_{23}{a}_{32}^{\′})(y-n_2)+(b_{21}{a}_{13}^{\′}+b_{22}{a}_{23}^{\′}+b_{23}{a}_{33}^{\′})(z-n_3)\\ (b_{31}{a}_{11}^{\′}+b_{32}{a}_{21}^{\′}+b_{33}{a}_{31}^{\′})(x-n_1)+(b_{31}{a}_{12}^{\′}+b_{32}{a}_{22}^{\′}+b_{33}{a}_{32}^{\′})(y-n_2)+(b_{31}{a}_{13}^{\′}+b_{32}{a}_{23}^{\′}+b_{33}{a}_{33}^{\′})(z-n_3)\end{array}\right]\end{array}---\left(3\right)$

Wherein,

$b^b={\left[\begin{array}{ccc}{n}_1& n_2& n_3\end{array}\right]}^T,{\hat{B}}^h={\left[\begin{array}{ccc}x& y& z\end{array}\right]}^T{\hat{B}}^l={\left[\begin{array}{ccc}{x}_0& y_0& z_0\end{array}\right]}^T;$

(a′₁₁,a′₁₂,a′₁₃,a′₂₁,a′₂₂,a′₂₃,a′₃₁,a′₃₂,a′₃₃) for matrix M each component, (b₁₁,b₁₂,b₁₃,b₂₁, b₂₂,b₂₃,b₃₁,b₃₂,b₃₃) it is matrixEach component, (n₁,n₂,n₃) it is vector b^bEach component, (x, y, z) be vectorEach component, (x₀,y₀,z₀) it is vectorEach component；

Step C2, first feature according to distribution in local horizontal coordinates for the three axle output vectors of magnetometer is to step Formula (3) in C1 carries out abbreviation, can be used for the error model of parameter estimation：

c₁a₁₁+c₂a₁₂+c₃a₁₃+c₄a₂₁+c₅a₂₂+c₆a₂₃+c₇a₃₁+c₈a₃₂+c₉a₃₃+c₁₀α+c₁₁β+c₁₂γ=1 (4)

Wherein,

Step D, the parameter in the error model for parameter estimation step C being obtained by two step total least squares methods Estimated, then navigation system course angle is calculated according to the parameter estimating, complete navigation system course angle and directly correct Compensate.

2. according to claim 1 for navigation system course angle direct rectification building-out method it is characterised in that：Step D In two step total least squares methods method that the parameter in parameter estimating error model is estimated, comprise the following steps：

Step D1, three axle output vectors of magnetometer is substituted into the parameter estimating error model drawing in step C2, obtains：

A σ=b (5)

Wherein,

I represents three axle output vectors of magnetometer I-th group of data, n represents the n-th group of data of three axle output vectors of magnetometer, altogether n group data；

Step D2, by Least Square Method parameter (a₁₁,a₁₂,a₁₃,a₂₁,a₂₂,a₂₃,a₃₁,a₃₂,a₃₃, α, beta, gamma), first, Matrix most preferably approached by A, the formula (5) in step D1 is converted into：

A ' σ=(A+E) σ=b (6)

Wherein, A ' be A most preferably approach matrix, A '=A+E, E be A in contain systematic error interference；

Then, remember, Matrix C=[A b], singular value decomposition is carried out to Matrix C, can obtain：U^TCV=Σ, wherein, U, V are orthogonal Matrix, Σ is diagonal matrix, subscript T representing matrix transposition；Note matrix V=[V₁V₂… V₁₂V₁₃], wherein (V₁,V₂,…,V₁₂, V₁₃) be respectively matrix V column vector；

Finally, the solution obtaining expression formula (5) is：

$\begin{array}{cc}\mathrm{\σ}=-\[\frac{V_i}{V_{13}}\]& i=1,2,...,12\end{array}$

This solution is parameter (a₁₁,a₁₂,a₁₃,a₂₁,a₂₂,a₂₃,a₃₁,a₃₂,a₃₃, α, beta, gamma) value,

Step D3, the parameter (a being obtained using step D2₁₁,a₁₂,a₁₃,a₂₁,a₂₂,a₂₃,a₃₁,a₃₂,a₃₃, α, β, γ) and calculating parameter (n₁,n₂,n₃), can be obtained by formula (4)：

$\left\{\begin{array}{c}{a}_{11}{n}_1+a_{12}{n}_2+a_{13}{n}_3=\mathrm{\α}\\ a_{21}{n}_1+a_{22}{n}_2+a_{23}{n}_3=\mathrm{\β}\\ a_{31}{n}_1+a_{32}{n}_2+a_{33}{n}_3=\mathrm{\γ}\end{array}\right.---\left(7\right)$

Formula (7) is one group of system of linear equations, solves above-mentioned system of linear equations and can obtain parameter (n₁,n₂,n₃) value.

3. according to claim 2 for navigation system course angle direct rectification building-out method it is characterised in that：Described reason Think that condition refers to that magnetometer does not have error interference, the Magnetic Field of output is exactly the real information in earth's magnetic field.