CN109141427B - EKF positioning method based on distance and angle probability model under non-line-of-sight environment - Google Patents

EKF positioning method based on distance and angle probability model under non-line-of-sight environment Download PDF

Info

Publication number
CN109141427B
CN109141427B CN201810992786.8A CN201810992786A CN109141427B CN 109141427 B CN109141427 B CN 109141427B CN 201810992786 A CN201810992786 A CN 201810992786A CN 109141427 B CN109141427 B CN 109141427B
Authority
CN
China
Prior art keywords
distance
line
sight
probability
base station
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
Application number
CN201810992786.8A
Other languages
Chinese (zh)
Other versions
CN109141427A (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.)
University of Shanghai for Science and Technology
Original Assignee
University of Shanghai for Science and Technology
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 University of Shanghai for Science and Technology filed Critical University of Shanghai for Science and Technology
Priority to CN201810992786.8A priority Critical patent/CN109141427B/en
Publication of CN109141427A publication Critical patent/CN109141427A/en
Application granted granted Critical
Publication of CN109141427B publication Critical patent/CN109141427B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mobile Radio Communication Systems (AREA)

Abstract

The invention provides an EKF positioning method based on distance and angle probability models in a non-line-of-sight environment, which is used for positioning a moving target (x, y) and is characterized by comprising the following steps: step 1, finding out sample points with non-line-of-sight factors among a plurality of base stations and the sample points; step 2, solving the probability of the sample points with non-line-of-sight factors in the distance interval; step 3, solving the probability of the sample points with non-line-of-sight factors in the angle interval; step 4, performing polynomial curve fitting to obtain distance-non-line-of-sight probability and angle-non-line-of-sight probability; step 5, find the ith base station (x)i,yi) The probability of non-line-of-sight factors with the moving target (x, y); step 6, solving a probability density function of each base station; step 7, estimating the position of the moving target (x, y); step 8, correcting the distance between the target and the base station according to the estimated position; and 9, predicting, correcting and updating the state of the moving target according to a Kalman filtering algorithm to complete positioning.

Description

