Vector particle filter method for range finding signal slam

Abstract

The application discloses a kind of vector particle filtering methods for ranging signal SLAM, comprising the following steps: running N groups of particle filters for N ranging signals, estimating signal source position and user positioning;Particle filter carries out filtering estimation, and the value of each particle is updated according to the motion model of user and signal source;The transient weight value of all particles of each group of particle filters is calculated;The transient mean and transient covariance of user positioning state and signal source position state in each group of particle filters are calculated respectively;The auxiliary mean and auxiliary covariance of each group of particle filters are calculated;Each group of particle filters is updated with joint weight: each group of particle filters calculates the local state quantity and local covariance of corresponding user positioning and signal source position;Resampling is carried out on each group of particle filters;Estimate N filters, output the state quantity and covariance of user positioning.This application can reduce the difficulty of particle sampling, improve the real-time performance of algorithm, and is suitable for practical engineering application.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of positioning and navigation, and particularly relates to a vector particle filtering method for SLAM of ranging signals. BACKGROUND

[0002] Simultaneous localization and mapping (SLAM) is an application for positioning a user and estimating features of a surrounding environment, and SLAM based on ranging signals is an important application environment, which is usually implemented by using a particle filtering technology. In an actual navigation positioning scene, a user can receive multiple ranging signals, which brings difficulty to particle sampling of the particle filtering, and a large number of particles are required to represent the user positioning and the signal source position, which directly affects the real-time performance of the SLAM system. SUMMARY

[0003] The application aims to provide a vector particle filtering method for SLAM of ranging signals, so as to reduce the difficulty of particle sampling and improve the real-time performance of the system.

[0004] To achieve the above object, the application provides a vector particle filtering method for SLAM of ranging signals, which comprises the following steps:

[0005] S1: running N sets of particle filters for N ranging signals to estimate the signal source position and the user positioning;

[0006] S2: performing filtering estimation by the particle filters, and updating the value of each particle according to the motion model of the user and the signal source;

[0007] S3: calculating the transient weight value of all particles of each set of particle filters;

[0008] S4: calculating the transient mean value and the transient covariance of the user positioning state and the signal source position state in each set of particle filters based on the transient weight value of step S3;

[0009] S5: calculating the auxiliary mean value and the auxiliary covariance of each set of particle filters based on the transient mean value and the transient covariance of the user positioning state in step S4;

[0010] S6: performing joint weight updating on each set of particle filters based on the transient mean value and the transient covariance of the user positioning state in step S4 and the auxiliary mean value and the auxiliary covariance of step S5:

[0011] S7: calculating the local state quantity and the local covariance of the corresponding user positioning and signal source position for each set of particle filters based on the joint weight of step S6;

[0012] S8: resampling each set of particle filters;

[0013] S9: Fusion estimation of N sets of filters based on the local state quantity and local covariance of user positioning calculated in step S7, outputting the state quantity and covariance of user positioning.

[0014] As a further improvement of the present application, in step S1, taking the i-th particle filter at time t as an example, the n-th particle contains wherein represents the state quantity of user positioning, represents the state quantity of position of the i-th signal source, represents the weight of the particle, i = 1, 2, …, N.

[0015] As a further improvement of the present application, in step S3, taking the transient weight value of the n-th particle of the i-th particle filter at time t as an example, the calculation method is

[0016]

[0017] wherein, is the transient observation covariance of the particle, is the i-th ranging signal at time t, is the estimated value of the i-th ranging signal calculated based on .

[0018] As a further improvement of the present application, in step S4, taking the i-th particle filter at time t as an example, the transient mean value of the state of user positioning is , and the transient covariance is , the transient mean value of the state of position of the signal source is , and the transient covariance is wherein, K i is the number of particles of the i-th particle filter.

[0019] As a further improvement of the present application, in step S5, taking the i-th particle filter at time t as an example, the auxiliary mean value is , and the auxiliary covariance is wherein, the subscript i represents that the corresponding variable excludes the i-th ranging signal, Z \i represents the set of all ranging signals received by the user except the i-th ranging signal, is the transient covariance of the state of user positioning of the j-th particle filter calculated in step S4; is the weight of the j-th particle filter.

