CN115235465A - Method for measuring attitude angle by magnetic force/GNSS combination - Google Patents

Method for measuring attitude angle by magnetic force/GNSS combination Download PDF

Info

Publication number
CN115235465A
CN115235465A CN202210087720.0A CN202210087720A CN115235465A CN 115235465 A CN115235465 A CN 115235465A CN 202210087720 A CN202210087720 A CN 202210087720A CN 115235465 A CN115235465 A CN 115235465A
Authority
CN
China
Prior art keywords
measurement
ellipsoid
equation
angle
data
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
Application number
CN202210087720.0A
Other languages
Chinese (zh)
Inventor
芦海巍
杨文靖
蔺大方
张伟
冯雪
蒋丽丽
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Clp Xingyuan Technology Co ltd
Original Assignee
Clp Xingyuan Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Clp Xingyuan Technology Co ltd filed Critical Clp Xingyuan Technology Co ltd
Priority to CN202210087720.0A priority Critical patent/CN115235465A/en
Publication of CN115235465A publication Critical patent/CN115235465A/en
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement

Abstract

The invention relates to a method for measuring an attitude angle by a magnetic force/GNSS combination, belonging to the technical field of attitude angle measurement. In a practical application scenario, factors influencing the measurement accuracy of the parameters mainly include the following: external or system interference (external magnetic field source interference, interference of an internal circuit, magnetization influence, and the like), measurement errors (high-frequency errors, noise interference, jumping points, and the like), system errors (different axial sensitivities, zero drift, and the like), algorithm errors, and the like (deviations due to small angle of attack assumptions). In an actual application scenario, factors influencing the measurement accuracy of these parameters mainly include the following: external or system interference (external magnetic field source interference, interference of an internal circuit, magnetization influence and the like), measurement errors (high-frequency errors, noise interference, jumping points and the like), system errors (different axial sensitivities, zero drift and the like), algorithm errors and the like (deviation caused by small attack angle assumption).

Description

