CN110702142A - Triaxial magnetometer full-parameter external field calibration method assisted by triaxial accelerometer - Google Patents
Triaxial magnetometer full-parameter external field calibration method assisted by triaxial accelerometer Download PDFInfo
- Publication number
- CN110702142A CN110702142A CN201910862363.9A CN201910862363A CN110702142A CN 110702142 A CN110702142 A CN 110702142A CN 201910862363 A CN201910862363 A CN 201910862363A CN 110702142 A CN110702142 A CN 110702142A
- Authority
- CN
- China
- Prior art keywords
- triaxial
- parameter
- magnetometer
- angle
- parameters
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01R—MEASURING ELECTRIC VARIABLES; MEASURING MAGNETIC VARIABLES
- G01R35/00—Testing or calibrating of apparatus covered by the other groups of this subclass
- G01R35/005—Calibrating; Standards or reference devices, e.g. voltage or resistance standards, "golden" references
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manufacturing & Machinery (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Measuring Magnetic Variables (AREA)
Abstract
The invention discloses a triaxial magnetometer full-parameter external field calibration method assisted by a triaxial accelerometer, which is used for external field autonomous calibration of 12-dimensional parameters of the triaxial magnetometer, wherein the parameters comprise 3 zero-offset parameters, 3 scale parameters, 3 cross-coupling parameters and 3 coordinate rotation parameters relative to the triaxial accelerometer, and no additional auxiliary equipment is needed except for the triaxial accelerometers arranged in parallel. According to the method, the attitude matrix to be solved at a certain sampling moment is represented as an angle to be solved according to the observation of the triaxial accelerometer at the certain sampling moment, then the triaxial magnetometer observation at all the sampling moments is utilized to solve the parameter to be calibrated and the angle to be solved at all the sampling moments together, and an expectation-maximum likelihood algorithm (EM algorithm) is adopted to solve the parameter in the specific solving process.
Description
Technical Field
The invention belongs to the technical field of navigation and guidance, and particularly relates to a triaxial magnetometer full-parameter external field calibration method assisted by a triaxial accelerometer.
Background
Under the condition that the background earth magnetic field vector is known, the measurement value of the three-axis magnetometer contains the attitude information of the carrier. In view of its advantages of low cost, low power consumption, small volume, light weight and the like, the three-axis magnetometer has wide application in the field of low-cost attitude measurement, for example, in a MARG sensor, the three-axis magnetometer is an important component, and the other two components are a three-axis accelerometer and a three-axis gyroscope.
In attitude measurement applications, a three-axis magnetometer is required to measure the three-axis component of a background geomagnetic field in a carrier coordinate system. However, in addition to the background earth magnetic field, the measurement of the three-axis magnetometer is also affected by environmental interference magnetic fields. In addition, due to the imperfection of the three-axis magnetometer, the measurement thereof has errors such as zero offset, scale, coupling and the like. The various error sources are sometimes indistinguishable, and need not be distinguished. Generally, a calibration model with 12 parameters can be adopted to completely describe the measurement error characteristics of the triaxial magnetometer, the parameters comprise 3 zero-offset parameters, 3 scale parameters, 3 cross-coupling parameters and 3 coordinate rotation parameters relative to the triaxial accelerometer, and the observation of the triaxial magnetometer can be compensated by estimating (calibrating) the 12 parameters, so that the accurate projection of the background geomagnetic field along three coordinate axes of the carrier system is obtained.
Because the measurement of the three-axis magnetometer is influenced by the environmental interference magnetic field, the system level can be carried out only in an external field, and the element level calibration cannot be carried out in a laboratory, because the interference magnetic field in the laboratory is generally different from the environmental interference magnetic field under the actual working condition of the three-axis magnetometer. The biggest difficulty of external field calibration is that attitude reference cannot be provided, and an ellipsoid fitting method is an effective method for solving the problem, but the method can only calibrate 9 of 12 parameters, and cannot calibrate the rotation of a magnetometer component coordinate system relative to a carrier coordinate system. In other words, in multi-sensor fusion applications, the ellipsoid fitting method cannot determine the rotational relationship between the coordinate system of the tri-axial magnetometer component and the coordinate systems of the other sensors. The method is an inherent defect of an ellipsoid fitting method, and because the method only adopts observation data of a three-axis magnetometer, an obtained calibration result only needs to meet self consistency required by the observation data of the magnetometer and cannot provide rotation information between the magnetometer and other sensors.
From the foregoing, to obtain the rotation information between the three-axis magnetometer and other sensors, the observation data of other sensors must be introduced. In view of this, the existing literature provides a method for calibrating parameters of a three-axis magnetometer 12 with the aid of a three-axis accelerometer (calibrated), which is called a point-by-point invariant method. This method is based on the fact that: the point product (i.e., inner product) of the triaxial acceleration vector and the triaxial magnetic field strength vector is the same in different rectangular coordinate systems. This method is equivalent to converting the 3 x 1 vector observed by the tri-axial accelerometer to a 1 x 1 scalar, i.e., a point product of the vector and the tri-axial magnetic field strength vector. It is clear that there is a loss of information from the conversion process of 3 x 1 vectors to 1 x 1 scalars. Even considering the property that the magnitude of the vector observation is independent of the attitude measurement in the attitude measurement, there is a loss of information from a 2-dimensional direction vector to a 1-dimensional scalar.
Disclosure of Invention
The purpose of the invention is as follows: the invention provides a triaxial magnetometer full-parameter external field calibration method assisted by a triaxial accelerometer, aiming at the calibration problem of the triaxial magnetometer, in particular to the full-parameter external field calibration problem of the triaxial magnetometer. The method needs a set of calibrated triaxial accelerometer assistance, and is different from a point multiplication invariant method in the prior art.
The technical scheme is as follows: in order to realize the purpose of the invention, the technical scheme adopted by the invention is as follows: a three-axis magnetometer full-parameter external field calibration method assisted by a three-axis accelerometer comprises the following steps:
step 1, collecting the measurement values of a triaxial magnetometer to be calibrated and the calibrated triaxial accelerometer at n moments;
step 2, preprocessing the measured values of the n sampling time triaxial accelerometers, and representing the attitude to be solved at each time as an angle to be solved;
step 3, establishing an observation model of the three-axis magnetometer, and constructing a cost function according to the observation model, the attitude matrix to be solved and the angle to be solved;
and 4, iteratively solving the cost function by adopting an Expectation-Maximization (EM) algorithm to minimize the cost function, so as to obtain the optimal solution of the angle to be solved and the parameter to be calibrated when the cost function reaches the minimum value, wherein the optimal solution of the calibration parameter is the final calibration parameter value.
Further, in step 1, data acquisition may be performed in an external field dynamic environment, and no limitation is imposed on the carrier dynamics.
Further, in the step 2, the measurement values of the n sampling time triaxial accelerometers are preprocessed in sequence, that is, data at a certain time is processed without involving data at other times; the input of the preprocessing process is the measured value of the triaxial accelerometer at the sampling moment, and the output is an expression of the attitude matrix at the sampling moment, wherein the expression comprises sine and cosine functions of the angle to be solved.
Preprocessing the measured values of the n time triaxial accelerometers, and representing the attitude matrix to be solved at each time as an angle to be solved; the method comprises the following specific steps:
the observation model of the calibrated triaxial accelerometer at the ith moment is as follows:
ai=Aiag(1)
wherein i is 1,2iSetting calibrated parameter compensation for an accelerometer observation vector at the ith moment; a. theiIs the attitude matrix at the ith time, agThe gravity acceleration vector in the navigation system is set to be known; attitude matrix A at the ith timeiThe expression is as follows:
wherein the superscript T denotes the matrix or vector transposition, ψiRepresents an angle; b isi、CiFor the introduced auxiliary variables, the expression is as follows:
wherein I3Is a 3 x 3 identity matrix.
An attitude matrix A to be solved is obtained by the formula (2)iInto the angle psi to be solvediOr its sine and cosine functions.
Further, the step 3 is to establish an observation model of the three-axis magnetometer and construct a cost function according to the observation model, the attitude matrix to be solved and the angle to be solved; the method comprises the following specific steps:
the observation model of the i-th time three-axis magnetometer is shown below
mi=MAime+m0(3)
Wherein i is 1,2iIs the i-th time observation vector of the three-axis magnetometer, AiThe posture matrix at the corresponding moment is expressed as formula (2); m iseFor the geomagnetic intensity vector in the navigation system, set as known, M and M0Is a parameter to be calibrated; wherein m is0Is a 3 multiplied by 1 vector and represents 3 zero offset parameters; m is a 3 x 3 matrix representing 3 scale parameters, 3 cross-coupling parameters, and 3 coordinate system rotation parameters relative to the tri-axial accelerometer.
Reorganizing the parameters to be calibrated into the following parameter vectors:
wherein β represents a calibration parameter vector, vec [ M ] is a vector formed by stacking each row of the matrix M in sequence;
in equation (3), except that the parameters to be calibrated are unknown, the attitude matrix AiIs also unknown, according to equation (2), as long as the angle ψ is determinediOr its sine and cosine functions, the attitude matrix A can be determinedi。
According to the observation model shown in the formula (3) and considering the expression (2) of the attitude matrix, the following cost function is constructed:
where n represents the number of sampling instants. Minimizing the cost functioniAnd beta is the optimal solution of the angle and the parameter to be calibrated.
Further, in the step 4, an Expectation-Maximization (EM) algorithm is used for iteratively solving the cost function to obtain an optimal solution of the angle and the parameter to be calibrated when the cost function reaches a minimum value, and the optimal solution of the calibration parameter is the final calibration parameter value. The method comprises the following specific steps:
4-1, calibrating parameters of the triaxial magnetometer by using a point-by-point invariant method to obtain calibration parameter values of the triaxial magnetometer, and entering the step 4-2 by taking the calibration parameter values as initial values;
4-2, solving the attitude under the condition of given calibration parameters, namely obtaining the sine and cosine of the angle to be solved at the corresponding sampling moment through solving a unitary quartic equation obtained by cost function conversion, and further solving an attitude matrix;
4-3, solving the calibration parameters under the condition of a given attitude, namely estimating 12 calibration parameters of the three-axis magnetometer by adopting a least square method according to the sine and cosine of the angle to be solved obtained in the step 4-2;
and 4-4, alternately and iteratively executing the steps 4-2 and 4-3 until the iterative process is converged, ending the iteration, and taking the calibration parameter estimation value in the last iteration as the final calibration parameter value.
Further, in the step 4-2, the attitude is solved under the condition of the given calibration parameter, namely the angle psi is solved under the condition of the given calibration parameter betai(ii) a The method comprises the following specific steps:
from equation (4), given β,. phi.iIs solved only with mi、Bi、CiIn this regard, it is necessary to solve n one-dimensional problems, rather than solving one n-dimensional problem, which greatly simplifies the amount of computation and allows parallel processing.
For the ith time, order
Let formula (4) to psiiIs zero, to obtain
2ρ02cosψi-2ρ01sinψi-ρ11sin2ψi+2ρ12cos2ψi-ρ22sin2ψi=0 (5)
Let sin psiiX, formula (5) is equivalent to
Squaring both sides of the formula (6) and normalizing the 4-degree term coefficients to obtain
x4+bx3+cx2+dx+e=0 (7)
Wherein
Solving a root formula by adopting a unitary quartic equation, and solving the root of equation (7); only two cases are possible among all possible solutions, namely one case of 4 real solutions and the other case of 2 real solutions and 2 complex conjugate solutions. For the case of 4 or 2 real solutions described above, only solutions falling within the range of [0,1] are feasible. The invention can adopt an open source software package with an equation root solving function to solve the root of the equation (7), and can also design a program by self to carry out equation root solving.
For feasible x, there are
When possible to solveWhen more than 1 group, all feasible solutions are substituted into the cost function formula (4) or the discriminant functionAnd selecting the solution which makes the discriminant function be the minimum value to output. A solution set will also minimize equation (4) if it minimizes the discriminant function.
Sequentially processing the measured values of the n sampling time triaxial magnetometer according to the method, and outputting sine and cosine of the angle to be solved at the n sampling time; this process involves a root-finding process for n univariate quartiles, and a unique solution determination (validation) process for a number of possible solutions.
Further, in step 4-3, the calibration parameters are solved under the condition of given attitude, which is specifically as follows:
for the ith time, firstly, the result obtained in step 4-2Substituted for formula (3) and rewritten as
mi=Fiβ (9)
WhereinRepresents the Kronecker product;
carrying out batch processing on the measured values of the triaxial magnetometer at all the n sampling moments, and estimating 12 calibration parameters of the triaxial magnetometer by adopting a least square method;
the following auxiliary variables F, m are calculated:
according to the least square theory there is
Obtained according to the formula (10)That is, the estimation value of the calibration parameter in the iterative process of the EM algorithm, and if the iterative process is not finished, the estimation value is used for the next iterative calculation.
Further, in step 4-4, a suitable iteration convergence criterion is selected, for example, a difference between the calibration parameter estimation values obtained in the previous iteration and the next iteration is smaller than a selected threshold, which means that the iteration process is considered to be converged.
The objective of the iterative algorithm is to minimize the cost function, iterative convergence is equivalent to parameter estimation when the cost function is minimum, and an estimated value obtained by the last iteration is a final solution.
Has the advantages that: compared with the prior art, the technical scheme of the invention has the following beneficial technical effects:
firstly, compared with an ellipsoid fitting method, the method provided by the invention can calibrate all 12 parameters of the three-axis magnetometer, and is a full-parameter calibration method; secondly, compared with other methods suitable for laboratory calibration, the method provided by the invention is suitable for the outfield dynamic condition, and no limitation is made on the carrier dynamics; thirdly, compared with a point multiplication invariant method, the method provided by the invention has higher redundancy and correspondingly higher calibration precision; fourthly, the root solving of the unitary quartic equation related in the invention can adopt some existing software packages, thereby simplifying the programming complexity; fifth, the method of the present invention can also provide the attitude at the observation time as a byproduct.
Drawings
FIG. 1 is a flow chart of the operation of the present invention;
FIG. 2 is a schematic diagram of the relationship between the coordinate system of the accelerometer and the coordinate system of the magnetometer.
Detailed Description
The technical solution of the present invention is further described below with reference to the accompanying drawings and examples.
The invention relates to a triaxial magnetometer full-parameter external field calibration method assisted by a triaxial accelerometer, which has a process shown in figure 1 and comprises the following steps:
step 1, collecting the measurement values of a triaxial magnetometer to be calibrated and the calibrated triaxial accelerometer at n moments in an external field dynamic environment;
step 2, preprocessing the measured values of the n moments of the triaxial accelerometer, and representing the attitude matrix to be solved at each moment as an angle to be solved; the method comprises the following specific steps:
the observation model of the calibrated triaxial accelerometer at the ith moment is as follows:
ai=Aiag(1)
wherein i is 1,2iSetting calibrated parameter compensation for an accelerometer observation vector at the ith moment; a. theiIs the attitude matrix at the ith time, agThe gravity acceleration vector in the navigation system is set to be known; attitude matrix A at the ith timeiThe expression is as follows:
wherein the superscript T denotes the matrix or vector transposition, ψiRepresents an angle; b isi、CiFor the introduced auxiliary variables, the expressions are as follows
Wherein I3Is a 3 x 3 identity matrix.
An attitude matrix A to be solved is obtained by the formula (2)iInto the angle psi to be solvediOr its sine and cosine functions.
Step 3, establishing an observation model of the three-axis magnetometer, and constructing a cost function according to the observation model, the attitude matrix to be solved, the angle to be solved and the angle to be solved; the method comprises the following specific steps:
the observation model of the i-th time three-axis magnetometer is shown below
mi=MAime+m0(3)
Wherein i is 1,2iIs the i-th time observation vector of the three-axis magnetometer, AiThe posture matrix at the corresponding moment is expressed as formula (2); m iseFor the geomagnetic intensity vector in the navigation system, set as known, M and M0Is a parameter to be calibrated; wherein m is0Is a 3 multiplied by 1 vector and represents 3 zero offset parameters; m is a 3 x 3 matrix representing 3 scale parameters, 3 cross-coupling parameters, and 3 coordinate system rotation parameters relative to the tri-axial accelerometer.
Reorganizing the parameters to be calibrated into the following parameter vectors:
wherein β represents a calibration parameter vector, vec [ M ] is a vector formed by stacking each row of the matrix M in sequence;
in equation (3), except that the parameters to be calibrated are unknown, the attitude matrix AiIs also unknown, according to equation (2), as long as the angle ψ is determinediOr its sine and cosine functions, the attitude matrix A can be determinedi。
According to the observation model shown in the formula (3) and considering the expression (2) of the attitude matrix, the following cost function is constructed:
wherein n denotes the sampling instantAnd (4) the number. Minimizing the cost functioniAnd beta is the optimal solution of the angle and the parameter to be calibrated.
And 4, iteratively solving the cost function by adopting an Expectation-Maximization (EM) algorithm to minimize the cost function, so as to obtain the optimal solution of the angle to be solved and the parameter to be calibrated when the cost function reaches the minimum value, wherein the optimal solution of the calibration parameter is the final calibration parameter value. The method comprises the following specific steps:
4-1, calibrating parameters of the triaxial magnetometer by using a point-by-point invariant method to obtain calibration parameter values of the triaxial magnetometer, and entering the step 4-2 by taking the calibration parameter values as initial values;
4-2, solving the attitude under the condition of given calibration parameters, namely obtaining the sine and cosine of the angle to be solved at the corresponding sampling moment through solving a unitary quartic equation obtained by cost function conversion, and further solving an attitude matrix;
4-3, solving the calibration parameters under the condition of a given attitude, namely estimating 12 calibration parameters of the three-axis magnetometer by adopting a least square method according to the sine and cosine of the angle to be solved obtained in the step 4-2;
and 4-4, alternately and iteratively executing the steps 4-2 and 4-3 until the iterative process is converged, ending the iteration, and taking the calibration parameter estimation value in the last iteration as the final calibration parameter value.
In the step 4-2, the attitude is solved under the condition of the given calibration parameter, namely the angle psi is solved under the condition of the given calibration parameter betai(ii) a The method comprises the following specific steps:
from equation (4), given β,. phi.iIs solved only with mi、Bi、CiIn this regard, it is necessary to solve n one-dimensional problems, rather than solving one n-dimensional problem, which greatly simplifies the amount of computation and allows parallel processing.
For the ith time, order
Introducing auxiliary variablesρjkValues are respectively rho01、ρ02、ρ11、ρ12、ρ22。
Let formula (4) to psiiIs zero, to obtain
2ρ02cosψi-2ρ01sinψi-ρ11sin2ψi+2ρ12cos2ψi-ρ22sin2ψi=0 (5)
Let sin psiiX, formula (5) is equivalent to
Squaring both sides of the formula (6) and normalizing the 4-degree term coefficients to obtain
x4+bx3+cx2+dx+e=0 (7)
Wherein
Solving a root formula by adopting a unitary quartic equation, and solving the root of equation (7); only two cases are possible among all possible solutions, namely one case of 4 real solutions and the other case of 2 real solutions and 2 complex conjugate solutions. For the case of 4 or 2 real solutions described above, only solutions falling within the range of [0,1] are feasible. The invention can adopt an open source software package with an equation root solving function to solve the root of the equation (7), and can also design a program by self to carry out equation root solving.
For feasible x, there are
When possible to solveWhen more than 1 group, all feasible solutions are substituted into the cost function formula (4) or the discriminant functionAnd selecting the solution which makes the discriminant function be the minimum value to output. A solution set will also minimize equation (4) if it minimizes the discriminant function.
Sequentially processing the measured values of the n sampling time triaxial magnetometer according to the method, and outputting sine and cosine of the angle to be solved at the n sampling time; this process involves a root-finding process for n univariate quartiles, and a unique solution determination (validation) process for a number of possible solutions.
In the step 4-3, the calibration parameters are solved under the condition of given attitude, which specifically comprises the following steps:
for the ith time, firstly, the result obtained in step 4-2Substituted for formula (3) and rewritten as
mi=Fiβ (9)
carrying out batch processing on the measured values of the triaxial magnetometer at all the n sampling moments, and estimating 12 calibration parameters of the triaxial magnetometer by adopting a least square method;
the following auxiliary variables F, m are calculated:
according to the least square theory there is
Obtained according to the formula (10)That is, the estimation value of the calibration parameter in the iterative process of the EM algorithm, and if the iterative process is not finished, the estimation value is used for the next iterative calculation.
In step 4-4, a suitable iteration convergence criterion is selected, for example, a difference between the calibration parameter estimation values obtained in the previous iteration and the next iteration is smaller than a selected threshold, which means that the iteration process is considered to be converged.
The objective of the iterative algorithm is to minimize the cost function, iterative convergence is equivalent to parameter estimation when the cost function is minimum, and an estimated value obtained by the last iteration is a final solution.
FIG. 2 is a schematic diagram of the relationship between the coordinate system of the accelerometer and the coordinate system of the magnetometer, where O-xayazaRepresenting the accelerometer coordinate system, O-xmymzmRepresenting the coordinate system of the magnetometer, and enabling the origins of the two coordinate systems to coincide without losing generality.
The invention adopts the auxiliary of the calibrated triaxial accelerometer, and is different from the construction of point multiplication scalar pseudo observation in a point multiplication invariant method. Firstly, an accelerometer measured value at any moment is preprocessed, an unknown attitude (3 dimensions) at the corresponding moment is expressed as a function (1 dimension) of an unknown angle, and then the three-axis magnetometer observation at all sampling moments is adopted to solve 12 parameters and the unknown angle degree at all the moments.
The method provided by the invention and the point multiplication invariant method are analyzed in comparison from the perspective of redundancy. The number of sampling instants is not assumed to be n. First, in the dot product invariant method, there are n dot product (pseudo) observations, and the unknowns to be estimated are 12 calibration parameters, so the redundancy of this method is d 1-n-12. Secondly, in the method proposed by the present invention, there are 3n observations (i.e. three-axis magnetometer observations at n epochs), and the unknowns to be estimated are 12 calibration parameters and n unknown angles, so the redundancy is d 2-3 n-12-n-2 n-12. Compared with a point multiplication invariant method, the method provided by the invention has higher redundancy rate, and has certain advantages from the aspect.
Claims (9)
1. A three-axis magnetometer full-parameter external field calibration method assisted by a three-axis accelerometer is characterized by comprising the following steps: the method comprises the following steps:
step 1, collecting the measurement values of a triaxial magnetometer to be calibrated and the calibrated triaxial accelerometer at n moments;
step 2, preprocessing the measured values of the n sampling time triaxial accelerometers, and representing the attitude to be solved at each time as an angle to be solved;
step 3, establishing an observation model of the three-axis magnetometer, and constructing a cost function according to the observation model, the attitude matrix to be solved and the angle to be solved;
and 4, iteratively solving the cost function by adopting an expected maximum likelihood algorithm to minimize the cost function, and obtaining the optimal solution of the angle to be solved and the parameter to be calibrated when the cost function reaches the minimum value, wherein the optimal solution of the calibration parameter is the final calibration parameter value.
2. The method for calibrating the full-parameter external field of the triaxial magnetometer assisted by the triaxial accelerometer according to claim 1, wherein the method comprises the following steps: in the step 2, the measured values of the n sampling time triaxial accelerometers are preprocessed in sequence, that is, data at a certain time is processed without being related to data at other times; the input of the preprocessing process is the measured value of the triaxial accelerometer at the sampling moment, and the output is an expression of an attitude matrix at the sampling moment, wherein the expression comprises sine and cosine functions of an angle to be solved;
preprocessing the measured values of the n time triaxial accelerometers, and representing the attitude matrix to be solved at each time as an angle to be solved; the method comprises the following specific steps:
the observation model of the calibrated triaxial accelerometer at the ith moment is as follows:
ai=Aiag(1)
wherein i is 1,2iThe accelerometer observation vector at the ith moment is subjected to calibration parameter compensation; a. theiIs the attitude matrix at the ith time, agIs the gravity acceleration vector in the navigation system; attitude matrix A at the ith timeiThe expression is as follows:
wherein the superscript T denotes the matrix or vector transposition, ψiRepresents an angle; b isi、CiFor the introduced auxiliary variables, the expression is as follows:
wherein I3Is a 3 × 3 identity matrix;
an attitude matrix A to be solved is obtained by the formula (2)iInto the angle psi to be solvediOr its sine and cosine functions.
3. The method for calibrating the full-parameter external field of the triaxial magnetometer assisted by the triaxial accelerometer according to claim 2, wherein the method comprises the following steps: step 3, establishing an observation model of the three-axis magnetometer, and establishing a cost function according to the observation model, the attitude matrix to be solved and the angle to be solved; the method comprises the following specific steps:
the observation model of the i-th time three-axis magnetometer is shown below
mi=MAime+m0(3)
Wherein i is 1,2iIs the i-th time observation vector of the three-axis magnetometer, AiIs the attitude matrix at the ith time, meFor geomagnetic intensity in navigation systemVector, M and M0Is a parameter to be calibrated; wherein m is0Is a 3 multiplied by 1 vector and represents 3 zero offset parameters; m is a 3 x 3 matrix representing 3 scale parameters, 3 cross-coupling parameters and 3 coordinate system rotation parameters relative to the triaxial accelerometer;
reorganizing the parameters to be calibrated into the following parameter vectors:
wherein β represents a calibration parameter vector, vec [ M ] is a vector formed by stacking each row of the matrix M in sequence;
according to the observation model shown in the formula (3) and considering the expression (2) of the attitude matrix, the following cost function is constructed:
wherein n represents the number of sampling instants; minimizing the cost functioniAnd beta is the optimal solution of the angle and the parameter to be calibrated.
4. The method of claim 3, wherein the method comprises the following steps: step 4, iterative solution of the cost function is carried out by adopting an expected maximum likelihood algorithm, so as to obtain the optimal solution of the angle and the parameter to be calibrated when the cost function reaches the minimum value, and the optimal solution of the calibration parameter is the final calibration parameter value; the method comprises the following specific steps:
4-1, calibrating parameters of the triaxial magnetometer by using a point-by-point invariant method to obtain calibration parameter values of the triaxial magnetometer, and entering the step 4-2 by taking the calibration parameter values as initial values;
4-2, solving the attitude under the condition of given calibration parameters, namely obtaining the sine and cosine of the angle to be solved at the corresponding sampling moment through solving a unitary quartic equation obtained by cost function conversion, and further solving an attitude matrix;
4-3, solving the calibration parameters under the condition of a given attitude, namely estimating 12 calibration parameters of the three-axis magnetometer by adopting a least square method according to the sine and cosine of the angle to be solved obtained in the step 4-2;
and 4-4, alternately and iteratively executing the steps 4-2 and 4-3 until the iterative process is converged, ending the iteration, and taking the calibration parameter estimation value in the last iteration as the final calibration parameter value.
5. The method of claim 4, wherein the method comprises the following steps: in the step 4-2, the attitude is solved under the condition of the given calibration parameter, namely the angle psi is solved under the condition of the given calibration parameter betai(ii) a The method comprises the following specific steps:
for the ith time, order
Let formula (4) to psiiIs zero, to obtain
2ρ02cosψi-2ρ01sinψi-ρ11sin2ψi+2ρ12cos2ψi-ρ22sin2ψi=0 (5)
Let sin psiiX, formula (5) is equivalent to
Squaring both sides of the formula (6) and normalizing the 4-degree term coefficients to obtain
x4+bx3+cx2+dx+e=0 (7)
Wherein
Solving a root formula by adopting a unitary quartic equation, and solving the root of equation (7); only two solutions of all solutions are feasible, namely 4 real solutions, 2 real solutions and 2 complex conjugate solutions; for the 4 or 2 real number solutions described above, only solutions falling within the [0,1] range are feasible;
for feasible x, there are
When possible to solveWhen more than 1 group, all feasible solutions are substituted into the cost function formula (4) or the discriminant functionSelecting a solution which enables the discrimination function to be the minimum value and outputting the solution;
and sequentially processing the measurement values of the n sampling time triaxial magnetometer according to the method, and outputting sine and cosine of the angle to be solved at the n sampling time.
6. The method of claim 4, wherein the method comprises the following steps: in the step 4-3, the calibration parameters are solved under the condition of given attitude, which specifically comprises the following steps:
for the ith time, firstly, the result obtained in step 4-2Substituted for formula (3) and rewritten as
mi=Fiβ (9)
carrying out batch processing on the measured values of the triaxial magnetometer at all the n sampling moments, and estimating 12 calibration parameters of the triaxial magnetometer by adopting a least square method;
the following auxiliary variables F, m are calculated:
according to the least square theory there is
7. The method of claim 4, wherein the method comprises the following steps: in the step 4-4, a suitable iteration convergence criterion is selected, that is, the difference value of the calibration parameter estimation values obtained in the two iterations is smaller than a selected threshold value, and the iteration process is considered to be converged.
8. The method for calibrating the full-parameter external field of the triaxial magnetometer assisted by the triaxial accelerometer according to any one of claims 1 to 7, wherein the method comprises the following steps: the step 1 executes data acquisition in an external field dynamic environment, and does not limit the carrier dynamics.
9. The method of claim 5, wherein the method comprises the following steps: and solving the root of the equation (7) by adopting an open source software package with an equation root solving function, or designing a program to carry out equation root solving.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910862363.9A CN110702142A (en) | 2019-09-12 | 2019-09-12 | Triaxial magnetometer full-parameter external field calibration method assisted by triaxial accelerometer |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910862363.9A CN110702142A (en) | 2019-09-12 | 2019-09-12 | Triaxial magnetometer full-parameter external field calibration method assisted by triaxial accelerometer |
Publications (1)
Publication Number | Publication Date |
---|---|
CN110702142A true CN110702142A (en) | 2020-01-17 |
Family
ID=69195239
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910862363.9A Pending CN110702142A (en) | 2019-09-12 | 2019-09-12 | Triaxial magnetometer full-parameter external field calibration method assisted by triaxial accelerometer |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110702142A (en) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112596015A (en) * | 2020-12-28 | 2021-04-02 | 上海矽睿科技有限公司 | Test method and system of three-axis magnetic sensor |
CN117589203A (en) * | 2024-01-18 | 2024-02-23 | 陕西太合智能钻探有限公司 | Gyroscope calibration method |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20130245984A1 (en) * | 2010-11-17 | 2013-09-19 | Hillcrest Laboratories, Inc. | Apparatuses and methods for magnetometer alignment calibration without prior knowledge of the local magnetic field |
CN105180968A (en) * | 2015-09-02 | 2015-12-23 | 北京天航华创科技股份有限公司 | IMU/magnetometer installation misalignment angle online filter calibration method |
CN109827571A (en) * | 2019-03-22 | 2019-05-31 | 北京壹氢科技有限公司 | A kind of dual acceleration meter calibration method under the conditions of no turntable |
CN110174122A (en) * | 2019-05-08 | 2019-08-27 | 苏州大学 | A kind of MEMS triaxial accelerometer scaling method based on maximum likelihood estimation algorithm |
-
2019
- 2019-09-12 CN CN201910862363.9A patent/CN110702142A/en active Pending
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20130245984A1 (en) * | 2010-11-17 | 2013-09-19 | Hillcrest Laboratories, Inc. | Apparatuses and methods for magnetometer alignment calibration without prior knowledge of the local magnetic field |
CN105180968A (en) * | 2015-09-02 | 2015-12-23 | 北京天航华创科技股份有限公司 | IMU/magnetometer installation misalignment angle online filter calibration method |
CN109827571A (en) * | 2019-03-22 | 2019-05-31 | 北京壹氢科技有限公司 | A kind of dual acceleration meter calibration method under the conditions of no turntable |
CN110174122A (en) * | 2019-05-08 | 2019-08-27 | 苏州大学 | A kind of MEMS triaxial accelerometer scaling method based on maximum likelihood estimation algorithm |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112596015A (en) * | 2020-12-28 | 2021-04-02 | 上海矽睿科技有限公司 | Test method and system of three-axis magnetic sensor |
CN117589203A (en) * | 2024-01-18 | 2024-02-23 | 陕西太合智能钻探有限公司 | Gyroscope calibration method |
CN117589203B (en) * | 2024-01-18 | 2024-05-10 | 陕西太合智能钻探有限公司 | Gyroscope calibration method |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111426318B (en) | Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering | |
Vasconcelos et al. | Geometric approach to strapdown magnetometer calibration in sensor frame | |
Huang et al. | Online initialization and automatic camera-IMU extrinsic calibration for monocular visual-inertial SLAM | |
CN108225370B (en) | Data fusion and calculation method of motion attitude sensor | |
WO2022160391A1 (en) | Magnetometer information assisted mems gyroscope calibration method and calibration system | |
CN108458709B (en) | Airborne distributed POS data fusion method and device based on vision-aided measurement | |
CN110702142A (en) | Triaxial magnetometer full-parameter external field calibration method assisted by triaxial accelerometer | |
CN111189474A (en) | Autonomous calibration method of MARG sensor based on MEMS | |
CN116147624B (en) | Ship motion attitude calculation method based on low-cost MEMS navigation attitude reference system | |
CN111982089A (en) | Real-time calibration and compensation method for magnetic compass total error | |
Wu et al. | Simultaneous hand–eye/robot–world/camera–IMU calibration | |
Kong et al. | Performance improvement of visual-inertial navigation system by using polarized light compass | |
CN111413651B (en) | Compensation method, device and system for total magnetic field and storage medium | |
CN107860382B (en) | Method for measuring attitude by applying AHRS under geomagnetic anomaly condition | |
Garcia et al. | Unscented Kalman filter for spacecraft attitude estimation using quaternions and euler angles | |
Bj et al. | Redesign and analysis of globally asymptotically stable bearing only SLAM | |
Wu et al. | Globally optimal symbolic hand-eye calibration | |
CN113375693B (en) | Geomagnetic heading error correction method | |
Stančin et al. | On the interpretation of 3D gyroscope measurements | |
CN110672127B (en) | Real-time calibration method for array type MEMS magnetic sensor | |
Liu et al. | LGC-Net: A lightweight gyroscope calibration network for efficient attitude estimation | |
Blachuta et al. | Attitude and heading reference system based on 3D complementary filter | |
CN109470267A (en) | A kind of attitude of satellite filtering method | |
Xuan | Application of real-time Kalman filter with magnetic calibration for MEMS sensor in attitude estimation | |
Neymann et al. | Minimization of Parameter Sensitivity to Pre-Estimation Errors and its Application to the Calibration of Magnetometer Arrays |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
AD01 | Patent right deemed abandoned |
Effective date of abandoning: 20231017 |
|
AD01 | Patent right deemed abandoned |