CN112797977A - Inertia/gravity matching combination method based on robust point group filtering - Google Patents

Inertia/gravity matching combination method based on robust point group filtering Download PDF

Info

Publication number
CN112797977A
CN112797977A CN202011544566.2A CN202011544566A CN112797977A CN 112797977 A CN112797977 A CN 112797977A CN 202011544566 A CN202011544566 A CN 202011544566A CN 112797977 A CN112797977 A CN 112797977A
Authority
CN
China
Prior art keywords
gravity
filtering
inertial navigation
error
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.)
Granted
Application number
CN202011544566.2A
Other languages
Chinese (zh)
Other versions
CN112797977B (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.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
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 National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202011544566.2A priority Critical patent/CN112797977B/en
Publication of CN112797977A publication Critical patent/CN112797977A/en
Application granted granted Critical
Publication of CN112797977B publication Critical patent/CN112797977B/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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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 invention belongs to the technical field of inertial navigation, relates to an inertia/gravity combination method, and discloses an inertia/gravity matching combination method based on robust point group filtering. The invention obtains the matched navigation information such as position and the like by processing the output results of the inertial navigation system and the gravity measurement system which are arranged on the carrier, and mainly comprises the following steps: firstly, establishing a two-stage filtering model; secondly, extracting a result of the gravity measurement system and a gravigram reading result of the inertial navigation indicating position; thirdly, substituting the result obtained in the last step into a first-stage filtering model for processing; and finally, substituting the result obtained in the last step into a secondary filtering model for calculation to realize the correction and filtering update of inertial navigation information. The method can obtain the matching result in real time under the condition of carrier mobility, and has the advantages of high calculation efficiency, high precision, comprehensive navigation information provision, strong robustness and the like.

Description

Inertia/gravity matching combination method based on robust point group filtering
The technical field is as follows:
the invention belongs to the technical field of inertial navigation, relates to an inertia/gravity combination method, and particularly relates to an inertia/gravity matching combination method based on robust point group filtering.
Background art:
the inertial navigation system has the advantages of high precision, all weather and complete autonomy, and is widely used for navigation of aviation, aerospace, ships and ground vehicles. In order to passively correct the error accumulation of the inertial navigation system, the abundant characteristic information of the earth gravity field can be fully utilized, a complete area is made in advance in a gravity reference map, the matching technology is utilized, the spatial variation characteristic of the gravity field is used as a reference, and the positioning information is acquired, so that the error accumulation of the inertial navigation system is passively corrected, the method is an effective means for improving the navigation precision, simultaneously the silence of the system is kept, and the safety of a carrier is ensured. The core of the matching technology is a matching method, which directly determines the final effect of matching under the existing hardware condition, and is therefore the focus of research. The common gravity matching method is developed from an inertia/terrain matching method, and the main development direction is a batch data correlation matching method and a filtering matching method.
The batch data correlation Matching method mainly comprises a Terrain Contour Matching method (TERCOM), an ICCP method, a gravity Matching method for obtaining an extreme value in a correlation manner and an auxiliary method based on Sandia. These methods all have respective merits: for example, the TERCOM method has high reliability and low requirement on the accuracy of an initial position, the ICCP method also has no high requirement on the matching accuracy of an initial point, and the carrier is allowed to make maneuvering navigation in the matching process. However, both methods need to store certain data, especially the TERCOM method, which results in too long matching time and incapability of real-time matching; the related extreme value method selects the matching points through the gravity matching constraint condition and eliminates a large amount of interference. However, when the position error is large, the iteration times are increased, so that the matching performance is influenced; the sandia-based method is verified by terrain matching and geomagnetic matching, has real-time performance, and can support the carrier to carry out maneuvering navigation in an area with obvious characteristics. However, this method is premised on that the reference on the course needs to be linearized and the initial error cannot be too large.
The above methods are all one-dimensional matching methods, and in addition to the above, there are two-dimensional matching methods, such as a phase correlation matching method, a matching method based on mutual information, and a matching method based on two-dimensional alignment. The methods have strong stability and reliability. However, the calculation is complex, the matching time is relatively long, and certain requirements are imposed on the navigation state of the carrier, so that the development of the method as an inertial gravity matching method is limited.
Besides the batch data correlation matching method, the one-dimensional method also comprises a filtering matching method. A common method is the extended kalman filter method. The principle is very direct, namely a difference value of a gravity abnormal value of a real position of a carrier and a gravity abnormal value of an inertial navigation indicating position is added into an inertial navigation equation to be used as a new observed quantity, and the new observed quantity is brought into the equation to carry out filtering estimation. Compared with the batch data correlation matching method, the filter matching method has higher matching precision and better matching effect, and can also provide estimation variance except position information, thereby providing richer information for the filter and improving the estimation effect.
The extended Kalman filter is used for linearizing a nonlinear system in principle, noise is assumed to be white noise with Gaussian distribution, but position information obtained by matching gravity information in practical application has high requirements on the accuracy of a background image and the stability of a gravity measurement system, so that mismatching is easy to occur, thick tail noise is generated, and certain challenge is brought to the anti-noise capability of a matching method. The matching method with good real-time performance, strong reliability and good robustness is designed, and the method has important significance for improving the precision and reliability of the inertia/gravity matching combined system and further realizing passive navigation.
The invention content is as follows:
the invention provides an inertia/gravity matching combination method based on robust point group filtering, aiming at the problems that the existing common inertia/gravity matching combination method is poor in real-time performance, low in precision and poor in robustness and other information except positions is difficult to provide. The main technical scheme is as follows:
the inertia/gravity matching combination method based on robust point group filtering comprises the following steps:
step S1: considering the horizontal movement of a water surface carrier, establishing an inertia/gravity matching two-stage filtering model, wherein the first-stage filtering focuses on position information, the second-stage filtering outputs complete information, and the two-stage filtering model is established according to the following modes:
constructing a primary filtering model:
denote the position of the system at time k as xkAnd is and
Figure RE-GDA0002957285410000021
wherein
Figure RE-GDA0002957285410000022
Is latitude, λkRepresents longitude; the constructed state equation is as follows: x is the number ofk＝xk-1+uk+wkWherein x iskRepresenting the true position at time k, xk-1Denotes the position at time k-1, ukRepresenting relative movement between two moments, wkProcess white noise representing a Gaussian distribution with a variance Qk(ii) a Taking the measurement result of the gravity measurement system as the input of the first-stage filtering measurement, wherein the measurement equation is as follows:
Figure RE-GDA0002957285410000031
wherein z iskRepresenting gravity anomaly information, g (x), measured by a gravity measurement systemk) Represents the position xkAbnormal reading value of gravity, xkCarrier position including inertial navigation system estimation
Figure RE-GDA0002957285410000032
And inertial navigation resolving error deltaxk；vkThe gravity chart reading error is measuring noise which is non-Gaussian distribution and has thick tail noise;
constructing a secondary filtering model:
selecting an inertial navigation system error equation under a geographic coordinate system as a secondary filtering state equation:
Figure RE-GDA0002957285410000033
wherein
Figure RE-GDA0002957285410000037
φNIs the error of the north attitude angle phiUIs the attitude angle error of the sky direction, phiEDelta V being an east attitude angle errorNFor north velocity error, δ VEIn the east direction of the speed error,
Figure RE-GDA0002957285410000038
the latitude error, the delta lambda longitude error, a matrix F (t) which is an inertial navigation state matrix equation, and W (t) which is system noise; the measurement equation for the second-order filtering is expressed as:
Figure RE-GDA0002957285410000034
wherein
Figure RE-GDA0002957285410000035
The state estimation result of the secondary filtering is obtained, and n is the estimation error of the secondary filtering;
step S2: navigating the carrier in an area in which the gravity map is prestored to obtain a measurement result of the gravity measurement system and an inertial navigation measurement result; carrying out data preprocessing, carrying out inertial navigation resolving on an inertial measurement unit, and carrying out data extraction and processing on a gravity measurement system; as shown in fig. 2, on one hand, the inertial navigation indicated position is used as a reference, and gravity anomaly data is extracted through a prestored gravity map; on the other hand, actually measured gravity anomaly data are obtained through a gravity measurement system;
step S3: substituting the matched data and inertial navigation data into a first-level point group for filtering for preprocessing; the method comprises the following steps of taking an inertial navigation calculation value as a state quantity, taking a gravity measurement result as an observed quantity, predicting and updating point group filtering, and matching through point group particles to obtain position estimation of a carrier, wherein the specific steps are as follows:
by ZkRepresents t1Time tkSet of observations of a time-of-day gravimetric measurement system, i.e. Zk＝{z1,z2,…,zk}; according to the Bayes estimation principle, the probability of estimating the system state is deduced and the inertial navigation resolving error delta x is obtainedkIs (d) is calculated by the probability distribution function P (δ x)k|Zk) Then, the mean value thereof
Figure RE-GDA0002957285410000036
And variance CkCalculated as follows:
Figure RE-GDA0002957285410000041
Figure RE-GDA0002957285410000042
using discrete grid point pairs for states deltax in point-group filteringk-1Is approximated, M grid points δ x are selectedk-1(i) 1,2, … M with a grid spacing δ, and assigning each grid point to a probability value P (δ x)k-1(i)|Zk-1) Correlation, and then converting the integral operation on the continuous probability space into summation on discrete grid points to realize P (delta x) of non-Gaussiank-1|Zk-1) Carrying out approximation;
after a state prediction and measurement update equation of the point group filtering is obtained respectively, the mean value and the variance of the carrier position considering the influence of the thick tail noise are obtained as follows:
Figure RE-GDA0002957285410000043
Figure RE-GDA0002957285410000044
step S4: estimating the position obtained in step S3
Figure RE-GDA0002957285410000045
Sum error estimate CkTaking the observation quantity as an observation quantity to be brought into secondary filtering, and updating by using robust Kalman filtering;
the step S4 includes the steps of:
step S41: for simplified models
Figure RE-GDA0002957285410000046
Obtaining a full source noise matrix
Figure RE-GDA0002957285410000047
Wherein
Figure RE-GDA0002957285410000048
Indicating position, measuring z for inertial navigationkIs the estimated value obtained in step S3, I is a unit matrix, HkTo measure the matrix, wkAnd vkRespectively state noise and measurement noise;
step S42: by pairs
Figure RE-GDA0002957285410000049
Cholesky decomposition to Bk(ii) a Make it
Figure RE-GDA00029572854100000410
Figure RE-GDA00029572854100000411
A cost function which is a criterion of maximum entropy, wherein d (i)kIs DkThe ith element of (1), w (i)kIs WkLine i of (1), DkIs L ═ n + m,
Figure RE-GDA00029572854100000412
Figure RE-GDA00029572854100000413
is x under the maximum entropy principlekOf (a) in which e (i)k＝d(i)k-w(i)kxkIs ekThe ith element of (1);
step S43: by making
Figure RE-GDA00029572854100000414
And
Figure RE-GDA00029572854100000415
equal and zero to obtain the optimal solution
Figure RE-GDA0002957285410000051
The above formula is taken as xkThe fixed point equation is obtained by iterative solution of the fixed point
Figure RE-GDA0002957285410000052
Wherein
Figure RE-GDA0002957285410000053
And C (x)k＝diag(Gσ(e(1)k),...,Gσ(e(n)k))，C(y)k＝diag(Gσ(e(n+1)k),...,Gσ(e(n+m)k))；
Step S44: converting by expanding and matrix inversion to obtain
Figure RE-GDA0002957285410000054
Wherein:
Figure RE-GDA0002957285410000055
wherein B (p)k|k-1By fitting covariance momentsArray Pk|k-1Cholesky decomposition to give B (r)kBy applying a noise matrix rkPerforming Cholesky decomposition to obtain;
substituting the inertial navigation information and the mean value and the variance obtained by the primary filtering into robust Kalman filtering to obtain the motion state estimation of the carrier; and carrying out omnibearing estimation on the position, the speed and the posture of the carrier according to the kinematic model of the carrier.
Compared with the prior art, the invention has the beneficial effects that:
the invention provides an inertia/gravity matching information fusion method, which is used for respectively carrying out inertia/gravity matching to obtain position estimation and secondary fusion filtering under the condition that a satellite rejects but has a prior gravity background image, updating an inertial navigation position and obtaining complete navigation information. Compared with the traditional method, the method has higher calculation precision and calculation efficiency and certain anti-noise capability.
Description of the drawings:
FIG. 1 is a schematic diagram of a first-level filtering according to the present invention;
FIG. 2 is a schematic diagram of the two-stage filtering according to the present invention;
FIG. 3 is a diagram of an inertia/gravity matching combination method based on robust point group filtering according to the present invention;
FIG. 4 is a flowchart of the inertia/gravity matching combination method based on robust point group filtering according to the present invention.
The specific implementation mode is as follows:
the technical solutions of the present invention will be described clearly and completely by the following embodiments, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all 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 inertia/gravity matching combination method based on robust point group filtering comprises the following steps:
step S1: considering the horizontal movement of a water surface carrier, establishing an inertia/gravity matching two-stage filtering model, wherein the first-stage filtering focuses on position information, the second-stage filtering outputs complete information, and the two-stage filtering model is established according to the following modes:
constructing a primary filtering model:
denote the position of the system at time k as xkAnd is and
Figure RE-GDA0002957285410000061
wherein
Figure RE-GDA0002957285410000062
Is latitude, λkRepresents longitude; the constructed state equation is as follows: x is the number ofk＝xk-1+uk+wkWherein x iskRepresenting the true position at time k, xk-1Denotes the position at time k-1, ukRepresenting relative movement between two moments, wkProcess white noise representing a Gaussian distribution with a variance Qk(ii) a Taking the measurement result of the gravity measurement system as the input of the first-stage filtering measurement, wherein the measurement equation is as follows:
Figure RE-GDA0002957285410000063
wherein z iskRepresenting gravity anomaly information, g (x), measured by a gravity measurement systemk) Represents the position xkAbnormal reading value of gravity, xkCarrier position including inertial navigation system estimation
Figure RE-GDA0002957285410000064
And inertial navigation resolving error deltaxk；vkThe gravity chart reading error is measuring noise which is non-Gaussian distribution and has thick tail noise;
constructing a secondary filtering model:
selecting an inertial navigation system error equation under a geographic coordinate system as a secondary filtering state equation:
Figure RE-GDA0002957285410000065
wherein
Figure RE-GDA0002957285410000066
φNIs the error of the north attitude angle phiUIs the attitude angle error of the sky direction, phiEDelta V being an east attitude angle errorNFor north velocity error, δ VEIn the east direction of the speed error,
Figure RE-GDA0002957285410000067
the latitude error, the delta lambda longitude error, a matrix F (t) which is an inertial navigation state matrix equation, and W (t) which is system noise; the measurement equation for the second-order filtering is expressed as:
Figure RE-GDA0002957285410000068
wherein
Figure RE-GDA0002957285410000069
The state estimation result of the secondary filtering is obtained, and n is the estimation error of the secondary filtering;
step S2: navigating the carrier in an area in which the gravity map is prestored to obtain a measurement result of the gravity measurement system and an inertial navigation measurement result; carrying out data preprocessing, carrying out inertial navigation resolving on an inertial measurement unit, and carrying out data extraction and processing on a gravity measurement system; as shown in fig. 2, on one hand, the inertial navigation indicated position is used as a reference, and gravity anomaly data is extracted through a prestored gravity map; on the other hand, actually measured gravity anomaly data are obtained through a gravity measurement system;
step S3: substituting the matched data and inertial navigation data into a first-level point group for filtering for preprocessing; the method comprises the following steps of taking an inertial navigation calculation value as a state quantity, taking a gravity measurement result as an observed quantity, predicting and updating point group filtering, and matching through point group particles to obtain position estimation of a carrier, wherein the specific steps are as follows:
by ZkRepresents t1Time tkSet of observations of a time-of-day gravimetric measurement system, i.e. Zk＝{z1,z2,…,zk}; according to the Bayes estimation principle, the probability of estimating the system state is deduced and the inertial navigation resolving error delta x is obtainedkIs (d) is calculated by the probability distribution function P (δ x)k|Zk) Then, the mean value thereof
Figure RE-GDA0002957285410000071
And variance CkCalculated as follows:
Figure RE-GDA0002957285410000072
Figure RE-GDA0002957285410000073
using discrete grid point pairs for states deltax in point-group filteringk-1Is approximated, M grid points δ x are selectedk-1(i) 1,2, … M with a grid spacing δ, and assigning each grid point to a probability value P (δ x)k-1(i)|Zk-1) Correlation, and then converting the integral operation on the continuous probability space into summation on discrete grid points to realize P (delta x) of non-Gaussiank-1|Zk-1) Carrying out approximation;
after a state prediction and measurement update equation of the point group filtering is obtained respectively, the mean value and the variance of the carrier position considering the influence of the thick tail noise are obtained as follows:
Figure RE-GDA0002957285410000074
Figure RE-GDA0002957285410000075
step S4: estimating the position obtained in step S3
Figure RE-GDA0002957285410000076
Sum error estimate CkTaking the observation quantity as an observation quantity to be brought into secondary filtering, and updating by using robust Kalman filtering;
the step S4 includes the steps of:
step S41: for simplified models
Figure RE-GDA0002957285410000081
Obtaining a full source noise matrix
Figure RE-GDA0002957285410000082
Wherein
Figure RE-GDA0002957285410000083
Indicating position, measuring z for inertial navigationkIs the estimated value obtained in step S3, I is a unit matrix, HkTo measure the matrix, wkAnd vkRespectively state noise and measurement noise;
step S42: by pairs
Figure RE-GDA0002957285410000084
Cholesky decomposition to Bk(ii) a Make it
Figure RE-GDA0002957285410000085
Figure RE-GDA0002957285410000086
A cost function which is a criterion of maximum entropy, wherein d (i)kIs DkThe ith element of (1), w (i)kIs WkLine i of (1), DkIs L ═ n + m,
Figure RE-GDA0002957285410000087
Figure RE-GDA0002957285410000088
is x under the maximum entropy principlekOf (a) in which e (i)k＝d(i)k-w(i)kxkIs ekThe ith element of (1);
step S43: by making
Figure RE-GDA0002957285410000089
Equal and zero to obtain the optimal solution
Figure RE-GDA00029572854100000810
The above formula is taken as xkThe fixed point equation is obtained by iterative solution of the fixed point
Figure RE-GDA00029572854100000811
Wherein
Figure RE-GDA00029572854100000812
And C (x)k＝diag(Gσ(e(1)k),...,Gσ(e(n)k))，C(y)k＝diag(Gσ(e(n+1)k),...,Gσ(e(n+m)k))；
Step S44: converting by expanding and matrix inversion to obtain
Figure RE-GDA00029572854100000813
Wherein:
Figure RE-GDA00029572854100000814
wherein B (p)k|k-1By aligning the covariance matrix Pk|k-1Cholesky decomposition to give B (r)kBy applying a noise matrix rkPerforming Cholesky decomposition to obtain;
substituting the inertial navigation information and the mean value and the variance obtained by the primary filtering into robust Kalman filtering to obtain the motion state estimation of the carrier; and carrying out omnibearing estimation on the position, the speed and the posture of the carrier according to the kinematic model of the carrier.
The above description is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above embodiments, and all technical solutions belonging to the idea of the present invention belong to the protection scope of the present invention. It should be noted that modifications and embellishments within the scope of the invention may occur to those skilled in the art without departing from the principle of the invention, and are considered to be within the scope of the invention.

