CN110095115B - Carrier attitude and heading measurement method based on geomagnetic information update - Google Patents

Carrier attitude and heading measurement method based on geomagnetic information update Download PDF

Info

Publication number
CN110095115B
CN110095115B CN201910480822.7A CN201910480822A CN110095115B CN 110095115 B CN110095115 B CN 110095115B CN 201910480822 A CN201910480822 A CN 201910480822A CN 110095115 B CN110095115 B CN 110095115B
Authority
CN
China
Prior art keywords
time
measurement
information
state vector
measuring
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
Application number
CN201910480822.7A
Other languages
Chinese (zh)
Other versions
CN110095115A (en
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.)
Hefei Institutes of Physical Science of CAS
Original Assignee
Hefei Institutes of Physical Science of CAS
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 Hefei Institutes of Physical Science of CAS filed Critical Hefei Institutes of Physical Science of CAS
Priority to CN201910480822.7A priority Critical patent/CN110095115B/en
Publication of CN110095115A publication Critical patent/CN110095115A/en
Application granted granted Critical
Publication of CN110095115B publication Critical patent/CN110095115B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/06—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving measuring of drift angle; involving correction for drift

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 invention relates to a carrier navigation attitude measurement method based on geomagnetic information updating, which comprises the following steps: initializing; calculating a magnetic declination angle; acquiring data at the next moment; measuring attitude information at the next time; updating magnetic declination information; the measurement method is continuously performed. The invention estimates and updates declination information in real time by using a discretized Kalman filtering algorithm based on the data measured by an accelerometer, a gyroscope and a magnetic sensor, and avoids the problems of course angle information distortion and unreliability caused by declination change due to the existence of ferromagnetic materials such as magnetic minerals, reinforcing steel bars and the like.

Description

