CN105716610B - A kind of attitude of carrier and course calculating method and system of Geomagnetic Field Model auxiliary - Google Patents
A kind of attitude of carrier and course calculating method and system of Geomagnetic Field Model auxiliary Download PDFInfo
- Publication number
- CN105716610B CN105716610B CN201610059817.5A CN201610059817A CN105716610B CN 105716610 B CN105716610 B CN 105716610B CN 201610059817 A CN201610059817 A CN 201610059817A CN 105716610 B CN105716610 B CN 105716610B
- Authority
- CN
- China
- Prior art keywords
- value
- magnetic sensor
- state
- magnetic
- measurement
- 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.)
- Active
Links
- 230000005358 geomagnetic field Effects 0.000 title claims abstract description 41
- 238000000034 method Methods 0.000 title claims abstract description 32
- 239000011159 matrix material Substances 0.000 claims abstract description 151
- 230000003044 adaptive effect Effects 0.000 claims abstract description 20
- 238000005259 measurement Methods 0.000 claims description 177
- 239000013598 vector Substances 0.000 claims description 36
- 238000001914 filtration Methods 0.000 claims description 34
- 230000007613 environmental effect Effects 0.000 claims description 32
- 238000004364 calculation method Methods 0.000 claims description 24
- 230000005484 gravity Effects 0.000 claims description 16
- 238000010606 normalization Methods 0.000 claims description 15
- 230000009466 transformation Effects 0.000 claims description 12
- 230000008569 process Effects 0.000 claims description 8
- 230000001133 acceleration Effects 0.000 claims description 7
- 230000007704 transition Effects 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 4
- 238000006467 substitution reaction Methods 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000005389 magnetism Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The present invention discloses a kind of attitude of carrier and course calculating method and system of Geomagnetic Field Model auxiliary, and this approach includes the following steps：According to the location information and Geomagnetic Field Model of carrier, ideal magnetic dip angle is calculated；According to the output valve of the output valve of accelerometer and Magnetic Sensor, estimation magnetic dip angle is calculated；According to ideal magnetic dip angle and estimation magnetic dip angle, environment magnetic disturbance is judged whether；When there are environment magnetic disturbance, according to the adaptive weighting of Magnetic Sensor measuring value, the equivalent weight matrix of Magnetic Sensor measuring value is built；According to accelerometer measuring value, the equivalent weight matrix of Magnetic Sensor measuring value and Magnetic Sensor measuring value, attitude of carrier information and course information are calculated.The present invention detects environment magnetic disturbance according to ideal magnetic dip angle and estimation magnetic dip angle, and when detecting environment magnetic disturbance, build the equivalent weight matrix of Magnetic Sensor measuring value, and realize the adaptive adjustment that Magnetic Sensor measuring value contributes state estimation, make the posture information being calculated and course information that there are Robustness least squares.
Description
Technical Field
The invention relates to the technical field of communication, in particular to a method and a system for calculating attitude and heading of a carrier assisted by a geomagnetic field model.
Background
An Attitude and Heading Reference System (AHRS) is a measuring device capable of accurately measuring the three-axis Attitude of a carrier under a space coordinate System, and is composed of hardware such as a three-axis magnetometer, a three-axis gyroscope, a three-axis accelerometer and the like and corresponding software, provides real-time Attitude and Heading information for the carrier, and is widely applied to the fields of aerospace, robots, the automobile industry and pedestrian navigation positioning. With the development of micro-electromechanical technology, attitude and heading reference systems based on micro-electromechanical system sensors are more widely applied. However, due to the drift of the gyroscope, the errors are accumulated continuously when the attitude and heading information of the carrier is calculated. In order to solve the problem, information of an accelerometer and a magnetic sensor is introduced into an attitude and heading reference system, a gravity field and a geomagnetic field are used as reference vectors, and attitude and heading information of a carrier is calculated in real time by using output values of the accelerometer and the magnetometer and output values of a gyroscope.
Although attitude and heading reference systems can provide accurate attitude and heading information for a carrier in real time, the carrier moving in the near ground is often interfered by the external environment, and particularly, a magnetic sensor is obviously interfered by environmental magnetism. When external magnetic interference occurs, the output value of the magnetic sensor is used as a measurement value to estimate the attitude and the heading information of the carrier, so that errors exist, and even the attitude and the heading information of the carrier cannot be correctly calculated, and great risk is brought to the autonomous control of the carrier.
Disclosure of Invention
The invention provides a geomagnetic field model-assisted carrier attitude and heading calculation method and system, which aim to solve the technical problem of real-time calculation of carrier attitude and heading in the presence of environmental magnetic interference.
The invention provides a geomagnetic field model-assisted carrier attitude and heading calculation method, which comprises the following steps of:
calculating an ideal magnetic inclination angle according to the position information of the carrier and the geomagnetic field model; calculating and estimating a magnetic inclination angle according to the output value of the accelerometer and the output value of the magnetic sensor;
judging whether environmental magnetic interference exists according to the ideal magnetic inclination angle and the estimated magnetic inclination angle;
when environmental magnetic interference exists, constructing a self-adaptive weight of a measured value of a magnetic sensor according to a normalized residual error of the measured value of the magnetic sensor and a covariance matrix of the measured value, and constructing an equivalent weight matrix of the measured value of the magnetic sensor according to the self-adaptive weight of the measured value of the magnetic sensor;
and calculating the attitude information and the course information of the carrier according to the accelerometer measurement value, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value.
Optionally, calculating the carrier attitude information and the heading information according to the accelerometer measurement value, the magnetic sensor measurement value, and the equivalent weight matrix of the magnetic sensor measurement value, specifically including:
updating the state prediction value according to the accelerometer measurement value to obtain a state update value, and updating the estimation value of the attitude quaternion by using the state update value;
updating the state updating value according to the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value, and updating the estimation value of the attitude quaternion by using the state estimation value;
and calculating the attitude information and the course information of the carrier according to the estimated value of the attitude quaternion and a conversion matrix from a navigation coordinate system to a carrier coordinate system.
Optionally, the updating the state prediction value according to the accelerometer measurement value to obtain a state update value specifically includes:
updating the model according to the inertial sensor error model and the attitude error quaternion, and constructing a Kalman filtering state equation; constructing a first measurement equation according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector;
and updating the state prediction value according to the Kalman filtering state equation, the first measurement equation and the accelerometer measurement value to obtain a state update value.
Optionally, updating the state update value according to the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value, specifically including:
updating the model according to the inertial sensor error model and the attitude error quaternion, and constructing a Kalman filtering state equation; constructing a second measurement equation according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model;
and updating a state updating value according to the Kalman filtering state equation, the second measurement equation, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value.
Optionally, the state equation of the Kalman filter specifically includes:
wherein X (t) ═ qebωba]T∈R9×1Is the state quantity, T is the transpose, F (T) is the state transition matrix, W (T) is the process noise,for gyroscope output valuesOf an inverse matrix, vωIs the random drift noise of the gyroscope,is the noise of the random constant model of the gyroscope,for accelerometer stochastic constant model noise, qeVector portion being error quaternion, bωIs a random constant of the gyroscope, baIs the accelerometer bias.
Optionally, the first measurement equation specifically includes:
wherein L isaFor the difference between the output value of the accelerometer and the output value of the accelerometer estimated from the gravity field reference vector,is the output value of the accelerometer and is,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, gnIs the projection of the gravitational acceleration in a navigation coordinate system, vaIs the random error of the accelerometer.
Optionally, the state prediction value is updated according to the Kalman filtered state equation, the first measurement equation, and the accelerometer measurement value, so as to obtain a state update value, specifically:
updating the state predicted value by adopting the following formula to obtain a state updated value:
wherein,the value is updated for the state at time k,is a predicted value of the state at time k, La，kThe accelerometer measurement at time k, Aa，kIs the measurement matrix of the first measurement equation at time K, I is the unit matrix, Ka，kIn order to be a matrix of gains, the gain matrix,is composed ofOf covariance matrix, RaMeasuring a covariance matrix of the noise for the accelerometer;is time kThe covariance matrix of (2).
Optionally, updating the estimation value of the attitude quaternion by using the state update value, specifically:
updating the estimated value of the attitude quaternion by adopting the following formula:
wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, qeThe components of the update values for the states,updating the state with the value.
Optionally, the second measurement equation specifically includes:
wherein L ismAs a difference between the output value of the magnetic sensor and the output value of the magnetic sensor estimated from the world geomagnetic field model,is an output value of the magnetic sensor,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, mnAs a result of the normalization of the ideal geomagnetic field output value, vmRandom error of the magnetic sensor.
Optionally, updating a state update value according to the Kalman filtered state equation, the second measurement equation, the magnetic sensor measurement value, and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value, specifically:
updating the state update value by adopting the following formula to obtain a state estimation value:
wherein,is an estimate of the state at time k,updating the value of the state at time k, Lm，kThe magnetic sensor measurement value at time k,is composed ofCovariance matrix of, Km，kIs the gain matrix at time k, Am，kIs a measurement matrix for the time k,an equivalent weight matrix of the magnetic sensor measurement values at time k.
Optionally, updating the estimated value of the attitude quaternion by using the state estimated value, specifically:
updating the estimated value of the attitude quaternion by adopting the following formula:
wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, qeIs a component of the state estimate that is,is the state estimate.
Optionally, the estimated magnetic tilt angle is calculated according to the output value of the accelerometer and the output value of the magnetic sensor, specifically:
the estimated declination angle is calculated using the following formula:
Dcacul＝arccos(na·nm)
wherein D iscaculFor said estimated declination angle, naFor the direction vector of the output value of the accelerometer in the carrier system, nmThe vector is a direction vector of an output value of the magnetic sensor in the carrier system.
Optionally, judging whether there is environmental magnetic interference according to the ideal magnetic inclination angle and the estimated magnetic inclination angle, specifically:
judging whether the ideal magnetic dip angle and the estimated magnetic dip angle meet the following conditions:
|Dcacul-Dreference|＞λD
wherein D isreferenceIs the ideal magnetic inclination angle, DcaculFor said estimation of the magnetic tilt angle, λDIs a preset threshold value;
if the above conditions are met, determining that environmental magnetic interference exists; otherwise, it is determined that there is no ambient magnetic interference.
Optionally, constructing an adaptive weight of the magnetic sensor measurement value according to the normalized residual of the magnetic sensor measurement value and the measurement value covariance matrix, specifically:
constructing an adaptive weight of the magnetic sensor measurement value by adopting the following formula:
in the absence of ambient magnetic interference;
in the presence of ambient magnetic interference;
wherein,adaptive weighting, p, of the i-th measured value of the magnetic sensor for time kkiIs the ith main diagonal element of the covariance matrix of the measured values, c is a constant, Vi' is the normalized residual of the magnetic sensor measurements.
Optionally, before constructing the adaptive weight of the magnetic sensor measurement value according to the normalized residual of the magnetic sensor measurement value and the measurement value covariance matrix, the method further includes:
calculating a normalized residual of the magnetic sensor measurement values using the following formula:
wherein L ismiIs the ith measurement equation of the second measurement equation, AmiIs a measurement matrixRow i of (1); i sigmaiI is the covariance matrix R of the measured noise of the magnetic sensormThe square root of the ith main diagonal element.
The invention also provides an attitude and heading reference system, comprising:
the first calculation module is used for calculating an ideal magnetic inclination angle according to the position information of the carrier and the geomagnetic field model;
the second calculation module is used for calculating and estimating a magnetic inclination angle according to the output value of the accelerometer and the output value of the magnetic sensor;
the judging module is used for judging whether environmental magnetic interference exists according to the ideal magnetic inclination angle and the estimated magnetic inclination angle;
the first construction module is used for constructing the self-adaptive weight of the magnetic sensor measuring value according to the normalized residual error of the magnetic sensor measuring value and the measuring value covariance matrix when the judgment module judges that the environmental magnetic interference exists;
the second construction module is used for constructing an equivalent weight matrix of the magnetic sensor measurement values according to the self-adaptive weights of the magnetic sensor measurement values;
and the third calculation module is used for calculating the attitude information and the course information of the carrier according to the accelerometer measurement value, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value.
Optionally, the third computing module comprises:
the first updating submodule is used for updating the state predicted value according to the accelerometer measurement value to obtain a state updating value;
a second update submodule for updating the estimated value of the attitude quaternion using the state update value;
the third updating submodule is used for updating the state updating value according to the magnetic sensor measuring value and the equivalent weight matrix of the magnetic sensor measuring value to obtain a state estimation value;
a fourth updating submodule for updating the estimated value of the attitude quaternion using the state estimated value;
and the calculation submodule is used for calculating the attitude information and the course information of the carrier according to the estimation value of the attitude quaternion and a conversion matrix from a navigation coordinate system to a carrier coordinate system.
Optionally, the first update submodule is specifically configured to update a model according to an inertial sensor error model and an attitude error quaternion, and construct a Kalman filtering state equation; constructing a first measurement equation according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector; and updating the state prediction value according to the Kalman filtering state equation, the first measurement equation and the accelerometer measurement value to obtain a state update value.
Optionally, the third update submodule is specifically configured to update a model according to an inertial sensor error model and an attitude error quaternion, and construct a Kalman filtering state equation; constructing a second measurement equation according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model; and updating a state updating value according to the Kalman filtering state equation, the second measurement equation, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value.
Optionally, the state equation of the Kalman filter specifically includes:
wherein X (t) ═ qebωba]T∈R9×1Is the state quantity, T is the transpose, F (T) is the state transition matrix, W (T) is the process noise,for gyroscope output valuesOf an inverse matrix, vωIs the random drift noise of the gyroscope,is the noise of the random constant model of the gyroscope,for accelerometer stochastic constant model noise, qeVector portion being error quaternion, bωIs a random constant of the gyroscope, baIs the accelerometer bias.
Optionally, the first measurement equation specifically includes:
wherein L isaFor the difference between the output value of the accelerometer and the output value of the accelerometer estimated from the gravity field reference vector,is the output value of the accelerometer and is,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, gnIs the projection of the gravitational acceleration in a navigation coordinate system, vaIs the random error of the accelerometer.
Optionally, the first update submodule is specifically configured to update a model according to an inertial sensor error model and an attitude error quaternion, and construct a Kalman filtering state equation; according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector, a first measurement equation is constructed, and a state prediction value is updated by adopting the following formula to obtain a state update value:
wherein,the value is updated for the state at time k,is a predicted value of the state at time k, La，kThe accelerometer measurement at time k, Aa，kIs the measurement matrix of the first measurement equation at time K, I is the unit matrix, Ka，kIn order to be a matrix of gains, the gain matrix,is composed ofOf covariance matrix, RaMeasuring a covariance matrix of the noise for the accelerometer;is time kThe covariance matrix of (2).
Optionally, the second updating sub-module is specifically configured to update the estimation value of the attitude quaternion by using the following formula:
wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, qeThe components of the update values for the states,updating the state with the value.
Optionally, the second measurement equation specifically includes:
wherein L ismAs a difference between the output value of the magnetic sensor and the output value of the magnetic sensor estimated from the world geomagnetic field model,is an output value of the magnetic sensor,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, mnAs a result of the normalization of the ideal geomagnetic field output value, vmRandom error of the magnetic sensor.
Optionally, the third update submodule is specifically configured to update a model according to an inertial sensor error model and an attitude error quaternion, and construct a Kalman filtering state equation; according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model, a second measurement equation is constructed, and a state updating value is updated by adopting the following formula to obtain a state estimation value:
wherein,is an estimate of the state at time k,updating the value of the state at time k, Lm，kThe magnetic sensor measurement value at time k,is composed ofCovariance matrix of, Km，kIs the gain matrix at time k, Am，kIs a measurement matrix for the time k,an equivalent weight matrix of the magnetic sensor measurement values at time k.
Optionally, the fourth updating sub-module is specifically configured to update the estimation value of the attitude quaternion by using the following formula:
wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, qeIs a component of the state estimate that is,is the state estimate.
Optionally, the second calculating module is specifically configured to calculate the estimated magnetic tilt angle by using the following formula:
Dcacul＝arccos(na·nm)
wherein D iscaculFor said estimated declination angle, naFor the direction vector of the output value of the accelerometer in the carrier system, nmThe vector is a direction vector of an output value of the magnetic sensor in the carrier system.
Optionally, the determining module is specifically configured to determine whether the ideal magnetic tilt angle and the estimated magnetic tilt angle satisfy the following conditions:
|Dcacul-Dreference|＞λD
wherein D isreferenceIs the ideal magnetic inclination angle, DcaculFor said estimation of the magnetic tilt angle, λDIs a preset threshold value;
if the above conditions are met, determining that environmental magnetic interference exists; otherwise, it is determined that there is no ambient magnetic interference.
Optionally, the first constructing module is specifically configured to, when the determining module determines that the environmental magnetic interference exists, construct an adaptive weight of the magnetic sensor measurement value by using the following formula:
absence of environmentWhen magnetic interference occurs;
in the presence of ambient magnetic interference;
wherein,adaptive weighting, p, of the i-th measured value of the magnetic sensor for time kkiIs the ith main diagonal element of the covariance matrix of the measured values, c is a constant, Vi' is the normalized residual of the magnetic sensor measurements.
Optionally, the system further includes:
a fourth calculating module, configured to calculate a normalized residual error of the magnetic sensor measurement value by using the following formula:
wherein L ismiIs the ith measurement equation of the second measurement equation, AmiIs a measurement matrixRow i of (1); i sigmaiI is the covariance matrix R of the measured noise of the magnetic sensormThe square root of the ith main diagonal element.
The method detects the environmental magnetic interference according to the ideal magnetic inclination angle and the estimated magnetic inclination angle, constructs an equivalent weight matrix of the measured value of the magnetic sensor when the environmental magnetic interference is detected, and realizes the self-adaptive adjustment of the contribution of the measured value of the magnetic sensor to the state estimated value, so that the attitude information and the course information obtained by calculation have the tolerance, can adapt to the geomagnetic distortion environment, and solves the technical problem of real-time calculation of the attitude and the course of a carrier when the environmental magnetic interference exists.
Drawings
FIG. 1 is a flowchart of a geomagnetic field model-assisted carrier attitude and heading calculation method in an embodiment of the present invention;
FIG. 2 is a flowchart of another geomagnetic model-assisted method for calculating attitude and heading of a carrier in an embodiment of the present invention;
fig. 3 is a schematic structural diagram of an attitude and navigation reference system in an 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. 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 embodiment of the invention provides a geomagnetic field model-assisted carrier attitude and heading calculation method, which comprises the following steps of:
step 101, calculating an ideal magnetic inclination angle according to the position information of the carrier and the geomagnetic field model; and calculating the estimated magnetic tilt angle according to the output value of the accelerometer and the output value of the magnetic sensor.
And 102, judging whether environmental magnetic interference exists or not according to the ideal magnetic inclination angle and the estimated magnetic inclination angle.
And 103, when the environmental magnetic interference exists, constructing an adaptive weight of the magnetic sensor measuring value according to the normalized residual error and the measuring value covariance matrix of the magnetic sensor measuring value, and constructing an equivalent weight matrix of the magnetic sensor measuring value according to the adaptive weight of the magnetic sensor measuring value.
And step 104, calculating the attitude information and the course information of the carrier according to the accelerometer measurement value, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value.
The embodiment of the invention detects the environmental magnetic interference according to the ideal magnetic inclination angle and the estimated magnetic inclination angle, constructs the equivalent weight matrix of the measured value of the magnetic sensor when the environmental magnetic interference is detected, and realizes the self-adaptive adjustment of the contribution of the measured value of the magnetic sensor to the state estimated value, so that the attitude information and the course information obtained by calculation have the tolerance, can adapt to the geomagnetic distortion environment, and solves the technical problem of real-time calculation of the attitude and the course of a carrier when the environmental magnetic interference exists.
The embodiment of the invention also provides another geomagnetic field model-assisted carrier attitude and heading calculation method, as shown in FIG. 2, which comprises the following steps:
step 201, calculating the total magnetic field intensity, the magnetic dip angle and the magnetic declination angle of the ideal geomagnetic field according to the position information of the carrier and the geomagnetic field model, and taking the magnetic dip angle of the ideal geomagnetic field as the ideal magnetic dip angle.
Specifically, the geomagnetic field information at the position of the carrier can be calculated in real time according to a World Magnetic Model (WMM) published by the national geographic information system (NGA) and trajectory data of the carrier motion, wherein the data is manufactured by the united states geophysical information center (NGDC) and the british geological survey Bureau (BGS), and the geomagnetic field information includes the total Magnetic field strength, the declination angle and the declination angle.
In step 202, an estimated magnetic tilt angle is calculated based on the output value of the accelerometer and the output value of the magnetic sensor.
Specifically, the estimated declination angle may be calculated using the following formula:
Dcacul＝arccos(na·nm) (1)
wherein D iscaculTo estimate the magnetic tilt angle, naAnd nmAre respectively asThe accelerometer and the magnetic sensor output a directional vector of values in the carrier system.
And step 203, judging whether environmental magnetic interference exists according to the ideal magnetic inclination angle and the estimated magnetic inclination angle.
Specifically, it may be determined whether the ideal and estimated declination angles satisfy the following conditions:
|Dcacul-Dreference|＞λD(2)
wherein D isreferenceTo an ideal magnetic inclination angle, DcaculTo estimate the magnetic tilt angle, λDIs a preset threshold.
If the above conditions are met, determining that environmental magnetic interference exists; otherwise, it is determined that there is no ambient magnetic interference.
In this embodiment, λD＝0.55°。
And 204, updating the model according to the inertial sensor error model and the attitude error quaternion, and constructing a Kalman filtering state equation.
In particular, the vector portion q of the error quaternion can be takeneGyroscope random constant value bωAnd accelerometer bias baAs the state quantities, a state equation of Kalman filtering is constructed:
wherein X (t) ═ qebωba]T∈R9×1Is a state quantity; t is transposition; f (t) is a state transition matrix; w (t) is process noise;for gyroscope output valuesAn inverse matrix of (d); v. ofωRandom drift noise for the gyroscope;noise of a random constant model of the gyroscope;random constant model noise for the accelerometer;
and step 205, establishing a measurement equation according to the difference between the projection values of the earth gravity field reference vector and the earth magnetic field reference vector in the carrier coordinate system and the actual measurement values of the accelerometer and the magnetic sensor.
In particular, the difference L between the output value of the accelerometer and the estimated value of the output value of the accelerometer is usedaAnd a difference L between the magnetic sensor output value and the estimated value of the magnetic sensor output valuemRespectively constructing a first measurement equation and a second measurement equation:
wherein,is the output value of the accelerometer;is the output value of the magnetic sensor;is a transformation matrix from a navigation coordinate system n to a carrier coordinate system b;is an estimate of a quaternion; gn＝[0 0 -g]TIs the projection of the gravity acceleration g under the navigation coordinate system n; m isn＝[0 cosα sinα]Tis the normalized result of ideal geomagnetic field output value, α is the magnetic dip angle, vaAnd vmRandom errors of the accelerometer and magnetometer, respectively.
In this example, gn＝9.8m/s2。
And step 206, estimating quaternion errors and inertial sensor errors by adopting a Kalman filtering algorithm of two-step observation updating, and realizing the estimation of the attitude and the heading of the carrier through an attitude quaternion updating equation.
Specifically, according to equation (6), the observed value L at time k is useda，kUpdating the state prediction value at time k
Wherein,for measuring the amount of equation (6) at time kMeasuring a matrix; i is a unit array;for state prediction at time kAn updated value of (d); ka，kIs a gain matrix;for state prediction at time kThe covariance matrix of (a); raMeasuring a covariance matrix of the noise for the accelerometer;is in a state at time kThe covariance matrix of (a);
in this example, Ra＝(500μg)2I。
Using state quantitiesComponent (b) ofUpdating
Wherein, the symbolMultiplication is carried out for quaternion;is a quaternionNormalization of (1);assigning the calculated value on the right side of the equation to the variable on the left side of the equation for assigning the sign;
according to equation (7), the measured value L at time k is usedm，kUpdating time kObtaining a state estimate at time kCovariance matrix of sum state estimate
Wherein, Km，kA gain matrix for time k; l ism，kA measurement value calculated by the magnetic sensor for the time k; a. them，kA measurement matrix at the time k; an equivalent weight matrix of the measured value of the magnetic sensor at the moment k;
using the state quantities according to equations (11) and (12)Component (b) ofTo updateAnd useTo calculate attitude and heading information of the carrier.
And step 207, utilizing the equivalent weight matrix to online adjust the weight of the measurement value of the magnetic sensor according to the detection result of the environmental magnetic interference in the step 203, so that the calculated attitude and heading information of the carrier have tolerance.
Calculating the normalized residual error of the observed value of the magnetic sensor according to the measurement equation (7) as
Wherein L ismiThe ith measurement equation which is measurement equation (7); a. themiIs a measurement matrixRow i of (1); i sigmaiI is the covariance matrix R of the measured noise of the magnetic sensormThe square root of the ith main diagonal element;
in this example, Rm＝0.001I；
Constructing a measurement value weight factor of
In the formula,an adaptive weight being an i-th measured value of the magnetic sensor at time k; p is a radical ofkiFor the ith main diagonal element of a given metrology covariance matrix; c is a constant;
in this example, c is 1.3 to 2.0.
The embodiment of the invention utilizes the self-adaptive weight of the measured value of the magnetic sensor at the moment kConstructing an equivalence weight matrix of magnetic sensor measurement valuesAnd the self-adaptive adjustment of the contribution of the measured value of the magnetic sensor to the state estimated value is realized, so that the attitude and heading information calculation has the tolerance.
The invention has the advantages that: the method comprises the following steps that the process of calculating attitude and heading reference system information is realized by adopting a two-step filtering method, and in the first step, an accelerometer is used as a measuring sensor to estimate the error of a quaternion and update the quaternion; and secondly, estimating the error of the quaternion by taking the magnetic sensor as a measuring sensor, detecting the magnetic interference of the environment where the carrier is positioned in real time by using a magnetic interference detection algorithm, adaptively adjusting the weight of the observation data of the magnetic sensor in filtering estimation by the ratio of a measuring residual error to an actual residual error, updating the quaternion and calculating the attitude of the carrier and the heading reference system information. The tolerance and the reliability of the carrier attitude and the heading reference system information are improved.
Based on the above geomagnetic field model-assisted carrier attitude and heading calculation method, an embodiment of the present invention further provides an attitude and heading reference system, as shown in fig. 3, including:
the first calculation module 310 is configured to calculate an ideal magnetic dip angle according to the position information of the carrier and the geomagnetic field model;
a second calculating module 320, configured to calculate an estimated magnetic tilt angle according to the output value of the accelerometer and the output value of the magnetic sensor;
the judging module 330 is configured to judge whether there is environmental magnetic interference according to the ideal magnetic inclination angle and the estimated magnetic inclination angle;
a first constructing module 340, configured to construct an adaptive weight of the magnetic sensor measurement value according to the normalized residual of the magnetic sensor measurement value and the measurement value covariance matrix when the determining module 330 determines that the environmental magnetic interference exists;
a second constructing module 350, configured to construct an equivalent weight matrix of the magnetic sensor measurement values according to the adaptive weights of the magnetic sensor measurement values;
and the third calculating module 360 is configured to calculate the carrier attitude information and the heading information according to the accelerometer measurement value, the magnetic sensor measurement value, and the equivalent weight matrix of the magnetic sensor measurement value.
Wherein, the third calculating module 360 comprises:
the first updating submodule is used for updating the state predicted value according to the accelerometer measurement value to obtain a state updating value;
a second update submodule for updating the estimated value of the attitude quaternion using the state update value;
the third updating submodule is used for updating the state updating value according to the magnetic sensor measuring value and the equivalent weight matrix of the magnetic sensor measuring value to obtain a state estimation value;
a fourth updating submodule for updating the estimated value of the attitude quaternion using the state estimated value;
and the calculation submodule is used for calculating the attitude information and the course information of the carrier according to the estimation value of the attitude quaternion and a conversion matrix from a navigation coordinate system to a carrier coordinate system.
Specifically, the first update submodule is specifically configured to update a model according to an inertial sensor error model and an attitude error quaternion, and construct a Kalman filtering state equation; constructing a first measurement equation according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector; and updating the state prediction value according to the Kalman filtering state equation, the first measurement equation and the accelerometer measurement value to obtain a state update value.
The third updating submodule is specifically configured to update the model according to the inertial sensor error model and the attitude error quaternion, and construct a Kalman filtering state equation; constructing a second measurement equation according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model; and updating a state updating value according to the Kalman filtering state equation, the second measurement equation, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value.
The Kalman filtering state equation specifically comprises the following steps:
wherein X (t) ═ qebωba]T∈R9×1Is the state quantity, T is the transpose, F (T) is the state transition matrix, W (T) is the process noise,for gyroscope output valuesOf an inverse matrix, vωIs the random drift noise of the gyroscope,is the noise of the random constant model of the gyroscope,for accelerometer stochastic constant model noise, qeVector portion being error quaternion, bωIs a random constant of the gyroscope, baIs the accelerometer bias.
The first measurement equation specifically includes:
wherein L isaFor the difference between the output value of the accelerometer and the output value of the accelerometer estimated from the gravity field reference vector,is the output value of the accelerometer and is,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, gnIs the projection of the gravitational acceleration in a navigation coordinate system, vaIs the random error of the accelerometer.
Correspondingly, the first updating submodule is specifically configured to update a model according to an inertial sensor error model and an attitude error quaternion, and construct a state equation of Kalman filtering; according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector, a first measurement equation is constructed, and a state prediction value is updated by adopting the following formula to obtain a state update value:
wherein,the value is updated for the state at time k,is a predicted value of the state at time k, La，kThe accelerometer measurement at time k, Aa，kIs the measurement matrix of the first measurement equation at time K, I is the unit matrix, Ka，kIn order to be a matrix of gains, the gain matrix,is composed ofOf covariance matrix, RaMeasuring a covariance matrix of the noise for the accelerometer;is time kThe covariance matrix of (2).
The second updating submodule is specifically configured to update the estimation value of the attitude quaternion by using the following formula:
wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to giveValue sign, qeThe components of the update values for the states,updating the state with the value.
The second measurement equation specifically includes:
wherein L ismAs a difference between the output value of the magnetic sensor and the output value of the magnetic sensor estimated from the world geomagnetic field model,is an output value of the magnetic sensor,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, mnAs a result of the normalization of the ideal geomagnetic field output value, vmRandom error of the magnetic sensor.
Correspondingly, the third updating submodule is specifically configured to update the model according to the inertial sensor error model and the attitude error quaternion, and construct a state equation of Kalman filtering; according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model, a second measurement equation is constructed, and a state updating value is updated by adopting the following formula to obtain a state estimation value:
wherein,is an estimate of the state at time k,updating the value of the state at time k, Lm，kThe magnetic sensor measurement value at time k,is composed ofCovariance matrix of, Km，kIs the gain matrix at time k, Am，kIs a measurement matrix for the time k,an equivalent weight matrix of the magnetic sensor measurement values at time k.
The fourth updating submodule is specifically configured to update the estimation value of the attitude quaternion by using the following formula:
wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, qeIs a component of the state estimate that is,is the state estimate.
The second calculating module 320 is specifically configured to calculate the estimated magnetic tilt angle by using the following formula:
Dcacul＝arccos(na·nm)
wherein D iscaculFor said estimated declination angle, naFor the direction vector of the output value of the accelerometer in the carrier system, nmThe vector is a direction vector of an output value of the magnetic sensor in the carrier system.
The determining module 330 is specifically configured to determine whether the ideal magnetic tilt angle and the estimated magnetic tilt angle satisfy the following conditions:
|Dcacul-Dreference|＞λD
wherein D isreferenceIs the ideal magnetic inclination angle, DcaculFor said estimation of the magnetic tilt angle, λDIs a preset threshold value;
if the above conditions are met, determining that environmental magnetic interference exists; otherwise, it is determined that there is no ambient magnetic interference.
The first constructing module 340 is specifically configured to, when the determining module 330 determines that the environmental magnetic interference exists, construct the adaptive weight of the magnetic sensor measurement value by using the following formula:
in the absence of ambient magnetic interference;
in the presence of ambient magnetic interference;
wherein,adaptive weighting, p, of the i-th measured value of the magnetic sensor for time kkiIs the ith main diagonal element of the covariance matrix of the measured values, c is a constant, Vi' is the normalized residual of the magnetic sensor measurements.
Further, the above system further includes:
a fourth calculating module, configured to calculate a normalized residual error of the magnetic sensor measurement value by using the following formula:
wherein L ismiIs said secondMeasurement equation of the ith measurement equation, AmiIs a measurement matrixRow i of (1); i sigmaiI is the covariance matrix R of the measured noise of the magnetic sensormThe square root of the ith main diagonal element.
The invention has the advantages that: the method comprises the following steps that the process of calculating attitude and heading reference system information is realized by adopting a two-step filtering method, and in the first step, an accelerometer is used as a measuring sensor to estimate the error of a quaternion and update the quaternion; and secondly, estimating the error of the quaternion by taking the magnetic sensor as a measuring sensor, detecting the magnetic interference of the environment where the carrier is positioned in real time by using a magnetic interference detection algorithm, adaptively adjusting the weight of the observation data of the magnetic sensor in filtering estimation by the ratio of a measuring residual error to an actual residual error, updating the quaternion and calculating the attitude of the carrier and the heading reference system information. The tolerance and the reliability of the carrier attitude and the heading reference system information are improved.
In the several embodiments provided in the present application, it should be understood that the disclosed method and system may be implemented in other ways. The above-described device embodiments are merely illustrative, for example, the division of the unit is only a logical functional division, and there may be other division ways in actual implementation, for example: multiple units or components may be combined, or may be integrated into another system, or some features may be omitted, or not implemented. Furthermore, the coupling, direct coupling, or communication between the components shown or discussed may be through some interfaces, and the indirect coupling or communication between the devices or units may be electrical, mechanical, or other forms.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, that is, may be located in one place, or may be distributed on a plurality of network units; some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
In addition, all the functional units in the embodiments of the present invention may be integrated into one processing module, or each unit may be separately used as one unit, or two or more units may be integrated into one unit; the integrated unit can be realized in a form of hardware, or in a form of hardware plus a software functional unit.
Those of ordinary skill in the art will understand that: all or part of the steps for implementing the method embodiments may be implemented by hardware related to program instructions, and the program may be stored in a computer readable storage medium, and when executed, the program performs the steps including the method embodiments; and the aforementioned storage medium includes: various media capable of storing program codes, such as a removable Memory device, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk, and an optical disk.
The method is suitable for calculating the carrier attitude and the heading reference system information. The above description is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and all the changes or substitutions should be covered within the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the appended claims.
The above description is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and all the changes or substitutions should be covered within the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.
Claims (22)
1. A geomagnetic field model-assisted carrier attitude and heading calculation method is characterized by comprising the following steps of:
calculating an ideal magnetic inclination angle according to the position information of the carrier and the geomagnetic field model; calculating and estimating a magnetic inclination angle according to the output value of the accelerometer and the output value of the magnetic sensor;
judging whether environmental magnetic interference exists according to the ideal magnetic inclination angle and the estimated magnetic inclination angle;
when environmental magnetic interference exists, constructing a self-adaptive weight of a measured value of a magnetic sensor according to a normalized residual error of the measured value of the magnetic sensor and a covariance matrix of the measured value, and constructing an equivalent weight matrix of the measured value of the magnetic sensor according to the self-adaptive weight of the measured value of the magnetic sensor;
wherein the adaptive weight of the magnetic sensor measurement value is constructed by adopting the following formula:
in the absence of ambient magnetic interference;
in the presence of ambient magnetic interference;
wherein,adaptive weighting, p, of the i-th measured value of the magnetic sensor for time kkiIs the ith main diagonal element of the covariance matrix of the measured values, c is a constant, Vi,k' is the normalized residual of the magnetic sensor measurements;
calculating carrier attitude information and course information according to an accelerometer measurement value, a magnetic sensor measurement value and an equivalent weight matrix of the magnetic sensor measurement value, and specifically comprising the following steps:
(1) updating the state prediction value according to the accelerometer measurement value to obtain a state update value, and updating the estimation value of the attitude quaternion by using the state update value;
(2) updating the state updating value according to the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value, and updating the estimation value of the attitude quaternion by using the state estimation value;
(3) calculating the attitude information and the course information of the carrier according to the estimated value of the attitude quaternion and a conversion matrix from a navigation coordinate system to a carrier coordinate system;
wherein the step (2) further comprises: updating the model according to the inertial sensor error model and the attitude error quaternion, and constructing a Kalman filtering state equation; constructing a second measurement equation according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model;
updating a state updating value according to the Kalman filtering state equation, the second measurement equation, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value, which specifically comprises:
updating the state update value by adopting the following formula to obtain a state estimation value:
is an estimate of the state at time k,updating the value of the state at time k, Lm,kThe magnetic sensor measurement value at time k,is composed ofThe covariance matrix of (a) is determined,is time kT is the transpose,for the transformation matrix from the navigation coordinate system to the carrier coordinate system, Km,kIs a gain matrix at time k, I is a unit matrix, Am,kIs a measurement matrix for the time k,an equivalent weight matrix for the magnetic sensor measurements at time k,is an estimate of the attitude quaternion.
2. The method of claim 1, wherein updating the state prediction value according to the accelerometer measurement value to obtain a state update value comprises:
updating the model according to the inertial sensor error model and the attitude error quaternion, and constructing a Kalman filtering state equation; constructing a first measurement equation according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector;
and updating the state prediction value according to the Kalman filtering state equation, the first measurement equation and the accelerometer measurement value to obtain a state update value.
3. The method of claim 2, wherein the Kalman filtering state equation is specifically:
wherein X (t) ═ qebωba]T∈R9×1Is the state quantity, T is the transpose, F (T) is the state transition matrix, W (T) is the process noise,for gyroscope output valuesV is a reverse symmetric matrix ofωIs the random drift noise of the gyroscope,is the noise of the random constant model of the gyroscope,for accelerometer stochastic constant model noise, qeVector portion being error quaternion, bωIs a random constant of the gyroscope, baIs the accelerometer bias.
4. The method of claim 2, wherein the first measurement equation is specifically:
wherein L isaIs the accelerationA difference between the output value of the meter and the output value of the accelerometer estimated from the gravity field reference vector,is the output value of the accelerometer and is,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, gnIs the projection of the gravitational acceleration in a navigation coordinate system, vaIs the random error of the accelerometer.
5. The method of claim 4, wherein the state prediction value is updated according to the Kalman filtered state equation, the first measurement equation, and the accelerometer measurement value to obtain a state update value, which is specifically:
wherein,the value is updated for the state at time k,is a predicted value of the state at time k, La,kThe acceleration measurement at time kMeasured value, Aa,kIs the measurement matrix of the first measurement equation at time K, I is the unit matrix, Ka,kIs the gain matrix for the time instant k,is composed ofRa is the covariance matrix of the noise measured by the accelerometer;is time kThe covariance matrix of (2).
6. The method of claim 1, wherein the state update value is used to update the estimate of the attitude quaternion by:
updating the estimated value of the attitude quaternion by adopting the following formula:
wherein,is an estimate of the attitude quaternion,is a four-elementThe multiplication of the number is carried out,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, qeThe components of the update values for the states,updating the state with the value.
7. The method of claim 1, wherein the second measurement equation is specifically:
wherein L ismAs a difference between the output value of the magnetic sensor and the output value of the magnetic sensor estimated from the world geomagnetic field model,is an output value of the magnetic sensor,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, mnNormalized result of the calculated value for the ideal earth magnetic field, vmIs the random error of the magnetic sensor.
8. The method of claim 1, wherein the state estimate is used to update the estimate of the attitude quaternion by:
wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, qeIs a component of the state estimate that is,is the state estimate.
9. The method according to claim 1, wherein the estimated declination angle is calculated from the output of the accelerometer and the output of the magnetic sensor by:
the estimated declination angle is calculated using the following formula:
Dcacul＝arccos(na·nm)
wherein D iscaculFor said estimated declination angle, naIs the direction vector, n, of the accelerometer output value in the carrier coordinate systemmAnd outputting a direction vector of a value for the magnetic sensor in a carrier coordinate system.
10. The method according to claim 1, wherein determining whether there is an environmental magnetic interference based on the ideal and estimated declination angles comprises:
judging whether the ideal magnetic dip angle and the estimated magnetic dip angle meet the following conditions:
|Dcacul-Dreference|＞λD
wherein D isreferenceIs the ideal magnetic inclination angle, DcaculFor said estimation of the magnetic tilt angle, λDIs a preset threshold value;
if the above conditions are met, determining that environmental magnetic interference exists; otherwise, it is determined that there is no ambient magnetic interference.
11. The method of claim 7, wherein prior to constructing the adaptive weights for the magnetic sensor measurements from the normalized residuals of the magnetic sensor measurements and the measurement covariance matrix, further comprising:
calculating a normalized residual of the magnetic sensor measurement values using the following formula:
wherein L ismi,kThe ith measurement value of the second measurement equation at time k, Ami,kMeasuring matrix for k timeRow i of (1); i sigmaiI is the covariance matrix R of the measured noise of the magnetic sensormIth main diagonalSquare root of line element.
12. An attitude and heading reference system, comprising:
the first calculation module is used for calculating an ideal magnetic inclination angle according to the position information of the carrier and the geomagnetic field model;
the second calculation module is used for calculating and estimating a magnetic inclination angle according to the output value of the accelerometer and the output value of the magnetic sensor;
the judging module is used for judging whether environmental magnetic interference exists according to the ideal magnetic inclination angle and the estimated magnetic inclination angle;
the first construction module is used for constructing the self-adaptive weight of the magnetic sensor measuring value according to the normalized residual error of the magnetic sensor measuring value and the measuring value covariance matrix when the judgment module judges that the environmental magnetic interference exists;
the first constructing module is specifically configured to, when the determining module determines that the environmental magnetic interference exists, construct the adaptive weight of the measured value of the magnetic sensor by using the following formula:
in the absence of ambient magnetic interference;
in the presence of ambient magnetic interference;
wherein,adaptive weighting, p, of the i-th measured value of the magnetic sensor for time kkiIs the ith main diagonal element of the covariance matrix of the measured values, c is a constant, Vi,k' is the normalized residual of the magnetic sensor measurements;
the second construction module is used for constructing an equivalent weight matrix of the magnetic sensor measurement values according to the self-adaptive weights of the magnetic sensor measurement values;
the third calculation module is used for calculating the attitude information and the course information of the carrier according to the measurement value of the accelerometer, the measurement value of the magnetic sensor and the equivalent weight matrix of the measurement value of the magnetic sensor;
the third computing module comprising:
the first updating submodule is used for updating the state predicted value according to the accelerometer measurement value to obtain a state updating value;
a second update submodule for updating the estimated value of the attitude quaternion using the state update value;
the third updating submodule is used for updating the state updating value according to the magnetic sensor measuring value and the equivalent weight matrix of the magnetic sensor measuring value to obtain a state estimation value;
a fourth updating submodule for updating the estimated value of the attitude quaternion using the state estimated value;
the calculation submodule is used for calculating the attitude information and the course information of the carrier according to the estimation value of the attitude quaternion and a conversion matrix from a navigation coordinate system to a carrier coordinate system;
the third updating submodule is specifically used for updating the model according to the inertial sensor error model and the attitude error quaternion and constructing a Kalman filtering state equation; constructing a second measurement equation according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model;
updating a state updating value according to the Kalman filtering state equation, the second measurement equation, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value, which specifically comprises:
and updating the state updating value by adopting the following formula to obtain a state estimation value:
wherein,is an estimate of the state at time k,updating the value of the state at time k, Lm,kThe magnetic sensor measurement value at time k,is composed ofCovariance matrix of, Km,kIs the gain matrix at time k, Am,kIs a measurement matrix for the time k,an equivalent weight matrix of the magnetic sensor measurement values at time k.
13. The system of claim 12,
the first updating submodule is specifically used for updating a model according to an inertial sensor error model and an attitude error quaternion and constructing a Kalman filtering state equation; constructing a first measurement equation according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector; and updating the state prediction value according to the Kalman filtering state equation, the first measurement equation and the accelerometer measurement value to obtain a state update value.
14. The system of claim 13, wherein the Kalman filter state equation is specifically:
wherein X (t) ═ qebωba]T∈R9×1Is the state quantity, T is the transpose, F (T) is the state transition matrix, W (T) is the process noise,for gyroscope output valuesV is a reverse symmetric matrix ofωIs the random drift noise of the gyroscope,is the noise of the random constant model of the gyroscope,for accelerometer stochastic constant model noise, qeVector portion being error quaternion, bωIs a random constant of the gyroscope, baIs the accelerometer bias.
15. The system of claim 13, wherein the first measurement equation is specifically:
wherein L isaFor the difference between the output value of the accelerometer and the accelerometer output value estimated from the gravity field reference vector,is the output value of the accelerometer and is,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, gnIs the projection of the gravitational acceleration in a navigation coordinate system, vaIs the random error of the accelerometer.
16. The system of claim 15,
the first updating submodule is specifically used for updating a model according to an inertial sensor error model and an attitude error quaternion and constructing a Kalman filtering state equation; according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector, a first measurement equation is constructed, and a state prediction value is updated by adopting the following formula to obtain a state update value:
wherein,the value is updated for the state at time k,is a predicted value of the state at time k, La,kThe accelerometer measurement at time k, Aa,kIs the measurement matrix of the first measurement equation at time K, I is the unit matrix, Ka,kIn order to be a matrix of gains, the gain matrix,is composed ofOf covariance matrix, RaMeasuring a covariance matrix of the noise for the accelerometer;is time kThe covariance matrix of (2).
17. The system of claim 12,
the second updating submodule is specifically configured to update the estimation value of the attitude quaternion by using the following formula:
wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, qeThe components of the update values for the states,the value is updated for the state at time k.
18. The system of claim 12, wherein the second measurement equation is specifically:
wherein L ismAs a difference between the output value of the magnetic sensor and the output value of the magnetic sensor estimated from the world geomagnetic field model,is an output value of the magnetic sensor,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, mnAs a result of the normalization of the ideal geomagnetic field output value, vmIs the random error of the magnetic sensor.
19. The system of claim 13,
the fourth updating submodule is specifically configured to update the estimation value of the attitude quaternion by using the following formula:
wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, qeIs a component of the state estimate that is,is the state estimate.
20. The system of claim 12,
the second calculating module is specifically configured to calculate the estimated declination angle by using the following formula:
Dcacul＝arccos(na·nm)
wherein D iscaculFor said estimated declination angle, naIs the direction vector, n, of the accelerometer output value in the carrier coordinate systemmAnd outputting a direction vector of a value for the magnetic sensor in a carrier coordinate system.
21. The system of claim 12,
the judging module is specifically configured to judge whether the ideal magnetic dip angle and the estimated magnetic dip angle satisfy the following conditions:
|Dcacul-Dreference|＞λD
wherein D isreferenceIs the ideal magnetic inclination angle, DcaculFor said estimation of the magnetic tilt angle, λDIs a preset threshold.
22. The system of claim 18, further comprising:
a fourth calculating module, configured to calculate a normalized residual error of the magnetic sensor measurement value by using the following formula:
wherein L ismi,kThe ith metrology value of the second metrology equation at time k,Ami,kmeasuring matrix for k timeRow i of (1); i sigmaiI is the covariance matrix R of the measured noise of the magnetic sensormThe square root of the ith main diagonal element.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610059817.5A CN105716610B (en) | 2016-01-28 | 2016-01-28 | A kind of attitude of carrier and course calculating method and system of Geomagnetic Field Model auxiliary |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610059817.5A CN105716610B (en) | 2016-01-28 | 2016-01-28 | A kind of attitude of carrier and course calculating method and system of Geomagnetic Field Model auxiliary |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105716610A CN105716610A (en) | 2016-06-29 |
| CN105716610B true CN105716610B (en) | 2018-09-04 |
Family
ID=56154242
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610059817.5A Active CN105716610B (en) | 2016-01-28 | 2016-01-28 | A kind of attitude of carrier and course calculating method and system of Geomagnetic Field Model auxiliary |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105716610B (en) |
Families Citing this family (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105937911A (en) * | 2016-07-01 | 2016-09-14 | 南京理工大学 | Magnetic sensor attitude calculation method |
| CN108955669A (en) * | 2017-05-17 | 2018-12-07 | 田亮 | A kind of heavy magnetic field combination navigation algorithm |
| CN107421535B (en) * | 2017-05-22 | 2021-11-26 | 上海交通大学 | Indoor pedestrian positioning system based on magnetic field characteristics and acceleration information step counting |
| CN107860382B (en) * | 2017-11-07 | 2021-04-06 | 吉林大学 | Method for measuring attitude by applying AHRS under geomagnetic anomaly condition |
| CN108844536B (en) * | 2018-04-04 | 2020-07-03 | 中国科学院国家空间科学中心 | Geomagnetic navigation method based on measurement noise covariance matrix estimation |
| CN109115241A (en) * | 2018-08-07 | 2019-01-01 | 中国航空无线电电子研究所 | Vector data source integrity monitoring method |
| CN111649763B (en) * | 2020-04-26 | 2021-08-27 | 中国人民解放军61540部队 | Submarine navigation method and system established based on gravity beacon |
| CN112050804B (en) * | 2020-07-31 | 2022-04-08 | 东南大学 | Near-field magnetic map construction method based on geomagnetic gradient |
| CN112013836B (en) * | 2020-08-14 | 2022-02-08 | 北京航空航天大学 | Attitude heading reference system algorithm based on improved adaptive Kalman filtering |
| CN112066984B (en) * | 2020-09-17 | 2022-08-30 | 深圳维特智能科技有限公司 | Attitude angle resolving method and device, processing equipment and storage medium |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101290229A (en) * | 2008-06-13 | 2008-10-22 | 哈尔滨工程大学 | Silicon micro-navigation attitude system inertia/geomagnetism assembled method |
| CN103528587B (en) * | 2013-10-15 | 2016-09-28 | 西北工业大学 | Independent combined navigation system |
| CN103822633B (en) * | 2014-02-11 | 2016-12-07 | 哈尔滨工程大学 | A kind of low cost Attitude estimation method measuring renewal based on second order |
-
2016
- 2016-01-28 CN CN201610059817.5A patent/CN105716610B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN105716610A (en) | 2016-06-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105716610B (en) | A kind of attitude of carrier and course calculating method and system of Geomagnetic Field Model auxiliary | |
| CN105606096B (en) | A kind of posture of carrier movement status information auxiliary and course calculate method and system | |
| CN109163721B (en) | Attitude measurement method and terminal equipment | |
| Ludwig et al. | Comparison of Euler estimate using extended Kalman filter, Madgwick and Mahony on quadcopter flight data | |
| US9417091B2 (en) | System and method for determining and correcting field sensors errors | |
| US11592512B2 (en) | Method for calibrating a magnetometer | |
| CN102654404B (en) | Method for improving resolving precision and anti-jamming capability of attitude heading reference system | |
| CN110231029B (en) | Underwater robot multi-sensor fusion data processing method | |
| CN108731664B (en) | Robot state estimation method, device, computer equipment and storage medium | |
| CN111076722B (en) | Attitude estimation method and device based on self-adaptive quaternion | |
| US20140222369A1 (en) | Simplified method for estimating the orientation of an object, and attitude sensor implementing such a method | |
| Jao et al. | Pedestrian inertial navigation system augmented by vision-based foot-to-foot relative position measurements | |
| Sabet et al. | Experimental analysis of a low-cost dead reckoning navigation system for a land vehicle using a robust AHRS | |
| KR101359758B1 (en) | Method for estimating movement of a solid | |
| Sokolović et al. | INS/GPS navigation system based on MEMS technologies | |
| CN117289322B (en) | High-precision positioning algorithm based on IMU, GPS and UWB | |
| Wang et al. | Attitude determination method by fusing single antenna GPS and low cost MEMS sensors using intelligent Kalman filter algorithm | |
| CN104913789A (en) | Apparatus and method for background calibration | |
| CN117739972B (en) | Unmanned aerial vehicle approach stage positioning method without global satellite positioning system | |
| CN110375773B (en) | Attitude initialization method for MEMS inertial navigation system | |
| Munguia et al. | An attitude and heading reference system (AHRS) based in a dual filter | |
| KR102547926B1 (en) | Method and apparatus for specifying direction of travel determined from magnetic field measurements | |
| JP7241732B2 (en) | Determination of orientation from the magnetic field measured by the magnetic sensor | |
| CN114964214B (en) | Extended Kalman filtering attitude calculation method of attitude heading reference system | |
| CN115950447A (en) | High-precision alignment method and system for underwater movable base based on magnetic compass and velocimeter |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |