CN116295538A - USBL (universal serial bus) installation error calibration method based on improved particle filtering - Google Patents

USBL (universal serial bus) installation error calibration method based on improved particle filtering Download PDF

Info

Publication number
CN116295538A
CN116295538A CN202310589081.2A CN202310589081A CN116295538A CN 116295538 A CN116295538 A CN 116295538A CN 202310589081 A CN202310589081 A CN 202310589081A CN 116295538 A CN116295538 A CN 116295538A
Authority
CN
China
Prior art keywords
representing
usbl
time
representation
matrix
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
CN202310589081.2A
Other languages
Chinese (zh)
Other versions
CN116295538B (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.)
Hohai University HHU
Original Assignee
Hohai University HHU
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 Hohai University HHU filed Critical Hohai University HHU
Priority to CN202310589081.2A priority Critical patent/CN116295538B/en
Publication of CN116295538A publication Critical patent/CN116295538A/en
Application granted granted Critical
Publication of CN116295538B publication Critical patent/CN116295538B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H17/00—Networks using digital techniques
    • H03H17/02—Frequency selective networks
    • H03H17/0211—Frequency selective networks using specific transformation algorithms, e.g. WALSH functions, Fermat transforms, Mersenne transforms, polynomial transforms, Hilbert transforms
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/004—Artificial life, i.e. computing arrangements simulating life
    • G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computing Systems (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Data Mining & Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Computer Hardware Design (AREA)
  • Health & Medical Sciences (AREA)
  • Algebra (AREA)
  • Artificial Intelligence (AREA)
  • Biomedical Technology (AREA)
  • Biophysics (AREA)
  • Computational Linguistics (AREA)
  • Mathematical Optimization (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Molecular Biology (AREA)
  • General Engineering & Computer Science (AREA)
  • Software Systems (AREA)
  • Manufacturing & Machinery (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

The invention discloses a USBL (universal serial bus) installation error calibration method based on improved particle filtering, which belongs to the technical field of underwater robot combined positioning, and is based on two azimuth information and diagonal information output by a USBL and position information under a navigation system provided by a satellite positioning system; the absolute position of the transponder under the navigation system is firstly constructed into a system state space model; based on the constructed system state space model, carrying out state estimation based on an improved particle filter algorithm to obtain an optimal state estimation value; and carrying out smoothing variable structure filtering processing on the basis of the obtained optimal state estimation value. The method realizes the anti-interference robust filter estimation of the state quantity, and finally obtains a relatively accurate state vector.

Description

USBL (universal serial bus) installation error calibration method based on improved particle filtering
Technical Field
The invention belongs to the technical field of underwater robot navigation and positioning, and particularly relates to a USBL (universal serial bus) installation error calibration method based on improved particle filtering.
Background
An ultra-short baseline positioning system (USBL) is an underwater positioning device that utilizes acoustic principles, is capable of providing relatively accurate positional information for an underwater robot, and errors do not accumulate over time. The Strapdown Inertial Navigation System (SINS) is an autonomous navigation system based on an Inertial Measurement Unit (IMU), and has the characteristics of high updating frequency and comprehensive navigation data, but errors of the SINS are accumulated with time. Therefore, the SINS/USBL-based integrated navigation system can provide absolute position information for underwater machines.
In order to realize the high-precision SINS/USBL integrated navigation function, the installation error angle between the IMU and the USBL is estimated first, and then the installation error angle is compensated to the integrated navigation system to improve the navigation precision. Considering that the calibration model of the USBL is nonlinear, the commonly used USBL installation error angle calibration algorithm is an extended kalman filter, or an improved algorithm thereof.
Considering the complexity of the underwater environment, the traditional calibration algorithm based on the extended Kalman filtering has the problems of poor robustness, low calibration precision and the like, and severely restricts the navigation precision of the SINS/USBL.
Disclosure of Invention
Aiming at the problems, the invention provides a USBL installation error calibration method based on improved particle filtering, which comprises the steps of firstly, constructing a state space model, and constructing a nonlinear state equation and a measurement equation according to USBL output information; second, improved particle filtering algorithms are introduced. The traditional particle filter algorithm is improved by designing Euclidean distance and weight factors, and optimal estimation of state vectors is realized; and finally, designing a smooth variable structure filter based on the state vector estimation to realize anti-interference robust filter estimation of the state quantity, and finally obtaining a relatively accurate state vector (installation error angle information).
In order to achieve the above purpose, the technical scheme of the invention is as follows:
a USBL installation error calibration method based on improved particle filtering is based on the known quantity of USBL output information:
Figure SMS_1
wherein->
Figure SMS_2
And->
Figure SMS_3
Two orientations representing USBL outputs, respectivelyCorner information->
Figure SMS_4
Skew information representing USBL output; position information under navigation system provided by satellite positioning system>
Figure SMS_5
The method comprises the steps of carrying out a first treatment on the surface of the Absolute position of transponder in navigation system +.>
Figure SMS_6
The method is characterized by comprising the following steps of:
s1, constructing a system state space model;
s2, carrying out state estimation based on an improved particle filtering algorithm on the basis of the system state space model constructed in the step S1 to obtain an optimal state estimation value;
s3, performing smoothing variable structure filtering processing on the basis of the optimal state estimated value obtained in the step S2.
Further, the specific method of step S1 is:
firstly, a state space model of a USBL installation error calibration system is established, wherein the state space model comprises a state equation and a measurement equation:
Figure SMS_7
Figure SMS_8
representation ofkState vector of time of day->
Figure SMS_9
Representing a state transfer function>
Figure SMS_10
Representing a system noise matrix>
Figure SMS_11
Representation ofkTime measurement matrix->
Figure SMS_12
Representing the measurement transfer function, +.>
Figure SMS_13
Representation ofkMeasuring a noise matrix at the moment;
kstate vector of time of day
Figure SMS_14
The definition is as follows:
Figure SMS_15
wherein,,
Figure SMS_16
respectively represent the USBL under the carrier coordinate systemx、y、zMounting error angles in three directions, superscriptTRepresenting a transpose of the matrix;
ktime measurement matrix
Figure SMS_17
The definition is as follows:
Figure SMS_18
considering that the installation error angle is constant, its derivative is 0, so the state transfer function is expressed as follows:
Figure SMS_19
measuring transfer function
Figure SMS_20
Is represented by the expression:
Figure SMS_21
wherein,,
Figure SMS_22
representation ofRelative position of transponder in acoustic coordinate system, angle signaThe acoustic coordinate system is represented as follows:
Figure SMS_23
wherein, the corner marknIndicating navigation coordinate system, corner markbThe coordinate system of the carrier is represented,
Figure SMS_24
representing position information in a navigation coordinate system provided by a satellite positioning system as a known quantity; />
Figure SMS_25
Representing the absolute position of the transponder at the navigational coordinates, which is a known quantity; />
Figure SMS_26
Representing a posture transfer matrix of a carrier coordinate system and a navigation coordinate system; />
Figure SMS_27
Representing the installation error attitude matrix of the acoustic coordinate system and the carrier system, namely the parameter to be calibrated, < ->
Figure SMS_28
And state vector->
Figure SMS_29
The relationship of (2) is expressed as follows:
Figure SMS_30
further, the specific method of step S2 is as follows:
s21, initializing parameters
Definition of the definition
Figure SMS_31
A priori probability density function representing a state vector, wherein the state vector prior probability density function at zero time is used in initializing parameters>
Figure SMS_32
Generating a sample particle population->
Figure SMS_33
Since the number of the initialized particle sets is m=120, the number of the initialized particle sets is set to be m=120i=1, 2,; initializing the weight of the individual particles to +.>
Figure SMS_34
；
S22, importance sampling processing
The weights of the particles are calculated by adopting a Euclidean distance-based method, and the formula is as follows:
Figure SMS_35
wherein,,
Figure SMS_36
representation ofkTime of day (time)iWeight of individual particles->
Figure SMS_37
Representing the measurement noise covariance matrix->
Figure SMS_38
Representation ofkTime measurement matrix->
Figure SMS_39
Representation ofkTime of day (time)iState estimation values of individual particles, in order to avoid the case that the weight value of the particles is 0, the weight minimum value is set to 0.0000001;
weighting and normalizing the weight value of each particle obtained by calculation based on the Euclidean distance method:
Figure SMS_40
wherein,,
Figure SMS_41
representation weightingNormalized processedkTime of day (time)iWeight of individual particles->
Figure SMS_42
Represents the weight adjustment factor introduced, satisfy->
Figure SMS_43
；
S23, resampling processing
First, high-weight particles close to USBL measured values at the previous moment are reserved; secondly, randomly sampling particles with small weights in a measurement error range by taking USBL measured values at the previous moment as the mean value in an initializing mode, and finally reconstructing M=120 particle weight sets
Figure SMS_44
；
S24, calculating an optimal state estimated value
Figure SMS_45
Wherein,,
Figure SMS_46
representing the processed particles after the improved particle filtering algorithmkState estimation value of time.
Further, the specific method of step S3 is as follows:
s31, according to the processing of the improved particle filtering algorithmkState estimation value of time
Figure SMS_47
Corresponding pre-measurement and prior information are calculated, and the method specifically comprises the following steps:
Figure SMS_48
wherein,,
Figure SMS_49
representation ofkPredicted measurement value at +1, +.>
Figure SMS_50
Representation ofkA priori innovation at time +1, +.>
Figure SMS_51
Representing the measurement function->
Figure SMS_52
Representation ofkMeasurement information at +1;
s32, calculating a smoothing variable structure filter gain
Figure SMS_53
Wherein,,
Figure SMS_54
representation ofkSmooth variable structure filter gain at +1 time, < ->
Figure SMS_55
Diagonal element matrix representing a matrix>
Figure SMS_56
Representation ofkA priori information about the moment->
Figure SMS_57
Representation ofkA priori innovation at time +1, +.>
Figure SMS_58
Representing the saturation function of the device,
Figure SMS_59
representing the regulatory factor->
Figure SMS_60
Representing a smooth boundary layer, expressed as follows:
Figure SMS_61
wherein,,
Figure SMS_62
representing a measurement transfer matrix; />
Figure SMS_63
Representing a covariance matrix; />
Figure SMS_64
Representation ofkA measurement noise covariance matrix at +1 moment;
s33, state estimation and covariance matrix update
Figure SMS_65
Wherein,,
Figure SMS_66
representation ofkState estimation value at +1 time, +.>
Figure SMS_67
Representation ofkCovariance matrix at +1, +.>
Figure SMS_68
Representing a unit matrix with a diagonal of 1.
Compared with the prior art, the invention has the following advantages:
(1) Aiming at the problem of calibration of the installation error angle of the IMU and the USBL in the underwater navigation system, the particle filter algorithm is introduced into the calibration of the installation error of the USBL, and the calibration method of the installation error angle of the USBL based on the particle filter algorithm is designed on the basis of a nonlinear space model.
(2) Aiming at the problem of particle depletion in the traditional particle filtering algorithm, the Euclidean distance and the weight factor are introduced, and the diversity and the effectiveness of particles in the particle filtering algorithm are improved.
(3) According to the method, the smooth variable structure filtering algorithm is introduced into the particle filtering algorithm aiming at the problem of influence of the outside on the USBL installation error calibration system, and the anti-interference capability of the system is further improved by designing the gain matrix.
Drawings
FIG. 1 is a flow chart of a USBL installation error calibration method based on improved particle filtering as described in the present invention;
FIG. 2 is a diagram of a motion profile of a test carrier;
fig. 3 shows the result of the USBL installation error angle estimated by the method of the present invention.
Detailed Description
The present invention is further illustrated in the following drawings and detailed description, which are to be understood as being merely illustrative of the invention and not limiting the scope of the invention.
A USBL installation error calibration method based on improved particle filtering is based on the known quantity of USBL output information:
Figure SMS_69
wherein->
Figure SMS_70
And->
Figure SMS_71
Two azimuth information respectively representing USBL output,/->
Figure SMS_72
Skew information representing USBL output; position information under navigation system provided by satellite positioning system>
Figure SMS_73
The method comprises the steps of carrying out a first treatment on the surface of the Absolute position of transponder in navigation system +.>
Figure SMS_74
The method specifically comprises the following steps:
s1, constructing a system state space model, wherein the state space model construction is a foundation for realizing a filtering method. Therefore, first, a state space model (including a state equation and a measurement equation) of the USBL installation error calibration system is established as follows:
Figure SMS_75
Figure SMS_76
representation ofkState vector of time of day->
Figure SMS_77
Representing a state transfer function>
Figure SMS_78
Representing a system noise matrix>
Figure SMS_79
Representation ofkTime measurement matrix->
Figure SMS_80
Representing the measurement transfer function, +.>
Figure SMS_81
Representation ofkMeasuring a noise matrix at the moment;
kstate vector of time of day
Figure SMS_82
The definition is as follows:
Figure SMS_83
wherein,,
Figure SMS_84
respectively represent the USBL under the carrier coordinate systemx、y、zMounting error angles in three directions, superscriptTRepresenting a transpose of the matrix;
ktime measurement matrix
Figure SMS_85
The definition is as follows:
Figure SMS_86
considering that the installation error angle is constant, its derivative is 0, so the state transfer function is expressed as follows:
Figure SMS_87
measuring transfer function
Figure SMS_88
Is represented by the expression:
Figure SMS_89
wherein,,
Figure SMS_90
indicating the relative position of the transponder in the acoustic coordinate system, corner markaThe acoustic coordinate system is represented as follows:
Figure SMS_91
wherein, the corner marknIndicating navigation coordinate system, corner markbThe coordinate system of the carrier is represented,
Figure SMS_92
representing position information in a navigation coordinate system provided by a satellite positioning system as a known quantity; />
Figure SMS_93
Representing the absolute position of the transponder at the navigational coordinates, which is a known quantity; />
Figure SMS_94
Representing a posture transfer matrix of a carrier coordinate system and a navigation coordinate system; />
Figure SMS_95
Representing the installation error attitude matrix of the acoustic coordinate system and the carrier system, namely the parameter to be calibrated, < ->
Figure SMS_96
And state vector->
Figure SMS_97
The relationship of (2) is expressed as follows:
Figure SMS_98
。
s2, carrying out state estimation based on an improved particle filter algorithm on the basis of the system state space model constructed in the step S1 to obtain an optimal state estimation value, wherein the method specifically comprises the following steps of:
s21, initializing parameters
Definition of the definition
Figure SMS_99
A priori probability density function representing a state vector, wherein the state vector prior probability density function at zero time is used in initializing parameters>
Figure SMS_100
Generating a sample particle population->
Figure SMS_101
Since the number of the initialized particle sets is m=120, the number of the initialized particle sets is set to be m=120i=1, 2,; initializing the weight of the individual particles to +.>
Figure SMS_102
；
S22, importance sampling processing
The weights of the particles are calculated by adopting a Euclidean distance-based method, and the formula is as follows:
Figure SMS_103
wherein,,
Figure SMS_104
representation ofkTime of day (time)iWeight of individual particles->
Figure SMS_105
Representing the measurement noise covariance matrix->
Figure SMS_106
Representation ofkTime measurement matrix->
Figure SMS_107
Representation ofkTime of day (time)iState estimation values of individual particles, in order to avoid the case that the weight value of the particles is 0, the weight minimum value is set to 0.0000001;
weighting and normalizing the weight value of each particle obtained by calculation based on the Euclidean distance method:
Figure SMS_108
wherein,,
Figure SMS_109
representing the weighted and normalized productskTime of day (time)iWeight of individual particles->
Figure SMS_110
Represents the weight adjustment factor introduced, satisfy->
Figure SMS_111
In this embodiment, select +.>
Figure SMS_112
；
S23, resampling processing.
First, high weight particles (error within 1 m) that were close to the USBL measurement at the previous time are retained; secondly, randomly sampling particles with small weights (error is larger than 1M) in a measurement error range by taking USBL measured values at the previous moment as a mean value according to an initialization mode, and finally reconstructing M=120 particle weight sets
Figure SMS_113
；
S24, calculating an optimal state estimated value
Figure SMS_114
Wherein,,
Figure SMS_115
representing the processed particles after the improved particle filtering algorithmkState estimation value of time.
S3, considering external interference, the method carries out smoothing variable structure filtering processing on the basis of the optimal state estimated value obtained in the step S2, and the specific method is as follows:
s31, according to the processing of the improved particle filtering algorithmkState estimation value of time
Figure SMS_116
Corresponding pre-measurement and prior information are calculated, and the method specifically comprises the following steps:
Figure SMS_117
wherein,,
Figure SMS_118
representation ofkPredicted measurement value at +1, +.>
Figure SMS_119
Representation ofkA priori innovation at time +1, +.>
Figure SMS_120
Representing the measurement function->
Figure SMS_121
Representation ofkMeasurement information at +1;
s32, calculating a smoothing variable structure filter gain
Figure SMS_122
Wherein,,
Figure SMS_123
representation ofkSmooth variable structure filter gain at +1 time, < ->
Figure SMS_124
Diagonal element matrix representing a matrix>
Figure SMS_125
Representation ofkA priori information about the moment->
Figure SMS_126
Representation ofkA priori innovation at time +1, +.>
Figure SMS_127
Representing the saturation function of the device,
Figure SMS_128
representing the regulatory factor->
Figure SMS_129
Representing a smooth boundary layer, expressed as follows:
Figure SMS_130
wherein,,
Figure SMS_131
representing a measurement transfer matrix; />
Figure SMS_132
Representing a covariance matrix; />
Figure SMS_133
Representation ofkA measurement noise covariance matrix at +1 moment;
s33, state estimation and covariance matrix update
Figure SMS_134
Wherein,,
Figure SMS_135
representation ofkState estimation value at +1 time, +.>
Figure SMS_136
Representation ofkCovariance matrix at +1, +.>
Figure SMS_137
Representing a unit matrix with a diagonal of 1.
In order to verify the effectiveness of the method, an USBL installation error calibration method experiment based on improved particle filtering is carried out. The test apparatus comprises: test boats, USBL sensing equipment, etc. Wherein figure 2 is a diagram of the motion profile of a test carrier, which is moving circumferentially around an underwater transponder. Fig. 3 shows the result of the USBL installation error angle estimated by the method of the present invention, and it can be seen from the figure that the method of the present invention can effectively estimate the installation error angle information of the USBL in three directions.
It should be noted that the foregoing merely illustrates the technical idea of the present invention and is not intended to limit the scope of the present invention, and that a person skilled in the art may make several improvements and modifications without departing from the principles of the present invention, which fall within the scope of the claims of the present invention.

Claims (4)

1. A USBL installation error calibration method based on improved particle filtering is based on the known quantity of USBL output information:
Figure QLYQS_1
wherein->
Figure QLYQS_2
And->
Figure QLYQS_3
Two azimuth information respectively representing USBL output,/->
Figure QLYQS_4
Skew information representing USBL output; provided by satellite positioning systemsPosition information under navigation system->
Figure QLYQS_5
The method comprises the steps of carrying out a first treatment on the surface of the Absolute position of transponder in navigation system +.>
Figure QLYQS_6
The method is characterized by comprising the following steps of:
s1, constructing a system state space model;
s2, carrying out state estimation based on an improved particle filtering algorithm on the basis of the system state space model constructed in the step S1 to obtain an optimal state estimation value;
s3, performing smoothing variable structure filtering processing on the basis of the optimal state estimated value obtained in the step S2.
2. The USBL installation error calibration method based on improved particle filtering of claim 1, wherein the specific method of step S1 is as follows:
firstly, a state space model of a USBL installation error calibration system is established, wherein the state space model comprises a state equation and a measurement equation:
Figure QLYQS_7
Figure QLYQS_8
representation ofkState vector of time of day->
Figure QLYQS_9
Representing a state transfer function>
Figure QLYQS_10
Representing a system noise matrix>
Figure QLYQS_11
Representation ofkTime measurement matrix->
Figure QLYQS_12
Representing the measurement transfer function, +.>
Figure QLYQS_13
Representation ofkMeasuring a noise matrix at the moment;
kstate vector of time of day
Figure QLYQS_14
The definition is as follows:
Figure QLYQS_15
wherein,,
Figure QLYQS_16
respectively represent the USBL under the carrier coordinate systemx、y、zMounting error angles in three directions, superscriptTRepresenting a transpose of the matrix;
ktime measurement matrix
Figure QLYQS_17
The definition is as follows:
Figure QLYQS_18
considering that the installation error angle is constant, its derivative is 0, so the state transfer function is expressed as follows:
Figure QLYQS_19
measuring transfer function
Figure QLYQS_20
Is represented by the expression:
Figure QLYQS_21
wherein,,
Figure QLYQS_22
indicating the relative position of the transponder in the acoustic coordinate system, corner markaThe acoustic coordinate system is represented as follows:
Figure QLYQS_23
wherein, the corner marknIndicating navigation coordinate system, corner markbThe coordinate system of the carrier is represented,
Figure QLYQS_24
representing position information in a navigation coordinate system provided by a satellite positioning system as a known quantity; />
Figure QLYQS_25
Representing the absolute position of the transponder at the navigational coordinates, which is a known quantity;
Figure QLYQS_26
representing a posture transfer matrix of a carrier coordinate system and a navigation coordinate system; />
Figure QLYQS_27
Representing the installation error attitude matrix of the acoustic coordinate system and the carrier system, namely the parameter to be calibrated, < ->
Figure QLYQS_28
And state vector->
Figure QLYQS_29
The relationship of (2) is expressed as follows:
Figure QLYQS_30
。
3. the USBL installation error calibration method based on improved particle filtering of claim 1, wherein the specific method of step S2 is as follows:
s21, initializing parameters
Definition of the definition
Figure QLYQS_31
A priori probability density function representing a state vector, wherein the state vector prior probability density function at zero time is used in initializing parameters>
Figure QLYQS_32
Generating a sample particle population->
Figure QLYQS_33
Since the number of the initialized particle sets is m=120, the number of the initialized particle sets is set to be m=120i=1, 2,; initializing the weight of the individual particles to +.>
Figure QLYQS_34
；
S22, importance sampling processing
The weights of the particles are calculated by adopting a Euclidean distance-based method, and the formula is as follows:
Figure QLYQS_35
wherein,,
Figure QLYQS_36
representation ofkTime of day (time)iWeight of individual particles->
Figure QLYQS_37
Representing the measurement noise covariance matrix->
Figure QLYQS_38
Representation ofkTime measurement matrix->
Figure QLYQS_39
Representation ofkTime of day (time)iState estimation values of individual particles, in order to avoid the case that the weight value of the particles is 0, the weight minimum value is set to 0.0000001;
weighting and normalizing the weight value of each particle obtained by calculation based on the Euclidean distance method:
Figure QLYQS_40
wherein,,
Figure QLYQS_41
representing the weighted and normalized productskTime of day (time)iWeight of individual particles->
Figure QLYQS_42
Represents the weight adjustment factor introduced, satisfy->
Figure QLYQS_43
；
S23, resampling processing
First, high-weight particles close to USBL measured values at the previous moment are reserved; secondly, randomly sampling particles with small weights in a measurement error range by taking USBL measured values at the previous moment as the mean value in an initializing mode, and finally reconstructing M=120 particle weight sets
Figure QLYQS_44
；
S24, calculating an optimal state estimated value
Figure QLYQS_45
Wherein,,
Figure QLYQS_46
representing the processed particles after the improved particle filtering algorithmkState estimation value of time.
4. The USBL installation error calibration method based on improved particle filtering of claim 1, wherein the specific method of step S3 is as follows:
s31, according to the processing of the improved particle filtering algorithmkState estimation value of time
Figure QLYQS_47
Corresponding pre-measurement and prior information are calculated, and the method specifically comprises the following steps:
Figure QLYQS_48
wherein,,
Figure QLYQS_49
representation ofkPredicted measurement value at +1, +.>
Figure QLYQS_50
Representation ofkA priori innovation at time +1, +.>
Figure QLYQS_51
Representing the measurement function->
Figure QLYQS_52
Representation ofkMeasurement information at +1;
s32, calculating a smoothing variable structure filter gain
Figure QLYQS_53
Wherein,,
Figure QLYQS_54
representation ofkSmooth variable structure filter gain at +1 time, < ->
Figure QLYQS_55
Diagonal element matrix representing a matrix>
Figure QLYQS_56
Representation ofkA priori information about the moment->
Figure QLYQS_57
Representation of kA priori innovation at time +1, +.>
Figure QLYQS_58
Representing the saturation function of the device,
Figure QLYQS_59
representing the regulatory factor->
Figure QLYQS_60
Representing a smooth boundary layer, expressed as follows:
Figure QLYQS_61
wherein,,
Figure QLYQS_62
representing a measurement transfer matrix; />
Figure QLYQS_63
Representing a covariance matrix; />
Figure QLYQS_64
Representation of kA measurement noise covariance matrix at +1 moment;
s33, state estimation and covariance matrix update
Figure QLYQS_65
Wherein,,
Figure QLYQS_66
representation ofkState estimation value at +1 time, +.>
Figure QLYQS_67
Representation ofkCovariance matrix at +1, +.>
Figure QLYQS_68
Representing a unit matrix with a diagonal of 1.
CN202310589081.2A 2023-05-24 2023-05-24 USBL (universal serial bus) installation error calibration method based on improved particle filtering Active CN116295538B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310589081.2A CN116295538B (en) 2023-05-24 2023-05-24 USBL (universal serial bus) installation error calibration method based on improved particle filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310589081.2A CN116295538B (en) 2023-05-24 2023-05-24 USBL (universal serial bus) installation error calibration method based on improved particle filtering

Publications (2)

Publication Number Publication Date
CN116295538A true CN116295538A (en) 2023-06-23
CN116295538B CN116295538B (en) 2023-08-01

Family

ID=86832703

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310589081.2A Active CN116295538B (en) 2023-05-24 2023-05-24 USBL (universal serial bus) installation error calibration method based on improved particle filtering

Country Status (1)

Country Link
CN (1) CN116295538B (en)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101755307B1 (en) * 2016-05-11 2017-07-10 한국해양과학기술원 A position measurement error correcting method of underwater moving objects
CN108562287A (en) * 2018-01-08 2018-09-21 哈尔滨工程大学 A kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter
CN109613520A (en) * 2018-12-14 2019-04-12 东南大学 A kind of ultra-short baseline installation error online calibration method based on filtering
CN110531618A (en) * 2019-08-27 2019-12-03 河海大学 Closed loop based on effective key frame detects robot self-localization error cancelling method
CN114666732A (en) * 2022-03-15 2022-06-24 江苏科技大学 Moving target positioning resolving and error evaluation method under noisy network
CN115855049A (en) * 2023-02-07 2023-03-28 河海大学 SINS/DVL navigation method based on particle swarm optimization robust filtering

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101755307B1 (en) * 2016-05-11 2017-07-10 한국해양과학기술원 A position measurement error correcting method of underwater moving objects
CN108562287A (en) * 2018-01-08 2018-09-21 哈尔滨工程大学 A kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter
CN109613520A (en) * 2018-12-14 2019-04-12 东南大学 A kind of ultra-short baseline installation error online calibration method based on filtering
CN110531618A (en) * 2019-08-27 2019-12-03 河海大学 Closed loop based on effective key frame detects robot self-localization error cancelling method
CN114666732A (en) * 2022-03-15 2022-06-24 江苏科技大学 Moving target positioning resolving and error evaluation method under noisy network
CN115855049A (en) * 2023-02-07 2023-03-28 河海大学 SINS/DVL navigation method based on particle swarm optimization robust filtering

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
DI WANG 等: "A Novel Calibration Algorithm of SINS/USBL Navigation System Based on Smooth Variable Structure", 《 IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT》, pages 1 - 14 *
张国良: "基于大深度的SINS/DVL/USBL 组合导航技术研究", 《中国优秀硕士学位论文全文数据库》, pages 56 - 61 *

Also Published As

Publication number Publication date
CN116295538B (en) 2023-08-01

Similar Documents

Publication Publication Date Title
Li et al. Effective adaptive Kalman filter for MEMS-IMU/magnetometers integrated attitude and heading reference systems
Vasconcelos et al. Geometric approach to strapdown magnetometer calibration in sensor frame
Vasconcelos et al. A geometric approach to strapdown magnetometer calibration in sensor frame
Tabatabaei et al. A fast calibration method for triaxial magnetometers
Narasimhappa et al. Fiber-optic gyroscope signal denoising using an adaptive robust Kalman filter
CN113155129B (en) Holder attitude estimation method based on extended Kalman filtering
CN114061591B (en) Contour line matching method based on sliding window data backtracking
CN110702113B (en) Method for preprocessing data and calculating attitude of strapdown inertial navigation system based on MEMS sensor
CN111649747A (en) IMU-based adaptive EKF attitude measurement improvement method
CN110209180A (en) A kind of UAV navigation method for tracking target based on HuberM-Cubature Kalman filtering
Liu et al. Tightly coupled modeling and reliable fusion strategy for polarization-based attitude and heading reference system
CN112710304A (en) Underwater autonomous vehicle navigation method based on adaptive filtering
CN112461224A (en) Magnetometer calibration method based on known attitude angle
Fedele et al. Magnetometer bias finite-time estimation using gyroscope data
CN106595669B (en) Method for resolving attitude of rotating body
Lee et al. Interference-compensating magnetometer calibration with estimated measurement noise covariance for application to small-sized UAVs
CN110703205A (en) Ultrashort baseline positioning method based on adaptive unscented Kalman filtering
CN116295538B (en) USBL (universal serial bus) installation error calibration method based on improved particle filtering
CN110849364B (en) Adaptive Kalman attitude estimation method based on communication-in-motion
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
CN117118398A (en) Discrete quaternion particle filter data processing method based on self-adaptive likelihood distribution
CN110940334A (en) Badge and method for measuring speed of human walking
CN115451946A (en) Indoor pedestrian positioning method combining MEMS-IMU and Wi-Fi
CN115469314A (en) Uniform circular array steady underwater target azimuth tracking method and system
Zhu et al. A hybrid step model and new azimuth estimation method for pedestrian dead reckoning

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