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 PDFInfo
- 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
Links
- 238000000691 measurement method Methods 0.000 title claims abstract description 33
- 230000005291 magnetic effect Effects 0.000 claims abstract description 37
- 238000001914 filtration Methods 0.000 claims abstract description 32
- 230000008859 change Effects 0.000 claims abstract description 5
- 239000013598 vector Substances 0.000 claims description 58
- 238000005259 measurement Methods 0.000 claims description 56
- 239000011159 matrix material Substances 0.000 claims description 42
- 238000000034 method Methods 0.000 claims description 38
- 230000026676 system process Effects 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 230000014509 gene expression Effects 0.000 claims description 3
- 150000007524 organic acids Chemical class 0.000 claims description 3
- 229910052500 inorganic mineral Inorganic materials 0.000 abstract description 3
- 239000011707 mineral Substances 0.000 abstract description 3
- 229910001294 Reinforcing steel Inorganic materials 0.000 abstract description 2
- 239000003302 ferromagnetic material Substances 0.000 abstract description 2
- 230000002159 abnormal effect Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000005358 geomagnetic field Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 229910000831 Steel Inorganic materials 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005294 ferromagnetic effect Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 239000010959 steel Substances 0.000 description 1
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
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 angleThe 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 angleAnd 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 angleAnd 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:
Zk+1=h(Xk+1,k+1)+vk+1=Xk+1+vk+1, (3)
ψm,k+1=αm,k+1+Dk, (7)
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 anglek 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;represents a time rate of change of the angle; dt is the time interval between t-k and t-k + 1;andrespectively 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;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 angleMeasuring the quantity of the sample;andmeasuring 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;andrespectively 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;andare respectively asAndconversion 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:
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)Andit is represented as follows:
the approximately linear equation of state and the measurement equation are obtained as follows:
wherein, Xk+1And Zk+1The real values of the state vector and the measurement vector at the next moment k +1 are respectively;
constructing a group of complete discretization extended Kalman filtering method iteration processes, which are expressed as follows:
wherein,a priori covariance matrix, P, of the state vector at time k +1kA covariance matrix of the state vector at time k;
combining equations (4) and (16), one can obtain,
the measurement updating equation of the discretization extended Kalman filtering method is as follows:
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.
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:
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:
wherein,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:
wherein,is the prior estimation value of the state vector at the moment k + 1;a priori estimate of the covariance matrix of the state vector at time k + 1;andrespectively UT changes to calculate weighted values used by prior estimated values of the state vector and the covariance matrix;
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:
obtaining a state vector X at the k +1 momentk+1The approximation of (d) is as follows:
the updating of the declination information comprises the following steps:
estimating course angle from the posterior at time k +1And measuring the magnetic heading angle alpham,k+1Calculating the declination at time k +1
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 angleInformation, the calculation formula is as follows:
wherein, thetam,kAndrespectively measuring information of a pitch angle and a roll angle when the carrier is in a static state at the current time t ═ k;andprojecting 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 angleInformation, 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:
wherein,andprojecting 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;andare respectively asAndconversion 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:
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 angleAnd 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:
Zk+1=h(Xk+1,k+1)+vk+1=Xk+1+vk+1, (3)
Ψm,k+1=αm,k+1+Dk,
wherein: state vectorThe heading angle ψ of the carrier coordinate system representing the current time t ═ k relative to the local horizontal coordinate systemkAngle of pitch thetakAnd roll anglek 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;represents a time rate of change of the angle; dt is t ═ k and tTime interval between k + 1;andrespectively 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;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 angleMeasuring the quantity of the sample; w is akAnd vk+1The statistical characteristics of the system process noise and the measurement noise are as follows:
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 filteringAndit is represented as follows:
the approximately linear equation of state and the measurement equation are obtained as follows:
wherein, Xk+1And Zk+1The real values of the state vector and the measurement vector at the next moment k +1 are respectively;
constructing a group of complete discretization extended Kalman filtering method iteration processes, which are expressed as follows:
wherein,a priori covariance matrix, P, of the state vector at time k +1kA covariance matrix of the state vector at time k;
the measurement updating equation of the discretization extended Kalman filtering method is as follows:
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.
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:
as a second embodiment of the present invention, when the discretized kalman filter method employs unscented kalman filtering:
constructing a Sigma scatter set:
wherein,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:
wherein,is the prior estimation value of the state vector at the moment k + 1;a priori estimate of the covariance matrix of the state vector at time k + 1;andrespectively UT changes to calculate weighted values used by prior estimated values of the state vector and the covariance matrix;
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:
obtaining a state vector X at the k +1 momentk+1The approximation of (d) is as follows:
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 angleAnd 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 angleAnd 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:
Zk+1=h(Xk+1,k+1)+vk+1=Xk+1+vk+1, (3)
ψm,k+1=αm,k+1+Dk, (7)
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 anglek 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;represents a time rate of change of the angle; dt is the time interval between t-k and t-k + 1;andrespectively 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;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 angleMeasuring the quantity of the sample;andmeasuring 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;andrespectively 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;andare respectively asAndconversion 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:
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)Andit is represented as follows:
the approximately linear equation of state and the measurement equation are obtained as follows:
wherein, Xk+1And Zk+1The real values of the state vector and the measurement vector at the next moment k +1 are respectively;
h is the Jacobian matrix of function H with respect to x partial derivatives,
v is the Jacobian matrix of the function h with respect to the V partial derivatives,
23) constructing a group of complete discretization extended Kalman filtering method iteration processes, which are expressed as follows:
wherein,a priori covariance matrix, P, of the state vector at time k +1kA covariance matrix of the state vector at time k;
combining equations (4) and (16), one can obtain,
24) the measurement updating equation of the discretization extended Kalman filtering method is as follows:
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;
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:
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:
wherein,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,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:
wherein,is the prior estimation value of the state vector at the moment k + 1;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;
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:
34) obtaining a state vector X at the k +1 momentk+1The approximation of (d) is as follows:
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 +1And measuring the magnetic heading angle alpham,k+1Calculating the declination at time k +1
42) Updating declination information and storing declination D at time k +1k+1。
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)
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)
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)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109405820A (en) * | 2018-09-05 | 2019-03-01 | 中原工学院 | Unmanned plane navigation attitude monitors system |
-
2019
- 2019-06-04 CN CN201910480822.7A patent/CN110095115B/en active Active
Patent Citations (4)
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)
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 |