[0020] As a further improvement of the present application, in step S6, taking the joint weight update of the n-th particle of the i-th particle filter at time t as an example, the updated joint weight is wherein,​ the auxiliary observation covariance of the particle,

[0021] As a further improvement of the application, in step S7, taking the i th particle filter at time t as an example, the local state quantity of user positioning is The local covariance is The local state quantity of the signal source position is The local covariance is

[0022] As a further improvement of the application, in step S9, taking time t as an example, the output user positioning state quantity is The covariance is Wherein, is the weight of the i th particle filter, which is obtained by formula , and the symbol [] l,l represents the l th row and l th column element of the corresponding matrix.

[0023] The beneficial effects of the present application are: compared with the prior art, the present application vectorizes the traditional high-dimensional particle filter into a group of low-dimensional particle filters, and realizes joint weight update through the auxiliary mean and auxiliary covariance of each group of particle filters, which can reduce the difficulty of particle sampling, improve the real-time performance of the algorithm, and is suitable for practical engineering application. BRIEF DESCRIPTION OF DRAWINGS

[0024] Figure 1 is the overall method flowchart of the present application.

[0025] Figure 2 is the flowchart of a single particle filter. DETAILED DESCRIPTION

[0026] In order to make the purpose, technical scheme and advantages of the present application clearer, the present application will be described in detail below in combination with the drawings and specific embodiments.

[0027] As Figure 1 and Figure 2 shown, the present application provides a vector particle filter method for ranging signal SLAM, comprising the following steps:

[0028] S1: running N groups of particle filters for N ranging signals to estimate the corresponding signal source position and user positioning;

[0029] S2: the particle filter performs filtering estimation, and updates the value of each particle according to the motion model of the user and the signal source;

[0030] S3: calculating the transient weight value of all particles of each group of particle filters;

[0031] S4: Calculate the transient mean and the transient covariance of the user positioning state and the signal source position state in each particle filter respectively based on the transient weight value of step S3;

[0032] S5: Calculate the auxiliary mean and the auxiliary covariance of each particle filter based on the transient mean and the transient covariance of the user positioning state in step S4;

[0033] S6: Perform joint weight update for each particle filter based on the transient mean and the transient covariance of the user positioning state in step S4, and the auxiliary mean and the auxiliary covariance of step S5:

[0034] S7: Calculate the local state quantity and the local covariance of the corresponding user positioning and signal source position for each particle filter based on the joint weight of step S6;

[0035] S8: Resample each particle filter;

[0036] S9: Fuse the N filters to output the state quantity and the covariance of the user positioning based on the local state quantity and the local covariance of the user positioning calculated in step S7.

[0037] The steps S1-S9 will be illustrated below.

[0038] In step S1, take the ith particle filter at time t as an example, the nth particle thereof contains wherein represents the positioning state quantity of the user, represents the position state quantity of the ith signal source, represents the weight of the particle, i = 1, 2, …, N.

[0039] In step S3, take the transient weight value of the nth particle of the ith particle filter at time t as an example, which is calculated as

[0040]

[0041] wherein, is the transient observation covariance of the particle, is the ith ranging signal at time t, is the ith ranging signal estimation value calculated based on .

[0042] In step S4, take the ith particle filter at time t as an example, the transient mean of the user positioning state is and the transient covariance is the transient mean of the signal source position state is and the transient covariance is ​wherein, K i is the number of particles of the i th particle filter.

[0043] In step S5, taking the i th particle filter at time t as an example, the auxiliary mean is The auxiliary covariance is wherein, the subscript i represents that the corresponding variable excludes the i th ranging signal, Z \i represents the set of all ranging signals received by the user except the i th ranging signal, is the transient covariance of the user positioning state of the j th particle filter calculated in step S4; is the weight of the j th particle filter.

[0044] In step S6, taking the joint weight update of the n th particle of the i th particle filter at time t as an example, the updated joint weight is wherein, is the auxiliary observation covariance of the particle,

[0045] In step S7, taking the i th particle filter at time t as an example, the local state quantity of the user positioning is The local covariance is The local state quantity of the signal source position is The local covariance is