EKF positioning method based on distance and angle probability model under non-line-of-sight environment
Technical Field
The invention relates to a positioning method, in particular to an EKF positioning method based on a distance and angle probability model in a non-line-of-sight environment.
Background
The mobile robot represents a higher level of mechanical-electrical integration, and the robot is widely applied on the premise of autonomous navigation, but higher navigation precision cannot be separated from higher positioning precision. The robot positioning and navigation functions can be well completed by arranging the sensor network in an indoor environment.
In the prior art, a plurality of distance information is usually used to form a plurality of distance equations, and a least square method is used to calculate coordinates of a target point, so that a residual error of the equation is minimum, but due to complexity of an indoor environment, a sensor network is shielded in a ranging process, and the like, the obtained distance information is greatly different from actual distance information, so that a large deviation exists in positioning information, and how to identify and weaken an error caused by non-line-of-sight according to the distance information of the sensor network, so that the problem that improvement of positioning accuracy in the non-line-of-sight environment is also an urgent need to be solved, and therefore, the EKF positioning method based on a distance and angle probability model in the non-line-of-sight environment is provided.
Disclosure of Invention
The present invention is made to solve the above problems, and an object of the present invention is to provide an EKF positioning method based on a distance and angle probability model in a non-line-of-sight environment.
The invention provides an EKF positioning method based on distance and angle probability models in a non-line-of-sight environment, which is used for positioning a moving target (x, y) and is characterized by comprising the following steps:
step 1, setting n base stations and a plurality of sample points, and setting the ith base station (x)i,yi) The actual distance from the jth sample point is denoted as dr,i,jAnd the direction angle is marked as thetar,i,jAnd the measured distance is recorded as zi,jMeasuring the distance zi,jSetting a threshold value and an actual distance d for taking an average value after multiple measurementsr,i,jAnd measuring the distance zi,jComparing the difference values to find out sample points with non-line-of-sight factors;
step 2, the ith base station (x)i,yi) The maximum distance from the plurality of sample points is denoted as drmax,iThe distance interval (0, d)rmax,i]Average division into ndiA distance between cells, wherein the ithdsA distance range between the cells is
Figure BDA0001781190380000021
The center point corresponding to each distance cell is
Figure BDA0001781190380000022
Will be the ithdsThe number of sample points in each distance cell is recorded as
Figure BDA0001781190380000023
The number of sample points in which non-line-of-sight factors exist is recorded as
Figure BDA0001781190380000024
Find at the idsProbability of non-line-of-sight factor among distance cells
Figure BDA0001781190380000025
Step 3, converting the direction angle thetar,i,jAngle interval (-pi, pi)]Average division into nθiAn angular cell, wherein the ithθsThe range between the angular cells is
Figure BDA0001781190380000026
IthθsThe center point corresponding to each angle cell is
Figure BDA0001781190380000027
Will be the ithθsThe number of sample points in each angular cell is recorded as
Figure BDA0001781190380000031
And the number of sample points in which the non-line-of-sight factor exists is recorded as
Figure BDA0001781190380000032
Find at the iθsProbability of non-line-of-sight factor among individual angular cells
Figure BDA0001781190380000033
Step 4, according to the central point d of each small distance intervalc,iCenter point theta of each angular cellc,iProbability of non-line-of-sight factor in each corresponding small-distance interval
Figure BDA0001781190380000034
And the probability of non-line-of-sight factors existing in each angular cell
Figure BDA0001781190380000035
Performing polynomial curve fitting to obtain distance-non-line-of-sight probability
Figure BDA0001781190380000036
Sum angle-non-line of sight probability
Figure BDA0001781190380000037
Step 5, obtaining the ith base station (x) according to the distance-non-line-of-sight probability and the angle-non-line-of-sight probabilityi,yi) Probability p of non-line-of-sight factor with target (x, y)i,nlos＝αi·pdi,nlos+(1-αi)·pθi,nlos；
Step 6, according to the probability density function of each base station in the line-of-sight environment
Figure BDA0001781190380000038
And probability density function in non-line-of-sight environment
Figure BDA0001781190380000039
Combining the ith base station (x)i,yi) Obtaining the probability density function of each base station after the probability of non-line-of-sight factors existing between the base station and the target (x, y)
p(zi)＝pnlos(zi)·pi,nlos+plos(zi)·(1-pi,nlos)，
Establishing a joint density function according to the probability density function of each base station
Figure BDA0001781190380000041
And 7, according to a distance correction formula:
Figure BDA0001781190380000042
to determine the position of the moving object (x, y), calculated
Figure BDA0001781190380000043
I.e. an optimal estimate of the position of the moving object (x, y),
Figure BDA0001781190380000044
estimated position including abscissa
Figure BDA0001781190380000045
And estimated position of ordinate
Figure BDA0001781190380000046
Step 8, according to the estimated position of the abscissa of the moving object (x, y)
Figure BDA0001781190380000047
And estimated position of ordinate
Figure BDA0001781190380000048
To correct the moving target (x, y) and the ith base station (x)i,yi) The number of base stations is n, and the distance correction formula is as follows:
Figure BDA0001781190380000049
step 9, constructing a prediction model of the moving target (x, y):
Figure BDA00017811903800000410
then, predicting, correcting and updating the state variable X of the moving target (X, y) by using a Kalman filtering algorithm, wherein the state variable X comprises the X coordinate, the y coordinate, the speed in the X direction and the speed in the y direction of the moving target (X, y),
a prediction stage: xk|k-1＝AXk-1|k-1，
Pk|k-1＝APk-1|k-1AT+Q，
In the above-mentioned formula, the compound of formula,
Figure BDA0001781190380000051
Xk-1|k-1＝[x(k-1),y(k-1),vx(k-1),vy(k-1)]T，Xk-1|k-1estimation of a state variable X for a time k-1, Xk|k-1For prediction of the state variable X at time k, Xk|k-1＝[xk|k-1，yk|k-1，vx，k|k-1，vy，k|k-1]T，Pk|k-1For prediction error covariance matrix at time k, Pk-1|k-1Is the corrected error covariance matrix at time k-1, Q is the process noise covariance matrix, which is set to the diagonal matrix,
a correction stage:
Figure BDA0001781190380000052
Figure BDA0001781190380000061
in the above formula, (x)k|k-1,yk|k-1) The state variable X of the moving object (X, y) at time k predicted from the prediction phasek|k-1Is obtained from h (X)k|k-1) For the predicted position (x) of the moving object (x, y) at time kk|k-1,yk|k-1) Distances from the n base stations, HkIs a vector h (X)k|k-1) The jacobian matrix of (a) is,
and (3) an updating stage:
Kk＝Pk|k-1Hk T(HkPk|k-1Hk T+R)，
Xk|k＝Xk|k-1+Kk(d′k-hk)，
Pk|k＝Pk|k-1-KkHkPk|k-1，
in the above formula, R is a measurement covariance matrix set as a diagonal matrix, and Kk obtained is a Kalman gain d'kFor the correction distance, h, determined in step 8kH (X) at time kk|k-1) The state vector X obtained in the update stagek|kI.e. the finally required state vector, the state vector comprises the x coordinate, the y coordinate, the x-direction speed and the y-direction speed of the moving target (x, y) at the time point of k,
wherein, a in step 4aiAnd abiFor coefficients after polynomial curve fitting, pdi,nlosAnd pθi,nlosAre each at a distance daiAnd angle thetabiThe probability of the presence of a non-line-of-sight factor,
in step 5
Figure BDA0001781190380000071
k0Is a constant that is positive in number,
Figure BDA0001781190380000072
θi＝a tan2(yi-y,xi-x)，
x (k) and v in step 9x(k) The position and speed of the moving target (X, y) in the X-axis direction in the world coordinate system at the time of k, y (k) and vy(k) The position and speed of the moving target (x, Y) in the Y-axis direction in the world coordinate system at time k.
The EKF positioning method based on the distance and angle probability model in the non-line-of-sight environment provided by the invention can also have the following characteristics: wherein the number of base stations is at least 4.
The EKF positioning method based on the distance and angle probability model in the non-line-of-sight environment provided by the invention can also have the following characteristics: the target (x, y) is provided with a tag for communicating with a base station, and the tag and the base station both use an ultra-wideband module occupying large bandwidth for communication.
The EKF positioning method based on the distance and angle probability model in the non-line-of-sight environment provided by the invention can also have the following characteristics: wherein the height of the target (x, y) and the height of the base station are known.
Action and Effect of the invention
According to the EKF positioning method based on the distance and angle probability models in the non-line-of-sight environment, the algorithm obtains the probability models based on the distance and the angle by sampling environment information, and determines the position of a moving target through the probability models, so that the positioning accuracy can be effectively improved; because the Kalman filtering algorithm is used for predicting, correcting and updating the state information of the moving target, the positioning accuracy of the moving target can be further enhanced; because the ultra-wideband module with large bandwidth is used between the base station and the moving target, the method can realize quick communication and complete the positioning of the moving target more quickly. Therefore, the EKF positioning method based on the distance and angle probability model under the non-line-of-sight environment can identify and weaken errors caused by non-line-of-sight, correct the state information of the moving target through the Kalman filtering algorithm, and effectively improve the positioning accuracy in the non-line-of-sight environment.
Detailed Description
In order to make the technical means and functions of the invention easy to understand, the invention is specifically described with reference to the following embodiments.
Example (b):
an EKF positioning method based on distance and angle probability models in non-line-of-sight environment is used for positioning a moving target (x, y), and is characterized by comprising the following steps: step 1, setting n base stations and a plurality of sample points, and setting the ith base station (x)i,yi) The actual distance from the jth sample point is denoted as dr,i,jAnd the direction angle is marked as thetar,i,jAnd the measured distance is recorded as zi,jMeasuring the distance zi,jSetting a threshold value and an actual distance d for taking an average value after multiple measurementsr,i,jAnd measuring the distance zi,jComparing the difference values to find out the sample points with non-line-of-sight factors, and when the obtained difference value is larger than a threshold value, the ith base station (x)i,yi) A non-line-of-sight factor exists between the current sample point and the jth sample point;
the number of base stations is at least 4.
And a tag used for communicating with the base station is arranged on the target (x, y), and the tag and the base station both use an ultra-wideband module occupying large bandwidth for communication.
The height of the target (x, y) and the height of the base station are both known.
Step 2, the ith base station (x)i,yi) The maximum distance from the plurality of sample points is denoted as drmax,iThe distance interval (0, d)rmax,i]Average division into ndiA distance between cells, wherein the ithdsA distance range between the cells is
Figure BDA0001781190380000091
The center point corresponding to each distance cell is
Figure BDA0001781190380000092
Will be the ithdsThe number of sample points in each distance cell is recorded as
Figure BDA0001781190380000093
The number of sample points in which non-line-of-sight factors exist is recorded as
Figure BDA0001781190380000094
Find at the idsProbability of non-line-of-sight factor among distance cells
Figure BDA0001781190380000095
Step 3, converting the direction angle thetar,i,jAngle interval (-pi, pi)]Average division into nθiAn angular cell, wherein the ithθsThe range between the angular cells is
Figure BDA0001781190380000096
IthθsThe center point corresponding to each angle cell is
Figure BDA0001781190380000097
Will be the ithθsThe number of sample points in each angular cell is recorded as
Figure BDA0001781190380000098
And the number of sample points in which the non-line-of-sight factor exists is recorded as
Figure BDA0001781190380000099
Find at the iθsProbability of non-line-of-sight factor among individual angular cells
Figure BDA00017811903800000912
Step 4, according to the central point d of each small distance intervalc,iCenter point theta of each angular cellc,iProbability of non-line-of-sight factor in each corresponding small-distance interval
Figure BDA00017811903800000910
And the probability of non-line-of-sight factors existing in each angular cell
Figure BDA00017811903800000911
Performing polynomial curve fitting to obtain distance-non-line-of-sight probability
Figure BDA0001781190380000101
Sum angle-non-line of sight probability
Figure BDA0001781190380000102
Step 5, obtaining the ith base station (x) according to the distance-non-line-of-sight probability and the angle-non-line-of-sight probabilityi,yi) Probability p of non-line-of-sight factor with target (x, y)i,nlos＝αi·pdi,nlos+(1-αi)·pθi,nlos；
Step 6, according to the probability density function of each base station in the line-of-sight environment
Figure BDA0001781190380000103
And probability density function in non-line-of-sight environment
Figure BDA0001781190380000104
Combining the ith base station (x)i,yi) Obtaining the probability density function of each base station after the probability of non-line-of-sight factors existing between the base station and the target (x, y)
p(zi)＝pnlos(zi)·pi,nlos+plos(zi)·(1-pi,nlos)，
Establishing a joint density function according to the probability density function of each base station
Figure BDA0001781190380000105
And 7, according to a distance correction formula:
Figure BDA0001781190380000106
to determine the position of the moving object (x, y), calculated
Figure BDA0001781190380000111
I.e. an optimal estimate of the position of the moving object (x, y),
Figure BDA0001781190380000112
estimated position including abscissa
Figure BDA0001781190380000113
And estimation of the ordinatePosition counting
Figure BDA0001781190380000114
Step 8, according to the estimated position of the abscissa of the moving object (x, y)
Figure BDA0001781190380000115
And estimated position of ordinate
Figure BDA0001781190380000116
To correct the moving target (x, y) and the ith base station (x)i,yi) The number of base stations is n, and the distance correction formula is as follows:
Figure BDA0001781190380000117
step 9, constructing a prediction model of the moving target (x, y):
Figure BDA0001781190380000118
then, predicting, correcting and updating the state variable X of the moving target (X, y) by using a Kalman filtering algorithm, wherein the state variable X comprises the X coordinate, the y coordinate, the speed in the X direction and the speed in the y direction of the moving target (X, y),
a prediction stage: xk|k-1＝AXk-1|k-1，
Pk|k-1＝APk-1|k-1AT+Q，
In the above-mentioned formula, the compound of formula,
Figure BDA0001781190380000121
Xk-1|k-1＝[x(k-1),y(k-1),vx(k-1),vy(k-1)]T，Xk-1|k-1estimation of a state variable X for a time k-1, Xk|k-1For prediction of the state variable X at time k, Xk|k-1＝[xk|k-1，yk|k-1，vx，k|k-1，vy，k|k-1]T，Pk|k-1For prediction error covariance matrix at time k, Pk-1|k-1Is the corrected error covariance matrix at time k-1, Q is the process noise covariance matrix, which is set to the diagonal matrix,
a correction stage:
Figure BDA0001781190380000122
Figure BDA0001781190380000123
in the above formula, (x)k|k-1,yk|k-1) The state variable X of the moving object (X, y) at time k predicted from the prediction phasek|k-1Is obtained from h (X)k|k-1) For the predicted position (x) of the moving object (x, y) at time kk|k-1,yk|k-1) Distances from the n base stations, HkIs a vector h (X)k|k-1) The jacobian matrix of (a) is,
and (3) an updating stage:
Kk＝Pk|k-1Hk T(HkPk|k-1Hk T+R)，
Xk|k＝Xk|k-1+Kk(d′k-hk)，
Pk|k＝Pk|k-1-KkHkPk|k-1，
in the above formula, R is a measurement covariance matrix set as a diagonal matrix, and Kk obtained is a Kalman gain d'kFor the correction distance, h, determined in step 8kH (X) at time kk|k-1)，
State vector X found in the update phasek|kI.e. the finally required state vector, the state vector comprises the x coordinate, the y coordinate, the x-direction speed and the y-direction speed of the moving target (x, y) at the time point of k,
wherein, a in step 4aiAnd abiFor coefficients after polynomial curve fitting, pdi,nlosAnd pθi,nlosAre each at a distance daiAnd angle thetabiThe probability of the presence of a non-line-of-sight factor,
in step 5
Figure BDA0001781190380000131
k0Is a constant that is positive in number,
Figure BDA0001781190380000132
θi＝a tan2(yi-y,xi-x)，
x (k) and v in step 9x(k) The position and speed of the moving target (X, y) in the X-axis direction in the world coordinate system at the time of k, y (k) and vy(k) The position and speed of the moving target (x, Y) in the Y-axis direction in the world coordinate system at time k.
Effects and effects of the embodiments
According to the EKF positioning method based on the distance and angle probability models in the non-line-of-sight environment, the algorithm acquires the probability models based on the distance and angle by sampling environment information, and determines the position of the moving target through the probability models, so that the positioning accuracy can be effectively improved; because the Kalman filtering algorithm is used for predicting, correcting and updating the state information of the moving target, the positioning accuracy of the moving target can be further enhanced; because the ultra-wideband module with large bandwidth is used between the base station and the moving target, the method can realize quick communication and complete the positioning of the moving target more quickly. Therefore, the EKF positioning method based on the distance and angle probability model in the non-line-of-sight environment can identify and weaken errors caused by non-line-of-sight, and can correct the state information of the moving target through the Kalman filtering algorithm, so that the positioning accuracy in the non-line-of-sight environment can be effectively improved.
The above embodiments are preferred examples of the present invention, and are not intended to limit the scope of the present invention.