Claims (1)

1. The inertia/gravity matching combination method based on robust point group filtering is characterized by comprising the following steps:
step S1: considering the horizontal movement of a water surface carrier, establishing an inertia/gravity matching two-stage filtering model, wherein the first-stage filtering focuses on position information, the second-stage filtering outputs complete information, and the two-stage filtering model is established according to the following modes:
constructing a primary filtering model:
denote the position of the system at time k as xkAnd is and
Figure RE-FDA0002957285400000011
wherein
Figure RE-FDA0002957285400000012
Is latitude, λkRepresents longitude; the constructed state equation is as follows: x is the number ofk＝xk-1+uk+wkWherein x iskRepresenting the true position at time k, xk-1Denotes the position at time k-1, ukRepresenting relative movement between two moments, wkProcess white noise representing a Gaussian distribution with a variance Qk(ii) a Taking the measurement result of the gravity measurement system as the input of the first-stage filtering measurement, wherein the measurement equation is as follows:
Figure RE-FDA0002957285400000013
wherein z iskRepresenting gravity anomaly information, g (x), measured by a gravity measurement systemk) Represents the position xkAbnormal reading value of gravity, xkCarrier position including inertial navigation system estimation
Figure RE-FDA0002957285400000014
And inertial navigation resolving error deltaxk；vkThe gravity chart reading error is measuring noise which is non-Gaussian distribution and has thick tail noise;
constructing a secondary filtering model:
selecting inertial navigation system under geographic coordinate systemTaking a systematic error equation as a two-stage filtering state equation:
Figure RE-FDA0002957285400000015
wherein
Figure RE-FDA0002957285400000016
φNIs the error of the north attitude angle phiUIs the attitude angle error of the sky direction, phiEDelta V being an east attitude angle errorNFor north velocity error, δ VEIn the east direction of the speed error,
Figure RE-FDA0002957285400000017
the latitude error, the delta lambda longitude error, a matrix F (t) which is an inertial navigation state matrix equation, and W (t) which is system noise; the measurement equation for the second-order filtering is expressed as:
Figure RE-FDA0002957285400000018
wherein
Figure RE-FDA0002957285400000019
The state estimation result of the secondary filtering is obtained, and n is the estimation error of the secondary filtering;
step S2: navigating the carrier in an area in which the gravity map is prestored to obtain a measurement result of the gravity measurement system and an inertial navigation measurement result; carrying out data preprocessing, carrying out inertial navigation resolving on an inertial measurement unit, and carrying out data extraction and processing on a gravity measurement system; as shown in fig. 2, on one hand, the inertial navigation indicated position is used as a reference, and gravity anomaly data is extracted through a prestored gravity map; on the other hand, actually measured gravity anomaly data are obtained through a gravity measurement system;
step S3: substituting the matched data and inertial navigation data into a first-level point group for filtering for preprocessing; the method comprises the following steps of taking an inertial navigation calculation value as a state quantity, taking a gravity measurement result as an observed quantity, predicting and updating point group filtering, and matching through point group particles to obtain position estimation of a carrier, wherein the specific steps are as follows:
by ZkRepresents t1Time tkSet of observations of a time-of-day gravimetric measurement system, i.e. Zk＝{z1,z2,…,zk}; according to the Bayes estimation principle, the probability of estimating the system state is deduced and the inertial navigation resolving error delta x is obtainedkIs (d) is calculated by the probability distribution function P (δ x)k|Zk) Then, the mean value thereof
Figure RE-FDA0002957285400000021
And variance CkCalculated as follows:
Figure RE-FDA0002957285400000022
Figure RE-FDA0002957285400000023
using discrete grid point pairs for states deltax in point-group filteringk-1Is approximated, M grid points δ x are selectedk-1(i) 1,2, … M with a grid spacing δ, and assigning each grid point to a probability value P (δ x)k-1(i)|Zk-1) Correlation, and then converting the integral operation on the continuous probability space into summation on discrete grid points to realize P (delta x) of non-Gaussiank-1|Zk-1) Carrying out approximation;
after a state prediction and measurement update equation of the point group filtering is obtained respectively, the mean value and the variance of the carrier position considering the influence of the thick tail noise are obtained as follows:
Figure RE-FDA0002957285400000024
Figure RE-FDA0002957285400000025
step S4: will be obtained in step S3Location estimation of arrival
Figure RE-FDA0002957285400000026
Sum error estimate CkTaking the observation quantity as an observation quantity to be brought into secondary filtering, and updating by using robust Kalman filtering;
the step S4 includes the steps of:
step S41: for simplified models
Figure RE-FDA0002957285400000027
Obtaining a full source noise matrix
Figure RE-FDA0002957285400000028
Wherein
Figure RE-FDA0002957285400000029
Indicating position, measuring z for inertial navigationkIs the estimated value obtained in step S3, I is a unit matrix, HkTo measure the matrix, wkAnd vkRespectively state noise and measurement noise;
step S42: by pairs
Figure RE-FDA00029572854000000210
Cholesky decomposition to Bk(ii) a Make it
Figure RE-FDA00029572854000000211
Figure RE-FDA0002957285400000031
A cost function which is a criterion of maximum entropy, wherein d (i)kIs DkThe ith element of (1), w (i)kIs WkLine i of (1), DkIs L ═ n + m,
Figure RE-FDA0002957285400000032
Figure RE-FDA0002957285400000033
is x under the maximum entropy principlekOf (a) in which e (i)k＝d(i)k-w(i)kxkIs ekThe ith element of (1);
step S43: by making
Figure RE-FDA0002957285400000034
And
Figure RE-FDA0002957285400000035
equal and zero to obtain the optimal solution
Figure RE-FDA0002957285400000036
The above formula is taken as xkThe fixed point equation is obtained by iterative solution of the fixed point
Figure RE-FDA0002957285400000037
Wherein
Figure RE-FDA0002957285400000038
And C (x)k＝diag(Gσ(e(1)k),...,Gσ(e(n)k))，C(y)k＝diag(Gσ(e(n+1)k),...,Gσ(e(n+m)k))；
Step S44: converting by expanding and matrix inversion to obtain
Figure RE-FDA0002957285400000039
Wherein:
Figure RE-FDA00029572854000000310
wherein B (p)k|k-1By aligning the covariance matrix Pk|k-1Cholesky decomposition to obtain，B(r)kBy applying a noise matrix rkPerforming Cholesky decomposition to obtain;
substituting the inertial navigation information and the mean value and the variance obtained by the primary filtering into robust Kalman filtering to obtain the motion state estimation of the carrier; and carrying out omnibearing estimation on the position, the speed and the posture of the carrier according to the kinematic model of the carrier.
CN202011544566.2A 2020-12-24 2020-12-24 Inertia/gravity matching combination method based on robust point group filtering Active CN112797977B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011544566.2A CN112797977B (en) 2020-12-24 2020-12-24 Inertia/gravity matching combination method based on robust point group filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011544566.2A CN112797977B (en) 2020-12-24 2020-12-24 Inertia/gravity matching combination method based on robust point group filtering