Carrier attitude and heading measurement method based on geomagnetic information update
Technical Field
The invention relates to the technical field of carrier navigation attitude measurement, in particular to a carrier navigation attitude measurement method based on geomagnetic information updating.
Background
The attitude heading measurement method is used for measuring a heading angle psi, a pitch angle theta and a roll angle
Figure GDA0002100714430000011
The method of (1). According to different measurement principles of the heading angle psi, the current heading attitude measurement method mainly comprises the following steps: an astronomical measurement method, a satellite navigation measurement method, an inertia measurement method, and a geomagnetic measurement method.
The astronomical measurement system utilizes the measurement of celestial bodies to realize the output of attitude and heading information, and is greatly influenced by atmosphere and has poor environmental adaptability. The satellite navigation measurement method is based on a satellite navigation system positioning technology, the heading and the pitch angle are measured by using double antennas, or the heading and attitude are calculated by using three antennas, the accuracy of the satellite navigation measurement method can be reduced due to the influence of the errors of the installation positions of the antennas, the height difference and the like, the general accuracy is improved along with the lengthening of a base line, and the satellite navigation measurement method is not suitable for being used on small-size equipment; the satellite data output frequency is generally not more than 10Hz, the response speed is slow, and high-frequency or real-time attitude and heading information cannot be given; in addition, the satellite navigation measurement method has the following limitations due to the characteristics that the satellite navigation measurement method needs to receive navigation satellite signals: (1) the signal is weak and is easy to be interfered and deceived; (2) it is difficult to reach indoors, jungles, and underwater and ground due to severe attenuation; (3) navigation satellites are at risk of being attacked. The inertial measurement method relies on a high-precision inertial device, namely a method for sensing earth rotation through a high-precision sky gyroscope to realize course angle observation, has large volume and high cost, generally needs a large amount of peripheral circuits for integration, and the defects greatly limit the application of the inertial measurement method in more fields, particularly in civil fields with higher requirements on low cost, small volume, light weight and the like.
The geomagnetic measurement method is to obtain attitude information by fully utilizing a geomagnetic field which is everywhere on the earth and has directivity and combining an included angle between magnetic north and true north, namely a magnetic declination, by measuring the direction of the magnetic north. Compared with an astronomical measurement method, an inertial measurement method and a satellite navigation measurement method, the geomagnetic measurement method has the advantages of strong environmental adaptability, low cost, passivity, autonomy and the like. At present, geomagnetic information-based attitude and heading measurement methods mainly include three modes, namely geomagnetic matching, geomagnetic filtering and electronic compass. Both the geomagnetic matching and the geomagnetic filtering depend on a high-precision geomagnetic map stored in advance. However, the geomagnetic field is complex and changeable, the abnormal field is difficult to predict, and a high-precision geomagnetic map is often difficult to obtain; especially in the near-surface area, the influence of the abnormal field is more obvious, so that the use of the attitude and heading measurement modes of geomagnetic matching and geomagnetic filtering is limited.
The electronic compass does not need a high-precision geomagnetic chart in the navigation attitude measurement mode, and usually obtains the information of the heading angle psi by utilizing the characteristics that the declination is not greatly changed and approximately keeps unchanged in a certain area azimuth; meanwhile, an accelerometer is adopted to sense gravity field information to realize measurement of a pitch angle theta and a roll angle phi and realize attitude compensation in a course angle psi measurement process. For the interference of the acceleration of the carrier on the detection of gravitational field information in the motion process of the carrier, a Micro Electro Mechanical System (MEMS) gyroscope is usually introduced, and a filtering algorithm is adopted to realize the navigation attitude measurement in the dynamic process. However, changes in the external magnetic environment, such as the existence of magnetic minerals, ferromagnetic buildings containing steel bars, etc., can cause changes in the declination angle, and distort the course angle information obtained by the electronic compass.
How to realize the real-time calibration of the declination based on the real-time update of the geomagnetic information and accurately carry out the attitude and heading measurement method becomes a technical problem which needs to be solved urgently.
Disclosure of Invention
The invention aims to solve the problem of the prior art that the external magnetic environment has a magnetic declination defect, and provides a carrier navigation attitude measurement method based on geomagnetic information updating to solve the problem.
In order to achieve the purpose, the technical scheme of the invention is as follows:
a carrier navigation attitude measurement method based on geomagnetic information updating comprises the following steps:
initialization: acquiring the pitch angle theta of the current time t-kkRoll angle
Figure GDA0002100714430000021
And heading angle psikInformation;
calculation of declination: using the magnetic sensor measuring data with the current time t being k to calculate and measure the magnetic heading angle alpham,k(ii) a According to the course angle psi at the current momentkAnd measuring the magnetic heading angle alpham,kCalculating the declination angle DkThe calculation formula is as follows:
Dk＝ψk-αm,k；
data acquisition at the next moment: acquiring measurement data of an accelerometer, a gyroscope and a magnetic sensor at the next moment t ═ k + 1;
and (3) measuring attitude information at the next time: calculating the pitch angle theta of the next moment t-k +1 by using a discretization Kalman filtering methodk+1Roll angle
Figure GDA0002100714430000022
And heading angle psik+1And measuring the magnetic heading angle alpham,k+1；
Updating declination information: heading angle ψ according to the next time t ═ k +1k+1And measuring the magnetic heading angle alpham,k+1Updating the declination D when the next time t is k +1k+1；
Continuous execution of the measurement method: returning to the data acquisition step at the next time, acquiring the data information at the time t equal to k +2, and updating the declination at the time t equal to k + 2.
In the step of measuring the attitude information at the next moment, the discretization Kalman filtering method is a discretization extended Kalman filtering method; the measurement of the attitude information at the next moment comprises the following steps:
establishing a discretized state equation and a discretized measurement equation, wherein the expressions are as follows:
Figure GDA0002100714430000031
Figure GDA0002100714430000032
Zk+1＝h(Xk+1,k+1)+vk+1＝Xk+1+vk+1， (3)
Figure GDA0002100714430000033
Figure GDA0002100714430000034
Figure GDA0002100714430000035
ψm,k+1＝αm,k+1+Dk， (7)
Figure GDA0002100714430000036
wherein: state vector
Figure GDA0002100714430000037
Figure GDA0002100714430000038
The heading angle ψ of the carrier coordinate system representing the current time t ═ k relative to the local horizontal coordinate systemkAngle of pitch thetakAnd roll angle
Figure GDA0002100714430000039
k represents a discrete time point;
the function f characterizes the state vector x at the next instant k +1k+1State vector x with current time kkThe relationship between;
Figure GDA0002100714430000041
represents a time rate of change of the angle; dt is the time interval between t-k and t-k + 1;
Figure GDA0002100714430000042
and
Figure GDA0002100714430000043
respectively measuring data of the calibrated gyroscope in the x axis, the y axis and the z axis in the carrier coordinate system at the moment k;
the function h characterizes the state quantity x at the next time k +1k+1Measurement of z with quantityk+1The relationship between the two;
Figure GDA0002100714430000044
the heading angle ψ of the carrier coordinate system representing the next time k +1 with respect to the local horizontal coordinate systemk+1Angle of pitch thetak+1And roll angle
Figure GDA0002100714430000045
Measuring the quantity of the sample;
Figure GDA0002100714430000046
and
Figure GDA0002100714430000047
measuring data of the calibrated acceleration sensor in the x, y and z axes in a carrier coordinate system at the next moment k +1 respectively;
Figure GDA0002100714430000048
and
Figure GDA0002100714430000049
respectively measuring data of the magnetic sensor in the x, y and z axes after calibration in a carrier coordinate system at the next moment k + 1;
Figure GDA00021007144300000410
and
Figure GDA00021007144300000411
are respectively as
Figure GDA00021007144300000412
And
Figure GDA00021007144300000413
conversion to geographical coordinate system i three axes xl、ylAnd zlObtaining measurement data; w is akAnd vk+1The statistical characteristics of the system process noise and the measurement noise are as follows:
Figure GDA00021007144300000414
wherein k and j represent discrete time points, Q is a covariance matrix of system process noise, and betakjThe symbol is Kronecker, and R is a measurement noise covariance matrix;
setting ignore System Process noisewkAnd the measurement noise vk+1，
Prior estimates of the state vector and the metrology vector are obtained from equations (1) and (2)
Figure GDA00021007144300000415
And
Figure GDA00021007144300000416
it is represented as follows:
Figure GDA00021007144300000417
Figure GDA00021007144300000418
wherein,
Figure GDA00021007144300000419
is the posterior estimate of the state vector at time k;
the approximately linear equation of state and the measurement equation are obtained as follows:
Figure GDA00021007144300000420
Figure GDA00021007144300000421
wherein, Xk+1And Zk+1The real values of the state vector and the measurement vector at the next moment k +1 are respectively;
b is the Jacobian matrix of the function f with respect to the x-partial,
Figure GDA00021007144300000422
w is the Jacobian matrix of the function f with respect to the W partial derivatives,
Figure GDA0002100714430000051
h is the Jacobian matrix of function H with respect to x partial derivatives,
Figure GDA0002100714430000052
v is the Jacobian matrix of the function h with respect to the V partial derivatives,
Figure GDA0002100714430000053
constructing a group of complete discretization extended Kalman filtering method iteration processes, which are expressed as follows:
Figure GDA0002100714430000054
Figure GDA0002100714430000055
wherein,
Figure GDA0002100714430000056
a priori covariance matrix, P, of the state vector at time k +1kA covariance matrix of the state vector at time k;
measurement innovation at time k +1
Figure GDA0002100714430000057
Comprises the following steps:
Figure GDA0002100714430000058
combining equations (4) and (16), one can obtain,
Figure GDA0002100714430000059
the measurement updating equation of the discretization extended Kalman filtering method is as follows:
in order to ensure that the water-soluble organic acid,
Figure GDA00021007144300000510
wherein (gamma)1,γ2,γ3) Taking a positive number for a parameter related to the sensor characteristic;
Rk+1＝f(η1,η2,η3)R， (19)
wherein, f (η)1,η2,η3) Is (eta)1,η2,η3) A function of the correlation.
Figure GDA0002100714430000061
Figure GDA0002100714430000062
Figure GDA0002100714430000063
Wherein, KkA filter gain matrix for time k, PkA covariance matrix of the state vector at time k;
state vector X at time k +1k+1Approximately as follows:
Figure GDA0002100714430000064
in the step of measuring the attitude and heading information at the next moment, the discretization Kalman filtering method is a discretization unscented Kalman filtering method; the measurement of the attitude information at the next moment comprises the following steps:
constructing a Sigma scatter set:
Figure GDA0002100714430000065
wherein,
Figure GDA0002100714430000066
is the posterior estimate of the state vector at time k; pkA covariance matrix of the state vector at time k; n is the state vector dimension; λ ═ α2(n + k) -n is a free parameter for obtaining information of a given distribution high-order moment, so that n + k is 3, 10-4Alpha is less than or equal to 1, represents a matrix (n + lambda) Pk-1Row i or column i of the square root;
the time updating equation of the discretization unscented Kalman filtering method is set as follows:
Figure GDA0002100714430000067
Figure GDA0002100714430000068
Figure GDA0002100714430000069
wherein,
Figure GDA0002100714430000071
is the prior estimation value of the state vector at the moment k + 1;
Figure GDA0002100714430000072
a priori estimate of the covariance matrix of the state vector at time k + 1;
Figure GDA0002100714430000073
and
Figure GDA0002100714430000074
respectively UT changes to calculate weighted values used by prior estimated values of the state vector and the covariance matrix;
Figure GDA0002100714430000075
wherein β contains prior information of the state distribution;
combining equation Rk+1＝f(η1,η2,η3)R，f(η1,η2,η3) Is (eta)1,η2,η3) The measurement update equation of the related function and the discretization unscented Kalman filtering method is as follows:
Figure GDA0002100714430000076
Figure GDA0002100714430000077
Figure GDA0002100714430000078
Figure GDA0002100714430000079
Figure GDA00021007144300000710
Figure GDA00021007144300000711
Figure GDA00021007144300000712
obtaining a state vector X at the k +1 momentk+1The approximation of (d) is as follows:
Figure GDA00021007144300000713
the updating of the declination information comprises the following steps:
estimating course angle from the posterior at time k +1
Figure GDA00021007144300000714
And measuring the magnetic heading angle alpham,k+1Calculating the declination at time k +1
Figure GDA00021007144300000715
Updating declination information and storing declination D at time k +1k+1。
Advantageous effects
Compared with the prior art, the carrier attitude and heading measurement method based on geomagnetic information updating is based on the measurement data of an accelerometer, a gyroscope and a magnetic sensor, utilizes a discretized Kalman filtering algorithm to estimate and update the declination information in real time, and avoids the problems of course angle information distortion and unreliability caused by declination change due to the existence of ferromagnetic materials such as magnetic minerals, reinforcing steel bars and the like.
The invention does not depend on a pre-stored high-precision geomagnetic chart, but is based on a navigation attitude measurement method with real-time updating of geomagnetic information, so that the real-time calibration of the declination is realized, the distortion of the navigation attitude information of the external magnetic environment is compensated, and the purpose of improving the reliability and precision of the navigation attitude measurement is achieved.
Drawings
FIG. 1 is a sequence diagram of the method of the present invention;
fig. 2 is a schematic relationship diagram of a carrier coordinate system b, a local horizontal coordinate system l and a geographic coordinate system g.
Detailed Description
So that the manner in which the above recited features of the present invention can be understood and readily understood, a more particular description of the invention, briefly summarized above, may be had by reference to embodiments, some of which are illustrated in the appended drawings, wherein:
as shown in fig. 1, the method for measuring a carrier attitude and heading based on geomagnetic information update according to the present invention includes the following steps:
the first step, initialization: at the carrierIn a static state, acquiring a course angle psi of the current time t ═ k based on external inputkInformation; acquiring a pitch angle theta of the current time t-k based on the calibrated measurement data of the triaxial accelerometerkRoll angle
Figure GDA0002100714430000081
Information, the calculation formula is as follows:
Figure GDA0002100714430000082
Figure GDA0002100714430000083
wherein, thetam,kAnd
Figure GDA0002100714430000084
respectively measuring information of a pitch angle and a roll angle when the carrier is in a static state at the current time t ═ k;
Figure GDA0002100714430000085
and
Figure GDA0002100714430000086
projecting the measured data of the calibrated triaxial acceleration sensor to three axes x of a carrier coordinate system b when the current time t is equal to kb、ybAnd zbObtaining the measurement data.
And secondly, calculating the declination.
Using the pitch angle theta at the current time t-kkRoll angle
Figure GDA0002100714430000091
Information, as shown in fig. 2, a coordinate system b of a magnetic sensor measured data carrier on a carrier sensitive axis at the current time t ═ k is converted into a local horizontal coordinate system l, and the conversion formula is as follows:
Figure GDA0002100714430000092
wherein,
Figure GDA0002100714430000093
and
Figure GDA0002100714430000094
projecting the measured data of the three-axis magnetic sensor after calibration to three axes x of a carrier coordinate system b when the current time t is equal to kb、ybAnd zbObtaining measurement data;
Figure GDA0002100714430000095
and
Figure GDA0002100714430000096
are respectively as
Figure GDA0002100714430000097
And
Figure GDA0002100714430000098
conversion to a local horizontal coordinate system i three axes xl、ylAnd zlObtaining the measurement data.
Therefore, the measured magnetic heading angle alpha of the current time t-k can be calculatedm,kThe calculation formula is as follows:
Figure GDA0002100714430000099
according to the course angle psi of the current time t ═ kkAnd measuring the magnetic heading angle alpham,kAnd (3) information, resolving a declination at the current moment:
Dk＝ψk-αm,k。
thirdly, acquiring data at the next moment: and acquiring the measurement data of the accelerometer, the gyroscope and the magnetic sensor at the next time t + 1.
And fourthly, measuring the attitude and heading information at the next moment.
Calculating the pitch angle theta of the next moment t-k +1 by using a discretization Kalman filtering methodk+1Roll angle
Figure GDA00021007144300000910
And heading angle psik+1And measuring the magnetic heading angle alpham,k+1；
Establishing a state equation and a measurement equation of discretized Kalman filtering, wherein the expressions are as follows:
Figure GDA0002100714430000101
Figure GDA0002100714430000102
Zk+1＝h(Xk+1,k+1)+vk+1＝Xk+1+vk+1， (3)
Figure GDA0002100714430000103
Ψm,k+1＝αm,k+1+Dk，
wherein: state vector
Figure GDA0002100714430000104
The heading angle ψ of the carrier coordinate system representing the current time t ═ k relative to the local horizontal coordinate systemkAngle of pitch thetakAnd roll angle
Figure GDA0002100714430000105
k represents a discrete time point;
the function f characterizes the state vector x at the next instant k +1k+1State vector x with current time kkThe relationship between;
Figure GDA0002100714430000106
represents a time rate of change of the angle; dt is t ═ k and tTime interval between k + 1;
Figure GDA0002100714430000107
and
Figure GDA0002100714430000108
respectively measuring data of the calibrated gyroscope in the x, y and z axes in the carrier coordinate system at the moment k;
the function h characterizes the state quantity x at the next time k +1k+1Measurement of z with quantityk+1The relationship between the two;
Figure GDA0002100714430000109
the heading angle ψ of the carrier coordinate system representing the next time k +1 with respect to the local horizontal coordinate systemk+1Angle of pitch thetak+1And roll angle
Figure GDA00021007144300001010
Measuring the quantity of the sample; w is akAnd vk+1The statistical characteristics of the system process noise and the measurement noise are as follows:
Figure GDA00021007144300001011
wherein k and j represent discrete time points, Q is a covariance matrix of system process noise,kjis the Kronecker symbol and R is the measured noise covariance matrix.
As a first embodiment of the present invention, when the discretized kalman filter method employs the extended kalman filter,
set ignore System Process noise wkAnd the measurement noise vk+1，
Obtaining a priori estimates of state vectors and measurement vectors from state equations and measurement equations of discretized Kalman filtering
Figure GDA0002100714430000111
And
Figure GDA0002100714430000112
it is represented as follows:
Figure GDA0002100714430000113
Figure GDA0002100714430000114
wherein,
Figure GDA0002100714430000115
is the posterior estimate of the state vector at time k;
the approximately linear equation of state and the measurement equation are obtained as follows:
Figure GDA0002100714430000116
Figure GDA0002100714430000117
wherein, Xk+1And Zk+1The real values of the state vector and the measurement vector at the next moment k +1 are respectively;
b is the Jacobian matrix of the function f with respect to the x-partial,
Figure GDA0002100714430000118
w is the Jacobian matrix of the function f with respect to the W partial derivatives,
Figure GDA0002100714430000119
h is the Jacobian matrix of function H with respect to x partial derivatives,
Figure GDA00021007144300001110
v is the Jacobian matrix of the function h with respect to the V partial derivatives,
Figure GDA00021007144300001111
constructing a group of complete discretization extended Kalman filtering method iteration processes, which are expressed as follows:
Figure GDA00021007144300001112
Figure GDA00021007144300001113
wherein,
Figure GDA00021007144300001114
a priori covariance matrix, P, of the state vector at time k +1kA covariance matrix of the state vector at time k;
measurement innovation at time k +1
Figure GDA00021007144300001115
Comprises the following steps:
Figure GDA0002100714430000121
the measurement updating equation of the discretization extended Kalman filtering method is as follows:
in order to ensure that the water-soluble organic acid,
Figure GDA0002100714430000122
wherein (gamma)1,γ2,γ3) Taking a positive number for a parameter related to the sensor characteristic;
Rk+1＝f(η1,η2,η3)R，
wherein, f (η)1,η2,η3) Is (eta)1,η2,η3) A function of the correlation.
Figure GDA0002100714430000123
Figure GDA0002100714430000124
Figure GDA0002100714430000125
Wherein, KkA filter gain matrix for time k, PkA covariance matrix of the state vector at time k;
state vector X at time k +1k+1Approximately as follows:
Figure GDA0002100714430000126
as a second embodiment of the present invention, when the discretized kalman filter method employs unscented kalman filtering:
constructing a Sigma scatter set:
Figure GDA0002100714430000131
wherein,
Figure GDA0002100714430000132
is the posterior estimate of the state vector at time k; pkA covariance matrix of the state vector at time k; n is the state vector dimension; λ ═ α2(n + k) -n is a free parameter for obtaining information of a given distribution high-order moment, so that n + k is 3, 10-4Alpha is less than or equal to 1, represents a matrix (n + lambda) Pk-1Row i or column i of the square root;
the time updating equation of the discretization unscented Kalman filtering method is set as follows:
Figure GDA0002100714430000133
Figure GDA0002100714430000134
Figure GDA0002100714430000135
wherein,
Figure GDA0002100714430000136
is the prior estimation value of the state vector at the moment k + 1;
Figure GDA0002100714430000137
a priori estimate of the covariance matrix of the state vector at time k + 1;
Figure GDA0002100714430000138
and
Figure GDA0002100714430000139
respectively UT changes to calculate weighted values used by prior estimated values of the state vector and the covariance matrix;
Figure GDA00021007144300001310
wherein β contains prior information of the state distribution;
combining equation Rk+1＝f(η1,η2,η3)R，f(η1,η2,η3) Is (eta)1,η2,η3) The measurement update equation of the related function and the discretization unscented Kalman filtering method is as follows:
Figure GDA00021007144300001311
Figure GDA00021007144300001312
Figure GDA00021007144300001313
Figure GDA00021007144300001314
Figure GDA0002100714430000141
Figure GDA0002100714430000142
Figure GDA0002100714430000143
obtaining a state vector X at the k +1 momentk+1The approximation of (d) is as follows:
Figure GDA0002100714430000144
and fifthly, updating the declination information.
Heading angle ψ according to the next time t ═ k +1k+1And measuring the magnetic heading angle alpham,k+1Updating the declination D when the next time t is k +1k+1；
Dk+1＝ψk+1-αm,k+1。
Sixthly, continuously executing the measuring method: returning to the data acquisition step at the next time, acquiring the data information at the time t equal to k +2, and updating the declination at the time t equal to k + 2.
The foregoing shows and describes the general principles, essential features, and advantages of the invention. It will be understood by those skilled in the art that the present invention is not limited to the embodiments described above, which are merely illustrative of the principles of the invention, but that various changes and modifications may be made without departing from the spirit and scope of the invention, which fall within the scope of the invention as claimed. The scope of the invention is defined by the appended claims and equivalents thereof.