Claims (4)

1. An EKF positioning method based on distance and angle probability models in non-line-of-sight environment is used for positioning a moving target (x, y), and is characterized by comprising the following steps:
step 1, setting n base stations and a plurality of sample points, and setting the ith base station (x)i,yi) The actual distance from the jth of the sample points is recorded as dr,i,jAnd the direction angle is marked as thetar,i,jAnd the measured distance is recorded as zi,jSaid measured distance zi,jSetting a threshold value and the actual distance d for taking the average value after multiple measurementsr,i,jAnd the measured distance zi,jComparing the difference values to find out the sample points with non-line-of-sight factors;
step 2, the ith base station (x)i,yi) The maximum distance from the plurality of sample points is recorded as drmax,iThe distance interval (0, d)rmax,i]Average division into ndiA distance between cells, wherein the ithdsA distance range between the distance cells is
Figure FDA0003297335020000011
Each distance cell corresponds to a central point of
Figure FDA0003297335020000012
Will be the ithdsThe number of the sample points in each distance cell is recorded as
Figure FDA0003297335020000013
Simultaneously recording the number of the sample points in which the non-line-of-sight factor exists
Figure FDA0003297335020000014
Find at the idsThe probability of the non-line-of-sight factor existing in each of the distance cells
Figure FDA0003297335020000015
Step 3, the direction angle theta is measuredr,i,jAngle interval (-pi, pi)]Average division into nθiAn angular cell, wherein the ithθsEach of the angular cells has a range of
Figure FDA0003297335020000016
IthθsThe central point corresponding to the angle cells is
Figure FDA0003297335020000021
Will be the ithθsThe number of the sample points in each angle cell is recorded as
Figure FDA0003297335020000022
And recording the number of the sample points in which the non-line-of-sight factor exists as
Figure FDA0003297335020000023
Find at the iθsThe probability of the non-line-of-sight factor existing in each of the angular cells
Figure FDA0003297335020000024
Step 4, according to the central point d between each distance cellc,iThe center point theta of each of the angular cellsc,iProbability of the presence of the non-line-of-sight factor in each of the corresponding range cells
Figure FDA0003297335020000025
And the probability of the non-line-of-sight factor existing in each of the angular cells
Figure FDA0003297335020000026
Performing polynomial curve fitting to obtain distance-non-line-of-sight probability
Figure FDA0003297335020000027
Sum angle-non-line of sight probability
Figure FDA0003297335020000028
Step 5, obtaining the ith base station (x) according to the distance-non-line-of-sight probability and the angle-non-line-of-sight probabilityi,yi) Probability p of the non-line-of-sight factor existing between the target (x, y)i,nlos＝αi·pdi,nlos+(1-αi)·pθi,nlos；
Step 6, according to the probability density function of each base station in the line-of-sight environment
Figure FDA0003297335020000029
And probability density function in non-line-of-sight environment
Figure FDA00032973350200000210
Incorporating the ith said base station (x)i,yi) Obtaining the probability density function of each base station after the probability of the non-line-of-sight factor existing between the mobile target (x, y)
p(zi)＝pnlos(zi)·pi,nlos+plos(zi)·(1-pi,nlos)，
Establishing a joint density function according to the probability density function of each base station
Figure FDA0003297335020000031
Step 7, determining a formula according to the position:
Figure FDA0003297335020000032
to determine the position of said moving object (x, y), calculated
Figure FDA0003297335020000033
I.e. an optimal estimate of the position of the moving object (x, y), including the estimated position of the abscissa
Figure FDA0003297335020000034
And estimated position of ordinate
Figure FDA0003297335020000035
Step 8, according to the obtained estimated position of the abscissa of the moving target (x, y)
Figure FDA0003297335020000036
And estimated position of ordinate
Figure FDA0003297335020000037
To correct said moving target (x, y) with the i-th said base station (x)i,yi) The number of the base stations is n, and the distance correction formula is as follows:
Figure FDA0003297335020000038
step 9, constructing a prediction model of the moving target (x, y):
Figure FDA0003297335020000039
then, predicting, correcting and updating the state variable X of the moving target (X, y) by using a Kalman filtering algorithm, wherein the state variable X comprises an X coordinate, a y coordinate, an X-direction speed and a y-direction speed of the moving target (X, y),
a prediction stage: xk|k-1＝AXk-1|k-1，
Pk|k-1＝APk-1|k-1AT+Q，
In the above-mentioned formula, the compound of formula,
Figure FDA0003297335020000041
Xk-1|k-1＝[x(k-1),y(k-1),vx(k-1),vy(k-1)]T，Xk-1|k-1estimation of the state variable X for the time k-1, Xk|k-1Prediction of the state variable X for time k, Xk|k-1＝[xk|k-1，yk|k-1，vx，k|k-1，vy，k|k-1]T，Pk|k-1For prediction error covariance matrix at time k, Pk-1|k-1Is the corrected error covariance matrix at time k-1, Q is the process noise covariance matrix, which is set to the diagonal matrix,
a correction stage:
Figure FDA0003297335020000042
Figure FDA0003297335020000051
in the above formula, (x)k|k-1,yk|k-1) The state variable X of the moving object (X, y) at time k predicted from the prediction phasek|k-1Is obtained from h (X)k|k-1) For the predicted position (x) of the moving object (x, y) at time kk|k-1,yk|k-1) Distances from the n base stations, HkIs a vector h (X)k|k-1) The jacobian matrix of (a) is,
and (3) an updating stage:
Kk＝Pk|k-1Hk T(HkPk|k-1Hk T+R)，
Xk|k＝Xk|k-1+Kk(dk′-hk)，
Pk|k＝Pk|k-1-KkHkPk|k-1，
in the above formula, R is a measurement covariance matrix set as a diagonal matrix, the obtained Kk is a Kalman gain, dk' is the correction distance, h, determined in said step 8kH (X) at time kk|k-1)，
The state vector X obtained in the update phasek|kNamely the finally needed state vector, the state vector comprises the x coordinate, the y coordinate, the speed in the x direction and the speed in the y direction of the moving target (x, y) at the time point of k,
wherein, a in the step 4aiAnd abiFor coefficients after polynomial curve fitting, pdi,nlosAnd pθi,nlosAre each at a distance daiAnd angle thetabiThe probability of the presence of the non-line-of-sight factor,
in said step 5
Figure FDA0003297335020000061
k0Is a constant that is positive in number,
Figure FDA0003297335020000062
θi＝atan2(yi-y,xi-x)，
x (k) and v in the step 9x(k) The position and the speed of the moving target (X, y) in the X-axis direction in the world coordinate system at the time of k, y (k) and vy(k) The position and the speed of the moving target (x, Y) in the Y-axis direction in the world coordinate system at the time k.
2. The EKF location method based on distance and angle probability model in non-line-of-sight environment as claimed in claim 1, wherein:
wherein the number of the base stations is at least 4.
3. The EKF location method based on distance and angle probability model in non-line-of-sight environment as claimed in claim 1, wherein:
and the mobile target (x, y) is provided with a tag for communicating with the base station, and the tag and the base station both use an ultra-wideband module occupying large bandwidth for communication.
4. The EKF location method based on distance and angle probability model in non-line-of-sight environment as claimed in claim 3, wherein:
wherein the height of the moving target (x, y) and the height of the base station are both known.
CN201810992786.8A 2018-08-29 2018-08-29 EKF positioning method based on distance and angle probability model under non-line-of-sight environment Active CN109141427B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810992786.8A CN109141427B (en) 2018-08-29 2018-08-29 EKF positioning method based on distance and angle probability model under non-line-of-sight environment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810992786.8A CN109141427B (en) 2018-08-29 2018-08-29 EKF positioning method based on distance and angle probability model under non-line-of-sight environment