Publications (2)

Publication Number Publication Date
CN112797977A true CN112797977A (en) 2021-05-14
CN112797977B CN112797977B (en) 2021-10-29

Family

ID=75805369

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011544566.2A Active CN112797977B (en) 2020-12-24 2020-12-24 Inertia/gravity matching combination method based on robust point group filtering

Country Status (1)

Country Link
CN (1) CN112797977B (en)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1642089A1 (en) * 2003-07-03 2006-04-05 Northrop Grumman Corporation Method and system for improving accuracy of inertial navigation measurements using measured and stored gravity gradients
CN103604430A (en) * 2013-11-06 2014-02-26 哈尔滨工程大学 Marginalized cubature Kalman filter (CKF)-based gravity aided navigation method
CN103743395A (en) * 2014-01-17 2014-04-23 哈尔滨工程大学 Time delay compensation method in inertia gravity matching combined navigation system
CN105737850A (en) * 2016-01-29 2016-07-06 北京理工大学 Variable-scale unidirectional gravity sampling vector matching and positioning method based on particle filtering
CN108444479A (en) * 2018-03-15 2018-08-24 北京理工大学 Gravity Matching method based on ADAPTIVE ROBUST Unscented kalman filtering

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1642089A1 (en) * 2003-07-03 2006-04-05 Northrop Grumman Corporation Method and system for improving accuracy of inertial navigation measurements using measured and stored gravity gradients
CN103604430A (en) * 2013-11-06 2014-02-26 哈尔滨工程大学 Marginalized cubature Kalman filter (CKF)-based gravity aided navigation method
CN103743395A (en) * 2014-01-17 2014-04-23 哈尔滨工程大学 Time delay compensation method in inertia gravity matching combined navigation system
CN105737850A (en) * 2016-01-29 2016-07-06 北京理工大学 Variable-scale unidirectional gravity sampling vector matching and positioning method based on particle filtering
CN108444479A (en) * 2018-03-15 2018-08-24 北京理工大学 Gravity Matching method based on ADAPTIVE ROBUST Unscented kalman filtering

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
SHAOKUN CAI，ET AL.: "An iterative method for the accurate determination of airborne gravity horizontal components using strapdown inertial navigation system/global navigation satellite system", 《GEOPHYSICS》 *
刘繁明等: "差分进化粒子滤波在惯性/重力组合导航中的应用研究", 《应用科技》 *
程传奇等: "鲁棒性地形匹配/惯性组合导航算法", 《中国惯性技术学报》 *

