Disclosure of Invention
Aiming at the problems in the prior art, the technical problem to be solved by the invention is to provide a distributed multi-source fusion positioning method based on FBPF-EIKF-FastSLAM, and the specific flow of the method is shown in FIG. 1.
The technical scheme comprises the following implementation steps:
(1) determining the weighted sum of the uniform PDFs p (x):
where x denotes a random variable, i denotes a count unit, N denotes the number of uniformly mixed PDFs, and w
iIndicates the probability density, [ x ] corresponding to the ith bin particle]Represents a box particle, U
[x]Represents PDF, [ x ] with bin particles as a support set
i]The (i) th bin of particles is represented,
(2) predicting probability density p (x) at time k +1k+1|z1,k):
At time k, assume xkThe PDF under the state is expressed as:
according to the time updating step, for the probability density at the k +1 moment:
in the formula, k represents time, x
kIs a system state vector, z
kIn order to observe the vector, the vector is,
the probability density of the ith bin particle at time k,
the state vector of the ith bin particle at time k,
the PDF for the support set at time k for the ith bin particle,
representing supporting header particles of
(ii) uniform PDF, [ f [)]To contain the function, [ V ]
k]A bin particle corresponding to state transition noise at time k +1, [ U
k]To control the input bin particles.
(3) Determination of State posterior PDFp (x)k+1|z1,k+1):
In the formula eta
k+1To normalize the coefficient, [ Z ]
k+1]Representing the actual observation of the bin particle, SS, at time k +1
ψiRepresented as a CSP, is shown to predict the bin particles and constrain them by the relationship of the observation function to the actual observed bin particles to eliminate the excess from the original bin particles.
The state value of the ith bin particle at the time k +1,
to be composed of
The constraint is carried out so that,
to be composed of
Confined box particle x
k+1In order to support the PDF of the set,
for a new box of particles after constraint, | [ X ]]And | is the volume of the box particle.
Selecting a proper threshold value, executing random subdivision resampling, and randomly selecting a certain one-dimensional state subinterval of the box particles obtained at the current moment for uniform division according to the resampling times, so that the box particles keep a proper size.
(4) Updating the fluorescence intensity of the box particle
In the formula (I), the compound is shown in the specification,
the fluorescence intensity of the ith bin at time k +1,
for the predicted observation of the ith bin particle at time k +1, [ Z ]
k+1]Is an actual observation. Instead of comparing the fluorescence intensity values between each bin of particles, a comparison is made using the actual observations and the predicted observations for each bin of particles.
(5) Updating global optimal box particle [ g ]k+1]:
Determining the degree of attraction β between the box particles:
wherein N (0,1) is a Gaussian distribution random vector having a mean value of 0 and a variance of 1,
as random weight terms, β
mIs the maximum attraction, gamma is the light intensity absorption coefficient, d
iThe spatial distance between the bin particle at time k +1 and the globally optimal bin particle. When the position update is completed, the fluorescence brightness value of the box particle is calculated and compared, and the global optimal box particle [ g ] is updated
k+1]:
In the formula, I ([ X ]) is the fluorescent brightness of the box particle.
(6) Updating bin particle positions
Where rand is a random number that is subject to uniform distribution. By updating the positions of the box particles, the box particles are guided to move towards the positions of the globally optimal box particles by utilizing the guiding function exerted by the attraction degree in the updating process. And setting the maximum iteration number, stopping the iteration of the algorithm when the fluorescence brightness function value is larger than the set iteration termination threshold value, and otherwise, continuing the iteration until the maximum iteration number is reached.
(7) Determining innovation and interval Kalman gain:
in the formula (I), the compound is shown in the specification,
to be novel, I is the identity matrix,
for actual observation, an uncertain interval vector is observed before, a common vector is observed after,
in order to observe the inclusion of the function,
the interval Kalman gain is represented by the gain of the interval Kalman,
in order to carry out the interval polymerization operation,
is a Jacobian matrix of intervals,
representing the observed noise interval covariance matrix. Updating the environment map construction:
in the formula (I), the compound is shown in the specification,
is a constrained interval position vector. And completing environmental characteristic estimation, observing and correlating data collected by the sensor, updating by using EIKF, and perfecting map information.
Compared with the prior art, the invention has the advantages that:
(1) the influence of the background noise of the complex environment on the positioning accuracy is effectively solved, and the filtering efficiency is improved.
(2) The method effectively improves the system operation efficiency, reduces the system response time, improves the system operation speed and accelerates the target positioning update frequency.
detailed description of the preferred embodiments
The present invention will be described in further detail below with reference to examples.
The data selected by the implementation case come from a typical demonstration area of an underground shielding space, 1000 groups of samples are shared in total, wherein 5 scenes including an underground tunnel, a railway tunnel, an underground mall, an underground parking lot and a subway station are provided, each scene has 200 groups of data, 140 groups of samples are extracted from each data in the 5 groups of scenes by adopting a random sampling method to serve as a training set, and the rest are used as a testing set. Finally, the total number of samples used as the training set is 700 and the total number of samples used as the test set is 300.
The overall flow of the distributed multi-source fusion positioning method provided by the invention is shown in figure 1, and the specific steps are as follows:
(1) determining the weighted sum of the uniform PDFs p (x):
in the formula, x represents a random variable, i represents a counting unit, N represents the number of uniformly mixed PDFs, the value is 20, and w
iIndicates the probability density, [ x ] corresponding to the ith bin particle]Represents a box particle, U
[x]Represents PDF, [ x ] with bin particles as a support set
i]The (i) th bin of particles is represented,
(2) predicting probability density p (x) at time k +1k+1|z1,k):
At time k, assume xkThe PDF under the state is expressed as:
according to the time updating step, for the probability density at the k +1 moment:
in the formula, k represents time, x
kIs a system state vector, z
kIn order to observe the vector, the vector is,
the probability density of the ith bin particle at time k,
the state vector of the ith bin particle at time k,
the PDF for the support set at time k for the ith bin particle,
representing supporting header particles of
(ii) uniform PDF, [ f [)]To contain the function, [ V ]
k]A bin particle corresponding to state transition noise at time k +1, [ U
k]To control the input bin particles.
(3) Determination of State posterior PDFp (x)k+1|z1,k+1):
In the formula eta
k+1To normalize the coefficient, [ Z ]
k+1]Indicating that the bin particle was actually observed at time k +1,
expressed as a CSP, expressed as a predicted box particle passage observation function andthe relation of actual observation box particles is restrained and used for eliminating redundant parts in the original box particles.
The state value of the ith bin particle at the time k +1,
to be composed of
The constraint is carried out so that,
to be composed of
Confined box particle x
k+1In order to support the PDF of the set,
for a new box of particles after constraint, | [ X ]]And | is the volume of the box particle.
Selecting a proper threshold value, executing random subdivision resampling, and randomly selecting a certain one-dimensional state subinterval of the box particles obtained at the current moment for uniform division according to the resampling times, so that the box particles keep a proper size.
(4) Updating the fluorescence intensity of the box particle
In the formula (I), the compound is shown in the specification,
the fluorescence intensity of the ith bin at time k +1,
for the predicted observation of the ith bin particle at time k +1, [ Z ]
k+1]Is an actual observation. Instead of comparing the fluorescence intensity values between each bin of particles, a comparison is made using the actual observations and the predicted observations for each bin of particles.
(5) Updating global optimal box particle [ g ]k+1]:
Determining the degree of attraction β between the box particles:
wherein N (0,1) is a Gaussian distribution random vector having a mean value of 0 and a variance of 1,
as random weight terms, β
mThe maximum attraction is 0.85, gamma is the light intensity absorption coefficient and is 1, and d
iThe spatial distance between the bin particle at time k +1 and the globally optimal bin particle. When the position update is completed, the fluorescence brightness value of the box particle is calculated and compared, and the global optimal box particle [ g ] is updated
k+1]:
In the formula, I ([ X ]) is the fluorescent brightness of the box particle.
(6) Updating bin particle positions
Where rand is a random number that is subject to uniform distribution. By updating the positions of the box particles, the box particles are guided to move towards the positions of the globally optimal box particles by utilizing the guiding function exerted by the attraction degree in the updating process. And setting the maximum iteration number, stopping the iteration of the algorithm when the fluorescence brightness function value is larger than the set iteration termination threshold value, and otherwise, continuing the iteration until the maximum iteration number is reached.
(7) Determining innovation and interval Kalman gain:
in the formula (I), the compound is shown in the specification,
to be novel, I is the identity matrix,
for actual observation, an uncertain interval vector is observed before, a common vector is observed after,
in order to observe the inclusion of the function,
the interval Kalman gain is represented by the gain of the interval Kalman,
in order to carry out the interval polymerization operation,
is a Jacobian matrix of intervals,
representing the observed noise interval covariance matrix. Updating the environment map construction:
in the formula (I), the compound is shown in the specification,
is a constrained interval position vector. And completing environmental characteristic estimation, observing and correlating data collected by the sensor, updating by using EIKF, and perfecting map information.
In order to verify the accuracy of the invention in positioning distributed multi-source information, four groups of multi-source information positioning experiments are carried out on the invention, and the experimental results are shown in fig. 3. As can be seen from fig. 3, the accuracy of the distributed multi-source fusion positioning method provided by the invention for positioning the distributed multi-source information is kept above 96%, and the method can achieve higher accuracy on the basis of ensuring stability, and has good positioning effect. The distributed multi-source fusion positioning method established by the invention is effective, provides a better method for establishing an accurate fusion positioning method model, and has certain practicability.