Claims (4)

1. A carrier navigation attitude measurement method based on geomagnetic information updating is characterized by comprising the following steps:
11) initialization: acquiring the pitch angle theta of the current time t-kkRoll angle
Figure FDA0002751201320000011
And heading angle psikInformation;
12) calculation of declination: using the magnetic sensor measuring data with the current time t being k to calculate and measure the magnetic heading angle alpham.k(ii) a According to the course angle psi at the current momentkAnd measuring the magnetic heading angle alpham,kCalculating the declination angle DkThe calculation formula is as follows:
Dk＝ψk-αm,k；
13) data acquisition at the next moment: acquiring measurement data of an accelerometer, a gyroscope and a magnetic sensor at the next moment t ═ k + 1;
14) and (3) measuring attitude information at the next time: calculating the pitch angle theta of the next moment t-k +1 by using a discretization Kalman filtering methodk+1Roll angle
Figure FDA0002751201320000012
And heading angle psik+1And measuring the magnetic heading angle alpham,k+1；
15) Updating declination information: heading angle ψ according to the next time t ═ k +1k+1And measuring the magnetic heading angle alpham,k+1Updating the declination D when the next time t is k +1k+1；
16) Continuous execution of the measurement method: returning to the data acquisition step at the next time, acquiring the data information at the time t equal to k +2, and updating the declination at the time t equal to k + 2.
2. The method for measuring the attitude of the carrier based on the update of the geomagnetic information according to claim 1, wherein in the step of measuring the attitude information at the next time, the discretized kalman filtering method is a discretized extended kalman filtering method; the measurement of the attitude information at the next moment comprises the following steps:
21) establishing a discretized state equation and a discretized measurement equation, wherein the expressions are as follows:
Figure FDA0002751201320000013
Figure FDA0002751201320000014
Zk+1＝h(Xk+1,k+1)+vk+1＝Xk+1+vk+1， (3)
Figure FDA0002751201320000021
Figure FDA0002751201320000022
Figure FDA0002751201320000023
ψm,k+1＝αm,k+1+Dk， (7)
Figure FDA0002751201320000024
wherein: state vector
Figure FDA0002751201320000025
Figure FDA0002751201320000026
The heading angle ψ of the carrier coordinate system representing the current time t ═ k relative to the local horizontal coordinate systemkAngle of pitch thetakAnd roll angle
Figure FDA0002751201320000027
k represents a discrete time point;
the function f characterizes the state vector x at the next instant k +1k+1State vector x with current time kkThe relationship between;
Figure FDA0002751201320000028
represents a time rate of change of the angle; dt is the time interval between t-k and t-k + 1;
Figure FDA0002751201320000029
and
Figure FDA00027512013200000210
respectively measuring data of the calibrated gyroscope in the x, y and z axes in the carrier coordinate system at the moment k;
the function h characterizes the state vector x at the next instant k +1k+1Measurement of z with quantityk+1The relationship between the two;
Figure FDA00027512013200000211
the heading angle ψ of the carrier coordinate system representing the next time k +1 with respect to the local horizontal coordinate systemk+1Angle of pitch thetak+1And roll angle
Figure FDA00027512013200000212
Measuring the quantity of the sample;
Figure FDA00027512013200000213
and
Figure FDA00027512013200000214
measuring data of the calibrated acceleration sensor in the x axis, the y axis and the z axis in a carrier coordinate system at the next moment k +1 respectively;
Figure FDA00027512013200000215
and
Figure FDA00027512013200000216
respectively measuring data of the magnetic sensor in the x, y and z axes after calibration in a carrier coordinate system at the next moment k + 1;
Figure FDA00027512013200000217
and
Figure FDA00027512013200000218
are respectively as
Figure FDA00027512013200000219
And
Figure FDA00027512013200000220
conversion to geographical coordinate system i three axes xl、ylAnd zlObtaining measurement data; w is akAnd vk+1The statistical characteristics of the system process noise and the measurement noise are as follows:
Figure FDA0002751201320000031
wherein k and j represent discrete time points, Q is a covariance matrix of system process noise,kjthe symbol is Kronecker, and R is a measurement noise covariance matrix;
22) set ignore System Process noise wkAnd the measurement noise vk+1，
Prior estimates of the state vector and the measurement vector are obtained from equations (1) - (4)
Figure FDA0002751201320000032
And
Figure FDA0002751201320000033
it is represented as follows:
Figure FDA0002751201320000034
Figure FDA0002751201320000035
wherein,
Figure FDA0002751201320000036
is the posterior estimate of the state vector at time k;
the approximately linear equation of state and the measurement equation are obtained as follows:
Figure FDA0002751201320000037
Figure FDA0002751201320000038
wherein, Xk+1And Zk+1The real values of the state vector and the measurement vector at the next moment k +1 are respectively;
b is the Jacobian matrix of the function f with respect to the x-partial,
Figure FDA0002751201320000039
w is the Jacobian matrix of the function f with respect to the W partial derivatives,
Figure FDA00027512013200000310
h is the Jacobian matrix of function H with respect to x partial derivatives,
Figure FDA00027512013200000311
v is the Jacobian matrix of the function h with respect to the V partial derivatives,
Figure FDA0002751201320000041
23) constructing a group of complete discretization extended Kalman filtering method iteration processes, which are expressed as follows:
Figure FDA0002751201320000042
Figure FDA0002751201320000043
wherein,
Figure FDA0002751201320000044
a priori covariance matrix, P, of the state vector at time k +1kA covariance matrix of the state vector at time k;
measurement innovation at time k +1
Figure FDA0002751201320000045
Comprises the following steps:
Figure FDA0002751201320000046
combining equations (4) and (16), one can obtain,
Figure FDA0002751201320000047
24) the measurement updating equation of the discretization extended Kalman filtering method is as follows:
in order to ensure that the water-soluble organic acid,
Figure FDA0002751201320000048
wherein (gamma)1,γ2,γ3) Taking a positive number for a parameter related to the sensor characteristic;
Rk+1＝f(η1,η2,η3)R， (19)
wherein, f (η)1,η2,η3) Is (eta)1,η2,η3) A function of the correlation;
Figure FDA0002751201320000051
Figure FDA0002751201320000052
Figure FDA0002751201320000053
wherein, KkA filter gain matrix for time k, PkA covariance matrix of the state vector at time k;
25) state vector X at time k +1k+1Approximately as follows:
Figure FDA0002751201320000054
3. the method for measuring the attitude of the carrier based on the update of the geomagnetic information according to claim 2, wherein in the step of measuring the attitude information at the next time, the discretized kalman filtering method is a discretized unscented kalman filtering method; the measurement of the attitude information at the next moment comprises the following steps:
31) constructing a Sigma scatter set:
Figure FDA0002751201320000055
wherein,
Figure FDA0002751201320000056
is the posterior estimate of the state vector at time k; pkA covariance matrix of the state vector at time k; n is the state vector dimension; λ ═ α2(n + k) -n is a free parameter for obtaining information of a given distribution high-order moment, so that n + k is 3, 10-4≤α≤1，
Figure FDA0002751201320000057
Representing the matrix (n + λ) Pk-1Row i or column i of the square root;
32) the time updating equation of the discretization unscented Kalman filtering method is set as follows:
Figure FDA0002751201320000058
Figure FDA0002751201320000059
Figure FDA0002751201320000061
wherein,
Figure FDA0002751201320000062
is the prior estimation value of the state vector at the moment k + 1;
Figure FDA00027512013200000612
a priori estimate of the covariance matrix of the state vector at time k + 1; wi (m)And Wi (c)Calculating weight values for prior estimated values of the state vector and the covariance matrix for the UT changes, respectively;
Figure FDA0002751201320000063
wherein β contains prior information of the state distribution;
33) combining equation Rk+1＝f(η1,η2,η3)R，f(η1,η2,η3) Is (eta)1,η2,η3) The measurement update equation of the related function and the discretization unscented Kalman filtering method is as follows:
Figure FDA0002751201320000064
Figure FDA0002751201320000065
Figure FDA0002751201320000066
Figure FDA0002751201320000067
Figure FDA0002751201320000068
Figure FDA0002751201320000069
Figure FDA00027512013200000610
34) obtaining a state vector X at the k +1 momentk+1The approximation of (d) is as follows:
Figure FDA00027512013200000611
4. the carrier attitude measurement method based on geomagnetic information update according to claim 1, wherein the update of the declination information comprises the following steps:
41) estimating course angle from the posterior at time k +1
Figure FDA0002751201320000071
And measuring the magnetic heading angle alpham,k+1Calculating the declination at time k +1
Figure FDA0002751201320000072
42) Updating declination information and storing declination D at time k +1k+1。
CN201910480822.7A 2019-06-04 2019-06-04 Carrier attitude and heading measurement method based on geomagnetic information update Active CN110095115B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910480822.7A CN110095115B (en) 2019-06-04 2019-06-04 Carrier attitude and heading measurement method based on geomagnetic information update

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910480822.7A CN110095115B (en) 2019-06-04 2019-06-04 Carrier attitude and heading measurement method based on geomagnetic information update