[0046] In step S9, taking time t as an example, the output user positioning state quantity is The covariance is wherein, is the weight of the i th particle filter, which is obtained by formula The symbol [] l, l represents the l th row and l th column element of the corresponding matrix.

[0047] In summary, the present application vectorizes the traditional high-dimensional particle filter into a group of low-dimensional particle filters, and realizes joint weight update through the auxiliary mean and auxiliary covariance of each group of particle filters, which can reduce the difficulty of particle sampling, improve the real-time performance of the algorithm, and is suitable for practical engineering application.

[0048] The above examples are only used to illustrate the technical solutions of the present application and not to limit it, although the present application has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present application can be modified or replaced equivalently without departing from the spirit and scope of the technical solutions of the present application.

Claims

1. A vector particle filtering method for ranging signal SLAM, characterized in that, Includes the following steps: S1: Run N sets of particle filters for N ranging signals to estimate the signal source location and user positioning; S2: The particle filter performs filtering estimation and updates the value of each particle based on the motion model of the user and the signal source; S3: Calculate the transient weight values ​​of all particles in each group of particle filters; S4: Based on the transient weight values ​​in step S3, calculate the transient mean and transient covariance of the user positioning state and the signal source position state in each group of particle filters. S5: Based on the transient mean and transient covariance of the user's positioning status in step S4, calculate the auxiliary mean and auxiliary covariance of each group of particle filters. S6: Based on the transient mean and transient covariance of the user's positioning state in step S4, and the auxiliary mean and auxiliary covariance in step S5, perform joint weight updates for each group of particle filters: S7: Based on the joint weights in step S6, each group of particle filters calculates the local state variables and local covariance of the corresponding user location and signal source location. S8: Resample each group of particle filters; S9: Based on the local state variables and local covariance of the user location calculated in step S7, fuse and estimate N sets of filters, and output the state variables and covariance of the user location.

2. The vector particle filtering method for ranging signal SLAM according to claim 1, characterized in that: In step S1, taking the i-th particle filter at time t as an example, its n-th particle contains in This represents the user's location status. This represents the position state of the i-th signal source. The weight of the particle is represented by i = 1, 2, ..., N.

3. The vector particle filtering method for ranging signal SLAM according to claim 2, characterized in that: In step S3, the transient weight value of the nth particle of the i-th particle filter at time t is used. For example, its calculation method is as follows: in, The transient observation covariance of the particle. Let i be the ranging signal at time t. For based on The estimated value of the i-th ranging signal is calculated.

4. The vector particle filtering method for ranging signal SLAM according to claim 3, characterized in that: In step S4, taking the i-th particle filter at time t as an example, its transient mean of user positioning state is Transient covariance is The transient mean of the signal source position state is Transient covariance is Among them, K i Let be the number of particles in the i-th particle filter.

5. The vector particle filtering method for ranging signal SLAM according to claim 4, characterized in that: In step S5, taking the i-th particle filter at time t as an example, its auxiliary mean is... Auxiliary covariance is Where the subscript \i represents the variable that excludes the i-th ranging signal, Z \i This represents the set of all ranging signals received by the user, excluding the i-th ranging signal. The transient covariance of the user positioning state of the j-th particle filter calculated in step S4; Let be the weight of the j-th particle filter.

6. The vector particle filtering method for ranging signal SLAM according to claim 5, characterized in that: In step S6, taking the joint weight update of the nth particle of the i-th particle filter at time t as an example, the updated joint weight is: in, The auxiliary observation covariance of this particle, 7. The vector particle filtering method for ranging signal SLAM according to claim 6, characterized in that: In step S7, taking the i-th particle filter at time t as an example, the local state variable of the user positioning is: Local covariance is The local state variables of the signal source location are Local covariance is 8. The vector particle filtering method for ranging signal SLAM according to claim 7, characterized in that: In step S9, taking time t as an example, the output user location status quantity is: covariance is in, The weights for filtering the i-th particle are determined by the formula... Calculated, symbol [] l,l This represents the element in the l-th row and l-th column of the corresponding matrix.

Images