Also Published As

Publication number Publication date
CN112797977B (en) 2021-10-29

Similar Documents

Publication Publication Date Title
CN108731670B (en) Inertial/visual odometer integrated navigation positioning method based on measurement model optimization
CN103644903B (en) Synchronous superposition method based on the tasteless particle filter of distributed edge
CN110514203B (en) Underwater integrated navigation method based on ISR-UKF
García et al. Augmented state Kalman filtering for AUV navigation
CN106772524B (en) A kind of agricultural robot integrated navigation information fusion method based on order filtering
CN110779518B (en) Underwater vehicle single beacon positioning method with global convergence
CN113252033B (en) Positioning method, positioning system and robot based on multi-sensor fusion
CN110595503B (en) Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation
WO2017215026A1 (en) Extended kalman filter positioning method based on height constraint
CN110567455B (en) Tightly-combined navigation method for quadrature updating volume Kalman filtering
CN103776453A (en) Combination navigation filtering method of multi-model underwater vehicle
CN113466890B (en) Light laser radar inertial combination positioning method and system based on key feature extraction
CN110779519B (en) Underwater vehicle single beacon positioning method with global convergence
CN104374405A (en) MEMS strapdown inertial navigation initial alignment method based on adaptive central difference Kalman filtering
CN108919283B (en) Satellite autonomous non-cooperative target relative navigation method and system
CN109813316B (en) Terrain-assisted underwater carrier tight combination navigation method
CN114777812A (en) Method for estimating alignment and attitude of underwater integrated navigation system during traveling
CN110926499A (en) Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation
CN112797977B (en) Inertia/gravity matching combination method based on robust point group filtering
CN114897942B (en) Point cloud map generation method and device and related storage medium
CN116577801A (en) Positioning and mapping method and system based on laser radar and IMU
CN115984463A (en) Three-dimensional reconstruction method and system suitable for narrow roadway
Zarei et al. Performance improvement for mobile robot position determination using cubature Kalman filter
CN115113646A (en) Satellite formation flat root state continuous estimation method and system based on Kalman filtering
CN114705223A (en) Inertial navigation error compensation method and system for multiple mobile intelligent bodies in target tracking

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