CN113175931A - Cluster networking collaborative navigation method and system based on constraint Kalman filtering - Google Patents
Cluster networking collaborative navigation method and system based on constraint Kalman filtering Download PDFInfo
- Publication number
- CN113175931A CN113175931A CN202110361651.3A CN202110361651A CN113175931A CN 113175931 A CN113175931 A CN 113175931A CN 202110361651 A CN202110361651 A CN 202110361651A CN 113175931 A CN113175931 A CN 113175931A
- Authority
- CN
- China
- Prior art keywords
- node
- matrix
- cluster
- inertial navigation
- representing
- 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
Links
- 230000006855 networking Effects 0.000 title claims abstract description 68
- 238000001914 filtration Methods 0.000 title claims abstract description 59
- 238000000034 method Methods 0.000 title claims abstract description 32
- 239000011159 matrix material Substances 0.000 claims description 90
- 238000005259 measurement Methods 0.000 claims description 22
- 230000007704 transition Effects 0.000 claims description 8
- 230000008859 change Effects 0.000 claims description 5
- 230000004048 modification Effects 0.000 claims description 4
- 238000012986 modification Methods 0.000 claims description 4
- 238000012937 correction Methods 0.000 claims description 3
- 230000000452 restraining effect Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 230000015572 biosynthetic process Effects 0.000 description 3
- 230000007774 longterm Effects 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 230000008569 process Effects 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/20—Instruments for performing navigational calculations
-
- 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
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
Abstract
The invention provides a cluster networking collaborative navigation method and system based on constraint Kalman filtering, and relates to the technical field of collaborative navigation, wherein the method comprises the following steps: step S1: determining the number of cluster networking nodes, and measuring the relative distance between the nodes; step S2: establishing a cluster networking collaborative navigation filtering equation based on the measured relative distance between the nodes; step S3: designing inertial navigation position error weighting and minimum constraint conditions of each node of a cluster networking; step S4: constructing constraint Kalman filtering of cluster networking collaborative navigation; step S5: and compensating pure inertial navigation errors by using the inertial navigation error estimation values of each node estimated by the constraint Kalman filtering. The method can recursion and estimate the inertial navigation error in real time to correct the inertial navigation system and inhibit pure inertial navigation error divergence of each node.
Description
Technical Field
The invention relates to the technical field of collaborative navigation, in particular to a cluster networking collaborative navigation method and system based on constraint Kalman filtering.
Background
At present, the inertial navigation technology is widely applied to various scenes in life such as aerospace, aviation, unmanned driving, underwater navigation, indoor positioning and the like, and becomes a common navigation mode.
The invention discloses a Chinese patent with publication number CN110207691A, and discloses a multi-unmanned vehicle cooperative navigation method based on data chain distance measurement, which measures the relative distance between unmanned vehicles by using a data chain, and further estimates the position of each unmanned vehicle node in a formation by a multi-navigation sensor information fusion method. Each unmanned vehicle node in the unmanned vehicle formation carries an inertial navigation system, a vehicle odometer and a data chain transceiving device, and the inertial navigation system and the vehicle odometer in each unmanned vehicle node are fused through a Kalman filter to obtain speed, position and attitude information of each unmanned vehicle node; and then optimizing the position estimation result of each unmanned vehicle through equality constraint by utilizing the distance information between the unmanned vehicles measured by the data chain. By the method, the navigation and positioning accuracy of the unmanned vehicle formation under the satellite rejection condition can be improved.
In summary, in the prior art, other navigation sensors must be used to assist in building constraints during collaborative navigation, such as vehicle odometers and the like, which brings additional economic cost and is limited in use in many scenarios. According to the invention, only by depending on the inertial navigation system carried by each carrier, the data chain is used for ranging, and the constraint Kalman filtering is constructed by designing geometric constraint conditions, so that the problem that the inertial navigation system has long-term error quick divergence and cannot be used for long-term navigation is solved.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide a cluster networking collaborative navigation method and system based on constraint Kalman filtering.
According to the cluster networking collaborative navigation method and system based on the constraint Kalman filtering, the scheme is as follows:
in a first aspect, a cluster networking collaborative navigation method based on constrained kalman filtering is provided, where the method includes:
step S1: determining the number of cluster networking nodes, and measuring the relative distance between the nodes;
step S2: establishing a cluster networking collaborative navigation filtering equation based on the measured relative distance between the nodes;
step S3: designing inertial navigation position error weighting and minimum constraint conditions of each node of a cluster networking;
step S4: constructing constraint Kalman filtering of cluster networking collaborative navigation;
step S5: and compensating pure inertial navigation errors by using the inertial navigation error estimation values of each node estimated by the constraint Kalman filtering.
Preferably, the cluster networking collaborative navigation filtering equation in step S2 includes a state equation, where the state equation specifically is:
X(t)=F(t)X(t)+G(t)W(t)
in the formula,respectively representing three-axis position errors of each node in the cluster under a geographic coordinate system, wherein n represents the number of the cluster nodes, F (t) represents a state transition matrix, G (t) represents a system noise coefficient matrix, and W (t) represents a system noise matrix;
in the formula, Ve iRepresenting the east speed of the inode; l isiRepresenting the latitude of the inode; h isiRepresenting the height of the inode; rNRepresenting the curvature radius of the earth fourth-unit circle; rMRepresents the radius of curvature of the earth's meridian; vn iRepresenting the i-node northbound speed.
Preferably, the cluster networking collaborative navigation filtering equation in step S2 further includes a measurement equation, where the measurement equation specifically is:
L(t)=HX(t)+Δ
wherein L (t) is an (n-1) × 1-dimensional observation quantity matrix, n-1 is the number of observation quantities, i.e., the number of distance measurement values, H is an (n-1) × 3 n-dimensional observation coefficient matrix, and Δ is an (n-1) × 1-dimensional observation noise matrix;
in the formula,the measured distance between node i and node j in the cluster,and calculating the distance for inertial navigation between the node i and the node j, wherein L changes in real time along with the change of the number of the nodes n of the missile group.
Preferably, in step S3, the inertial navigation position error weighting and minimum constraint condition of each node of the cluster networking are provided, where the constraint condition includes:
GTPxDaX=0
in the formula, GTIs formed by HTA 6 x 3 n-dimensional matrix composed of eigenvectors corresponding to the zero eigenvalue of PH, P being a diagonal weight matrix of L, HTA positive definite matrix of a diagonal weight matrix with PH of L; pxIs a 3n multiplied by 3n dimensional weight matrix of a parameter vector X to be estimated, which is determined according to the precision level of each node inertial navigation system, wherein n is the number of cluster nodes,
in the formula,for the inertial navigation position of the cluster node i under the earth-centered earth-fixed system, PxVarying in real time as the number of cluster nodes n varies.
Preferably, in step S3, the inertial navigation position error weighting and minimum constraint condition of each node of the cluster networking is that a transformation matrix for transforming the latitude and longitude high error in the geographic coordinate system to the XYZ triaxial error in the geocentric geostationary coordinate system is:
in the formula, Rn represents the curvature radius of the earth unitary fourth of twelve earthly branches;representing the i-node inertial navigation resolving height;representing the i-node inertial navigation resolving latitude;to representResolving longitude by i-node inertial navigation; e.g. of the type2Representing the curvature of the earth.
Preferably, in step S4, the constraint condition is projected to the conventional kalman filter by using a projection method, and the constraint kalman filter for constructing the clustered network collaborative navigation is as follows:
constraint condition matrix: b isk=GTPxDa;
estimating the mean square error: pk=(I-KkHk)Pk/k-1;
In the formula, BkIn order to be a matrix of the constraint conditions,in order to predict the state quantity matrix in one step,is a previous state quantity matrix, phik,k-1In order to be a state transition matrix,correcting for constraintsPost-prediction state quantity matrix, Pk-1Is a state error covariance matrix, KkFor filtering the gain matrix, ZkAs a matrix of observations, HkFor observing the coefficient matrix, Pk/k-1Predicting a state error covariance matrix, R, for one stepkTo observe the noise matrix, QkIs a state noise matrix and I is an identity matrix.
Preferably, in step S5, the inertial navigation error estimation value of each node estimated by using constrained kalman filteringThe method for compensating the pure inertial navigation error comprises the following steps:
in the formula,representing the i-node inertial navigation resolving height;representing the i-node inertial navigation resolving latitude;representing the i-node inertial navigation resolving longitude,the latitude error estimated for the inode,for the longitude error estimated for the inode,the estimated height error for the inode.
In a second aspect, a cluster networking collaborative navigation system based on constrained kalman filtering is provided, the system includes:
module M1: determining the number of cluster networking nodes, and measuring the relative distance between the nodes;
module M2: establishing a cluster networking collaborative navigation filtering equation based on the measured relative distance between the nodes;
module M3: designing inertial navigation position error weighting and minimum constraint conditions of each node of a cluster networking;
module M4: constructing constraint Kalman filtering of cluster networking collaborative navigation;
module M5: and compensating pure inertial navigation errors by using the inertial navigation error estimation values of each node estimated by the constraint Kalman filtering.
Preferably, the cluster networking collaborative navigation filtering equation in the module M2 includes a state equation, where the state equation specifically is:
X(t)=F(t)X(t)+G(t)W(t)
in the formula,respectively representing three-axis position errors of each node in the cluster under a geographic coordinate system, wherein n represents the number of the cluster nodes, F (t) represents a state transition matrix, G (t) represents a system noise coefficient matrix, and W (t) represents a system noise matrix;
in the formula, Ve iRepresenting the east speed of the inode; l isiRepresenting the latitude of the inode; h isiRepresenting the height of the inode; rNRepresenting the curvature radius of the earth fourth-unit circle; rMRepresents the radius of curvature of the earth's meridian; vn iRepresenting the i-node northbound speed.
Preferably, the cluster networking collaborative navigation filtering equation in the module M2 further includes a measurement equation, where the measurement equation specifically is:
L(t)=HX(t)+Δ
wherein L (t) is an (n-1) × 1-dimensional observation quantity matrix, n-1 is the number of observation quantities, i.e., the number of distance measurement values, H is an (n-1) × 3 n-dimensional observation coefficient matrix, and Δ is an (n-1) × 1-dimensional observation noise matrix;
in the formula,the measured distance between node i and node j in the cluster,(i, j ≠ 1, …, n, and i ≠ j) is an inertial navigation calculation distance between the node i and the node j, and L changes in real time along with the change of the number of the missile group nodes n.
Compared with the prior art, the invention has the following beneficial effects:
the method is independent of other navigation equipment, only utilizes networking ranging among nodes in the cluster, designs networking geometric constraint conditions, constructs a state equation and a measurement equation based on the constraint conditions, designs a new Kalman filter, and carries out real-time recursive estimation on inertial navigation errors to correct the inertial navigation system so as to inhibit pure inertial navigation error divergence of each node.
Drawings
Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments with reference to the following drawings:
FIG. 1 is a flowchart of a cluster networking collaborative navigation method based on constraint Kalman filtering according to an embodiment of the present invention;
FIG. 2 is a diagram of a constrained Kalman filter update process.
Detailed Description
The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the invention, but are not intended to limit the invention in any way. It should be noted that it would be obvious to those skilled in the art that various changes and modifications can be made without departing from the spirit of the invention. All falling within the scope of the present invention.
The embodiment of the invention provides a cluster networking collaborative navigation method based on constraint Kalman filtering, aiming at the problem of long-term accumulation of inertial navigation errors, the networking ranging between nodes in a cluster is utilized, the networking constraint condition is designed, the constraint Kalman filtering is constructed, the inertial navigation errors are estimated, and the pure inertial navigation error divergence is restrained. The scheme is as follows: referring to fig. 1 and 2, first, the number of nodes of the cluster networking is determined, and the relative distance between the two nodes is measured.
In the embodiment of the invention, the nodes participating in networking are determined according to the geometric configuration, the distance measurement among all the nodes in the networking is realized through means such as a data link, a millimeter wave radar and the like, and the distance measurement is used as the measurement distance among all the nodes in the collaborative navigation and is used for distance measurement networking.
Secondly, establishing a collaborative navigation filtering equation of the cluster networking, which comprises a state equation and a measurement equation, wherein the collaborative navigation filtering state equation:
X(t)=F(t)X(t)+G(t)W(t)
in the formula,three-axis position error of each node in the cluster in the geographic coordinate system, n is the number of cluster nodes, F (t) is a state transition matrix, G (t) is a system noise coefficient matrix,w (t) is a system noise matrix;
in the formula, Ve iRepresenting the east speed of the inode; l isiRepresenting the latitude of the inode; h isiRepresenting the height of the inode; rNRepresenting the curvature radius of the earth fourth-unit circle; rMRepresents the radius of curvature of the earth's meridian; vn iRepresenting the i-node northbound speed.
The collaborative navigation filtering measurement equation specifically comprises:
L(t)=HX(t)+Δ
wherein L (t) is an (n-1) × 1-dimensional observation quantity matrix, n-1 is the number of observation quantities, i.e., the number of distance measurement values, H is an (n-1) × 3 n-dimensional observation coefficient matrix, and Δ is an (n-1) × 1-dimensional observation noise matrix;
in the formula,the measured distance between node i and node j in the cluster,(i, j ≠ 1, …, n, and i ≠ j) is an inertial navigation calculation distance between the node i and the node j, and L changes in real time along with the change of the number of the missile group nodes n.
And then, designing inertial navigation position error weighting and minimum constraint conditions of each node of the cluster network.
Specifically, in the embodiment of the present invention, the weighted sum minimum constraint condition of inertial navigation position errors of each node in the cluster networking is:
GTPxDaX=0
in the formula, GTIs formed by HTA 6 x 3 n-dimensional matrix composed of eigenvectors corresponding to the zero eigenvalue of PH, P being a diagonal weight matrix of L, HTA positive definite matrix of a diagonal weight matrix with PH of L; pxIs a 3n multiplied by 3n dimensional weight matrix of a parameter vector X to be estimated, which is determined according to the precision level of each node inertial navigation system, wherein n is the number of cluster nodes,
in the formula,for the inertial navigation position of the cluster node i under the earth-centered earth-fixed system, PxVarying in real time as the number of cluster nodes n varies.
The transformation matrix for transforming the high errors of the latitude and longitude under the geographic coordinate system to the errors of the three X, Y, Z axes under the geocentric geostationary coordinate system is as follows:
in the formula, Rn represents the curvature radius of the earth unitary fourth of twelve earthly branches;representing the i-node inertial navigation resolving height;representing the i-node inertial navigation resolving latitude;representing the i-node inertial navigation resolving longitude; e.g. of the type2Representing the curvature of the earth.
And constructing constraint Kalman filtering of cluster networking collaborative navigation.
In the embodiment of the invention, a 'projection method' is adopted to project the constraint condition to the traditional Kalman filtering, and the constraint Kalman filtering for constructing the cluster networking cooperative navigation is as follows:
constraint condition matrix: b isk=GTPxDa;
one-step prediction of mean square error: pk/k-1=φk,k-1Pk-1φT k,k-1+Qk;
Estimating the mean square error: pk=(I-KkHk)Pk/k-1;
In the formula, BkIn order to be a matrix of the constraint conditions,in order to predict the state quantity matrix in one step,is a previous state quantity matrix, phik,k-1In order to be a state transition matrix,predicting the state quantity matrix after constraint modification, Pk-1Is a state error covariance matrix, KkFor filtering the gain matrix, ZkAs a matrix of observations, HkFor observing the coefficient matrix, Pk/k-1Predicting a state error covariance matrix, R, for one stepkTo observe the noise matrix, QkIs a state noise matrix and I is an identity matrix.
And finally, compensating the pure inertial navigation error by using the inertial navigation error estimation value of each node estimated by the constraint Kalman filtering.
In the embodiment of the invention, the inertial navigation error estimation value of each node is estimated by using the constraint Kalman filteringThe method for compensating the pure inertial navigation error comprises the following steps:
in the formula,representing the i-node inertial navigation resolving height;representing the i-node inertial navigation resolving latitude;representing the i-node inertial navigation resolving longitude,the latitude error estimated for the inode,for the longitude error estimated for the inode,the estimated height error for the inode.
The embodiment of the invention provides a cluster networking collaborative navigation method based on constraint Kalman filtering, which is independent of other navigation equipment, only utilizes networking ranging between nodes in a cluster, designs networking constraint conditions, constructs a state equation and a measurement equation based on the constraint conditions, designs a new Kalman filter, carries out real-time recursive estimation on inertial navigation errors to correct an inertial navigation system, inhibits the pure inertial navigation error divergence of each node, and realizes pure inertial navigation high-precision positioning.
Those skilled in the art will appreciate that, in addition to implementing the system and its various devices, modules, units provided by the present invention as pure computer readable program code, the system and its various devices, modules, units provided by the present invention can be fully implemented by logically programming method steps in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like. Therefore, the system and various devices, modules and units thereof provided by the invention can be regarded as a hardware component, and the devices, modules and units included in the system for realizing various functions can also be regarded as structures in the hardware component; means, modules, units for performing the various functions may also be regarded as structures within both software modules and hardware components for performing the method.
The foregoing description of specific embodiments of the present invention has been presented. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes or modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention. The embodiments and features of the embodiments of the present application may be combined with each other arbitrarily without conflict.
Claims (10)
1. A cluster networking collaborative navigation method based on constraint Kalman filtering is characterized by comprising the following steps:
step S1: determining the number of cluster networking nodes, and measuring the relative distance between the nodes;
step S2: establishing a cluster networking collaborative navigation filtering equation based on the measured relative distance between the nodes;
step S3: designing inertial navigation position error weighting and minimum constraint conditions of each node of a cluster networking;
step S4: constructing constraint Kalman filtering of cluster networking collaborative navigation;
step S5: and compensating pure inertial navigation errors by using the inertial navigation error estimation values of each node estimated by the constraint Kalman filtering.
2. The constrained kalman filter-based clustered networking collaborative navigation method according to claim 1, wherein the clustered networking collaborative navigation filter equation in the step S2 includes a state equation, and the state equation specifically is:
X(t)=F(t)X(t)+G(t)W(t)
in the formula, respectively representing three-axis position errors of each node in the cluster under a geographic coordinate system, wherein n represents the number of the cluster nodes, F (t) represents a state transition matrix, G (t) represents a system noise coefficient matrix, and W (t) represents a system noise matrix;
in the formula, Ve iRepresenting the east speed of the inode; l isiRepresenting the latitude of the inode; h isiRepresenting the height of the inode; rNRepresenting the curvature radius of the earth fourth-unit circle; rMRepresents the radius of curvature of the earth's meridian; vn iRepresenting the i-node northbound speed.
3. The constrained kalman filter-based clustered networking collaborative navigation method according to claim 1, wherein the clustered networking collaborative navigation filtering equation in step S2 further includes a measurement equation, and the measurement equation specifically is:
L(t)=HX(t)+Δ
wherein L (t) is an (n-1) × 1-dimensional observation quantity matrix, n-1 is the number of observation quantities, i.e., the number of distance measurement values, H is an (n-1) × 3 n-dimensional observation coefficient matrix, and Δ is an (n-1) × 1-dimensional observation noise matrix;
4. The collaborative navigation method for cluster networking based on constrained kalman filtering according to claim 3, wherein the inertial navigation position error weighting and the minimum constraint condition of each node of the cluster networking in the step S3 are included in the following steps:
GTPxDaX=0
in the formula, GTIs formed by HTA 6 x 3 n-dimensional matrix composed of eigenvectors corresponding to the zero eigenvalue of PH, P being a diagonal weight matrix of L, HTPositive definite matrix of diagonal weight matrix with PH L, PxIs a 3n multiplied by 3n dimensional weight matrix of a parameter vector X to be estimated, which is determined according to the precision level of each node inertial navigation system, wherein n is the number of cluster nodes,
5. The collaborative navigation method for cluster networking based on constrained kalman filtering according to claim 4, wherein in step S3, the inertial navigation position error weighting and the minimum constraint condition of each node of the cluster networking are obtained by transforming the latitude and longitude high error in the geographic coordinate system to the XYZ triaxial error in the geocentric geostationary coordinate system by using a transformation matrix:
in the formula, Rn represents the curvature radius of the earth unitary fourth of twelve earthly branches;representing the i-node inertial navigation resolving height;representing the i-node inertial navigation resolving latitude;representing the i-node inertial navigation resolving longitude; e.g. of the type2Representing the curvature of the earth.
6. The constraint Kalman filtering-based trunking networking collaborative navigation method according to claim 1, wherein in the step S4, the constraint condition is projected to the traditional Kalman filtering by a projection method, and the constraint Kalman filtering for constructing trunking networking collaborative navigation is as follows:
constraint condition matrix: b isk=GTPxDa;
estimating the mean square error: pk=(I-KkHk)Pk/k-1;
In the formula, BkIn order to be a matrix of the constraint conditions,in order to predict the state quantity matrix in one step,is a previous state quantity matrix, phik,k-1In order to be a state transition matrix,predicting the state quantity matrix after constraint modification, Pk-1Is a state error covariance matrix, KkFor filtering the gain matrix, ZkAs a matrix of observations, HkFor observing the coefficient matrix, Pk/k-1Predicting a state error covariance matrix, R, for one stepkTo observe the noise matrix, QkIs a state noise matrix and I is an identity matrix.
7. The collaborative navigation method for cluster networking based on constrained Kalman filtering according to claim 6, wherein the step S5 is implemented by using inertial navigation error estimation values of nodes estimated by constrained Kalman filteringThe method for compensating the pure inertial navigation error comprises the following steps:
in the formula,representing the i-node inertial navigation resolving height;representing the i-node inertial navigation resolving latitude;representing the i-node inertial navigation resolving longitude,the latitude error estimated for the inode,for the longitude error estimated for the inode,the estimated height error for the inode.
8. A cluster networking collaborative navigation system based on constraint Kalman filtering is characterized by comprising:
module M1: determining the number of cluster networking nodes, and measuring the relative distance between the nodes;
module M2: establishing a cluster networking collaborative navigation filtering equation based on the measured relative distance between the nodes;
module M3: designing inertial navigation position error weighting and minimum constraint conditions of each node of a cluster networking;
module M4: constructing constraint Kalman filtering of cluster networking collaborative navigation;
module M5: and compensating pure inertial navigation errors by using the inertial navigation error estimation values of each node estimated by the constraint Kalman filtering.
9. The constraint kalman filter-based trunking networking collaborative navigation system according to claim 8, wherein the trunking networking collaborative navigation filter equations in the module M2 include state equations, and the state equations are specifically:
X(t)=F(t)X(t)+G(t)W(t)
in the formula, respectively representing three-axis position errors of each node in the cluster under a geographic coordinate system, wherein n represents the number of the cluster nodes, F (t) represents a state transition matrix, G (t) represents a system noise coefficient matrix, and W (t) represents a system noise matrix;
in the formula, Ve iRepresenting the east speed of the inode; l isiRepresenting the latitude of the inode; h isiRepresenting the height of the inode; rNRepresenting the curvature radius of the earth fourth-unit circle; rMRepresents the radius of curvature of the earth's meridian; vn iRepresenting the i-node northbound speed.
10. The constraint Kalman filtering-based trunking networking collaborative navigation system according to claim 6, wherein the clustering networking collaborative navigation filtering equation in the module M2 further comprises a measurement equation, and the measurement equation is specifically:
L(t)=HX(t)+Δ
wherein L (t) is an (n-1) × 1-dimensional observation quantity matrix, n-1 is the number of observation quantities, i.e., the number of distance measurement values, H is an (n-1) × 3 n-dimensional observation coefficient matrix, and Δ is an (n-1) × 1-dimensional observation noise matrix;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110361651.3A CN113175931B (en) | 2021-04-02 | 2021-04-02 | Cluster networking collaborative navigation method and system based on constraint Kalman filtering |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110361651.3A CN113175931B (en) | 2021-04-02 | 2021-04-02 | Cluster networking collaborative navigation method and system based on constraint Kalman filtering |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113175931A true CN113175931A (en) | 2021-07-27 |
CN113175931B CN113175931B (en) | 2022-08-16 |
Family
ID=76923105
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110361651.3A Active CN113175931B (en) | 2021-04-02 | 2021-04-02 | Cluster networking collaborative navigation method and system based on constraint Kalman filtering |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113175931B (en) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114279466A (en) * | 2021-12-23 | 2022-04-05 | 中国电子科技集团公司第十四研究所 | Sensor error correction method |
CN114964228A (en) * | 2022-05-05 | 2022-08-30 | 上海机电工程研究所 | Inertial navigation cooperation method based on rank deficiency constraint cascade filtering |
CN116147429A (en) * | 2023-03-10 | 2023-05-23 | 中国电子科技集团公司第二十六研究所 | Aided projectile navigation method based on data chain and radar |
CN116401618A (en) * | 2023-03-03 | 2023-07-07 | 南京航空航天大学 | Cross-domain unmanned cluster collaborative navigation information fusion method based on geometric distribution sampling |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102901514A (en) * | 2012-09-25 | 2013-01-30 | 北京航空航天大学 | Collaborative initial alignment method based on multiple-inertia-unit informational constraint |
CN109471102A (en) * | 2018-10-23 | 2019-03-15 | 湖北航天技术研究院总体设计所 | A kind of used grouping error modification method |
CN109471103A (en) * | 2018-10-23 | 2019-03-15 | 湖北航天技术研究院总体设计所 | A kind of missile-borne Bistatic SAR data fusion positioning error correcting method |
CN110207691A (en) * | 2019-05-08 | 2019-09-06 | 南京航空航天大学 | A kind of more unmanned vehicle collaborative navigation methods based on data-link ranging |
CN110208792A (en) * | 2019-06-26 | 2019-09-06 | 哈尔滨工业大学 | The arbitrary line constraint tracking of dbjective state and trajectory parameters is estimated simultaneously |
CN110285804A (en) * | 2019-06-26 | 2019-09-27 | 南京航空航天大学 | Vehicle collaborative navigation method based on the constraint of relative motion model |
CN110988880A (en) * | 2019-12-12 | 2020-04-10 | 南京莱斯电子设备有限公司 | Geographic information extraction and target tracking method based on SMR target track |
CN111221018A (en) * | 2020-03-12 | 2020-06-02 | 南京航空航天大学 | GNSS multi-source information fusion navigation method for inhibiting marine multipath |
-
2021
- 2021-04-02 CN CN202110361651.3A patent/CN113175931B/en active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102901514A (en) * | 2012-09-25 | 2013-01-30 | 北京航空航天大学 | Collaborative initial alignment method based on multiple-inertia-unit informational constraint |
CN109471102A (en) * | 2018-10-23 | 2019-03-15 | 湖北航天技术研究院总体设计所 | A kind of used grouping error modification method |
CN109471103A (en) * | 2018-10-23 | 2019-03-15 | 湖北航天技术研究院总体设计所 | A kind of missile-borne Bistatic SAR data fusion positioning error correcting method |
CN110207691A (en) * | 2019-05-08 | 2019-09-06 | 南京航空航天大学 | A kind of more unmanned vehicle collaborative navigation methods based on data-link ranging |
CN110208792A (en) * | 2019-06-26 | 2019-09-06 | 哈尔滨工业大学 | The arbitrary line constraint tracking of dbjective state and trajectory parameters is estimated simultaneously |
CN110285804A (en) * | 2019-06-26 | 2019-09-27 | 南京航空航天大学 | Vehicle collaborative navigation method based on the constraint of relative motion model |
CN110988880A (en) * | 2019-12-12 | 2020-04-10 | 南京莱斯电子设备有限公司 | Geographic information extraction and target tracking method based on SMR target track |
CN111221018A (en) * | 2020-03-12 | 2020-06-02 | 南京航空航天大学 | GNSS multi-source information fusion navigation method for inhibiting marine multipath |
Non-Patent Citations (3)
Title |
---|
PEYMAN HADI MOHAMMADABADI: "Cooperative Node Positioning In Vehicular Networks Using Inter-Node Distance Measurements", 《2014 IEEE 25TH INTERNATIONAL SYMPOSIUM ON PERSONAL, INDOOR AND MOBILE RADIO COMMUNICATIONS》 * |
刘俊成: "基于相互测距信息的机群组网协同定位技术", 《北 京 航 空 航 天 大 学 学 报》 * |
张京娟: "基于飞行器三维组网测距信息的导航误差估计技术研究", 《空天防御》 * |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114279466A (en) * | 2021-12-23 | 2022-04-05 | 中国电子科技集团公司第十四研究所 | Sensor error correction method |
CN114279466B (en) * | 2021-12-23 | 2024-02-27 | 中国电子科技集团公司第十四研究所 | Sensor error correction method |
CN114964228A (en) * | 2022-05-05 | 2022-08-30 | 上海机电工程研究所 | Inertial navigation cooperation method based on rank deficiency constraint cascade filtering |
CN116401618A (en) * | 2023-03-03 | 2023-07-07 | 南京航空航天大学 | Cross-domain unmanned cluster collaborative navigation information fusion method based on geometric distribution sampling |
CN116401618B (en) * | 2023-03-03 | 2023-12-01 | 南京航空航天大学 | Cross-domain unmanned cluster collaborative navigation information fusion method based on geometric distribution sampling |
CN116147429A (en) * | 2023-03-10 | 2023-05-23 | 中国电子科技集团公司第二十六研究所 | Aided projectile navigation method based on data chain and radar |
CN116147429B (en) * | 2023-03-10 | 2024-07-19 | 中国电子科技集团公司第二十六研究所 | Aided projectile navigation method based on data chain and radar |
Also Published As
Publication number | Publication date |
---|---|
CN113175931B (en) | 2022-08-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113175931B (en) | Cluster networking collaborative navigation method and system based on constraint Kalman filtering | |
CN110487301B (en) | Initial alignment method of radar-assisted airborne strapdown inertial navigation system | |
Li et al. | A robust graph optimization realization of tightly coupled GNSS/INS integrated navigation system for urban vehicles | |
US8452536B2 (en) | Method of definition of a navigation system | |
CN111337020A (en) | Factor graph fusion positioning method introducing robust estimation | |
CN111156987A (en) | Inertia/astronomical combined navigation method based on residual compensation multi-rate CKF | |
CN102353378B (en) | Adaptive federal filtering method of vector-form information distribution coefficients | |
CN109883426A (en) | Dynamic allocation and correction multi-sources Information Fusion Method based on factor graph | |
CN111337029B (en) | Polarized light inertia tight combination navigation method based on self-learning multi-rate residual error correction | |
CN109631883B (en) | Method for accurately estimating local attitude of aircraft based on node information sharing | |
CN109507706B (en) | GPS signal loss prediction positioning method | |
CN108931799A (en) | Train combined positioning method and system based on the search of recurrence fast orthogonal | |
CN112525188B (en) | Combined navigation method based on federal filtering | |
CN114264301B (en) | Vehicle-mounted multi-sensor fusion positioning method, device, chip and terminal | |
CN112346104A (en) | Unmanned aerial vehicle information fusion positioning method | |
CN111220151B (en) | Inertia and milemeter combined navigation method considering temperature model under load system | |
CN115711616B (en) | Smooth positioning method and device for indoor and outdoor traversing unmanned aerial vehicle | |
CN114689047A (en) | Deep learning-based integrated navigation method, device, system and storage medium | |
Wang et al. | An adaptive federated filter based on variational Bayes with application to multisource navigation | |
Taghizadeh et al. | A low-cost integrated navigation system based on factor graph nonlinear optimization for autonomous flight | |
CN117824632A (en) | Gravity matching secondary filtering method and device based on noise distribution assumption | |
CN113434806A (en) | Robust adaptive multi-model filtering method | |
CN117419723A (en) | Interactive model self-adaptive integrated navigation method based on factor graph | |
CN108981689B (en) | UWB/INS combined navigation system based on DSP TMS320C6748 | |
CN111982126A (en) | Design method of full-source BeiDou/SINS elastic state observer model |
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 |