CN115096307B - Autonomous splitting and fusion filtering method for probability function of matched navigation system - Google Patents
Autonomous splitting and fusion filtering method for probability function of matched navigation system Download PDFInfo
- Publication number
- CN115096307B CN115096307B CN202210460132.7A CN202210460132A CN115096307B CN 115096307 B CN115096307 B CN 115096307B CN 202210460132 A CN202210460132 A CN 202210460132A CN 115096307 B CN115096307 B CN 115096307B
- Authority
- CN
- China
- Prior art keywords
- positioning
- terrain
- terrain matching
- navigation system
- probability distribution
- 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
- 230000004927 fusion Effects 0.000 title claims abstract description 43
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000001914 filtration Methods 0.000 title claims abstract description 26
- 238000005315 distribution function Methods 0.000 claims abstract description 40
- 238000005259 measurement Methods 0.000 claims description 21
- 238000012545 processing Methods 0.000 claims description 10
- 238000012937 correction Methods 0.000 claims description 7
- 238000012876 topography Methods 0.000 claims description 4
- 230000003321 amplification Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 238000003199 nucleic acid amplification method Methods 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 230000006978 adaptation Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- GNFTZDOKVXKIBK-UHFFFAOYSA-N 3-(2-methoxyethoxy)benzohydrazide Chemical compound COCCOC1=CC=CC(C(=O)NN)=C1 GNFTZDOKVXKIBK-UHFFFAOYSA-N 0.000 description 1
- FGUUSXIOTUKUDN-IBGZPJMESA-N C1(=CC=CC=C1)N1C2=C(NC([C@H](C1)NC=1OC(=NN=1)C1=CC=CC=C1)=O)C=CC=C2 Chemical compound C1(=CC=CC=C1)N1C2=C(NC([C@H](C1)NC=1OC(=NN=1)C1=CC=CC=C1)=O)C=CC=C2 FGUUSXIOTUKUDN-IBGZPJMESA-N 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
The invention discloses an autonomous splitting and fusion filtering method for probability functions of a matched navigation system, which comprises the following steps: s01, inputting positioning information of a main navigation system into a terrain matching navigation system, and inputting initial input information in the terrain matching navigation system in advance; s02, when the main navigation system is positioned to the starting point of the terrain matching navigation planning point, the terrain matching navigation system starts a filter initialization program to complete a probability distribution function splitting step, S03, the terrain matching navigation system starts a real-time terrain detection program, a data packet is input into a filtering module of the terrain matching navigation system, and finally fusion of positioning probability distribution functions is carried out; s04, outputting navigation positioning information after the operation of the terrain matching navigation program is finished. The probability function autonomous splitting and fusion filtering method of the matched navigation system provided by the invention can reduce the interference of the pseudo wave crest through the autonomous splitting of the positioning probability distribution function in the initial operation stage of the terrain matched navigation system.
Description
Technical Field
The invention relates to an autonomous splitting and fusion filtering method for probability functions of a matched navigation system, and belongs to the technical field of terrain matched navigation systems.
Background
The deep sea space is an electromagnetic wave shielding space, so that satellite positioning signals cannot be received, and long-term accurate navigation positioning cannot be performed. The underwater topography matching navigation is a positioning navigation method based on environment information feature matching tracking, and the navigation mode utilizes an underwater robot to sense environment features and performs matching recognition with an priori environment map so as to obtain positioning estimation, thereby realizing complete autonomous positioning and navigation of the underwater robot. However, for underwater robots performing deep sea or long range exploration tasks, a large initial positioning deviation is often caused at the initial moment of terrain matching navigation due to the accumulation of errors of a long-time main navigation system. The positioning probability distribution function of the terrain matching navigation system filter has pseudo wave peaks, information is mutually coupled and interfered between the pseudo wave peaks, risks of unstable filtering and divergent filtering exist in the running process of the system, and the reliability of the terrain matching navigation system is directly affected.
Disclosure of Invention
The invention aims to overcome the defects of the prior art, and provides an autonomous splitting and fusion filtering method for a probability function of a matched navigation system, which can reduce the interference of a pseudo wave crest through autonomous splitting of a positioning probability distribution function in the initial operation stage of the terrain matched navigation system.
In order to solve the technical problems, the invention adopts the following technical scheme:
a probability function autonomous splitting and fusion filtering method of a matched navigation system comprises the following steps:
s01, a terrain matching navigation system inputs a terrain matching navigation planning point sequence Wp i in advance, i=1, 2 and … I, wherein I represents the total number of the terrain matching navigation planning points, a main navigation system inputs positioning information into the terrain matching navigation system, the terrain matching navigation system acquires a terrain matching navigation planning point to be processed currently from the terrain matching navigation planning point sequence according to the index number, and the terrain matching navigation system judges whether a carrier reaches the terrain matching navigation planning point according to the positioning information;
S02, when the main navigation system is positioned to the starting point that the carrier reaches the terrain matching navigation planning point, at the moment, the index number i of the terrain matching navigation planning point sequence is 1, the terrain measurement module acquires real-time terrain measurement information, and a data packet formed by the real-time terrain measurement information, the positioning information and the terrain matching navigation planning point is input into the terrain matching positioning module for processing, and then is input into the probability distribution function splitting module for probability distribution function splitting, and finally, the index number i of the terrain matching navigation planning point is increased by 1, namely i=i+1;
S03, when the main navigation system is positioned to the point that the carrier reaches the non-starting point of the terrain matching navigation planning point, at the moment, the index number i of the terrain matching navigation planning point sequence is larger than 1, the real-time terrain measurement module acquires real-time terrain measurement information, the data packet formed by the real-time terrain measurement information, the positioning information and the terrain matching navigation planning point is input into the filtering module for processing, the processed data packet is input into the probability distribution function fusion module for probability distribution function fusion, and finally, the index number i of the terrain matching navigation planning point is increased by 1, namely i=i+1;
S04, repeating S03, judging whether I > I is met, if yes, ending the operation of the terrain matching navigation program, outputting navigation positioning information by the probability distribution function fusion module, and if not, continuing to execute S03 until the situation is met.
In S01, the positioning information includes a pilot positioning point D t of the navigation t time node and a pilot positioning error De t, an index number i of the terrain matching navigation planning point sequence Wp i is initialized to be equal to 1, and when an absolute value of a difference value between Wp i and the pilot positioning point D t is smaller than an error e, the carrier is judged to reach an ith terrain matching navigation planning point.
In S02, the terrain matching and positioning module obtains matching and positioning information after processing, the matching and positioning information comprises a terrain matching and positioning likelihood function, a terrain matching and positioning point and a terrain matching and positioning error, and the matching and positioning information is input into the probability distribution function splitting module to carry out a probability distribution function splitting flow.
The probability function splitting flow in the probability distribution function splitting module specifically comprises the following steps: s021, a threshold value is setRespectively represent statistical feature thresholds of a two-dimensional sequence, wherein/>Variance statistics representing sequences, ε μ representing the mean of the sequences, ε r representing the correlation coefficients of the sequences;
S022, obtaining a terrain matching positioning likelihood function L from the terrain matching positioning module, and calculating all peak points of the terrain matching positioning likelihood function L Where n 0 represents the total number of peak points,Coordinate values representing peak points; /(I)Covariance matrix representing positioning error of peak point is solved by FISHER information quantity of topography;
S023, fitting by using a Gaussian check likelihood function L, wherein the formula is as follows:
Wherein, L 1 represents a Gaussian kernel fitting function of a terrain matching positioning likelihood function of the 1 st terrain matching positioning point; n represents the number of Gaussian kernels; Coefficients representing gaussian kernel functions; /(I) Mean value is/>Variance isA gaussian kernel function of (c);
On this basis, a fitting residual Δl 1 is calculated:
ΔL1＝L-L1 (2)
then judging whether the statistical parameter of the residual error delta L 1 meets the formula (3),
If yes, completing the probability distribution function splitting step; if not, continuing to transfer to S024;
S024, after obtaining ΔL 1, all peak points of ΔL 1 are calculated as in S021 Where n 1 represents the total number of peak points,/>Coordinate value representing peak point,/>The variance of the positioning error of the peak point is represented, and the variance is solved through the FISHER information quantity of the terrain;
s025, again using equation (1), calculate the gaussian fit L 2 for Δl 1:
on this basis, the fitting residual is calculated:
ΔL2＝ΔL1-L2 (5)
On the basis, deltaL 1 in the formula (3) is replaced by DeltaL 2, whether the statistical parameter of the residual error meets the formula (3) is judged, and if so, the splitting step is finished; if not, repeating the processes of S024 and S025, calculating all peak points and Gaussian fitting of the last fitting residual error, and obtaining a new fitting residual error until the statistical parameters of the new fitting residual error meet the conditions.
The filtering module comprises a terrain matching positioning module, an observation correction module and a positioning prediction module, wherein the terrain matching positioning module estimates terrain matching positioning probability distribution through real-time terrain measurement information, the positioning prediction module predicts the positioning probability distribution at the current moment based on the positioning probability distribution at the moment t-1 to obtain positioning prediction probability distribution, then the observation correction module carries out fusion estimation on the terrain matching positioning probability distribution and the positioning prediction probability distribution to obtain the observed corrected terrain matching positioning probability distribution, and then the observed corrected terrain matching positioning probability distribution is input into the probability distribution function fusion module to carry out probability distribution function fusion.
The probability distribution function fusion specifically comprises the following steps:
S031, firstly, calculating the positioning point of the terrain matching navigation system at the ith navigation planning point Wp i, i > 1 And a positioning error P i estimation result, the calculation formula is as follows:
S032, inputting a positioning sequence expressed as Wherein: /(I)Representing the weight value,/>Representing the anchor point,/>Representing the variance of the positioning point, and k represents the index number of the positioning point;
k represents the total number of navigation positioning points at the current moment;
S033, alignment sequence Fusion judgment is carried out, and the judgment method is as follows: first, any two anchor points/>, in the sequence are calculatedAnd/>Is the fusion error of (2)
Then, calculateAnd/>And according to whether the distance is within a distance threshold range determined by fusion error, whether the two positioning information are combined or not is judged according to the judgment basis, and the judgment method is as follows:
Where γ represents the amplification factor.
The value of gamma is more than 1 and less than 3.
The invention has the beneficial effects that: according to the autonomous splitting and fusion filtering method for the probability function of the matched navigation system, provided by the invention, in the initial operation stage of the terrain matched navigation system, the interference of the pseudo wave crest is reduced through the autonomous splitting of the positioning probability distribution function, the stability and the convergence speed of the filter can be improved, and the system is ensured to be rapidly transited from the unstable state of the initial alignment process to the stable tracking state.
Drawings
FIG. 1 is a schematic flow chart of a method for autonomous splitting and fusion filtering of probability functions of a matched navigation system.
Detailed Description
The present invention will be further described with reference to the accompanying drawings, and the following examples are only for more clearly illustrating the technical aspects of the present invention, and are not to be construed as limiting the scope of the present invention.
The invention discloses an autonomous splitting and fusion filtering method for probability functions of a matched navigation system, which comprises the following steps:
Step one, a terrain matching navigation system inputs a terrain matching navigation planning point sequence Wp i in advance, i=1, 2 and … I (see flow 3 in fig. 1), wherein I represents the total number of the terrain matching navigation planning points, a main navigation system inputs positioning information into the terrain matching navigation system, the terrain matching navigation system acquires a terrain matching navigation planning point which needs to be processed currently from the terrain matching navigation planning point sequence according to an index number, and the terrain matching navigation system judges whether a carrier reaches the terrain matching navigation planning point according to the positioning information. Wherein, the positioning information includes a main navigation positioning point D t of the navigation t time node and a main navigation positioning error De t (see flow 1 in fig. 1), an index i of the terrain matching navigation planning point sequence Wp i is initialized to be equal to 1 (see flow 2 in fig. 1), and when an absolute value of a difference between Wp i and the main navigation positioning point D t is smaller than the error e (see flow 4 in fig. 1), the carrier is determined to reach the ith terrain matching navigation planning point.
When the main navigation system locates that the carrier reaches the starting point of the terrain matching navigation planning point, at this time, the index number i of the terrain matching navigation planning point sequence is 1, the terrain measurement module acquires real-time terrain measurement information (see flow 5 in fig. 1), a data packet formed by the real-time terrain measurement information, the positioning information and the terrain matching navigation planning point (see flow 6 in fig. 1) is input into the terrain matching positioning module for processing (see flow 7 in fig. 1), the terrain matching positioning module acquires the matching positioning information after processing, the matching positioning information comprises a terrain matching positioning likelihood function, a terrain matching positioning point and a terrain matching positioning error, the matching positioning information is input into the probability distribution function splitting module for a probability distribution function splitting flow (see flow 8 in fig. 1), and finally, the index number i of the terrain matching navigation planning point is increased by 1, namely i=i+1.
The probability function splitting flow in the probability distribution function splitting module specifically comprises the following steps:
Step a, setting a threshold value Respectively represent statistical feature thresholds of a two-dimensional sequence, wherein/>Variance statistics representing sequences, ε μ representing the mean of the sequences, ε r representing the correlation coefficients of the sequences;
step b, obtaining a terrain matching positioning likelihood function L from the terrain matching positioning module, and calculating all peak points of the terrain matching positioning likelihood function L Where n 0 represents the total number of peak points,Coordinate values representing peak points; /(I)Covariance matrix representing positioning error of peak point is solved by FISHER information quantity of topography;
and c, fitting by using a Gaussian kernel likelihood function L, wherein the formula is as follows:
Wherein, L 1 represents a Gaussian kernel fitting function of a terrain matching positioning likelihood function of the 1 st terrain matching positioning point; n represents the number of Gaussian kernels; Coefficients representing gaussian kernel functions; /(I) Mean value is/>Variance isA gaussian kernel function of (c);
On this basis, a fitting residual Δl 1 is calculated:
ΔL1＝L-L1 (2)
then judging whether the statistical parameter of the residual error delta L 1 meets the formula (3), If yes, completing the probability distribution function splitting step; if not, continuing to transfer to the step d;
Step d, after obtaining ΔL 1, with S021, all peak points of ΔL 1 are calculated Where n 1 represents the total number of peak points,/> Coordinate value representing peak point,/>The variance of the positioning error of the peak point is represented, and the variance is solved through the FISHER information quantity of the terrain;
Step e, again using equation (1), calculate the gaussian fit L 2 for Δl 1:
on this basis, the fitting residual is calculated:
ΔL2＝ΔL1-L2 (5)
On the basis, deltaL 1 in the formula (3) is replaced by DeltaL 2, whether the statistical parameter of the residual error meets the formula (3) is judged, and if so, the splitting step is finished; and d, if not, repeating the processes of the step d and the step e, and calculating all peak points and Gaussian fitting of the last fitting residual to obtain a new fitting residual until the statistical parameters of the new fitting residual meet the conditions.
And thirdly, when the main navigation system is positioned to the point that the carrier reaches the non-starting point of the terrain matching navigation planning point, the index number i of the terrain matching navigation planning point sequence is larger than 1, the real-time terrain measurement module acquires real-time terrain measurement information, and a data packet formed by the real-time terrain measurement information, the positioning information and the terrain matching navigation planning point is input into the filtering module for processing (see a flow 9 in fig. 1). The filtering module comprises a terrain matching positioning module, an observation correction module and a positioning prediction module, wherein the terrain matching positioning module estimates terrain matching positioning probability distribution through real-time terrain measurement information, the positioning prediction module predicts the positioning probability distribution at the current moment based on the positioning probability distribution at the t-1 moment to obtain positioning prediction probability distribution, and then the observation correction module carries out fusion estimation on the terrain matching positioning probability distribution and the positioning prediction probability distribution to obtain the observed and corrected terrain matching positioning probability distribution. And then inputting the terrain matching positioning probability distribution after observation and correction into a probability distribution function fusion module, and fusing the sub-probability distribution functions by utilizing a fusion rule.
And after the processing, inputting the processed data into a probability distribution function fusion module for probability distribution function fusion (see a flow 10 in fig. 1), and finally increasing the index number i of the terrain matching navigation planning point by 1, namely i=i+1. The probability distribution function fusion specifically comprises the following steps:
Step f, firstly, calculating the positioning point of the terrain matching navigation system at the ith navigation planning point Wp i, i > 1 And a positioning error P i estimation result, the calculation formula is as follows:
Step g, inputting a positioning sequence, which is expressed as Wherein: /(I)Representing the weight value,/>Representing the anchor point,/>Representing the variance of the positioning point, and k represents the index number of the positioning point; k represents the total number of navigation positioning points at the current moment;
step h, for the positioning sequence Fusion judgment is carried out, and the judgment method is as follows: first, any two anchor points/>, in the sequence are calculatedAnd/>Fusion error/>
Then, calculateAnd/>And according to whether the distance is within a distance threshold range determined by fusion error, whether the two positioning information are combined or not is judged according to the judgment basis, and the judgment method is as follows:
wherein, gamma represents the amplification factor, and the value of gamma is 1 < gamma < 3.
And step four, repeating the step three, judging whether I is more than I, if so, ending the operation of the terrain matching navigation program, outputting navigation positioning information by the probability distribution function fusion module (see the flow 11 in fig. 1), and if not, continuing to execute the step three until the terrain matching navigation program is met.
The foregoing is only a preferred embodiment of the invention, it being noted that: it will be apparent to those skilled in the art that various modifications and adaptations can be made without departing from the principles of the present invention, and such modifications and adaptations are intended to be comprehended within the scope of the invention.
Claims (7)
1. A probability function autonomous splitting and fusion filtering method of a matched navigation system is characterized in that: the method comprises the following steps:
S01, a terrain matching navigation system inputs a terrain matching navigation planning point sequence Wp i in advance, i=1, 2, & I, wherein I represents the total number of the terrain matching navigation planning points, a main navigation system inputs positioning information into the terrain matching navigation system, the terrain matching navigation system acquires a terrain matching navigation planning point to be processed currently from the terrain matching navigation planning point sequence according to the index number, and the terrain matching navigation system judges whether a carrier reaches the terrain matching navigation planning point according to the positioning information;
S02, when the main navigation system is positioned to the starting point that the carrier reaches the terrain matching navigation planning point, at the moment, the index number i of the terrain matching navigation planning point sequence is 1, the terrain measurement module acquires real-time terrain measurement information, and a data packet formed by the real-time terrain measurement information, the positioning information and the terrain matching navigation planning point is input into the terrain matching positioning module for processing, and then is input into the probability distribution function splitting module for probability distribution function splitting, and finally, the index number i of the terrain matching navigation planning point is increased by 1, namely i=i+1;
S03, when the main navigation system is positioned to the point that the carrier reaches the non-starting point of the terrain matching navigation planning point, at the moment, the index number i of the terrain matching navigation planning point sequence is larger than 1, the real-time terrain measurement module acquires real-time terrain measurement information, the data packet formed by the real-time terrain measurement information, the positioning information and the terrain matching navigation planning point is input into the filtering module for processing, the processed data packet is input into the probability distribution function fusion module for probability distribution function fusion, and finally, the index number i of the terrain matching navigation planning point is increased by 1, namely i=i+1;
S04, repeating S03, judging whether I > I is met, if yes, ending the operation of the terrain matching navigation program, outputting navigation positioning information by the probability distribution function fusion module, and if not, continuing to execute S03 until the situation is met.
2. The method for autonomously splitting and fusing the filtering of the probability function of the matched navigation system according to claim 1, wherein the method comprises the following steps: in S01, the positioning information includes a pilot positioning point D t of the navigation t time node and a pilot positioning error De t, an index number i of the terrain matching navigation planning point sequence Wp i is initialized to be equal to 1, and when an absolute value of a difference value between Wp i and the pilot positioning point D t is smaller than an error e, the carrier is judged to reach an ith terrain matching navigation planning point.
3. The method for autonomously splitting and fusing the filtering of the probability function of the matched navigation system according to claim 2, wherein the method comprises the following steps: in S02, the terrain matching and positioning module obtains matching and positioning information after processing, the matching and positioning information comprises a terrain matching and positioning likelihood function, a terrain matching and positioning point and a terrain matching and positioning error, and the matching and positioning information is input into the probability distribution function splitting module to carry out a probability distribution function splitting flow.
4. A method for autonomous splitting and fusion filtering of probability functions of a matched navigation system according to claim 3, wherein: the probability function splitting flow in the probability distribution function splitting module specifically comprises the following steps:
S021, a threshold value is set Respectively represent statistical feature thresholds of a two-dimensional sequence, wherein/>Variance statistics representing sequences, ε μ representing the mean of the sequences, ε r representing the correlation coefficients of the sequences;
S022, obtaining a terrain matching positioning likelihood function L from the terrain matching positioning module, and calculating all peak points of the terrain matching positioning likelihood function L Where n 0 represents the total number of peak points,Coordinate values representing peak points; /(I)Covariance matrix representing positioning error of peak point is solved by FISHER information quantity of topography;
S023, fitting by using a Gaussian check likelihood function L, wherein the formula is as follows:
Wherein, L 1 represents a Gaussian kernel fitting function of a terrain matching positioning likelihood function of the 1 st terrain matching positioning point; n represents the number of Gaussian kernels; Coefficients representing gaussian kernel functions; /(I) Mean value is/>Variance is/>A gaussian kernel function of (c);
On this basis, a fitting residual Δl 1 is calculated:
ΔL1＝L-L1 (2)
then judging whether the statistical parameter of the residual error delta L 1 meets the formula (3),
If yes, completing the probability distribution function splitting step; if not, continuing to transfer to S024;
S024, after obtaining ΔL 1, all peak points of ΔL 1 are calculated as in S021 Where n 1 represents the total number of peak points,/>Coordinate value representing peak point,/>The variance of the positioning error of the peak point is represented, and the variance is solved through the FISHER information quantity of the terrain;
s025, again using equation (1), calculate the gaussian fit L 2 for Δl 1:
on this basis, the fitting residual is calculated:
ΔL2＝ΔL1-L2 (5)
On the basis, deltaL 1 in the formula (3) is replaced by DeltaL 2, whether the statistical parameter of the residual error meets the formula (3) is judged, and if so, the splitting step is finished; if not, repeating the processes of S024 and S025, calculating all peak points and Gaussian fitting of the last fitting residual error, and obtaining a new fitting residual error until the statistical parameters of the new fitting residual error meet the conditions.
5. The method for autonomously splitting and fusing the filtering of the probability function of the matched navigation system according to claim 4, wherein the method comprises the following steps: the filtering module comprises a terrain matching positioning module, an observation correction module and a positioning prediction module, wherein the terrain matching positioning module estimates terrain matching positioning probability distribution through real-time terrain measurement information, the positioning prediction module predicts the positioning probability distribution at the current moment based on the positioning probability distribution at the moment t-1 to obtain positioning prediction probability distribution, then the observation correction module carries out fusion estimation on the terrain matching positioning probability distribution and the positioning prediction probability distribution to obtain the observed corrected terrain matching positioning probability distribution, and then the observed corrected terrain matching positioning probability distribution is input into the probability distribution function fusion module to carry out probability distribution function fusion.
6. The method for autonomously splitting and fusing the filtering of the probability function of the matched navigation system according to claim 5, wherein the method comprises the following steps: the probability distribution function fusion specifically comprises the following steps:
S031, firstly, calculating the positioning point of the terrain matching navigation system at the ith navigation planning point Wp i, i > 1 And a positioning error P i estimation result, the calculation formula is as follows:
S032, inputting a positioning sequence expressed as Wherein: /(I)Representing the weight value,/>Representing the anchor point,/>Representing the variance of the positioning point, and k represents the index number of the positioning point; k represents the total number of navigation positioning points at the current moment;
S033, alignment sequence Fusion judgment is carried out, and the judgment method is as follows: first, any two anchor points/>, in the sequence are calculatedAnd/>Is the fusion error of (2)
Then, calculateAnd/>And according to whether the distance is within a distance threshold range determined by fusion error, whether the two positioning information are combined or not is judged according to the judgment basis, and the judgment method is as follows:
Where γ represents the amplification factor.
7. The method for autonomously splitting and fusing the filtering of the probability function of the matched navigation system according to claim 6, wherein the method comprises the following steps: the value of gamma is more than 1 and less than 3.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210460132.7A CN115096307B (en) | 2022-04-28 | 2022-04-28 | Autonomous splitting and fusion filtering method for probability function of matched navigation system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210460132.7A CN115096307B (en) | 2022-04-28 | 2022-04-28 | Autonomous splitting and fusion filtering method for probability function of matched navigation system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115096307A CN115096307A (en) | 2022-09-23 |
| CN115096307B true CN115096307B (en) | 2024-05-14 |
Family
ID=83287352
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210460132.7A Active CN115096307B (en) | 2022-04-28 | 2022-04-28 | Autonomous splitting and fusion filtering method for probability function of matched navigation system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115096307B (en) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111121778A (en) * | 2019-12-03 | 2020-05-08 | 河海大学 | Navigation system initialization method |
| CA3067575A1 (en) * | 2019-01-14 | 2020-07-14 | Harbin Engineering University | Self-learning autonomous navigation systems and methods for unmanned underwater vehicle |
| CN112880659A (en) * | 2021-01-09 | 2021-06-01 | 西北工业大学 | Fusion positioning method based on information probability |
| CN114111767A (en) * | 2021-10-28 | 2022-03-01 | 北京自动化控制设备研究所 | Method for optimizing line design line type based on multi-information fusion |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11327492B2 (en) * | 2019-12-03 | 2022-05-10 | Mitsubishi Electric Research Laboratories, Inc. | Adaptive control of autonomous or semi-autonomous vehicle |
-
2022
- 2022-04-28 CN CN202210460132.7A patent/CN115096307B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CA3067575A1 (en) * | 2019-01-14 | 2020-07-14 | Harbin Engineering University | Self-learning autonomous navigation systems and methods for unmanned underwater vehicle |
| CN111121778A (en) * | 2019-12-03 | 2020-05-08 | 河海大学 | Navigation system initialization method |
| CN112880659A (en) * | 2021-01-09 | 2021-06-01 | 西北工业大学 | Fusion positioning method based on information probability |
| CN114111767A (en) * | 2021-10-28 | 2022-03-01 | 北京自动化控制设备研究所 | Method for optimizing line design line type based on multi-information fusion |
Non-Patent Citations (3)
| Title |
|---|
| AUV地形匹配导航初始定位研究;王汝鹏;《中国优秀硕士论文全文数据库（工程科技Ⅱ辑）》;20200115;全文 * |
| Intial positioning of terrain relative navigation under pseudo-peaks interference;王汝鹏等;《IEEE transactions on instrumentation and measurement》;20231019;全文 * |
| 水下无源导航系统仿真匹配算法研究;张飞舟, 侣文芳, 晏磊, 葛远声;武汉大学学报(信息科学版);20030425(02);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115096307A (en) | 2022-09-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108134640B (en) | Cooperative positioning system and method based on node motion state constraint | |
| CN112097774B (en) | Distributed map fusion method based on adaptive Kalman filtering and average tracking | |
| CN112632202A (en) | Dynamic map matching method based on high-order hidden Markov model | |
| CN110889862B (en) | Combined measurement method for multi-target tracking in network transmission attack environment | |
| CN115096307B (en) | Autonomous splitting and fusion filtering method for probability function of matched navigation system | |
| Nakatani et al. | Terrain based localization for pinpoint observation of deep seafloors | |
| CN113218421B (en) | Robust self-adaptive dynamic alignment method for strapdown inertial navigation system under Beidou rejection condition | |
| CN108680162B (en) | Human body target tracking method based on progressive unscented Kalman filtering | |
| CN116681733B (en) | Near-distance real-time pose tracking method for space non-cooperative target | |
| CN112581610A (en) | Robust optimization method and system for establishing map from multi-beam sonar data | |
| Palmier et al. | Adaptive approximate bayesian computational particle filters for underwater terrain aided navigation | |
| CN116883460A (en) | Visual perception positioning method and device, electronic equipment and storage medium | |
| CN113840230B (en) | Unmanned cluster cooperative positioning method and device | |
| CN110007298B (en) | Target advanced prediction tracking method | |
| CN113296139B (en) | Self-adaptive image optical flow and RTK fusion attitude determination method | |
| CN113156368B (en) | Error parameter identification co-location method based on factor graph | |
| US20230078005A1 (en) | Navigation assistance method for a mobile carrier | |
| FR3073947A1 (en) | METHOD AND SYSTEM FOR POSITION ESTIMATING BY COLLABORATIVE REPRESENTATION ON UNKNOWN AMERS | |
| CN115115702A (en) | Autonomous positioning method, device, equipment and computer readable storage medium | |
| EP4042197A1 (en) | Method and device for estimating the status of a maneuvering target by a mobile radar | |
| CN114035154A (en) | Motion parameter assisted single-station radio frequency signal positioning method | |
| CN107590509B (en) | Cherenov fusion method based on maximum expectation approximation | |
| KR20210073079A (en) | Apparatus and method for estimating position of underwater robot | |
| Perera et al. | A gm-phd filter with estimation of probability of detection and survival for individual targets | |
| CN118031951A (en) | Multisource fusion positioning method, multisource fusion positioning device, electronic equipment and storage medium |
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 |