Publications (2)

Publication Number Publication Date
CN109141427A CN109141427A (en) 2019-01-04
CN109141427B true CN109141427B (en) 2022-01-25

Family

ID=64828831

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810992786.8A Active CN109141427B (en) 2018-08-29 2018-08-29 EKF positioning method based on distance and angle probability model under non-line-of-sight environment

Country Status (1)

Country Link
CN (1) CN109141427B (en)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109947116B (en) * 2019-04-18 2022-10-18 北京主线科技有限公司 Positioning method and device for unmanned vehicle
CN110401915B (en) * 2019-08-27 2021-02-05 杭州电子科技大学 SEKF and distance reconstruction combined moving target positioning method under NLOS condition
CN110824453A (en) * 2020-01-10 2020-02-21 四川傲势科技有限公司 Unmanned aerial vehicle target motion estimation method based on image tracking and laser ranging
CN111537950B (en) * 2020-04-14 2023-04-21 哈尔滨工业大学 Satellite position prediction tracking method based on position fingerprint and two-step polynomial fitting
CN112710312B (en) * 2020-12-24 2023-12-01 长三角哈特机器人产业技术研究院 Laser SLAM front end matching method integrating distance residual error and probability residual error
CN113108791B (en) * 2021-03-05 2023-08-04 深圳大学 Navigation positioning method and navigation positioning equipment
CN115147475B (en) * 2022-09-02 2022-12-06 汕头大学 Target position positioning method, device, equipment and storage medium

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101483805A (en) * 2009-02-11 2009-07-15 江苏大学 Wireless positioning method under visual distance and non-visual distance mixed environment
CN101509969A (en) * 2009-03-31 2009-08-19 江苏大学 Wireless positioning method for combining Non-line-of-sight error elimination and motion state estimation
CN101526605A (en) * 2009-03-31 2009-09-09 江苏大学 Robust positioning method with non-visual-range error elimination function
CN102088769A (en) * 2010-12-23 2011-06-08 南京师范大学 Wireless location method for directly estimating and eliminating non-line-of-sight (NLOS) error
CN103618997A (en) * 2013-11-22 2014-03-05 北京邮电大学 Indoor positioning method and device based on signal intensity probability
CN106885575A (en) * 2017-02-17 2017-06-23 浙江工商职业技术学院 A kind of indoor positioning bootstrap technique and system
CN107817469A (en) * 2017-10-18 2018-03-20 上海理工大学 Indoor orientation method is realized based on ultra-wideband ranging under nlos environment

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7383053B2 (en) * 2004-04-28 2008-06-03 Lawrence Livermore National Security, Llc Position estimation of transceivers in communication networks
US8966390B2 (en) * 2009-12-29 2015-02-24 Nokia Corporation Method and apparatus for visually indicating location probability

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101483805A (en) * 2009-02-11 2009-07-15 江苏大学 Wireless positioning method under visual distance and non-visual distance mixed environment
CN101509969A (en) * 2009-03-31 2009-08-19 江苏大学 Wireless positioning method for combining Non-line-of-sight error elimination and motion state estimation
CN101526605A (en) * 2009-03-31 2009-09-09 江苏大学 Robust positioning method with non-visual-range error elimination function
CN102088769A (en) * 2010-12-23 2011-06-08 南京师范大学 Wireless location method for directly estimating and eliminating non-line-of-sight (NLOS) error
CN103618997A (en) * 2013-11-22 2014-03-05 北京邮电大学 Indoor positioning method and device based on signal intensity probability
CN106885575A (en) * 2017-02-17 2017-06-23 浙江工商职业技术学院 A kind of indoor positioning bootstrap technique and system
CN107817469A (en) * 2017-10-18 2018-03-20 上海理工大学 Indoor orientation method is realized based on ultra-wideband ranging under nlos environment

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
A Line-of-Sight probability model for VANETs;Christina Stadler,et.al;《2017 13th International Wireless Communications and Mobile Computing Conference (IWCMC)》;20171231;第466-471页 *
基于基站LOS鉴别的TDOA室内定位算法;张雅等;《云南大学学报(自然科学版)》;20171130;第39卷(第6期);第967-972页 *
融合异质传感器信息的机器人精准室内定位;张彬等;《控制工程》;20180731;第25卷(第7期);第1335-1343页 *