Method for measuring attitude angle by magnetic force/GNSS combination
Technical Field
The invention relates to a method for measuring an attitude angle by a magnetic force/GNSS combination, belonging to the technical field of measuring the attitude angle.
Background
In recent years, the performance and reliability of Micro Electro Mechanical Systems (MEMS) have been greatly improved, and the manufacturing and use costs have been greatly reduced. Therefore, various MEMS sensors with small volume, low power consumption and simple algorithm are widely adopted in the fields of navigation, unmanned planes, navigation grenades, robots and the like.
The geomagnetic sensor is used as an important gate in an MEMS device, is combined with a satellite positioning system (GNSS) to carry out a combined measuring tool, has the advantages of low cost, strong overload resistance, strong adaptability, easy realization of resolving and the like, and becomes a main attitude angle measuring mode of a low-cost unmanned aerial vehicle, a projectile, a robot and an unmanned boat. Calibrating and filtering the sensor to overcome these problems is an effective way to improve the measurement results.
Roll angle error depends on magnetometer measurement to get h by 、h bz Error, pitch angle theta and yaw angle psi and their incremental errors delta theta, delta psi. For high frequency measurements, h is also derived from magnetometer measurements, as can be seen from equation (4) by 、h bz Error, error in pitch angle theta and yaw angle psi, and in pitch angle rate theta and yaw rate theta
Figure BDA0003487698160000011
The errors are correlated.
In an actual application scenario, factors influencing the measurement accuracy of these parameters mainly include the following: external or system interference (external magnetic field source interference, interference of an internal circuit, magnetization influence and the like), measurement errors (high-frequency errors, noise interference, jumping points and the like), system errors (different axial sensitivities, zero drift and the like), algorithm errors and the like (deviation caused by small attack angle assumption).
Disclosure of Invention
In view of this, the present invention provides a method for measuring an attitude angle by using a magnetic force/GNSS combination, which corrects a system error and a measurement error by using data fitting and filtering algorithms, and compensates external or system interference such as external interference, electromagnetic interference and magnetization interference of a system itself by using a compensation algorithm.
The invention provides a method for measuring attitude angle by combining magnetic force/GNSS, which comprises the following conversion relation among magnetometer measurement data, geomagnetic data and carrier attitude angle:
Figure BDA0003487698160000021
the magnetometer measurement data is the body axial magnetic field strength, the geomagnetic data is the magnetic field strength in the navigation coordinate system, and the data comprises: h is nx 、h ny 、h nz Is geomagnetic component under navigation coordinate system; h is bx 、h by 、h bz The projection component of the geomagnetic vector under the carrier coordinate system is used; psi, theta and gamma are respectively a yaw angle, a pitch angle and a roll angle under a navigation coordinate system;
in a speed and navigation coordinate system, the speed information provided by the measuring equipment is utilized to obtain the trajectory inclination angle and trajectory deflection angle as shown in the following formula:
Figure BDA0003487698160000022
v is x 、v y 、v z North, east and ground speed, respectively;
in a low-rotation application scene, when the yaw angle and the pitch angle are known, the value of the roll angle can be uniquely determined by the formula (1):
Figure BDA0003487698160000023
the following steps:
m＝-sinψh nx +cosψh ny
n＝cosψsinθh nx +sinψsinθh ny +cosθh nz ；
in a high-rotation application scene, the roll rate of the carrier in the whole flight process is far greater than the pitch angle and the yaw rate, namely
Figure BDA0003487698160000024
Therefore, during the satellite data updating time, it can be assumed that only the roll angle of the carrier is changed, and equation (1) can be simplified as follows:
Figure BDA0003487698160000025
the A and a are defined as follows:
Figure BDA0003487698160000026
Figure BDA0003487698160000027
Figure BDA0003487698160000031
Figure BDA0003487698160000032
Figure BDA0003487698160000033
Figure BDA0003487698160000034
defining geomagnetic output under a navigation coordinate system as follows:
Figure BDA0003487698160000035
a is a magnetic measurement value and a geomagnetic intensity proportional coefficient; f is the measurement variation frequency; t is a magnetic measurement time series; phi is an initial offset angle;
the roll rate obtainable from equations (4) and (5) is:
Figure BDA0003487698160000036
roll angle information may be obtained by integrating equation (6) before the satellite measurement device updates velocity information.
For the magnetometers which are vertically arranged in three sensitive axial directions, the acquired raw data is a space ellipsoid which is generally distributed in three axial directions parallel to a body axis;
the general equation for an ellipsoid can be expressed as:
a 1 x 2 +a 2 y 2 +a 3 z 2 +a 4 xy+a 5 xz+a 6 yz+a 7 x+a 8 y+a 9 z＝1 (7)
the standard equation is written in the form:
Figure BDA0003487698160000037
the points distributed on the ellipsoid can be expressed in a matrix form from a geometrical point of view as follows:
Figure BDA0003487698160000038
Figure BDA0003487698160000041
the above formula can be represented as:
[X-C]M[X-C] T ＝1+CMC T (10)
XMX T -2CMX T +CMC T ＝1+CMC T (11)
the X = [ X y z ]]Denotes a point on an ellipsoid, C = [ C = x c y c z ]Is a coordinate point of the center of the ellipsoid sphere,
Figure BDA0003487698160000042
according to the ellipsoid model, the following ellipsoid parameters:
coordinates of the center of sphere:
C＝0.5[a 7 a 8 a 9 ]M -1
length of x axis:
Figure BDA0003487698160000043
length of y-axis:
Figure BDA0003487698160000044
length of z-axis:
Figure BDA0003487698160000045
said SS = CMC T +1
If n sets of data are collected by the magnetometer, the data can be expressed as follows by substituting the data into an ellipsoid equation:
Figure BDA0003487698160000046
order:
Figure BDA0003487698160000051
k＝[a 1 a 2 ... a 9 ] T
the ellipsoid fitting is to determine a set of ellipsoid parameter values K with the minimum variance according to the measurement raw data and the equation. According to the minimum second-order multiplication algorithm, the ellipsoid parameter with the minimum measurement error is as follows:
k＝(H T H) -1 H T (13)
the ellipsoid parameters obtained by fitting can be used for calibrating the data measured by the magnetometer.
Firstly, carrying out ellipsoid coordinate origin calibration:
h bt ＝h b -C (14)
after the correction, the measurement data is distributed by an ellipsoid taking the coordinate origin as the center of the sphere, so that zero point errors are eliminated, and meanwhile, spherical calibration is required to eliminate different axial sensitivities. In performing the spherical calibration, a reference axis needs to be determined first. In the magnetometer-satellite attitude determination scheme, the Y-axis is important, so the Y-axis can be selected as the reference for spherical correction, namely:
h b ＝y scale *h bt .*[x scale y scale z scale ] (15)。
according to the practical characteristics of the combined measurement attitude of the magnetometer and the satellite positioning, the state model generally has the following form:
Figure BDA0003487698160000052
x is a state quantity, a space position in the case of a satellite measurement system, a change equation of local geomagnetic intensity in each axial direction in the case of a magnetometer, and N is a change rate of each state,
Figure BDA0003487698160000053
for the various interference noise faced by each state,
the "current" statistical model is used herein to optimize the motion model. Based on the "current" statistical model, the following:
Figure BDA0003487698160000054
Figure BDA0003487698160000055
the above-mentioned
Figure BDA0003487698160000056
Is the "current" average of the rate change amount, and can be considered as a constant in the same sampling period of the sensor;
order:
Figure BDA0003487698160000061
then:
Figure BDA0003487698160000062
within the same sampling period, it can be considered that
Figure BDA0003487698160000063
Then there is
Figure BDA0003487698160000064
From formula (19):
Figure BDA0003487698160000065
bringing this formula and formula (18) into formula
Figure BDA0003487698160000066
The following can be obtained:
Figure BDA0003487698160000067
namely:
Figure BDA0003487698160000068
this is the equation of state, i.e., the "current" statistical model of the mobile vehicle, when equation (16) is expressed as:
Figure BDA0003487698160000069
the result estimated by the discrete system kalman filter algorithm is to minimize the overall mean square error, i.e.:
Figure BDA00034876981600000610
the kalman filter result is an unbiased estimate, namely:
Figure BDA00034876981600000611
from equation (23), the system equation and the measurement equation are described here in the form:
X K ＝φ k，k-1 X k-1 +Γ k-1 W k-1 (24)
Z k ＝H k X k +V K (25)
the X is an n-dimensional state vector; z is m as a measurement vector; phi is an n multiplied by n dimensional system matrix; Γ is an n × r dimensional system noise matrix; h is mxn is a measurement matrix; w and V are r and m-dimensional mean white noise, respectively, and X 0 W, V cross correlation
Order: e { X } 0 }＝m x0 ，E{[X 0 -m x0 ][X 0 -m x0 ] T }＝P 0 ，
Figure BDA00034876981600000612
Figure BDA00034876981600000613
The order is as follows: e { X } 0 }＝m x0 ，E{[X 0 -m x0 ][X 0 -m x0 ] T }＝P 0 ，
Figure BDA0003487698160000071
Figure BDA0003487698160000072
The discrete system kalman filter equation is as follows:
Figure BDA0003487698160000073
Figure BDA0003487698160000074
Figure BDA0003487698160000075
Figure BDA0003487698160000076
P k ＝P k，k-1 -K k H k P k，k-1 。
the invention has the beneficial effects that:
the invention provides a method for measuring an attitude angle by a magnetic force/GNSS combination, which adopts data fitting and filtering algorithms to process and correct system errors and measurement errors, and adopts a compensation algorithm to compensate external interference, system electromagnetic interference, system magnetization interference and other external or system interference. .
Detailed Description
The preferred embodiments of the present invention will be described in detail below.
When the magnetometer and the satellite data are used for measuring the attitude angle of the carrier, the magnetometer and the carrier are installed in a strapdown mode, and the direction of the sensitive axis of the magnetometer is the same as the direction of a carrier coordinate system. During use, the magnetometer measures data of the axial component of the earth magnetism on the carrier in each axial direction in real time. According to the Euler's theorem, the following conversion relationship exists among magnetometer measurement data (carrier axial magnetic field intensity), geomagnetic data (magnetic field intensity in a navigation coordinate system) and carrier attitude angles:
Figure BDA0003487698160000077
here: h is a total of nx 、h ny 、h nz Is geomagnetic component under navigation coordinate system; h is bx 、h by 、h bz The projection component of the geomagnetic vector under the carrier coordinate system is used; psi, theta and gamma are respectively a yaw angle, a pitch angle and a roll angle under the navigation coordinate system.
Countless sets of attitude angle feasible solutions can be solved by the formula (1). Therefore, the measurement requirements of engineering practice cannot be met, and the fusion of satellite measurement data is also needed to complete the measurement of the attitude angle.
The satellite measuring equipment can measure the longitude, latitude, altitude and speed information of the carrier in real time. In a speed and navigation coordinate system, the speed information provided by the measuring equipment is utilized to obtain the trajectory inclination angle and trajectory deflection angle as shown in the following formula:
Figure BDA0003487698160000081
here: v. of x 、v y 、v z North, east and ground speed, respectively.
For a low-rotation application scene, when the yaw angle and the pitch angle are known, the value of the roll angle can be uniquely determined by the formula (1):
Figure BDA0003487698160000082
here:
m＝-sinψh nx +cosψh ny
n＝cosψsinθh nx +sinψsinθh ny +cosθh nz
in a high-rotation application scene, the roll rate of the carrier in the whole flight process is far greater than the pitch angle and the yaw rate, namely
Figure BDA0003487698160000083
Thus, during the time of satellite data update, it can be assumed that only the roll angle of the carrier has changed. Under this assumption, equation (1) can be simplified as:
Figure BDA0003487698160000084
here: a and α are defined as follows:
Figure BDA0003487698160000085
Figure BDA0003487698160000086
Figure BDA0003487698160000087
Figure BDA0003487698160000088
Figure BDA0003487698160000089
Figure BDA00034876981600000810
defining geomagnetic output under a navigation coordinate system as follows:
Figure BDA0003487698160000091
here: a is a magnetic measurement value and a geomagnetic intensity proportional coefficient; f is the measurement variation frequency; t is a magnetic measurement time series; phi is the initial offset angle.
The roll rate obtainable from equations (4) and (5) is:
Figure BDA0003487698160000092
roll angle information may be obtained by integrating equation (6) before the satellite measurement device updates velocity information.
Within a certain area, the earth's magnetic field is homogeneous. If three magnetosensitive devices of the magnetometer are installed along three mutually perpendicular directions of the projectile body axial direction, after enough data in each direction are collected, the spatial distribution of the data should be theoretically a sphere with a sphere center at the origin of coordinates. However, due to the influences of factors such as zero drift of the sensitive device, different axial sensitivities and the like, the spherical center of the original data obtained by real acquisition is neither in the origin of coordinates nor in a sphere shape. In order to make the measured data have relativity with the theoretical model, the raw data needs to be calibrated. Generally, the calibration process mainly comprises two parts: ellipsoid calibration and spherical calibration.
For the magnetometers with three sensitive axes arranged perpendicular to each other, the collected raw data is a space ellipsoid which is roughly distributed in three axes parallel to the body axis. The ellipsoid calibration aims to determine and acquire an ellipsoid equation with the minimum mean variance of data by adopting a statistical method, and the magnetometer is calibrated by adopting a minimum quadratic factorial method.
The general equation for an ellipsoid can be expressed as:
a 1 x 2 +a 2 y 2 +a 3 z 2 +a 4 xy+a 5 xz+a 6 yz+a 7 x+a 8 y+a 9 z＝1 (7)
the standard equation is written in the form:
Figure BDA0003487698160000093
the points distributed on the ellipsoid can be expressed in a matrix form from a geometrical point of view as follows:
Figure BDA0003487698160000094
Figure BDA0003487698160000101
the above formula can be represented as:
[X-C]M[X-C] T ＝1+CMC T (10)
XMX T -2CMX T +CMC T ＝1+CMC T (11)
the X = [ X y z ]]Denotes a point on an ellipsoid, C = [ C ] x c y c z ]Is an ellipsoid spherical center coordinate point,
Figure BDA0003487698160000102
according to the ellipsoid model, the following ellipsoid parameters:
and (3) coordinates of the center of sphere:
C＝0.5[a 7 a 8 a 9 ]M -1
length of x-axis:
Figure BDA0003487698160000103
length of y-axis:
Figure BDA0003487698160000104
length of z axisDegree:
Figure BDA0003487698160000105
said SS = CMC T +1
If n sets of data are collected by the magnetometer, the data can be expressed in the following form by substituting the data into an ellipsoid equation:
Figure BDA0003487698160000106
order:
Figure BDA0003487698160000111
k＝[a 1 a 2 ... a 9 ] T
the ellipsoid fitting is to determine a set of ellipsoid parameter values K with the smallest variance from the measured raw data and the equation. According to the minimum second-order multiplication algorithm, the ellipsoid parameter with the minimum measurement error is as follows:
k＝(H T H) -1 H T (13)
the data obtained by the measurement of the magnetometer can be calibrated by the ellipsoid parameters obtained by fitting.
Firstly, carrying out ellipsoid coordinate origin calibration:
h bt ＝h b -C (14)
after the correction, the measurement data is distributed by an ellipsoid with the coordinate origin as the sphere center, so that zero point errors are eliminated, and meanwhile, spherical calibration is required to eliminate different axial sensitivities. In performing the spherical calibration, a reference axis needs to be determined first. In the magnetometer-satellite attitude determination scheme, the Y axis is more important, so the Y axis can be selected as the reference for spherical correction, that is:
h b ＝y scale *h bt .*[x scale y scale z scale ] (15)。
kalman filtering is an effective means for processing data measurement errors of a dynamic system, and can effectively improve dynamic measurement precision. In order to eliminate the measurement errors of the magnetometer and the satellite positioning system, a Kalman filtering algorithm is adopted for solving.
In a combined magnetometer and satellite positioning attitude data measurement method, the state equations of the sensors are continuous, while the metrology equations are discrete. However, in terms of practical applications, the accuracy of the measurement information is more concerned, and the discrete system model is more concise and convenient to add, so that the discrete system model is used for filtering in the aspect of applications.
According to the practical characteristics of the combined measurement attitude of the magnetometer and the satellite positioning, the state model generally has the following form:
Figure BDA0003487698160000112
x is a state quantity, a space position in the case of a satellite measurement system, a change equation of local geomagnetic intensity in each axial direction in the case of a magnetometer, and N is a change rate of each state,
Figure BDA0003487698160000121
for the various interference noise faced by each state,
the motion model is optimized using a "current" statistical model. Based on the "current" statistical model, the following:
Figure BDA0003487698160000122
Figure BDA0003487698160000123
the described
Figure BDA0003487698160000124
As the "current" average of the rate change amount, at sensingThe sampling period of the device can be regarded as a constant;
order:
Figure BDA0003487698160000125
then:
Figure BDA0003487698160000126
within the same sampling period, it can be considered that
Figure BDA0003487698160000127
Then there is
Figure BDA0003487698160000128
From formula (19):
Figure BDA0003487698160000129
bringing this formula and formula (18) into formula
Figure BDA00034876981600001210
The following can be obtained:
Figure BDA00034876981600001211
namely:
Figure BDA00034876981600001212
this is the equation of state, i.e., the "current" statistical model of the mobile vehicle, when equation (16) is expressed as:
Figure BDA00034876981600001213
the result of the estimation by the discrete system kalman filter algorithm minimizes the overall mean square error, namely:
Figure BDA00034876981600001214
the kalman filter result is an unbiased estimate, namely:
Figure BDA00034876981600001215
from equation (23), the system equations and measurement equations are described here in the form:
X K ＝φ k，k-1 X k-1 +Γ k-1 W k-1 (24)
Z k ＝H k X k +V K (25)
the X is an n-dimensional state vector; z is m as a measurement vector; phi is an n multiplied by n dimensional system matrix; Γ is an n × r dimensional system noise matrix; h is mxn is a measurement matrix; w and V are r and m-dimensional mean white noise, respectively, and X 0 W, V cross correlation
Order: e { X } 0 }＝m x0 ，E{[X 0 -m x0 ][X 0 -m x0 ] T }＝P 0 ，
Figure BDA0003487698160000131
Figure BDA0003487698160000132
The order is as follows: e { X } 0 }＝m x0 ，E{[X 0 -m x0 ][X 0 -m x0 ] T }＝P 0 ，
Figure BDA0003487698160000133
Figure BDA0003487698160000134
The discrete system kalman filter equation is as follows:
Figure BDA0003487698160000135
Figure BDA0003487698160000136
Figure BDA0003487698160000137
Figure BDA0003487698160000138
P k ＝P k，k-1 -K k H k P k，k-1 。
the present invention and the embodiments thereof have been described above without limitation, and it is within the scope of the present invention that those skilled in the art should be able to devise similar structural modes and embodiments without inventive changes without departing from the spirit and scope of the present invention.