Publications (2)

Publication Number Publication Date
CN110095115A CN110095115A (en) 2019-08-06
CN110095115B true CN110095115B (en) 2020-12-25

Family

ID=67450164

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910480822.7A Active CN110095115B (en) 2019-06-04 2019-06-04 Carrier attitude and heading measurement method based on geomagnetic information update

Country Status (1)

Country Link
CN (1) CN110095115B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111895994A (en) * 2020-06-29 2020-11-06 西北工业大学 Geomagnetic bionic navigation method based on magnetic trend course strategy

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1908584A (en) * 2006-08-23 2007-02-07 北京航空航天大学 Method for determining initial status of strapdown inertial navigation system
CN108955671A (en) * 2018-04-25 2018-12-07 珠海全志科技股份有限公司 A kind of Kalman filtering air navigation aid based on magnetic declination, magnetic dip angle
CN109163721A (en) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 Attitude measurement method and terminal device
CN109443342A (en) * 2018-09-05 2019-03-08 中原工学院 NEW ADAPTIVE Kalman's UAV Attitude calculation method

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109405820A (en) * 2018-09-05 2019-03-01 中原工学院 Unmanned plane navigation attitude monitors system

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1908584A (en) * 2006-08-23 2007-02-07 北京航空航天大学 Method for determining initial status of strapdown inertial navigation system
CN108955671A (en) * 2018-04-25 2018-12-07 珠海全志科技股份有限公司 A kind of Kalman filtering air navigation aid based on magnetic declination, magnetic dip angle
CN109443342A (en) * 2018-09-05 2019-03-08 中原工学院 NEW ADAPTIVE Kalman's UAV Attitude calculation method
CN109163721A (en) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 Attitude measurement method and terminal device

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
EKF和UKF在惯导初始对准中的应用研究;陈焕清 等;《长江工程职业技术学院学报》;20090630;第26卷(第2期);第40-43页 *
基于无迹卡尔曼滤波的四旋翼无人飞行器姿态估计算法;朱岩 等;《测试技术学报》;20141231;第28卷(第3期);第194-198页 *