Also Published As

Publication number Publication date
CN109141427A (en) 2019-01-04

Similar Documents

Publication Publication Date Title
CN109141427B (en) EKF positioning method based on distance and angle probability model under non-line-of-sight environment
CN108490473B (en) GNSS and UWB integrated unmanned aerial vehicle enhanced positioning method and system
CN109548141B (en) Indoor environment base station coordinate position calibration method based on Kalman filtering algorithm
CN107817469B (en) Indoor positioning method based on ultra-wideband ranging in non-line-of-sight environment
EP2584372B1 (en) RSS based positioning method with limited sensitivity receiver
US20160066157A1 (en) Method and apparatus for real-time, mobile-based positioning according to sensor and radio frequency measurements
CN105704652B (en) Fingerprint base acquisition and optimization method in a kind of positioning of WLAN/ bluetooth
CN108307301B (en) Indoor positioning method based on RSSI ranging and track similarity
CN113074727A (en) Indoor positioning navigation device and method based on Bluetooth and SLAM
CN108414974B (en) Indoor positioning method based on ranging error correction
KR20120003578A (en) Indoor localization system and method
CN110806561B (en) Self-calibration method for multiple base stations
CN106932752A (en) A kind of real-time indoor orientation method based on RF phse monitoring
CN111198567B (en) Multi-AGV collaborative dynamic tracking method and device
CN110907903B (en) Self-adaptive tracking processing method based on multiple sensors
CN109856616A (en) A kind of radar fix relative systematic error modification method
Sun et al. Simultaneous WiFi ranging compensation and localization for indoor NLoS environments
US9258679B1 (en) Modifying a history of geographic locations of a computing device
CN109041209B (en) Wireless sensor network node positioning error optimization method based on RSSI
CN107371133A (en) A kind of method for improving architecture precision
CN113324544A (en) Indoor mobile robot co-location method based on UWB/IMU (ultra wide band/inertial measurement unit) of graph optimization
CN112540345A (en) Dual-model positioning method and system for detecting UWB quality based on Gaussian distribution
CN108848447B (en) Differential DV _ Distance node positioning method adopting unknown node correction
CN114521014B (en) Method for improving positioning precision in UWB positioning process
CN113484822B (en) Wireless signal compensation method, system, computer 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