Compass calibration method and device
Technical Field
The invention relates to the technical field of computers, in particular to a compass calibration method and device.
Background
An electronic magnetic compass is generally installed on the unmanned aerial vehicle to control the course of the unmanned aerial vehicle so as to ensure the accuracy of the flight track. The accuracy of the measurement of the electronic magnetic compass plays an important role in the flight control of the unmanned aerial vehicle.
Due to the influence of environmental factors and the factors of the electronic magnetic compass, the electronic magnetic compass usually has errors in measurement, and therefore, the electronic magnetic compass needs to be calibrated. In the prior art, only the influence of the hard magnetic disturbance on the compass, namely the offset of the magnetic field strength measured by the compass relative to the origin in the body coordinate system, is generally considered. However, the measurement error of the compass is not limited to the error caused by the hard magnetic interference, and the prior art solution makes the accuracy of the calibration of the electronic magnetic compass insufficient.
Disclosure of Invention
The invention aims to solve the technical problems that: how to improve the accuracy of the calibration of the electronic magnetic compass.
According to an embodiment of the present invention, there is provided a compass calibration method including: determining a relationship representation of the calibrated magnetic field strength and the calibration data, the soft magnetic interference correction amount and the hard magnetic interference correction amount; determining a soft magnetic interference correction amount and a hard magnetic interference correction amount by enabling the error between the calibrated magnetic field intensity and the normal magnetic field intensity to meet a preset condition; and calibrating the compass by using the soft magnetic interference correction quantity and the hard magnetic interference correction quantity.
In one embodiment, determining soft and hard magnetic disturbance corrections comprises: i) substituting the calibration data, the soft magnetic interference correction quantity and the hard magnetic interference correction quantity into the error relation expression of the magnetic field intensity after calibration and the normal magnetic field intensity to obtain a converted error relation expression; ii) obtaining a converted error relation representing a jacobian matrix with respect to each independent variable, wherein the independent variable is one or more of a normal magnetic field strength, a soft magnetic disturbance correction amount and a hard magnetic disturbance correction amount, and a preset value is not adopted as a term of the independent variable; iii) constructing an increment normal equation according to the Jacobian matrix, the iteration error, the iteration step length and the independent variable; iv) substituting the values of the independent variables into a Jacobian matrix to calculate a Jacobian matrix, and substituting the values of the independent variables into the converted error relation to express the calculation iteration error; v) substituting the calculated Jacobian matrix, the calculated iteration error and the first iteration step value into an increment normal equation to be solved into a self-variable first value, and substituting the calculated Jacobian matrix, the calculated iteration error and the second iteration step value into the increment normal equation to be solved into a self-variable second value; vi) substituting the first value of the independent variable into the converted error relation representation to obtain a first updating iteration error, and substituting the second value of the independent variable into the converted error relation representation to obtain a second updating iteration error; vii) comparing the first update iteration error and the second update iteration error with preset conditions respectively: if the first update iteration error and the second update iteration error are both greater than the preset condition, multiplying the first iteration step value by the preset iteration gradient to update the first iteration step value, multiplying the second iteration step value by the preset iteration gradient to update the second iteration step value, if the first update iteration error is less than the second update iteration error, updating the value of the argument with the argument first value, if the second update iteration error is less than the first update iteration error, updating the value of the argument with the argument second value, repeating steps iv) through vii) until the first update iteration error value or the second update iteration error value is less than or equal to the preset condition; if the first updating iteration error is smaller than the preset condition, outputting the first value of the independent variable as the value of the independent variable, and if the second updating iteration error is smaller than the preset condition, outputting the second value of the independent variable as the value of the independent variable; the values of the independent variables adopt preset values in the initial iteration, and the first iteration step value and the second iteration step value adopt preset values in the initial iteration.
In one embodiment, determining soft and hard magnetic disturbance corrections comprises: presetting a value of a soft magnetic interference correction quantity, and determining a normal magnetic field strength and a hard magnetic interference correction quantity by enabling an error between the calibrated magnetic field strength and the normal magnetic field strength to meet a preset condition; and determining the soft magnetic interference correction quantity by enabling the error between the calibrated magnetic field strength and the normal magnetic field strength to meet a preset condition based on the determined normal magnetic field strength and the soft magnetic interference correction quantity.
In one embodiment, the incremental normal equation is expressed as:
wherein,kan argument representing the kth iteration, JkJacobian matrix representing the kth iteration, ekDenotes the iteration error, λ, of the kth iterationkDenotes the iteration step for the kth iteration and I denotes the identity matrix.
In one embodiment, the relationship between the calibrated magnetic field strength and the calibration data, soft magnetic disturbance correction, and hard magnetic disturbance correction is expressed as: the magnetic field intensity after calibration is equal to the sum of the calibration data and the correction quantity of the hard magnetic interference multiplied by the correction quantity of the soft magnetic interference; and adding the differences obtained by subtracting the square of the normal magnetic field intensity from the sum of the squares of the triaxial magnetic field intensity values of all the calibrated magnetic field intensities to obtain the error between the calibrated magnetic field intensity and the normal magnetic field intensity.
According to another embodiment of the present invention, there is provided a compass calibration apparatus including: the relationship determination module is used for determining the relationship representation of the magnetic field intensity after calibration and the calibration data, the soft magnetic interference correction quantity and the hard magnetic interference correction quantity; the parameter determining module is used for determining soft magnetic interference correction quantity and hard magnetic interference correction quantity by enabling the error between the calibrated magnetic field intensity and the normal magnetic field intensity to meet a preset condition; and the calibration module is used for calibrating the compass by utilizing the soft magnetic interference correction quantity and the hard magnetic interference correction quantity.
In one embodiment, the parameter determination module is to: i) substituting the calibration data, the soft magnetic interference correction quantity and the hard magnetic interference correction quantity into the error relation expression of the magnetic field intensity after calibration and the normal magnetic field intensity to obtain a converted error relation expression; ii) obtaining a converted error relation representing a jacobian matrix with respect to each independent variable, wherein the independent variable is one or more of a normal magnetic field strength, a soft magnetic disturbance correction amount and a hard magnetic disturbance correction amount, and a preset value is not adopted as a term of the independent variable; iii) constructing an increment normal equation according to the Jacobian matrix, the iteration error, the iteration step length and the independent variable; iv) substituting the values of the independent variables into a Jacobian matrix to calculate a Jacobian matrix, and substituting the values of the independent variables into the converted error relation to express the calculation iteration error; v) substituting the calculated Jacobian matrix, the calculated iteration error and the first iteration step value into an increment normal equation to be solved into a self-variable first value, and substituting the calculated Jacobian matrix, the calculated iteration error and the second iteration step value into the increment normal equation to be solved into a self-variable second value; vi) substituting the first value of the independent variable into the converted error relation representation to obtain a first updating iteration error, and substituting the second value of the independent variable into the converted error relation representation to obtain a second updating iteration error; vii) comparing the first update iteration error and the second update iteration error with preset conditions respectively: if the first update iteration error and the second update iteration error are both greater than the preset condition, multiplying the first iteration step value by the preset iteration gradient to update the first iteration step value, multiplying the second iteration step value by the preset iteration gradient to update the second iteration step value, if the first update iteration error is less than the second update iteration error, updating the value of the argument with the argument first value, if the second update iteration error is less than the first update iteration error, updating the value of the argument with the argument second value, repeating steps iv) through vii) until the first update iteration error value or the second update iteration error value is less than or equal to the preset condition; if the first updating iteration error is smaller than the preset condition, outputting the first value of the independent variable as the value of the independent variable, and if the second updating iteration error is smaller than the preset condition, outputting the second value of the independent variable as the value of the independent variable; the values of the independent variables adopt preset values in the initial iteration, and the first iteration step value and the second iteration step value adopt preset values in the initial iteration.
In one embodiment, the parameter determining module is configured to preset a value of the soft magnetic interference correction amount, determine the normal magnetic field strength and the hard magnetic interference correction amount by making an error between the calibrated magnetic field strength and the normal magnetic field strength satisfy a preset condition, and determine the soft magnetic interference correction amount by making an error between the calibrated magnetic field strength and the normal magnetic field strength satisfy the preset condition based on the determined normal magnetic field strength and the soft magnetic interference correction amount.
In one embodiment, the incremental normal equation is expressed as:
wherein,kan argument representing the kth iteration, JkJacobian matrix representing the kth iteration, ekDenotes the iteration error, λ, of the kth iterationkDenotes the iteration step for the kth iteration and I denotes the identity matrix.
In one embodiment, the relationship between the calibrated magnetic field strength and the calibration data, soft magnetic disturbance correction, and hard magnetic disturbance correction is expressed as: the magnetic field intensity after calibration is equal to the sum of the calibration data and the correction quantity of the hard magnetic interference multiplied by the correction quantity of the soft magnetic interference; and the parameter determination module is used for adding the difference value obtained by subtracting the square of the normal magnetic field intensity from the sum of the squares of the triaxial magnetic field intensity values of all the calibrated magnetic field intensities to obtain the error between the calibrated magnetic field intensity and the normal magnetic field intensity.
According to still another embodiment of the present invention, there is provided a compass calibration apparatus including: a memory; and a processor coupled to the memory, the processor configured to perform a compass calibration method as in any one of the preceding embodiments based on instructions stored in the memory device.
According to a further embodiment of the invention, a computer-readable storage medium is provided, on which a computer program is stored which, when being executed by a processor, carries out the steps of the compass calibration method of any of the preceding embodiments.
The invention simultaneously considers the influence of the hard magnetic interference and the soft magnetic interference and solves the two, thereby calibrating the compass, improving the accuracy of the compass calibration and enabling the compass to realize more accurate navigation function.
Other features of the present invention and advantages thereof will become apparent from the following detailed description of exemplary embodiments thereof, which proceeds with reference to the accompanying drawings.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.
Fig. 1 shows a schematic structural diagram of a compass calibration device according to an embodiment of the present invention.
Fig. 2 shows a schematic structural diagram of a compass calibration device according to another embodiment of the present invention.
FIG. 3 shows a schematic flow chart of a compass calibration method according to an embodiment of the present invention.
FIG. 4 shows a schematic flow chart of a compass calibration method according to another embodiment of the present invention.
Fig. 5 shows a schematic structural diagram of a compass calibrating device according to a further embodiment of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. The following description of at least one exemplary embodiment is merely illustrative in nature and is in no way intended to limit the invention, its application, or uses. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The method and the device for calibrating the electronic magnetic compass aim at the problem that the accuracy of the electronic magnetic compass is insufficient due to the fact that only the influence of hard magnetic interference on the compass is considered in the prior art.
The compass calibration apparatus in the embodiments of the present invention may each be implemented by various computing devices or computer systems, which are described below in conjunction with fig. 1 and 2.
FIG. 1 is a block diagram of one embodiment of a compass calibration apparatus of the present invention. As shown in fig. 1, the apparatus 10 of this embodiment includes: a memory 110 and a processor 120 coupled to the memory 110, the processor 120 being configured to perform a compass calibration method in any of the embodiments of the present invention based on instructions stored in the memory 110.
Memory 110 may include, for example, system memory, fixed non-volatile storage media, and the like. The system memory stores, for example, an operating system, an application program, a Boot Loader (Boot Loader), a database, and other programs.
Fig. 2 is a block diagram of another embodiment of the compass calibration device of the present invention. As shown in fig. 2, the apparatus 10 of this embodiment includes: the memory 110 and the processor 120 may further include an input/output interface 230, a network interface 240, a storage interface 250, and the like. These interfaces 230, 240, 250 and the connection between the memory 110 and the processor 120 may be, for example, via a bus 260. The input/output interface 230 provides a connection interface for input/output devices such as a display, a mouse, a keyboard, and a touch screen. The network interface 240 provides a connection interface for various networking devices, such as a database server or a cloud storage server. The storage interface 250 provides a connection interface for external storage devices such as an SD card and a usb disk.
The present invention provides a method of compass calibration, which is described below in conjunction with fig. 3.
FIG. 3 is a flow chart of one embodiment of a compass calibration method of the present invention. As shown in fig. 4, the method of this embodiment includes:
step S302, determining the relationship expression of the magnetic field intensity after calibration and the calibration data, the soft magnetic interference correction amount and the hard magnetic interference correction amount.
The relationship between the magnetic field intensity after calibration and the calibration data, the soft magnetic disturbance correction amount, and the hard magnetic disturbance correction amount is expressed by, for example, the following equation:
H=M(A+B)T(1)
wherein H ═ H (H)x,Hy,Hz) Which represents the strength of the magnetic field after calibration, denotes the soft magnetic interference correction amount, A ═ Ax,Ay,Az) The calibration data is actually acquired by the compass, or the acquired data may be used after being subjected to certain preprocessing, for example, data which is close in distance and has poor calibration effect may be deleted, and B ═ may be used (B ═ may bex,By,Bz) The correction amount of hard magnetic interference is shown, and x, y and z respectively show an x axis, a y axis and a z axis in a body coordinate system.
It should be noted that if the compass also has a three-axis proportional error, M may include not only a pure soft magnetic interference correction but also a correction of the three-axis proportional error, that is, M ═ KN, K ═ diag (K)x,Ky,Kz),K represents the triaxial proportional error correction, and N represents the pure soft magnetic interference correction. Since K and N can be calibrated without separate calculation, only by calculating their product, KN is still calculated as one term for convenience.
And step S304, determining soft magnetic interference correction quantity and hard magnetic interference correction quantity by enabling the error between the calibrated magnetic field intensity and the normal magnetic field intensity to meet a preset condition.
The normal magnetic field strength value of the geomagnetic field is between 0.4 and 0.6mG, and the normal magnetic field strength value of the geomagnetic field is basically kept unchanged in a smaller geographical range, so that the soft magnetic interference correction amount and the hard magnetic interference correction amount can be determined by enabling the error between the calibrated magnetic field strength and the normal magnetic field strength to meet a preset condition.
Adding the sum of the squares of the triaxial magnetic field strength values of all the calibrated magnetic field strengths minus the square of the normal magnetic field strength to obtain the error between the calibrated magnetic field strength and the normal magnetic field strength, for example, using the following formula to represent:
wherein HiThe data after the ith calibration is shown, n is the number of the data after the calibration and is determined by the number of the calibration data, and R is the normal magnetic field intensity.
Substituting equation (1) into equation (2) yields a transformed error relationship representation:
as can be seen from equation (3), the calibration data A is knowniAsTraining data (for example, n is 300 calibration data), a value range of e is preset, if a normal magnetic field strength R is obtained through other test methods as a known parameter, a hard magnetic interference correction amount B and a soft magnetic interference correction amount M can be obtained by solving formula (3), if the normal magnetic field strength R is also used as an unknown variable, a normal magnetic field strength R, a hard magnetic interference correction amount B and a soft magnetic interference correction amount M can be obtained, and the solving method is described in detail later.
And step S306, calibrating the compass by using the soft magnetic interference correction quantity and the hard magnetic interference correction quantity.
After the soft magnetic interference correction amount and the hard magnetic interference correction amount are obtained, when the compass is used for navigation, the actual measurement data of the compass, the soft magnetic interference correction amount and the hard magnetic interference correction amount are directly substituted into the formula (1) to obtain the magnetic field intensity after calibration, so that the compass can realize a more accurate navigation function.
In the prior art, only the influence of hard magnetic interference is generally considered when the compass is calibrated, but the influence of soft magnetic interference is ignored.
In the above embodiment, the independent variables to be solved in the formula (3) may be the hard magnetic interference correction amount B and the soft magnetic interference correction amount M, or the normal magnetic field strength R, the hard magnetic interference correction amount B and the soft magnetic interference correction amount M, since the formula (3) belongs to the solution of the nonlinear multi-unknowns equation, the solution may be performed by using an LM (Levenberg Marquardt, Levenberg-Marquardt) algorithm, and in order to improve the calculation efficiency, the LM algorithm is improved by the present invention.
Firstly, if the normal magnetic field strength R is also used as an independent variable to be solved, the solving process can be divided into two times to improve the calculation efficiency, preferably, the value of the soft magnetic interference correction quantity M is preset, and the normal magnetic field strength R and the hard magnetic interference correction quantity B are determined by enabling the error between the calibrated magnetic field strength and the normal magnetic field strength to meet the preset condition; and determining the soft magnetic interference correction quantity by enabling the error between the calibrated magnetic field strength and the normal magnetic field strength to meet a preset condition based on the determined normal magnetic field strength R and the soft magnetic interference correction quantity B. The method is divided into two solving processes, the number of unknown parameters at each time is reduced, the calculation efficiency is improved, in addition, the normal magnetic field intensity is also calculated, and the calibration accuracy can be further improved.
How to solve the argument in equation (3) using the improved LM algorithm in the present invention is described below with reference to fig. 4.
FIG. 4 is a flow chart of another embodiment of a compass calibration method of the present invention. As shown in fig. 4, step S306 includes:
in step S3061, a converted error relationship representation is obtained for each independent variable as a jacobian matrix.
The independent variable is one or more of the normal magnetic field strength R, the soft magnetic interference correction quantity M and the hard magnetic interference correction quantity B, and a preset value is adopted as a term which is not used as the independent variable. Namely, the solving process can be completed once or for multiple times, and the improved LM algorithm can be adopted in each solving.
And S3062, constructing an increment normal equation according to the Jacobian matrix, the iteration error, the iteration step length and the independent variable.
The incremental normal equation is expressed as:
wherein,kthe independent variable representing the kth iteration can be represented in a vector form, namely, all unknowns needing to be solved are arranged according to a certain sequence to form a vector, JkJacobian matrix representing the kth iteration, ekIndicating iteration error of kth iterationDifference, λkDenotes the iteration step for the kth iteration and I denotes the identity matrix.
In step S3063, the number of iterations k is 0, and the value of the argument is initialized0First iteration step size value λ01And a second iteration step size value λ02Setting a preset condition e for meeting the termination iteration error, namely the error between the magnetic field intensity after calibration and the normal magnetic field intensitythAnd iterating the gradient V.
Wherein,0r may be set to 200 at initialization if the normal magnetic field strength R is included, the diagonal line is set to 1 if the soft magnetic disturbance correction amount M is included, the other elements are 0, the elements in the hard magnetic disturbance correction amount B may all be set to 0 if included, and the first iteration step value λ01Can be set as 10-3,λ02Can be set to lambda02=λ01/V,V=10,λ02Or with λ01The other values are set without the above proportional relationship. e.g. of the typethMay be set to 0.001.
Step S3064, the value of the argumentkCalculating the Jacobian matrix J by substituting the Jacobian matrixkThe value of the independent variablekSubstituting formula (3) to calculate the iteration error ek。
kValues for each element of B or M may be included as well as the value of R.
Step S3065, calculating Jacobian matrix JkCalculated iteration error ekFirst iteration step value lambdak1Solving for the first value of the variable by substituting the normal equation (4) for the incrementk+1,1(ii) a Calculating the Jacobian matrix JkCalculated iteration error ekSecond iteration step size lambdak2Solving the normal equation (4) substituted into the increment to obtain a second value of the self-variablek+1,2。
Step S3066, set the first value of the argumentk+1,1Substituting the formula (3) to obtain a first updated iteration error ek+1,1Setting the second value of the independent variablek+1,2Substituting the formula (3) to obtain a second updated iteration error ek+1,2。
Wherein the iteration error ekFirst updated iteration error ek+1,1Obtaining a second updated iteration error ek+1,2All the values are positive numbers, namely, if the values are substituted into the formula (3), the absolute values of the values are taken if negative numbers are obtained.
Step S3067, update the first iteration error ek+1,1And a second update iteration error ek+1,2Respectively corresponding to the preset conditions ethMaking an alignment if ek+1,1>ethAnd e is ak+1,2>ethStep S3068 is performed if ek+1,1≤ethOr ek+1,2≤ethThen step S3069 is performed.
Step S3068, apply the first iteration step value λk1Multiplying by a preset iteration gradient V to update a first iteration step value lambdak+1,1I.e. λk+1,1=λk1V, with a second iteration step value λk2Multiplying by a preset iteration gradient V degree to update a second iteration step value lambdak+1,2I.e. λk+1,2=λk2V; if the first update iteration error ek+1,1Less than the second update iteration error ek+1,2Using the first value of the argumentk+1,1To update the value of an argumentk+1I.e. if ek+1,1<ek+1,2Then, thenk+1=k+1,1If the second update iteration error ek+1,2Less than the first update iteration error ek+1,1Using the second value of the argumentk+1,2To update the value of an argumentk+1I.e. if ek+1,2≤ek+1,1Then, thenk+1=k+1,2Returning to step S3064, the next iteration cycle, i.e., k + +, is entered.
The preset iterative gradient may also be set to two V1And V2Respectively for updating the first iteration stepA long value and a second iteration step value, i.e. lambdak+1,1=λk1·V1,λk+1,2=λk2·V2. More different iteration step lengths can be set in the scheme.
Step S3069, if the first update iteration error is less than the preset condition, i.e. ek+1,1<ethThen the argument is set to the first valuek+1,1As the value output of the argument, e if the second update iteration error is less than the preset conditionk+1,2<ethThen the argument is set to the second valuek+1,2As the value of the argument.
In the embodiment, different iteration step lengths are set, so that the algorithm can reach the termination condition more quickly, the iteration judgment condition of the LM algorithm is optimized, and the efficiency of the algorithm is further improved.
Preferably, as described in the foregoing embodiment, if the normal magnetic field strength R, the hard magnetic disturbance correction amount B and the soft magnetic disturbance correction amount M are all used as unknown independent variables, the values of the normal magnetic field strength R and the hard magnetic disturbance correction amount B are first preset as independent variables, the values of the soft magnetic disturbance correction amount M are calculated by the methods of steps S3061 to S3069 to obtain the values of the normal magnetic field strength R and the hard magnetic disturbance correction amount B, the values of the normal magnetic field strength R and the hard magnetic disturbance correction amount B are further substituted into the formula (3), the value of the soft magnetic disturbance correction amount M is used as an independent variable, the value of the soft magnetic disturbance correction amount M is calculated by the methods of steps S3061 to S3069 again, so as to improve the calculation efficiency, of course, the solving process may be divided into three times, and one of the normal magnetic field strength R, the hard magnetic disturbance correction amount B and, or firstly solving one of the normal magnetic field intensity R, the hard magnetic interference correction quantity B and the soft magnetic interference correction quantity M, and then solving the remaining two, or completing the solving process once.
The normal magnetic field intensity R, the hard magnetic interference correction quantity B and the soft magnetic interference correction quantity M can be compared with a preset normal value, and if the normal magnetic field intensity R, the hard magnetic interference correction quantity B and the soft magnetic interference correction quantity M do not meet the requirement, the nearby strong magnetic field interference is considered. The interference source needs to be removed and recalibrated. For example, the normal magnetic field strength R needs to be within 150-600 mG, the hard magnetic disturbance correction amount B needs to be less than 1000mG, and the diagonal value of the soft magnetic disturbance correction amount M is within 0.2-5.
The steps S3062 to S3069 may be replaced by the following method:
i) the inverted error relationships are obtained representing a jacobian matrix for each independent variable.
The independent variable is one or more of a normal magnetic field intensity, a soft magnetic interference correction amount and a hard magnetic interference correction amount, and a preset value is adopted as an item which is not used as the independent variable;
ii) constructing an increment normal equation according to the Jacobian matrix, the iteration error, the iteration step size and the independent variable increment.
And iii) substituting the values of the independent variables into the Jacobian matrix to calculate the Jacobian matrix, and substituting the values of the independent variables into the converted error relation to express the calculation iteration error.
iv) substituting the calculated Jacobian matrix, the calculated iteration error and the first iteration step value into an increment normal equation to solve to obtain a first independent variable increment, and substituting the calculated Jacobian matrix, the calculated iteration error and the second iteration step value into the increment normal equation to solve to obtain a second independent variable increment.
v) substituting the sum of the first independent variable increment and the independent variable value into the converted error relation expression to obtain a first updating iteration error, and substituting the sum of the second independent variable increment and the independent variable value into the converted error relation expression to obtain a second updating iteration error.
vi) comparing the first updated iteration error and the second updated iteration error with a preset condition and an iteration error respectively:
if the first update iteration error and the second update iteration error are both greater than the iteration error, multiplying the first iteration step value by the preset iteration gradient to update the first iteration step value, multiplying the second iteration step value by the preset iteration gradient to update the second iteration step value, keeping the value of the independent variable unchanged, and repeating the steps iii) to vi) until the first update iteration error value or the second update iteration error value is less than or equal to the preset condition;
if the first update iteration error and the second update iteration error are both greater than the preset condition and the first update iteration error and the second update iteration error are both less than the iteration error, updating the value of the argument with the sum of the first argument increment and the argument if the first update iteration error is less than the second update iteration error, updating the value of the argument with the sum of the second argument increment and the argument if the second update iteration error is less than the first update iteration error, updating the value of the first iteration step with the first iteration step divided by the preset iteration gradient, updating the value of the second iteration step with the second iteration step divided by the preset iteration gradient, and repeating steps iii) to vi) until the value of the first update iteration error or the value of the second update iteration error is less than or equal to the preset condition;
if the first update iteration error and the second update iteration error are both greater than the preset condition, the first update iteration error is less than the iteration error, and the second update iteration error is greater than the iteration error, updating the value of the argument by the sum of the first argument increment and the argument, keeping the iteration step constant, and repeating the steps iii) to vi) until the first update iteration error value or the second update iteration error value is less than or equal to the preset condition;
and if the first updating iteration error and the second updating iteration error are both larger than the preset condition, the first updating iteration error is larger than the iteration error, and the second updating iteration error is smaller than the iteration error, updating the value of the argument by using the sum of the second argument increment and the argument, keeping the iteration step unchanged, and repeating the steps iii) to vi) until the first updating iteration error value or the second updating iteration error value is smaller than or equal to the preset condition.
The present invention also provides a compass calibration device, which is described below with reference to fig. 5.
Fig. 5 is a block diagram of an embodiment of the compass calibrating device of the present invention, and as shown in fig. 5, the compass calibrating device 50 includes:
and the relation determining module 502 is used for determining the relation representation of the calibrated magnetic field strength and the calibration data, the soft magnetic interference correction quantity and the hard magnetic interference correction quantity.
In one embodiment, the relationship between the calibrated magnetic field strength and the calibration data, soft magnetic disturbance correction, and hard magnetic disturbance correction is expressed as: the calibrated field strength is equal to the sum of the calibration data and the hard magnetic disturbance correction multiplied by the soft magnetic disturbance correction.
And the parameter determining module 504 is configured to determine a soft magnetic interference correction amount and a hard magnetic interference correction amount by enabling an error between the calibrated magnetic field strength and the normal magnetic field strength to satisfy a preset condition.
In one embodiment, the parameter determination module 504 is configured to add the sum of the squares of the three-axis magnetic field strength values of all calibrated magnetic field strengths minus the square of the normal magnetic field strength to obtain the error between the calibrated magnetic field strength and the normal magnetic field strength.
And the calibration module 506 is used for calibrating the compass by using the soft magnetic interference correction quantity and the hard magnetic interference correction quantity.
In one embodiment, the parameter determination module 504 is configured to:
i) and substituting the calibration data, the soft magnetic interference correction quantity and the hard magnetic interference correction quantity into the error relation expression of the magnetic field intensity after calibration and the normal magnetic field intensity to obtain a converted error relation expression.
ii) obtaining a transformed error relationship representing a jacobian matrix with respect to each independent variable, wherein the independent variable is one or more of a normal magnetic field strength, a soft magnetic disturbance correction amount and a hard magnetic disturbance correction amount, and a preset value is not adopted as a term of the independent variable.
iii) constructing an increment normal equation according to the Jacobian matrix, the iteration error, the iteration step size and the independent variable.
iv) substituting the values of the independent variables into the Jacobian matrix to calculate the Jacobian matrix, and substituting the values of the independent variables into the converted error relation to express the calculation iteration error.
v) substituting the calculated Jacobian matrix, the calculated iteration error and the first iteration step value into an increment normal equation to be solved into a self-variable first value, and substituting the calculated Jacobian matrix, the calculated iteration error and the second iteration step value into the increment normal equation to be solved into a self-variable second value.
And vi) substituting the first value of the independent variable into the converted error relation representation to obtain a first updating iteration error, and substituting the second value of the independent variable into the converted error relation representation to obtain a second updating iteration error.
vii) comparing the first update iteration error and the second update iteration error with preset conditions respectively:
if the first update iteration error and the second update iteration error are both greater than the preset condition, multiplying the first iteration step value by the preset iteration gradient to update the first iteration step value, multiplying the second iteration step value by the preset iteration gradient to update the second iteration step value, if the first update iteration error is less than the second update iteration error, updating the value of the argument with the argument first value, if the second update iteration error is less than the first update iteration error, updating the value of the argument with the argument second value, repeating steps iv) through vii) until the first update iteration error value or the second update iteration error value is less than or equal to the preset condition;
and if the first updating iteration error is smaller than the preset condition, outputting the first value of the independent variable as the value of the independent variable, and if the second updating iteration error is smaller than the preset condition, outputting the second value of the independent variable as the value of the independent variable.
The values of the independent variables adopt preset values in the initial iteration, and the first iteration step value and the second iteration step value adopt preset values in the initial iteration.
In one embodiment, the parameter determining module is configured to preset a value of the soft magnetic interference correction amount, determine the normal magnetic field strength and the hard magnetic interference correction amount by making an error between the calibrated magnetic field strength and the normal magnetic field strength satisfy a preset condition, and determine the soft magnetic interference correction amount by making an error between the calibrated magnetic field strength and the normal magnetic field strength satisfy the preset condition based on the determined normal magnetic field strength and the soft magnetic interference correction amount.
The compass calibration device in the above embodiment may be disposed on an electronic compass or an unmanned aerial vehicle, or may adopt a separate device, and output the finally calculated soft magnetic interference correction amount and hard magnetic interference correction amount to the compass for compass calibration.
The invention also provides a computer-readable storage medium, on which a computer program is stored which, when being executed by a processor, carries out the steps of the compass calibration method of any of the preceding embodiments.
As will be appreciated by one skilled in the art, embodiments of the present invention may be provided as a method, system, or computer program product. Accordingly, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of a computer program product embodied on one or more computer-usable non-transitory storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
The present invention is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.