Also Published As

Publication number Publication date
CN110095115A (en) 2019-08-06

Similar Documents

Publication Publication Date Title
CN109556632B (en) INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering
CN104736963B (en) mapping system and method
CN107270893B (en) Lever arm and time asynchronous error estimation and compensation method for real estate measurement
CN109556631B (en) INS/GNSS/polarization/geomagnetic combined navigation system alignment method based on least squares
CN110398245B (en) Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit
US9417091B2 (en) System and method for determining and correcting field sensors errors
CN111947652A (en) Inertia/vision/astronomy/laser ranging combined navigation method suitable for lunar lander
CN110174123B (en) Real-time calibration method for magnetic sensor
CN112923924B (en) Method and system for monitoring posture and position of anchoring ship
CN110231029B (en) Underwater robot multi-sensor fusion data processing method
CN109000639B (en) Attitude estimation method and device of multiplicative error quaternion geomagnetic tensor field auxiliary gyroscope
CN111649747A (en) IMU-based adaptive EKF attitude measurement improvement method
CN111121820B (en) MEMS inertial sensor array fusion method based on Kalman filtering
Tomaszewski et al. Concept of AHRS algorithm designed for platform independent IMU attitude alignment
CN113551669B (en) Combined navigation positioning method and device based on short base line
CN110095115B (en) Carrier attitude and heading measurement method based on geomagnetic information update
CN112284388B (en) Unmanned aerial vehicle multisource information fusion navigation method
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
CN110160530B (en) Spacecraft attitude filtering method based on quaternion
CN117053802A (en) Method for reducing positioning error of vehicle navigation system based on rotary MEMS IMU
CN116380052A (en) Indoor positioning method based on UWB fusion IMU
CN115523919A (en) Nine-axis attitude calculation method based on gyro drift optimization
CN115950447A (en) High-precision alignment method and system for underwater movable base based on magnetic compass and velocimeter
Mathiassen et al. A low cost navigation unit for positioning of personnel after loss of GPS position
Tomaszewski et al. Analysis of the noise parameters and attitude alignment accuracy of INS conducted with the use of MEMS-based integrated navigation system

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
GR01 Patent grant
GR01 Patent grant