Claims (3)

1. A method for measuring attitude angle by a magnetic force/GNSS combination is characterized in that: the following conversion relationship exists between the data measured by the magnetometer, the geomagnetic data, and the attitude angle of the carrier:
Figure FDA0003487698150000011
the magnetometer measurement data is the axial magnetic field intensity, the geomagnetic data is the magnetic field intensity under the navigation coordinate system, here: h is nx 、h ny 、h nz Is the geomagnetic component under the navigation coordinate system; h is bx 、h by 、h bz The projection component of the geomagnetic vector under the carrier coordinate system is used; psi, theta and gamma are respectively a yaw angle, a pitch angle and a roll angle under the navigation coordinate system;
in a speed and navigation coordinate system, the speed information provided by the measuring equipment is utilized to obtain the trajectory inclination angle and trajectory deflection angle as shown in the following formula:
Figure FDA0003487698150000012
v is x 、v y 、v z North, east and ground speed, respectively;
in a low-rotation application scene, when the yaw angle and the pitch angle are known, the value of the roll angle can be uniquely determined by the formula (1):
Figure FDA0003487698150000013
the following steps:
m＝-sinψh nx +cosψh ny
n＝cosψsinθh nx +sinψsinθh ny +cosθh nz ；
in a high-rotation application scene, the roll rate of the carrier in the whole flight process is far greater than the pitch angle and the yaw rate, namely
Figure FDA0003487698150000014
Therefore, during the satellite data updating time, it can be assumed that only the roll angle of the carrier is changed, and equation (1) can be simplified as follows:
Figure FDA0003487698150000015
said A and α are defined as follows:
Figure FDA0003487698150000016
Figure FDA0003487698150000017
Figure FDA0003487698150000018
Figure FDA0003487698150000021
Figure FDA0003487698150000022
Figure FDA0003487698150000023
defining geomagnetic output under a navigation coordinate system as follows:
Figure FDA0003487698150000024
a is a magnetic measurement value and a geomagnetic intensity proportional coefficient; f is the measurement variation frequency; t is a magnetic measurement time series; phi is an initial offset angle;
the roll rate obtainable from equations (4) and (5) is:
Figure FDA0003487698150000025
before the satellite measurement device updates the velocity information, roll angle information may be obtained by integrating equation (6).
2. The method of claim 1, wherein the combined magnetic force/GNSS comprises: for the magnetometers which are vertically arranged in three sensitive axial directions, the acquired raw data is a space ellipsoid which is generally distributed in three axial directions parallel to a body axis;
the general equation for an ellipsoid can be expressed as:
a 1 x 2 +a 2 y 2 +a 3 z 2 +a 4 xy+a 5 xz+a 6 yz+a 7 x+a 8 y+a 9 z＝1 (7)
the standard equation is written in the form:
Figure FDA0003487698150000026
the points distributed on the ellipsoid can be expressed in the form of a matrix from a geometrical point of view as follows:
Figure FDA0003487698150000027
the above formula can be represented as:
[X-C]M[X-C] T ＝1+CMC T (10)
XMX T -2CMX T +CMC T ＝1+CMC T (11)
the X = [ X y z ]]Denotes a point on an ellipsoid, C = [ C = x c y c z ]Is a coordinate point of the center of the ellipsoid sphere,
Figure FDA0003487698150000031
according to the ellipsoid model, the following ellipsoid parameters:
coordinates of the center of sphere:
C＝0.5[a 7 a 8 a 9 ]M -1
length of x axis:
Figure FDA0003487698150000032
length of y-axis:
Figure FDA0003487698150000033
length of z-axis:
Figure FDA0003487698150000034
said SS = CMC T +1
If n sets of data are collected by the magnetometer, the data can be expressed in the following form by substituting the data into an ellipsoid equation:
Figure FDA0003487698150000035
order:
Figure FDA0003487698150000036
k＝[a 1 a 2 … a 9 ] T
the ellipsoid fitting is to determine a set of ellipsoid parameter values K with the minimum variance according to the measurement raw data and the equation. According to the minimum second-order multiplication algorithm, the ellipsoid parameter with the minimum measurement error is as follows:
k＝(H T H) -1 H T (13)
the ellipsoid parameters obtained by fitting can be used for calibrating the data measured by the magnetometer.
Firstly, calibrating an ellipsoid coordinate origin:
h bt ＝h b -C (14)
after the correction, the measurement data is distributed by an ellipsoid with the coordinate origin as the sphere center, so that zero point errors are eliminated, and meanwhile, spherical calibration is required to eliminate different axial sensitivities. In performing spherical calibration, a reference axis is first determined. In the magnetometer-satellite attitude determination scheme, the Y-axis is important, so the Y-axis can be selected as the reference for spherical correction, namely:
h b ＝y scale *h bt .*[x scale y scale z scale ] (15)。
3. the method of claim 1, wherein the combined magnetic force/GNSS comprises: according to the practical characteristics of the combined measurement attitude of the magnetometer and the satellite positioning, the state model generally has the following form:
Figure FDA0003487698150000041
x is a state quantity, a space position in the case of a satellite measurement system, a change equation of local geomagnetic intensity in each axial direction in the case of a magnetometer, and N is a change rate of each state,
Figure FDA0003487698150000042
for the various interference noise faced by each state,
the "current" statistical model is used herein to optimize the motion model. Based on the "current" statistical model, the following:
Figure FDA0003487698150000043
Figure FDA0003487698150000044
the above-mentioned
Figure FDA0003487698150000045
Is the "current" average of the rate change amount, and can be considered as a constant in the same sampling period of the sensor;
order:
Figure FDA0003487698150000046
then:
Figure FDA0003487698150000047
within the same sampling period, it can be considered that
Figure FDA0003487698150000048
Then there is
Figure FDA0003487698150000049
From formula (19):
Figure FDA00034876981500000410
bringing this formula and formula (18) into formula
Figure FDA00034876981500000411
The following can be obtained:
Figure FDA00034876981500000412
namely:
Figure FDA00034876981500000413
this is the equation of state, i.e., the "current" statistical model of the mobile vehicle, when equation (16) is expressed as:
Figure FDA0003487698150000051
the result of the estimation by the discrete system kalman filter algorithm minimizes the overall mean square error, namely:
Figure FDA0003487698150000052
the kalman filter result is an unbiased estimate, namely:
Figure FDA0003487698150000053
from equation (23), the system equation and the measurement equation are described here in the form:
X K ＝φ k，k-1 X k-1 +Γ k-1 W k-1 (24)
Z k ＝H k X k +V K (25)
the X is an n-dimensional state vector; z is m as a measurement vector; phi is an n multiplied by n dimensional system matrix; the gamma is an n multiplied by r dimension system noise matrix; h is mxn is a measurement matrix; w and V are r and m-dimensional mean white noise respectively, and X 0 W, V cross-correlation order: e { X } 0 }＝m x0 ，E{[X 0 -m x0 ][X 0 -m x0 ] T }＝P 0 ，E{W K W i T }＝Q k δ kj ，E{V K V i T }＝R k δ kj (ii) a The order is as follows: e { X } 0 }＝m x0 ，E{[X 0 -m x0 ][X 0 -m x0 ] T }＝P 0 ，E{W K W i T }＝Q k δ kj ，E{V K V i T }＝R k δ kj (ii) a The discrete system kalman filter equation is as follows:
Figure FDA0003487698150000054
Figure FDA0003487698150000055
Figure FDA0003487698150000056
Figure FDA0003487698150000057
P k ＝P k，k-1 -K k H k P k，k-1 。
CN202210087720.0A 2022-01-25 2022-01-25 Method for measuring attitude angle by magnetic force/GNSS combination Pending CN115235465A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210087720.0A CN115235465A (en) 2022-01-25 2022-01-25 Method for measuring attitude angle by magnetic force/GNSS combination

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210087720.0A CN115235465A (en) 2022-01-25 2022-01-25 Method for measuring attitude angle by magnetic force/GNSS combination

