CN109548141A - Indoor environment base station coordinates position calibration method based on Kalman filtering algorithm - Google Patents
Indoor environment base station coordinates position calibration method based on Kalman filtering algorithm Download PDFInfo
- Publication number
- CN109548141A CN109548141A CN201811296954.6A CN201811296954A CN109548141A CN 109548141 A CN109548141 A CN 109548141A CN 201811296954 A CN201811296954 A CN 201811296954A CN 109548141 A CN109548141 A CN 109548141A
- Authority
- CN
- China
- Prior art keywords
- base station
- target
- label
- distance
- indicate
- 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
Links
Classifications
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
- H04W64/006—Locating users or terminals or network equipment for network management purposes, e.g. mobility management with additional information processing, e.g. for direction or speed determination
Landscapes
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
The invention discloses a kind of indoor environment base station coordinates position calibration method based on Kalman filtering algorithm, this method construct indoor locating system by wireless sensor, solve the horizontal distance between label and base station using geometrical relationship;Synchronised clock is arranged in all base stations and label, and base station sends the wireless signal comprising sending instant timestamp around, and label receives the wireless signal, calculates the distance between label and base station by the time difference;Base station number and coordinate are arranged in setting in space, and four coordinate points for being linked to be rectangle are demarcated in space, and mobile target is moved along coordinate points, and obtains the measurement range information between mobile target and all base stations in each sampling period;It is measured at a distance from each base station using mobile target in initial point, according to a preliminary estimate the position of each base station;The co-ordinate position information of base station after two step expanded Kalman filtration algorithms are optimized.This method can accurately complete base station location under indoor complex environment and demarcate, and improve positioning accuracy.
Description
Technical field
The present invention relates to a kind of indoor environment base station coordinates position calibration method based on Kalman filtering algorithm.
Background technique
Mobile robot represents the higher level of electromechanical integration, the extensive use of the robot using independent navigation as
Premise, and navigation accuracy be unable to do without higher positioning accuracy.Placement sensor network can preferably complete machine in environment indoors
The positioning and navigation feature of device people.
In the prior art, multiple range informations are generallyd use and form multiple range equations, find out mesh using least square method
The coordinate of punctuate, so that the residual error of equation is minimum;But due to the complexity of indoor environment, sensor network is in ranging
Due to noise jamming in journey, obtained range information can generate larger difference with actual range information, to can to position
There are relatively large deviations for information.Therefore, there are measurement error, it is urgently to be resolved to guarantee that positioning accuracy just becomes the present invention
The problem of.
Current most location algorithm is intended to known base station position, to estimate the location information of measured target.However
In biggish space, calibration base station location becomes an important problem, and most classic method is will to move target to be placed on three
In a fixation and known position, then the position of base station is estimated with 3 mensurations.It, should due to the presence of noise
Kind method can not obtain very accurate location information, and positioning accuracy is poor.
Summary of the invention
Technical problem to be solved by the invention is to provide a kind of, and the indoor environment base station based on Kalman filtering algorithm is sat
Cursor position scaling method, this method overcome the defect of conventional mapping methods, can accurately complete under indoor complex environment
Base station location calibration, and anti-interference ability is stronger, effectively improves positioning accuracy.
In order to solve the above technical problems, the present invention is based on the indoor environment base station coordinates location positions of Kalman filtering algorithm
Method includes the following steps:
Step 1: indoor locating system of the setting based on wireless sensor network, wherein single goal positioning is by a label
It is formed with multiple base stations, label and base station are all made of ultra wide band module and realize the transmission of ultra-wideband information, and all base stations are mounted on together
On one horizontal plane, by positioning target and the fixed height of the horizontal plane, label is installed to mobile robot, using right angled triangle
Geometrical relationship solves the horizontal distance between label and base station:
Wherein, d is the distance between label and base station, and Δ h is the difference in height of label and base station, dIt is horizontalFor label and base
The horizontal distance stood;
Step 2: ranging uses TOF principle between each base station and label, when all base stations is synchronous with label setting
Clock, base station send wireless signal around, include the timestamp of sending instant in the wireless signal, and label receives the wireless communication
Number when, be compared according to the timestamp of label with the timestamp received in wireless messages, by the time difference calculate label with
The distance between base station:
D=c (treceive-tsend) (2)
Wherein, d is the distance between label and base station, and c is the spread speed of light in a vacuum, treceiveTo receive nothing
The time of line signal, tsendFor the time for sending wireless signal;
Step 3: there are noises for the distance between label and base station information, it is described as follows:
Dis (t)=dreal(t)+n(t) (3)
Wherein, dis (t) is the distance value measured by t moment, drealIt (t) is the actual distance of t moment, n (t) is full
Sufficient mean value is 0, variance σ2Gaussian random variable;
Step 4: assuming that arrangement base station number is n in space, and i-th of base station coordinates is (xi,yi), in space
Four coordinate points are demarcated, four coordinate points are expressed as (xa,ya), (xb,yb), (xc,yc), (xd,yd), and meet ya=yb,
xb=xc, yc=yd, xa=xd, i.e. four coordinate points are linked to be rectangle;
Step 5: four mobile targets are individually positioned in four calibration coordinate points, and move along a straight line counterclockwise
To next calibration point, i.e. target A is from (xa,ya) point move to (xb,yb) point, target B is from (xb,yb) point move to (xc,yc)
Point, target C is from (xc,yc) point move to (xd,yd) point, target D is from (xd,yd) point move to (xa,ya) point, and four targets
Movement velocity is at the uniform velocity;In target motion process, each sampling period obtains each movement target and all base stations
Between measurement range information dM,i,k, wherein dM,i,kIndicate target M in the measurement distance at k moment and i-th of base station;
Step 6: the position of each base station according to a preliminary estimate, using four targets respectively in initial point measurement and each base
The distance d to standA,i,0, dB,i,0, dC,i,0, dD,i,0, tentatively seek the location information of each base station using least square method, i-th
A base station and the range formula equation group of four targets are as follows:
First three equation of formula (4) is subtracted each other with the 4th equation respectively, arrangement obtains following formula:
A0Yi=B0 (5)
Wherein,
Yi=[xi,yi]T, wherein [*]TThe transposed matrix of representing matrix [*],
Y is solved using least square methodi, solution formula is as follows:
Initial point (the x of each base station is tentatively acquired according to formula (6)i,0,yi,0);
Step 7: defining dbjective state vector is X=[x, y, vx,vy]T, wherein x and y indicates the position coordinates of target, vx
And vyIndicate target the direction x and the direction y speed,
Then the state vector of four targets is respectively defined as XA,XB,XC,XD,
Using first step expanded Kalman filtration algorithm, predictor formula is as follows:
XM,k|k-1=AMXM,k-1|k-1 (7)
Wherein, M=A, B, C or D respectively indicate four targets, XM,k-1|k-1Indicate the state of the target M at k-1 moment,
XM,k|k-1=[xM,k|k-1,yM,k|k-1,vx,M,k|k-1,vy,M,k|k-1]TFor the state vector of forecast period target, PM,k-1|k-1Indicate k-
The state error covariance of 1 moment target M, PM,k|k-1Indicate that the covariance matrix of forecast period, Q indicate process covariance square
Battle array, is set as diagonal matrix, when for the first moment, (xi,yi) value use base station initial point (xi,0,yi,0), hM
(XM,k|k-1) indicate forecast period target M and each base station Prediction distance vector;
As M=A or C, matrix AMAnd HMMeet:
As M=B or D, matrix AMAnd HMMeet:
Wherein HMFor hM(XM,k|k-1) Jacobian matrix, hM,i(XM,k|k-1) indicate hM(XM,k|k-1) i-th yuan of vector
Element, Δ t are the sampling period;
Kalman gain formula is as follows:
Wherein R is measurement covariance matrix, is set as diagonal matrix,
Correction formula is as follows:
XM,k|k=XM,k|k-1+KM,k(mM,k-hM) (11)
PM,k|k=PM,k|k-1-KM,kHMPM,k|k-1 (12)
Wherein, mM,kFor target M column vector composed by the distance measure of k moment Yu all base stations, XM,k|kFor amendment
The state vector of target M afterwards, PM,k|kFor revised error co-variance matrix, hMIndicate forecast period target M and each base
The Prediction distance vector stood;
Step 8: getting target A, B, C, the location information of D, (x according to step 7A,k,yA,k),(xB,k,yB,k),
(xC,k,yC,k),(xD,k,yD,k),
Using second step expanded Kalman filtration algorithm,
Xi,k|k-1=AiXi,k-1|k-1 (13)
Wherein,Xi,k-1|k-1Indicate the state of the base station i at k-1 moment, Xi,k|k-1=
[xi,k|k-1,yi,k|k-1,vx,i,k|k-1,vy,i,k|k-1]TIndicate the state vector of the base station i of forecast period, Pi,k-1|k-1When indicating k-1
Carve the state error covariance of base station i, Pi,k|k-1Indicate the covariance matrix of forecast period base station i, Q ' expression process covariance
Matrix is set as diagonal matrix, hi(Xi,k|k-1) for composed by the distance between the predicted position of base station i and four target points
Column vector,
Wherein, HiFor hi(Xi,k|k-1) Jacobian matrix, hi,p(Xi,k|k-1) it is hi(Xi,k|k-1) p-th yuan of column vector
Element;
Kalman gain formula is as follows:
Wherein R ' is measurement covariance matrix, is set as diagonal matrix,
Correction formula is as follows:
Xi,k|k=Xi,k|k-1+Ki,k(mi,k-hi) (18)
Pi,k|k=Pi,k|k-1-Ki,kHiPi,k|k-1 (19)
Wherein, mi,kFor base station i column vector composed by the distance measure of k moment and all targets, Xi,k|kFor amendment
The state vector of base station i afterwards, Pi,k|kFor revised error co-variance matrix;
Step 9: being iterated in each sampling instant using step 7 and step 8, until four target points reach
Its corresponding terminal, after four target points reach target point, the location information of the state vector of each base station is as excellent
The co-ordinate position information of base station after change.
Since the present invention is based on the indoor environment base station coordinates position calibration methods of Kalman filtering algorithm using above-mentioned
Technical solution, i.e. this method construct indoor locating system using wireless sensor network, and all base stations are mounted on same level
On, by positioning target and the fixed height of the horizontal plane, label is installed to mobile robot, using the geometrical relationship of right angled triangle
Solve the horizontal distance between label and base station;Synchronised clock is arranged in all base stations and label, and base station is sent around comprising hair
The wireless signal of the timestamp at moment is sent, when label receives the wireless signal, is calculated between label and base station by the time difference
Distance;The noise of range information between label and base station is described, base station number and coordinate are arranged in setting in space, and in sky
Interior calibration four is linked to be the coordinate points of rectangle, and mobile target is moved along coordinate points, and obtains mobile mesh in each sampling period
Measurement range information between mark and all base stations;It is measured at a distance from each base station using mobile target in initial point, just
Step estimates the position of each base station;The co-ordinate position information of base station after two step expanded Kalman filtration algorithms are optimized.This
Method overcomes the defect of conventional mapping methods, can accurately complete the base station location calibration under indoor complex environment, and
And anti-interference ability is stronger, effectively improves positioning accuracy.
Detailed description of the invention
The present invention will be further described in detail below with reference to the accompanying drawings and embodiments:
Fig. 1 is that four coordinate points are linked to be rectangle schematic diagram in this method step 4.
Specific embodiment
The present invention is based on the indoor environment base station coordinates position calibration methods of Kalman filtering algorithm to include the following steps:
Step 1: indoor locating system of the setting based on wireless sensor network, wherein single goal positioning is by a label
It is formed with multiple base stations, label and base station are all made of ultra wide band module and realize the transmission of ultra-wideband information, and all base stations are mounted on together
On one horizontal plane, by positioning target and the fixed height of the horizontal plane, label is installed to mobile robot, using right angled triangle
Geometrical relationship solves the horizontal distance between label and base station:
Wherein, d is the distance between label and base station, and Δ h is the difference in height of label and base station, dIt is horizontalFor label and base
The horizontal distance stood;
Step 2: ranging uses TOF principle between each base station and label, when all base stations is synchronous with label setting
Clock, base station send wireless signal around, include the timestamp of sending instant in the wireless signal, and label receives the wireless communication
Number when, be compared according to the timestamp of label with the timestamp received in wireless messages, by the time difference calculate label with
The distance between base station:
D=c (treceive-tsend) (2)
Wherein, d is the distance between label and base station, and c is the spread speed of light in a vacuum, treceiveTo receive nothing
The time of line signal, tsendFor the time for sending wireless signal;
Step 3: there are noises for the distance between label and base station information, it is described as follows:
Dis (t)=dreal(t)+n(t) (3)
Wherein, dis (t) is the distance value measured by t moment, drealIt (t) is the actual distance of t moment, n (t) is full
Sufficient mean value is 0, variance σ2Gaussian random variable;
Step 4: assuming that arrangement base station number is n in space, and i-th of base station coordinates is (xi,yi), in space
Four coordinate points are demarcated, four coordinate points are expressed as (xa,ya), (xb,yb), (xc,yc), (xd,yd), and meet ya=yb,
xb=xc, yc=yd, xa=xd, i.e. four coordinate points are linked to be rectangle;
Step 5: four mobile targets are individually positioned in four calibration coordinate points, and move along a straight line counterclockwise
To next calibration point, i.e. target A is from (xa,ya) point move to (xb,yb) point, target B is from (xb,yb) point move to (xc,yc)
Point, target C is from (xc,yc) point move to (xd,yd) point, target D is from (xd,yd) point move to (xa,ya) point, and four targets
Movement velocity is at the uniform velocity;In target motion process, each sampling period obtains each movement target and all base stations
Between measurement range information dM,i,k, wherein dM,i,kIndicate target M in the measurement distance at k moment and i-th of base station;
Step 6: the position of each base station according to a preliminary estimate, using four targets respectively in initial point measurement and each base
The distance d to standA,i,0, dB,i,0, dC,i,0, dD,i,0, tentatively seek the location information of each base station using least square method, i-th
A base station and the range formula equation group of four targets are as follows:
First three equation of formula (4) is subtracted each other with the 4th equation respectively, arrangement obtains following formula:
A0Yi=B0 (5)
Wherein,
Yi=[xi,yi]T, wherein [*]TThe transposed matrix of representing matrix [*].,
Y is solved using least square methodi, solution formula is as follows:
Initial point (the x of each base station is tentatively acquired according to formula (56)i,0,yi,0);
Step 7: defining dbjective state vector is X=[x, y, vx,vy]T, wherein x and y indicates the position coordinates of target, vx
And vyIndicate target the direction x and the direction y speed,
Then the state vector of four targets is respectively defined as XA,XB,XC,XD,
Using first step expanded Kalman filtration algorithm, predictor formula is as follows:
XM,k|k-1=AMXM,k-1|k-1 (7)
Wherein, M=A, B, C or D respectively indicate four targets, XM,k-1|k-1Indicate the state of the target M at k-1 moment,
XM,k|k-1=[xM,k|k-1,yM,k|k-1,vx,M,k|k-1,vy,M,k|k-1]TFor the state vector of forecast period target, PM,k-1|k-1Indicate k-
The state error covariance of 1 moment target M, PM,k|k-1Indicate that the covariance matrix of forecast period, Q indicate process covariance square
Battle array, is set as diagonal matrix, (xi,yi) be i-th of base station location information, when for the first moment, (xi,yi) value use
Initial point (the x of base stationi,0,yi,0), hM(XM,k|k-1) indicate forecast period target M and each base station Prediction distance vector;
As M=A or C, matrix AMAnd HMMeet:
As M=B or D, matrix AMAnd HMMeet:
Wherein HMFor hM(XM,k|k-1) Jacobian matrix, hM,i(XM,k|k-1) indicate hM(XM,k|k-1) i-th yuan of vector
Element, Δ t are the sampling period;
Kalman gain formula is as follows:
Wherein R is measurement covariance matrix, is set as diagonal matrix,
Correction formula is as follows:
XM,k|k=XM,k|k-1+KM,k(mM,k-hM) (11)
PM,k|k=PM,k|k-1-KM,kHMPM,k|k-1 (12)
Wherein, mM,kFor target M column vector composed by the distance measure of k moment Yu all base stations, XM,k|kFor amendment
The state vector of target M afterwards, PM,k|kFor revised error co-variance matrix, hMIndicate forecast period target M and each base
The Prediction distance vector stood;
Step 8: getting target A, B, C, the location information of D, (x according to step 7A,k,yA,k),(xB,k,yB,k),
(xC,k,yC,k),(xD,k,yD,k),
Using second step expanded Kalman filtration algorithm,
Xi,k|k-1=AiXi,k-1|k-1 (13)
Wherein,Xi,k-1|k-1Indicate the state of the base station i at k-1 moment, Xi,k|k-1=
[xi,k|k-1,yi,k|k-1,vx,i,k|k-1,vy,i,k|k-1]TIndicate the state vector of the base station i of forecast period, Pi,k-1|k-1When indicating k-1
Carve the state error covariance of base station i, Pi,k|k-1Indicate the covariance matrix of forecast period base station i, Q ' expression process covariance
Matrix is set as diagonal matrix, hi(Xi,k|k-1) for composed by the distance between the predicted position of base station i and four target points
Column vector,
Wherein, HiFor hi(Xi,k|k-1) Jacobian matrix, hi,p(Xi,k|k-1) it is hi(Xi,k|k-1) p-th yuan of column vector
Element;
Kalman gain formula is as follows:
Wherein R ' is measurement covariance matrix, is set as diagonal matrix,
Correction formula is as follows:
Xi,k|k=Xi,k|k-1+Ki,k(mi,k-hi) (18)
Pi,k|k=Pi,k|k-1-Ki,kHiPi,k|k-1 (19)
Wherein, mi,kFor base station i column vector composed by the distance measure of k moment and all targets, Xi,k|kFor amendment
The state vector of base station i afterwards, Pi,k|kFor revised error co-variance matrix;
Step 9: being iterated in each sampling instant using step 7 and step 8, until four target points reach
Its corresponding terminal, after four target points reach target point, the location information of the state vector of each base station is as excellent
The co-ordinate position information of base station after change.
This method realizes the calibration of base station coordinates position under indoor environment using expanded Kalman filtration algorithm, extends karr
The principles illustrated of graceful filtering algorithm is as follows:
Construct prediction model:
Forecast period calculates:
Xk|k-1=AXk-1|k-1
Pk|k-1=APk-1|k-1AT+Q
Wherein, Xk-1|k-1For the optimal estimation of k-1 moment coordinate information,
X=[x y vx vy]T, Pk|k-1For predicting covariance matrix, Q makes an uproar for process
Sound covariance matrix;
Calibration phase calculates:
Kk=Pk|k-1Hk T(HkPk|k-1Hk T+R)
Xk|k=Xk|k-1+Kk(mk-hk)
Pk|k=Pk|k-1-KkHkPk|k-1
Wherein, R is measurement covariance matrix, mkFor the distance vector of measurement, Pk|kFor correction error covariance matrix, Xk|k
As in the coordinate optimal estimation at k moment.
This method considers sensor network noise jamming present in ranging process, communicates and adopts between base station and label
With ultra-wideband transmission technology, there are the spies such as strong anti-interference performance, transmission rate are high, power system capacity is big, transmission power is small, precision is high
Point, while solving to generate larger difference between obtained range information and actual range information using expanded Kalman filtration algorithm
Different problem, overcomes location information to have the defects that relatively large deviation, to ensure that positioning accuracy, the perfect positioning of robot
With navigation feature.
Claims (1)
1. a kind of indoor environment base station coordinates position calibration method based on Kalman filtering algorithm, it is characterised in that this method packet
Include following steps:
Step 1: indoor locating system of the setting based on wireless sensor network, wherein single goal positioning is by a label and more
A base station composition, label and base station are all made of ultra wide band module and realize the transmission of ultra-wideband information, and all base stations are mounted on same water
In plane, by positioning target and the fixed height of the horizontal plane, label is installed to mobile robot, using the geometry of right angled triangle
Relationship solves the horizontal distance between label and base station:
Wherein, d is the distance between label and base station, and Δ h is the difference in height of label and base station, dIt is horizontalFor label and base station
Horizontal distance;
Step 2: ranging uses TOF principle between each base station and label, synchronised clock is arranged in all base stations and label,
Base station sends wireless signal around, includes the timestamp of sending instant in the wireless signal, and label receives the wireless signal
When, it is compared according to the timestamp of label with the timestamp received in wireless messages, label and base is calculated by the time difference
The distance between stand:
D=c (treceive-tsend) (2)
Wherein, d is the distance between label and base station, and c is the spread speed of light in a vacuum, treceiveTo receive wireless communication
Number time, tsendFor the time for sending wireless signal;
Step 3: there are noises for the distance between label and base station information, it is described as follows:
Dis (t)=dreal(t)+n(t) (3)
Wherein, dis (t) is the distance value measured by t moment, drealIt (t) is the actual distance of t moment, n (t) is to meet mean value
For 0, variance σ2Gaussian random variable;
Step 4: assuming that arrangement base station number is n in space, and i-th of base station coordinates is (xi,yi), it is demarcated in space
Four coordinate points, four coordinate points are expressed as (xa,ya), (xb,yb), (xc,yc), (xd,yd), and meet ya=yb, xb=
xc, yc=yd, xa=xd, i.e. four coordinate points are linked to be rectangle;
Step 5: four mobile targets are individually positioned in four calibration coordinate points, and move along a straight line counterclockwise under
One calibration point, i.e. target A are from (xa,ya) point move to (xb,yb) point, target B is from (xb,yb) point move to (xc,yc) point, mesh
C is marked from (xc,yc) point move to (xd,yd) point, target D is from (xd,yd) point move to (xa,ya) point, and the movement speed of four targets
Degree is at the uniform velocity;In target motion process, each sampling period is obtained between each movement target and all base stations
Measure range information dM,i,k, wherein dM,i,kIndicate target M in the measurement distance at k moment and i-th of base station;
Step 6: the position of each base station according to a preliminary estimate, using four targets respectively in initial point measurement and each base station
Distance dA,i,0, dB,i,0, dC,i,0, dD,i,0, the location information of each base station, i-th of base are tentatively sought using least square method
It stands as follows with the range formula equation group of four targets:
First three equation of formula (4) is subtracted each other with the 4th equation respectively, arrangement obtains following formula:
A0Yi=B0 (5)
Wherein,
Yi=[xi,yi]T, wherein [*]TThe transposed matrix of representing matrix [*],
Y is solved using least square methodi, solution formula is as follows:
Initial point (the x of each base station is tentatively acquired according to formula (6)i,0,yi,0);
Step 7: defining dbjective state vector is X=[x, y, vx,vy]T, wherein x and y indicates the position coordinates of target, vxAnd vy
Indicate target the direction x and the direction y speed,
Then the state vector of four targets is respectively defined as XA,XB,XC,XD,
Using first step expanded Kalman filtration algorithm, predictor formula is as follows:
XM,k|k-1=AMXM,k-1|k-1 (7)
Wherein, M=A, B, C or D respectively indicate four targets, XM,k-1|k-1Indicate the state of the target M at k-1 moment, XM,k|k-1=
[xM,k|k-1,yM,k|k-1,vx,M,k|k-1,vy,M,k|k-1]TFor the state vector of forecast period target, PM,k-1|k-1Indicate k-1 moment mesh
Mark the state error covariance of M, PM,k|k-1Indicate that the covariance matrix of forecast period, Q indicate process covariance matrix, be set as
Diagonal matrix, when for the first moment, (xi,yi) value use base station initial point (xi,0,yi,0), hM(XM,k|k-1) indicate pre-
Survey the Prediction distance vector of phase targets M and each base station;
As M=A or C, matrix AMAnd HMMeet:
As M=B or D, matrix AMAnd HMMeet:
Wherein HMFor hM(XM,k|k-1) Jacobian matrix, hM,i(XM,k|k-1) indicate hM(XM,k|k-1) vector i-th of element, Δ t
For the sampling period;
Kalman gain formula is as follows:
Wherein R is measurement covariance matrix, is set as diagonal matrix,
Correction formula is as follows:
XM,k|k=XM,k|k-1+KM,k(mM,k-hM) (11)
PM,k|k=PM,k|k-1-KM,kHMPM,k|k-1 (12)
Wherein, mM,kFor target M column vector composed by the distance measure of k moment Yu all base stations, XM,k|kIt is revised
The state vector of target M, PM,k|kFor revised error co-variance matrix, hMIndicate forecast period target M and each base station
Prediction distance vector;
Step 8: getting target A, B, C, the location information of D, (x according to step 7A,k,yA,k),(xB,k,yB,k),(xC,k,
yC,k),(xD,k,yD,k),
Using second step expanded Kalman filtration algorithm,
Xi,k|k-1=AiXi,k-1|k-1 (13)
Wherein,Xi,k-1|k-1Indicate the state of the base station i at k-1 moment, Xi,k|k-1=[xi,k|k-1,
yi,k|k-1,vx,i,k|k-1,vy,i,k|k-1]TIndicate the state vector of the base station i of forecast period, Pi,k-1|k-1Indicate k-1 moment base station i
State error covariance, Pi,k|k-1The covariance matrix of expression forecast period base station i, Q ' expression process covariance matrix, if
It is set to diagonal matrix, hi(Xi,k|k-1) be base station i predicted position and the distance between four target points composed by column vector,
Wherein, HiFor hi(Xi,k|k-1) Jacobian matrix, hi,p(Xi,k|k-1) it is hi(Xi,k|k-1) column vector p-th of element;
Kalman gain formula is as follows:
Wherein R ' is measurement covariance matrix, is set as diagonal matrix,
Correction formula is as follows:
Xi,k|k=Xi,k|k-1+Ki,k(mi,k-hi) (18)
Pi,k|k=Pi,k|k-1-Ki,kHiPi,k|k-1 (19)
Wherein, mi,kFor base station i column vector composed by the distance measure of k moment and all targets, Xi,k|kIt is revised
The state vector of base station i, Pi,k|kFor revised error co-variance matrix;
Step 9: be iterated in each sampling instant using step 7 and step 8, until to reach its right for four target points
The terminal answered, after four target points reach target point, the location information of the state vector of each base station is after optimizing
The co-ordinate position information of base station.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811296954.6A CN109548141B (en) | 2018-11-01 | 2018-11-01 | Indoor environment base station coordinate position calibration method based on Kalman filtering algorithm |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811296954.6A CN109548141B (en) | 2018-11-01 | 2018-11-01 | Indoor environment base station coordinate position calibration method based on Kalman filtering algorithm |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109548141A true CN109548141A (en) | 2019-03-29 |
CN109548141B CN109548141B (en) | 2020-11-10 |
Family
ID=65845752
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811296954.6A Active CN109548141B (en) | 2018-11-01 | 2018-11-01 | Indoor environment base station coordinate position calibration method based on Kalman filtering algorithm |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109548141B (en) |
Cited By (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110022574A (en) * | 2019-04-16 | 2019-07-16 | 江苏科技大学 | A kind of method of automatic configuration of UWB indoor positioning base station |
CN110174346A (en) * | 2019-05-06 | 2019-08-27 | 江苏耀群工业技术有限公司 | Autoregistration strainer scan method and device |
CN110557720A (en) * | 2019-09-12 | 2019-12-10 | 南京工程学院 | Ultra-wideband indoor positioning system and blind-complementing positioning method based on dynamic reference label |
CN110580054A (en) * | 2019-08-21 | 2019-12-17 | 东北大学 | Control system and method of photoelectric pod based on autonomous visual tracking |
CN110657799A (en) * | 2019-09-26 | 2020-01-07 | 广东省海洋工程装备技术研究所 | Space real-time positioning method, computer device and computer readable storage medium |
CN110711701A (en) * | 2019-09-16 | 2020-01-21 | 华中科技大学 | Grabbing type flexible sorting method based on RFID space positioning technology |
CN111818460A (en) * | 2020-06-08 | 2020-10-23 | 南京邮电大学 | Position verification method and system based on dual-threshold sequential detection |
CN111954153A (en) * | 2020-08-12 | 2020-11-17 | 南京工程学院 | Intelligent taxi calling navigation positioning method based on UWB positioning |
CN113189541A (en) * | 2021-04-23 | 2021-07-30 | 北京邮电大学 | Positioning method, device and equipment |
CN113259884A (en) * | 2021-05-19 | 2021-08-13 | 桂林电子科技大学 | Indoor positioning base station layout optimization method based on multi-parameter fusion |
CN113514797A (en) * | 2021-07-09 | 2021-10-19 | 中国人民解放军战略支援部队信息工程大学 | Automatic calibration method of UWB base station |
CN113709662A (en) * | 2021-08-05 | 2021-11-26 | 北京理工大学重庆创新中心 | Ultra-wideband-based autonomous three-dimensional inversion positioning method |
CN114040327A (en) * | 2021-11-25 | 2022-02-11 | 江苏科技大学 | Construction method of space visual benchmarking system based on UWB |
CN114143804A (en) * | 2021-02-02 | 2022-03-04 | 浙江机电职业技术学院 | Receiving station path optimization method for maximizing measurement information gain |
CN114666732A (en) * | 2022-03-15 | 2022-06-24 | 江苏科技大学 | Moving target positioning resolving and error evaluation method under noisy network |
CN115639521A (en) * | 2022-10-24 | 2023-01-24 | 青岛柯锐思德电子科技有限公司 | UWB-based moving object motion state judgment method |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102914303A (en) * | 2012-10-11 | 2013-02-06 | 江苏科技大学 | Navigation information acquisition method and intelligent space system with multiple mobile robots |
CN103471595A (en) * | 2013-09-26 | 2013-12-25 | 东南大学 | Inertial navigation system (INS)/wireless sensor network (WSN) indoor mobile robot tight-integration navigation-oriented iterative extended RTS average filtering method |
CN105953802A (en) * | 2016-07-22 | 2016-09-21 | 马宏宾 | Indoor positioning system and method based on iBeacon |
CN106352869A (en) * | 2016-08-12 | 2017-01-25 | 上海理工大学 | Indoor localization system for mobile robot and calculation method thereof |
CN106793077A (en) * | 2017-01-05 | 2017-05-31 | 重庆邮电大学 | The UWB localization methods and system of dynamic object in a kind of self adaptation room |
US20180023953A1 (en) * | 2008-03-28 | 2018-01-25 | Regents Of The University Of Minnesota | Extended kalman filter for 3d localization and vision-aided inertial navigation |
CN107817469A (en) * | 2017-10-18 | 2018-03-20 | 上海理工大学 | Indoor orientation method is realized based on ultra-wideband ranging under nlos environment |
-
2018
- 2018-11-01 CN CN201811296954.6A patent/CN109548141B/en active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20180023953A1 (en) * | 2008-03-28 | 2018-01-25 | Regents Of The University Of Minnesota | Extended kalman filter for 3d localization and vision-aided inertial navigation |
CN102914303A (en) * | 2012-10-11 | 2013-02-06 | 江苏科技大学 | Navigation information acquisition method and intelligent space system with multiple mobile robots |
CN103471595A (en) * | 2013-09-26 | 2013-12-25 | 东南大学 | Inertial navigation system (INS)/wireless sensor network (WSN) indoor mobile robot tight-integration navigation-oriented iterative extended RTS average filtering method |
CN105953802A (en) * | 2016-07-22 | 2016-09-21 | 马宏宾 | Indoor positioning system and method based on iBeacon |
CN106352869A (en) * | 2016-08-12 | 2017-01-25 | 上海理工大学 | Indoor localization system for mobile robot and calculation method thereof |
CN106793077A (en) * | 2017-01-05 | 2017-05-31 | 重庆邮电大学 | The UWB localization methods and system of dynamic object in a kind of self adaptation room |
CN107817469A (en) * | 2017-10-18 | 2018-03-20 | 上海理工大学 | Indoor orientation method is realized based on ultra-wideband ranging under nlos environment |
Cited By (24)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110022574A (en) * | 2019-04-16 | 2019-07-16 | 江苏科技大学 | A kind of method of automatic configuration of UWB indoor positioning base station |
CN110174346A (en) * | 2019-05-06 | 2019-08-27 | 江苏耀群工业技术有限公司 | Autoregistration strainer scan method and device |
CN110580054A (en) * | 2019-08-21 | 2019-12-17 | 东北大学 | Control system and method of photoelectric pod based on autonomous visual tracking |
CN110580054B (en) * | 2019-08-21 | 2022-06-14 | 东北大学 | Control system and method of photoelectric pod based on autonomous visual tracking |
CN110557720A (en) * | 2019-09-12 | 2019-12-10 | 南京工程学院 | Ultra-wideband indoor positioning system and blind-complementing positioning method based on dynamic reference label |
CN110557720B (en) * | 2019-09-12 | 2024-02-13 | 南京工程学院 | Ultra-wideband indoor positioning system and blind compensation positioning method based on dynamic reference label |
CN110711701A (en) * | 2019-09-16 | 2020-01-21 | 华中科技大学 | Grabbing type flexible sorting method based on RFID space positioning technology |
CN110657799B (en) * | 2019-09-26 | 2022-09-09 | 广东省海洋工程装备技术研究所 | Space real-time positioning method, computer device and computer readable storage medium |
CN110657799A (en) * | 2019-09-26 | 2020-01-07 | 广东省海洋工程装备技术研究所 | Space real-time positioning method, computer device and computer readable storage medium |
CN111818460A (en) * | 2020-06-08 | 2020-10-23 | 南京邮电大学 | Position verification method and system based on dual-threshold sequential detection |
CN111954153B (en) * | 2020-08-12 | 2022-11-29 | 南京工程学院 | Intelligent taxi calling navigation positioning method based on UWB positioning |
CN111954153A (en) * | 2020-08-12 | 2020-11-17 | 南京工程学院 | Intelligent taxi calling navigation positioning method based on UWB positioning |
CN114143804A (en) * | 2021-02-02 | 2022-03-04 | 浙江机电职业技术学院 | Receiving station path optimization method for maximizing measurement information gain |
CN113189541A (en) * | 2021-04-23 | 2021-07-30 | 北京邮电大学 | Positioning method, device and equipment |
CN113189541B (en) * | 2021-04-23 | 2023-12-12 | 北京邮电大学 | Positioning method, device and equipment |
CN113259884A (en) * | 2021-05-19 | 2021-08-13 | 桂林电子科技大学 | Indoor positioning base station layout optimization method based on multi-parameter fusion |
CN113514797A (en) * | 2021-07-09 | 2021-10-19 | 中国人民解放军战略支援部队信息工程大学 | Automatic calibration method of UWB base station |
CN113514797B (en) * | 2021-07-09 | 2023-08-08 | 中国人民解放军战略支援部队信息工程大学 | Automatic calibration method of UWB base station |
CN113709662A (en) * | 2021-08-05 | 2021-11-26 | 北京理工大学重庆创新中心 | Ultra-wideband-based autonomous three-dimensional inversion positioning method |
CN113709662B (en) * | 2021-08-05 | 2023-12-01 | 北京理工大学重庆创新中心 | Autonomous three-dimensional inversion positioning method based on ultra-wideband |
CN114040327A (en) * | 2021-11-25 | 2022-02-11 | 江苏科技大学 | Construction method of space visual benchmarking system based on UWB |
CN114666732A (en) * | 2022-03-15 | 2022-06-24 | 江苏科技大学 | Moving target positioning resolving and error evaluation method under noisy network |
CN114666732B (en) * | 2022-03-15 | 2024-05-10 | 江苏科技大学 | Moving target positioning calculation and error evaluation method under noisy network |
CN115639521A (en) * | 2022-10-24 | 2023-01-24 | 青岛柯锐思德电子科技有限公司 | UWB-based moving object motion state judgment method |
Also Published As
Publication number | Publication date |
---|---|
CN109548141B (en) | 2020-11-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109548141A (en) | Indoor environment base station coordinates position calibration method based on Kalman filtering algorithm | |
CN107817469B (en) | Indoor positioning method based on ultra-wideband ranging in non-line-of-sight environment | |
CN105547305B (en) | A kind of pose calculation method based on wireless location and laser map match | |
CN110856106B (en) | Indoor high-precision three-dimensional positioning method based on UWB and barometer | |
CN107205268A (en) | A kind of 3-D positioning method based on radio communication base station | |
CN112533163B (en) | Indoor positioning method based on NB-IoT (NB-IoT) improved fusion ultra-wideband and Bluetooth | |
CN102419180A (en) | Indoor positioning method based on inertial navigation system and WIFI (wireless fidelity) | |
CN108318861A (en) | One kind being based on CSI precision distance measurement localization methods | |
CN108226860B (en) | RSS (received signal strength) -based ultra-wideband mixed dimension positioning method and positioning system | |
CN103402258A (en) | Wi-Fi (Wireless Fidelity)-based indoor positioning system and method | |
CN102231912A (en) | RSSI ranging-based positioning method for indoor wireless sensor network | |
CN105357754B (en) | A kind of mobile node combined positioning method based on wireless network | |
CN110026993B (en) | Human body following robot based on UWB and pyroelectric infrared sensor | |
CN109511085B (en) | UWB fingerprint positioning method based on MeanShift and weighted k nearest neighbor algorithm | |
CN104507097A (en) | Semi-supervised training method based on WiFi (wireless fidelity) position fingerprints | |
Benaissa et al. | Phone application for indoor localization based on Ble signal fingerprint | |
CN110636436A (en) | Three-dimensional UWB indoor positioning method based on improved CHAN algorithm | |
CN112729301A (en) | Indoor positioning method based on multi-source data fusion | |
Lategahn et al. | Tdoa and rss based extended kalman filter for indoor person localization | |
CN108413966A (en) | Localization method based on a variety of sensing ranging technology indoor locating systems | |
CN103596265B (en) | A kind of multi-user's indoor orientation method based on sound ranging and motion-vector | |
Yin et al. | UWB-based indoor high precision localization system with robust unscented Kalman filter | |
CN116567531A (en) | Sensor fusion indoor positioning method and system based on particle filter algorithm | |
CN103888979A (en) | Indoor positioning method based on wireless local area network | |
Kuxdorf-Alkirata et al. | Reliable and low-cost indoor localization based on bluetooth low energy |
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 |