Publications (1)

Publication Number Publication Date
CN115235465A true CN115235465A (en) 2022-10-25

Family

ID=83667819

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210087720.0A Pending CN115235465A (en) 2022-01-25 2022-01-25 Method for measuring attitude angle by magnetic force/GNSS combination

Country Status (1)

Country Link
CN (1) CN115235465A (en)

Similar Documents

Publication Publication Date Title
CN107270893B (en) Lever arm and time asynchronous error estimation and compensation method for real estate measurement
US7119533B2 (en) Method, system and device for calibrating a magnetic field sensor
Gebre-Egziabher et al. A non-linear, two-step estimation algorithm for calibrating solid-state strapdown magnetometers
CN109443349A (en) A kind of posture Course Measure System and its fusion method, storage medium
US11408735B2 (en) Positioning system and positioning method
CN104698485A (en) BD, GPS and MEMS based integrated navigation system and method
CN110133692B (en) Inertial navigation technology-assisted high-precision GNSS dynamic inclination measurement system and method
CN112833917B (en) Three-axis magnetic sensor calibration method based on magnetic course angle and least square method
KR101211703B1 (en) Calibration method of the magnetometer error using a line of sight vector and the integrated navigation system using the same
CN111189474A (en) Autonomous calibration method of MARG sensor based on MEMS
CN109000639B (en) Attitude estimation method and device of multiplicative error quaternion geomagnetic tensor field auxiliary gyroscope
CN112461262A (en) Device and method for correcting errors of three-axis magnetometer
CN101122637A (en) SINS/GPS combined navigation self-adaptive reduced-dimensions filtering method for SAR movement compensation
CN111189442A (en) Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF
JPH095104A (en) Method and apparatus for measurement of three-dimensional attitude angle of moving body
CN116817896A (en) Gesture resolving method based on extended Kalman filtering
Zhang et al. A multi-position calibration algorithm for inertial measurement units
CN114459466A (en) MEMS multi-sensor data fusion processing method based on fuzzy control
CN112525144B (en) Nonlinear attitude detection compensation method and terminal
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
CN115235465A (en) Method for measuring attitude angle by magnetic force/GNSS combination
Hemanth et al. Calibration of 3-axis magnetometers
CN115523919A (en) Nine-axis attitude calculation method based on gyro drift optimization
CN112284388B (en) Unmanned aerial vehicle multisource information fusion navigation method
KR100674194B1 (en) Method of controlling magnetic sensor, control device therefor, and portable terminal

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