CN109282820B - Indoor positioning method based on distributed hybrid filtering - Google Patents

Indoor positioning method based on distributed hybrid filtering Download PDF

Info

Publication number
CN109282820B
CN109282820B CN201811415999.0A CN201811415999A CN109282820B CN 109282820 B CN109282820 B CN 109282820B CN 201811415999 A CN201811415999 A CN 201811415999A CN 109282820 B CN109282820 B CN 109282820B
Authority
CN
China
Prior art keywords
representing
matrix
denotes
ith node
ith
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
CN201811415999.0A
Other languages
Chinese (zh)
Other versions
CN109282820A (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.)
Zhejiang University of Technology ZJUT
Original Assignee
Zhejiang University of Technology ZJUT
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 Zhejiang University of Technology ZJUT filed Critical Zhejiang University of Technology ZJUT
Priority to CN201811415999.0A priority Critical patent/CN109282820B/en
Publication of CN109282820A publication Critical patent/CN109282820A/en
Application granted granted Critical
Publication of CN109282820B publication Critical patent/CN109282820B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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)
  • Feedback Control In General (AREA)

Abstract

An indoor positioning method based on distributed hybrid filtering is characterized in that a continuous dynamic model is established for a moving object under the assumption that the moving object exists indoors, and the actual driving condition is simulated; then, when the object moves, the inaccuracy and the interference of the model are considered as a mixed disturbance signal consisting of random disturbance and non-random disturbance; and further adopting distributed mixed filtering, observing a target by a plurality of sensors, fusing, and carrying out real-time high-precision estimation on the position (abscissa a and ordinate b) of the moving object on the premise of ensuring the robustness and the anti-interference capability of the system. The estimation result can meet the precision and real-time requirements of practical application, and no matter which indoor positioning technology is used, the algorithm can be well matched, and the requirement of indoor positioning is met.

Description

Indoor positioning method based on distributed hybrid filtering
Technical Field
The invention relates to an indoor positioning method of an object.
Background
The global positioning system is a global positioning system based on satellite communication, and can solve various outdoor positioning problems in military and civil fields with good precision. Due to the shielding of buildings, the indoor positioning system cannot receive enough satellite signals for positioning because of insufficient signal accuracy. The current indoor positioning technology is mainly realized based on a wireless communication system, and the following technologies are mainly available: Wi-Fi technology, Bluetooth technology, infrared technology, ultra wideband technology, RFID technology, ZigBee technology, and ultrasonic technology.
In indoor positioning, a kalman filter algorithm is commonly used. Because the Kalman filtering does not need to store a large amount of observation data during solving and is convenient for real-time processing and estimation, the Kalman filtering is widely applied to the dynamic data processing, in particular to the fields of GPS dynamic data processing, navigation and the like. Meanwhile, the Kalman filtering is an optimal linear filter based on the minimum mean square error, so that the Kalman filtering has higher precision. But kalman filtering requires model accuracy. In indoor positioning, the model is inaccurate, and some external non-random interference occurs, which may cause the estimation effect of the kalman filtering algorithm to be reduced.
Disclosure of Invention
The invention solves the problems in the prior art and provides an indoor positioning method based on distributed hybrid filtering.
The working principle of the invention is as follows: assuming that a moving object exists indoors, firstly establishing a continuous dynamics model for the moving object to simulate the actual moving condition; then, the inaccuracy and the interference of the object model are considered as a mixed disturbance signal consisting of random disturbance and non-random disturbance; and distributed mixed filtering is further adopted, a plurality of sensors observe targets, fusion is carried out, and the precision is improved.
The indoor positioning estimation method based on distributed hybrid filtering specifically comprises the following steps:
1) establishing a continuous dynamic model for the motion situation of an indoor moving object;
2) considering mixed disturbance and model inaccuracy in a complex environment, establishing a system state equation and an output equation, and establishing an observation equation of each sensor;
3) constructing a corresponding filter according to the observed value of each sensor;
4) and (3) providing a system error model, further providing a system error model based on worst non-random disturbance, and designing and solving the gain of each filter through an iterative algorithm.
Further, in step 1), a plane rectangular coordinate system is established by the indoor environment, and then the plane rectangular coordinate system is used
Figure GDA0002809639170000021
To represent the position of the moving object, where a represents the abscissa of the object and b represents the ordinate of the object.
Further, in the step 2), considering the mixed disturbance and the model inaccuracy in the indoor environment, establishing a system state equation and an observation equation of each sensor comprises the following steps:
(2.1) the equation of state of the system is:
x(k+1)＝Ax(k)+B0ω0(k)+B1ω(k) (1)
where k denotes the current discretization time, k +1 denotes the next discretization time, and the estimation object x denotes the position of the object, that is, x ═ a b]TThe superscript "T" denotes the transpose of the matrix, A denotes the state transition matrix of the estimation object x, ω0Denotes white Gaussian noise with mean of zero and variance of 1, B0Representing white gaussian noise omega0ω represents a non-random bounded perturbation signal, B1An input matrix representing a non-random bounded perturbation signal ω.
(2.2) the observation equation for the ith sensor is:
yi(k)＝C2；ix(k)+D1；iω0(k)+Diω(k) (2)
where k denotes the discretization time, yiAn observation vector representing the ith sensor, x representing an estimation object, C2；iAn observation matrix, ω, representing an estimated object of the i-th sensor0Denotes white Gaussian noise with mean 0 and variance 1, D1；iWhite Gaussian noise omega representing the ith sensor0ω is a non-random bounded perturbation signal, DiAn observation matrix representing the non-random bounded perturbation signal of the ith sensor.
(2.3) output equation of the system:
Figure GDA0002809639170000031
where k denotes the discretization time, z0Representing the output of the system, x representing the object of estimation, C1,C0An output matrix representing the estimation object x.
Further, in step 3), assuming that there are N nodes and each node has a filter and a sensor, defining η to represent the set of all nodes, i.e., η: { 1.., N }. In distributed filtering, for the ith filter, the ith filter can not only receive an observed value y from the own sensori(k) And can receive the observed value of its neighbor sensor
Figure GDA0002809639170000032
Definition etaiMeans that the ith node receives a set of neighbor observations, namely
Figure GDA0002809639170000033
Definition JiIs the set of the ith node itself and all its neighbors, i.e., Ji＝{i}∪ηi. Definition of
Figure GDA0002809639170000034
A set of all observations representing an ith node;
Figure GDA0002809639170000035
a set of all observation matrices representing the ith node;
Figure GDA0002809639170000036
a set of observation matrices representing all white gaussian noise for the ith node;
Figure GDA0002809639170000037
a set of observation matrices representing all non-random bounded perturbation signals of the ith node.
In the indoor positioning technology, data transmission is performed through a wireless sensor network, for an ith node, the observed value of a neighbor node can be received through the wireless sensor network, but disturbance is received in the transmission process, so that data is lost. Assuming that all observed values of the ith node receive m disturbances of independent and uniformly distributed random processes in the transmission process, recording as:
Figure GDA0002809639170000038
where k represents the discretized time,
Figure GDA0002809639170000039
representing all the data actually received by the ith node,
Figure GDA00028096391700000310
all neighbor node observations representing the ith node
Figure GDA00028096391700000311
Whether data transmitted to the ith node is lost.
Designing a filter for each node:
Figure GDA0002809639170000041
where k denotes the current discretization time, k +1 denotes the next discretization time, a denotes the state transition matrix of the estimation object x,
Figure GDA00028096391700000415
an estimated value, L, representing an estimated object xiRepresenting the filter gain of the i-th node, C1，C0An output matrix representing the estimated object x,
Figure GDA0002809639170000042
a set of all observation matrices representing the ith node,
Figure GDA0002809639170000043
a set of observation matrices representing all white gaussian noise for the ith node,
Figure GDA0002809639170000044
a set of observation matrices representing all non-random bounded perturbation signals of the ith node,
Figure GDA0002809639170000045
indicating the data actually received by the ith node,
Figure GDA0002809639170000046
all neighbor node observations representing the ith node
Figure GDA0002809639170000047
Whether data transmitted to the ith node is lost.
The effect of the filter is to make the estimationEvaluating value
Figure GDA0002809639170000048
Proximity system output z, z0Thereby realizing real-time high-precision estimation of the estimation object x, i.e., the moving object.
Further, in the step 4), a system error model is given, a system error model based on worst non-disturbance is further given, and filter gain is designed and solved through an iterative algorithm, specifically comprising the following steps:
(4.1) obtaining an error system model through (1) (2) (3) (5):
Figure GDA0002809639170000049
where k denotes the current discrete time, k +1 denotes the next discrete time, ex；iRepresenting the estimated object x and the corresponding estimated value
Figure GDA00028096391700000410
Difference of (e)iRepresenting the system output z and the corresponding estimate
Figure GDA00028096391700000411
Difference of (e)0；iRepresenting system output z0And corresponding estimated value
Figure GDA00028096391700000412
A represents a state transition matrix of the estimation object, LiIndicating the gain of the filter that needs to be designed,
Figure GDA00028096391700000413
a set of all observation matrices representing the ith node;
Figure GDA00028096391700000414
a set of observation matrices representing all white gaussian noise for the ith node; dJiSet of observation matrices, B, representing all the non-random bounded perturbation signals of the ith node0Is Gaussian white noise omega0Input matrix of, B1An input matrix of non-random bounded perturbation signals omega, omega0Representing white Gaussian noise with mean 0 and variance 1, omega being a non-random bounded perturbation signal, C1，C0An output matrix representing the estimation object x.
(4.2) defining the worst non-random bounded perturbation signal omega of the ith node based on an error systemi(k):
ωi(k)＝Wiex；i(k) (7)
Substituting (7) into an equation (6), and further obtaining a system error model under worst non-random disturbance:
Figure GDA0002809639170000051
(4.3) when k is 0, centering the intermediate variable P1；iAnd P2；iAnd an intermediate matrix WiAnd filter gain LiGiving an initial value, i.e.
P1；i(0)，P2；i(0)，Wi(0)，Li(0) (9)
(4.4) error-based System, from H∞The worst non-random bounded disturbance signal omega is obtained by the filtering algorithmi：
ωi(k)＝γ-2Δi -1(B1 TP1；iAL；i+D1；Ji TDμ；iAΩ；i)ex；i(k) (10)
Therefore:
Wi＝γ-2Δi -1(B1 TP1；iAL；i+D1；i TDμ；iAΩ；i) (11)
where k denotes the current discrete time, ωiRepresenting a non-random bounded perturbation signal, gamma being H representing a preset∞The parameter, superscript "-1" denotes the inverse of the matrix, superscript "T" denotes the transpose of the matrix, B1An input matrix representing a non-random bounded perturbation signal omega,
Figure GDA0002809639170000052
set of observation matrices, D, representing all white Gaussian noises of the ith nodeμ；iRepresents DΦ；iExpectation of (D)Φ；iAn observed value y of the node itself representing the ith nodei(k) Whether data of (2) is lost, C1，C0An output matrix representing an estimated object x, AL；i、P1；i、AΩ；iAnd ΔiAre all intermediate matrices, ex；iRepresenting the estimated object x and the corresponding estimated value
Figure GDA0002809639170000061
The difference of (a).
(4.5) based on the system error model under the worst non-random disturbance, passing through H2The filter algorithm obtains the filter gain LiExpression (c):
Figure GDA0002809639170000062
wherein the superscript "-1" represents the inverse of the matrix, the superscript "T" represents the transpose of the matrix,
Figure GDA0002809639170000063
a set of all observation matrices representing the ith node,
Figure GDA0002809639170000064
a set of observation matrices representing all white gaussian noise for the ith node,
Figure GDA0002809639170000065
a set of observation matrices representing all non-random bounded perturbation signals of the ith node,
Figure GDA0002809639170000066
to represent
Figure GDA0002809639170000067
Expectation of (A), B0Is Gaussian white noise omega0Input matrix of, B1Input matrix, P, for non-random bounded perturbation signal omega2；i、Wi、AW；iAnd ΛiAre all intermediate matrices, AWRepresents all intermediate matrices AW；iA collection of (a).
(4.6) when k is 1, it is obtained from formula (11) (12):
Wi(1)＝γ-2Δi -1(B1 TP1；i(0)AL；i+D1 TDμ；iAΩ；i) (13)
Figure GDA0002809639170000068
where A represents a state transition matrix of an estimation object, and γ represents a preset H∞The parameter, superscript "-1" denotes the inverse of the matrix, superscript "T" denotes the transpose of the matrix, LiIndicating the gain of the filter that needs to be designed,
Figure GDA0002809639170000069
to represent
Figure GDA00028096391700000610
In the expectation that the position of the target is not changed,
Figure GDA00028096391700000611
a set of all observation matrices representing the ith node,
Figure GDA00028096391700000612
set of observation matrices, B, representing all the non-random bounded perturbation signals of the ith node0Is Gaussian white noise omega0Input matrix of, B1Input matrix, C, for non-random bounded perturbation signal omega1，C0An output matrix representing an estimated object x, AL；i、AΩ；i、Δi、Λi、AW；i、P1；iAnd P2；iAre all intermediate matrices.
(4.7) intermediate matrix P1；iThe following equation is satisfied:
Figure GDA00028096391700000613
thus obtaining an intermediate matrix P1；i(1)：
Figure GDA0002809639170000071
Where k denotes the current discretization time, k +1 denotes the next discretization time, a denotes a state transition matrix of the estimation object, and γ denotes a preset H∞The parameter, superscript "-1" denotes the inverse of the matrix, superscript "T" denotes the transpose of the matrix, LiIndicating the gain of the filter that needs to be designed,
Figure GDA0002809639170000072
to represent
Figure GDA0002809639170000073
In the expectation that the position of the target is not changed,
Figure GDA0002809639170000074
a set of all observation matrices representing the ith node,
Figure GDA0002809639170000075
set of observation matrices, B, representing all the non-random bounded perturbation signals of the ith node1Input matrix, C, for non-random bounded perturbation signal omega1An output matrix representing an estimated object x, AL；i、Γi、AΩ；i、ΔiAnd P1；iAre all intermediate matrices.
(4.8) obtaining an intermediate matrix P2；i(1)；
Figure GDA0002809639170000076
Thus obtaining an intermediate matrix P2；i(1)：
Figure GDA0002809639170000077
Where k denotes the current discretization time, k +1 denotes the next discretization time, a denotes a state transition matrix of the estimation object, and γ denotes a preset H∞The parameter, superscript "-1" denotes the inverse of the matrix, superscript "T" denotes the transpose of the matrix, LiIndicating the gain of the filter that needs to be designed,
Figure GDA0002809639170000078
set of all observation matrices, D, representing the ith nodeJiSet of observation matrices, B, representing all the non-random bounded perturbation signals of the ith node1Input matrix for non-random bounded perturbation signal omega, AW；i、Wi、ΛiAnd P1；iAre all intermediate matrices.
(4.9) repeating steps (4.6) (4.7) (4.8).
If k equals T time, the matrix P1(T) and matrix P1(T-1) the two-norm difference is less than a given error, resulting in:
P1；i＝P1；i(T)＝P1；i(T-1) (19)
similarly, if k equals T, the matrix P2(T) and matrix P2(T-1) the two-norm difference is less than a given error, resulting in:
P2；i＝P2；i(T)＝P2；i(T-1) (20)
wherein, P1；iAnd P2；iAre all intermediate matrices.
(4.10) combining the intermediate matrices P2；iSubstituting formula (12) to obtain filter gain matrix L of ith nodei. Thus, the filter equation (12) realizes real-time high-accuracy estimation of the estimation target x (i.e., the position of the moving object).
The invention designs an indoor positioning algorithm based on distributed mixed filtering, which solves two groups of equations through an iterative algorithm, constructs a plurality of filters to realize the multi-point real-time high-precision estimation of the coordinates of a moving object under the worst non-random disturbance signal.
The invention has the advantages that: the influence of a complex environment is considered, a system state equation and an observation equation are established aiming at the inaccuracy of an indoor positioning technology model, a filter is further constructed, and the position (an abscissa a and an ordinate b) of a moving object is estimated in real time and high-precision on the premise of ensuring the robustness and the anti-interference capacity of the system. The estimation result can meet the precision and real-time requirements of practical application, and no matter which indoor positioning technology is used, the algorithm can be well matched, and the requirement of indoor positioning is met.
Drawings
FIG. 1 is a graph showing the effect of the experiment of the present invention
Detailed Description
The technical scheme of the invention is further explained by combining the attached drawings.
The invention provides an indoor positioning method based on distributed robust optimal hybrid filtering. The working principle is as follows: assuming that a moving object exists indoors, firstly establishing a continuous dynamics model for the moving object to simulate the actual driving condition; then, when the object moves, the inaccuracy and the interference of the model are considered as a mixed disturbance signal consisting of random disturbance and non-random disturbance; and further, distributed mixed filtering is adopted, a plurality of sensors observe targets and are fused, and the precision is improved.
The indoor positioning estimation method based on distributed hybrid filtering specifically comprises the following steps:
1) establishing continuous dynamic model for indoor moving object motion condition
2) Considering mixed disturbance and model inaccuracy under complex environment, establishing system state equation and output equation, and establishing observation equation of each sensor
3) Constructing a plurality of system filters from observations of each sensor
4) Providing a system error model, further providing a system error model based on worst non-random disturbance, designing and solving the gain of each filter through an iterative algorithm
In step 1), a planar rectangular coordinate system is established for the indoor environment, and then x is used as [ a b ]]TTo represent the position of the moving object, where the superscript "T" represents the transpose of the matrix, a represents the abscissa of the object, and b represents the ordinate of the object, so that we can know the position of the object at each moment.
In the step 2), considering mixed disturbance and model inaccuracy in an indoor environment, establishing a system state equation and an observation equation of each sensor comprises the following steps:
(2.1) the discretized equation of state is:
x(k+1)＝Ax(k)+B0ω0(k)+B1ω(k) (1)
where k denotes the current discretization time, k +1 denotes the next discretization time, and the estimation object x denotes the position of the object, that is, x ═ a b]TThe superscript "T" denotes the transpose of the matrix, the state transition matrix of the estimation object x being
Figure GDA0002809639170000091
ω0Representing white gaussian noise with mean 0 and variance 1,
Figure GDA0002809639170000092
representing white gaussian noise omega0The input matrix of (1), the non-random bounded perturbation signal is modeled by ω ═ 0.1 × sin (0.5 × k) |, the input matrix of the non-random bounded perturbation signal ω is
Figure GDA0002809639170000101
(2.2) after discretization, the observation equation of the ith sensor is as follows:
yi(k)＝C2；ix(k)+D1；iω0(k)+Diω(k) (2)
where k denotes the discretization time, yJiAn observation vector representing the ith sensor, x representing an estimation object, C2；iAn observation matrix indicating an estimation object of the ith sensor, wherein if i is an odd number, the observation matrix indicates that the ith sensor is a target of estimation
Figure GDA0002809639170000102
If i is an even number, then
Figure GDA0002809639170000103
ω0Denotes white Gaussian noise with mean 0 and variance 1, D1；iWhite Gaussian noise omega representing the ith sensor0If i is an odd number,
Figure GDA0002809639170000104
if i is an even number, then,
Figure GDA0002809639170000105
omega is a non-random bounded perturbation signal, DiAn observation matrix representing the non-random bounded perturbation signal of the ith sensor, if i is odd,
Figure GDA0002809639170000106
if i is an even number, then,
Figure GDA0002809639170000107
(2.3) output equation of the discretized system:
Figure GDA0002809639170000108
where k denotes the discretization time, z0Representing the output of the system, x representing the object to be estimated, the output matrix of object x being C1＝[1 1]，C0＝[1 1]。
Further, in step 3), designing a filter of each node based on the system in step 2):
Figure GDA0002809639170000109
where k denotes the current discretization time, k +1 denotes the next discretization time, and the state transition matrix of the estimation object x is
Figure GDA0002809639170000111
The output matrix of object x is C1＝[1 1]，C0＝[1 1]。
Figure GDA0002809639170000112
An estimated value, L, representing an estimated object xiRepresents the filter gain of the ith node,
Figure GDA0002809639170000113
a set of all observation matrices representing the ith node,
Figure GDA0002809639170000114
a set of observation matrices representing all white gaussian noise for the ith node,
Figure GDA0002809639170000115
a set of observation matrices representing all non-random bounded perturbation signals of the ith node,
Figure GDA0002809639170000116
representing all the data actually received by the ith node,
Figure GDA0002809639170000117
all neighbor node observations representing the ith node
Figure GDA0002809639170000118
Whether data transmitted to the ith node is lost
In the step 4), a system error model is given, a system error model based on worst non-disturbance is further given, and filter gain is designed and solved through an iterative algorithm, and the method specifically comprises the following steps:
(4.1) obtaining an error system model through (1) (2) (3) (4):
Figure GDA0002809639170000119
where k denotes the current discrete time, k +1 denotes the next discrete time, ex；iRepresenting the estimated object x and the corresponding estimated value
Figure GDA00028096391700001110
Difference of (e)iRepresenting the system output z and the corresponding estimate
Figure GDA00028096391700001111
Difference of (e)0；iRepresenting system output z0And corresponding estimated value
Figure GDA00028096391700001112
Estimating a state transition matrix of the object
Figure GDA00028096391700001113
LiIndicating the gain of the filter that needs to be designed,
Figure GDA00028096391700001114
a set of all observation matrices representing the ith node;
Figure GDA00028096391700001115
a set of observation matrices representing all white gaussian noise for the ith node;
Figure GDA00028096391700001116
set of observation matrices, white Gaussian noise omega, representing all non-random bounded perturbation signals of the ith node0Input matrix of
Figure GDA00028096391700001117
Input matrix of non-random bounded perturbation signals omega
Figure GDA00028096391700001118
ω0Representing white gaussian noise with mean 0 and variance 1, the nonrandom bounded perturbation signal is ω (k) |0.1 × sin (0.5 × k) |, and the output matrix of object x is C1＝[1 1]，C0＝[1 1]。
(4.2) defining the worst non-random bounded perturbation signal ω based on an error system:
ωi(k)＝Wiex；i(k) (6)
and (13) is substituted into an equation (12), and a system error model under worst non-random disturbance is further obtained:
Figure GDA0002809639170000121
wherein k represents the current discretization time, k +1 represents the next discretization time, and the preset H is∞The parameter γ is 2, and the state transition matrix of the object is estimated
Figure GDA0002809639170000122
LiIndicating the gain of the filter that needs to be designed,
Figure GDA0002809639170000123
to represent
Figure GDA0002809639170000124
In the expectation that the position of the target is not changed,
Figure GDA0002809639170000125
a set of all observation matrices representing the ith node,
Figure GDA0002809639170000126
set of observation matrices, white Gaussian noise omega, representing all non-random bounded perturbation signals of the ith node0Input matrix of
Figure GDA0002809639170000127
Input matrix of non-random bounded perturbation signals omega
Figure GDA0002809639170000128
ω0Expressing white gaussian noise with mean 0 and variance 1, the nonrandom bounded perturbation signal is ω (k) |0.1 | (0.5 |) and the output matrix C of the object x is estimated1＝[1 1]，C0＝[1 1]。
(4.3) when k is 0, centering the intermediate variable P1；iAnd P2；iAnd an intermediate matrix WiAnd filter gain LiGiving an initial value, i.e.
Figure GDA0002809639170000129
(4.4) error-based System, from H∞The worst non-random bounded disturbance signal omega is obtained by the filtering algorithmi：
ωi(k)＝γ-2Δi -1(B1 TP1；iAL；i+D1 TDμ；iAΩ；i)ex；i(k) (9)
Therefore:
Wi＝γ-2Δi -1(B1 TP1；iAL；i+D1 TDμ；iAΩ；i) (10)
where k denotes the current discrete time, ωiRepresenting a non-random bounded perturbation signal, preset H∞The parameter γ is 2, the superscript "-1" denotes the inverse of the matrix, the superscript "T" denotes the transpose of the matrix, LiInput matrix representing filter gain to be designed, non-random bounded perturbation signal omega
Figure GDA0002809639170000131
Estimating an output matrix C of an object x1＝[1 1]，C0＝[1 1]，P1；i、AL；i、AΩ；i、Δi、AW；iAre all intermediate matrices, ex；iRepresenting the estimated object x and the corresponding estimated value
Figure GDA0002809639170000132
The difference of (a).
(4.5) based on the system error model under the worst non-random disturbance, passing through H2The filter algorithm obtains the filter gain LiExpression (c):
Figure GDA0002809639170000133
wherein the superscript "-1" represents the inverse of the matrix, the superscript "T" represents the transpose of the matrix,
Figure GDA0002809639170000134
a set of all observation matrices representing the ith node,
Figure GDA0002809639170000135
a set of observation matrices representing all white gaussian noise for the ith node,
Figure GDA0002809639170000136
a set of observation matrices representing all non-random bounded perturbation signals of the ith node,
Figure GDA0002809639170000137
to represent
Figure GDA0002809639170000138
Expectation of (1), white Gaussian noise ω0Input matrix of
Figure GDA0002809639170000139
Input matrix of non-random bounded perturbation signals omega
Figure GDA00028096391700001310
P2；i、Wi、AW；iAnd ΛiAre all intermediate matrices.
(4.6) when k is 1, it is obtained from formula (10) (11):
Wi(1)＝γ-2Δi -1(B1 TP1；i(0)AL；i(1)+D1 TDμ；iAΩ；i(1)) (12)
Figure GDA00028096391700001311
wherein k represents the current discretization time, k +1 represents the next discretization time, and the preset H is∞The parameter γ is 2, and the state transition matrix of the object is estimated
Figure GDA00028096391700001312
LiIndicating the gain of the filter that needs to be designed,
Figure GDA00028096391700001313
to represent
Figure GDA00028096391700001314
In the expectation that the position of the target is not changed,
Figure GDA00028096391700001315
a set of all observation matrices representing the ith node,
Figure GDA00028096391700001316
set of observation matrices, white Gaussian noise omega, representing all non-random bounded perturbation signals of the ith node0Input matrix of
Figure GDA00028096391700001317
Input matrix of non-random bounded perturbation signals omega
Figure GDA00028096391700001318
ω0Expressing white gaussian noise with mean 0 and variance 1, the nonrandom bounded perturbation signal is ω (k) |0.1 | (0.5 |) and the output matrix C of the object x is estimated1＝[1 1]，C0＝[1 1]，P1；i、AL；i、AΩ；i、Δi、P2；i、Wi、AW；iAnd ΛiAre all intermediate matrices.
(4.7) intermediate matrix P1；iThe following equation is satisfied:
Figure GDA0002809639170000141
thus obtaining an intermediate matrix P1；i(1)：
Figure GDA0002809639170000142
Wherein k represents the current discretization time, k +1 represents the next discretization time, a superscript of "-1" represents the inverse of the matrix, a superscript of "T" represents the transpose of the matrix, and a preset H∞The parameter γ is 2, and the state transition matrix of the object is estimated
Figure GDA0002809639170000143
LiIndicating the gain of the filter that needs to be designed,
Figure GDA0002809639170000144
to represent
Figure GDA0002809639170000145
In the expectation that the position of the target is not changed,
Figure GDA0002809639170000146
a set of all observation matrices representing the ith node,
Figure GDA0002809639170000147
set of observation matrices, white Gaussian noise omega, representing all non-random bounded perturbation signals of the ith node0Input matrix of
Figure GDA0002809639170000148
Input matrix of non-random bounded perturbation signals omega
Figure GDA0002809639170000149
ω0Expressing white gaussian noise with mean 0 and variance 1, the nonrandom bounded perturbation signal is ω (k) |0.1 | (0.5 |) and the output matrix C of the object x is estimated1＝[1 1]，C0＝[1 1]，P1；i、AL；i、AΩ；i、Δi、P2；i、Wi、AW；iAnd ΛiAre all intermediate matrices.
(4.8) obtaining an intermediate matrix P2；i(1)；
Figure GDA00028096391700001410
Thus obtaining an intermediate matrix P2；i(1)：
Figure GDA0002809639170000151
Wherein k represents the current discretization time, k +1 represents the next discretization time, a superscript of "-1" represents the inverse of the matrix, a superscript of "T" represents the transpose of the matrix, and a preset H∞The parameter γ is 2, and the state transition matrix of the object is estimated
Figure GDA0002809639170000152
LiIndicating the gain of the filter that needs to be designed,
Figure GDA0002809639170000153
to represent
Figure GDA0002809639170000154
In the expectation that the position of the target is not changed,
Figure GDA0002809639170000155
a set of all observation matrices representing the ith node,
Figure GDA0002809639170000156
observations representing all non-random bounded perturbation signals of the ith nodeSet of matrices, Gaussian white noise omega0Input matrix of
Figure GDA0002809639170000157
Input matrix of non-random bounded perturbation signals omega
Figure GDA0002809639170000158
The non-random bounded perturbation signal is taken as omega (k) |0.1 × sin (0.5 × k) |, omega0An output matrix C representing white Gaussian noise with a mean value of 0 and a variance of 1, which is an estimation target x1＝[1 1]，C0＝[1 1]，P1；i、AL；i、AΩ；i、Δi、P2；i、Wi、AW；iAnd ΛiAre all intermediate matrices.
(4.9) repeating the step (4.6), the step (4.7) and the step (4.8).
When k equals 150, the matrix P1；i(T) and matrix P1；i(T-1) the two-norm difference is less than a given error, resulting in:
Figure GDA0002809639170000159
similarly, if k equals T, the matrix P2；i(T) and matrix P2；i(T-1) the two-norm difference is less than a given error, resulting in:
Figure GDA00028096391700001510
wherein, P1；iAnd P2；iAll are intermediate matrices, and we select the data of the fifth node.
(4.10) combining the intermediate matrices P2；iSubstituting formula (11) to obtain filter gain matrix L of ith nodei. Thus, the filter equation (11) realizes real-time high-accuracy estimation of the estimation target x (i.e., the position of the moving object).
The invention designs an indoor positioning algorithm based on distributed mixed filtering, which solves two groups of equations through an iterative algorithm, constructs a plurality of filters to realize the multi-point real-time high-precision estimation of the coordinates of a moving object under the worst non-random disturbance signal.
The invention has the advantages that: the influence of a complex environment is considered, a system state equation and an observation equation are established aiming at the inaccuracy of an indoor positioning technology model, a filter is further constructed, and the position (an abscissa a and an ordinate b) of a moving object is estimated in real time and high-precision on the premise of ensuring the robustness and the anti-interference capacity of the system. The estimation result can meet the precision and real-time requirements of practical application, and no matter which indoor positioning technology is used, the algorithm can be well matched, and the requirement of indoor positioning is met.
The embodiments described in this specification are merely illustrative of implementations of the inventive concept and the scope of the present invention should not be considered limited to the specific forms set forth in the embodiments but rather by the equivalents thereof as may occur to those skilled in the art upon consideration of the present inventive concept.

Claims (1)

1. The indoor positioning estimation method based on distributed hybrid filtering specifically comprises the following steps:
1) establishing a plane rectangular coordinate system of the indoor environment, and then using
Figure FDA0002809639160000011
To represent the position of the moving object, wherein a represents the abscissa of the object and b represents the ordinate of the object;
2) considering mixed disturbance and model inaccuracy in the indoor environment, and establishing a system state equation and an observation equation of each sensor comprises the following steps:
(2.1) the discretized equation of state is:
x(k+1)＝Ax(k)+B0ω0(k)+B1ω(k) (1)
where k denotes the current discretization time, k +1 denotes the next discretization time, and the estimation object x denotes the position of the object, that is, x ═ is[a b]TThe superscript "T" denotes the transpose of the matrix, A denotes the state transition matrix of the estimation object x, ω0(k) Denotes white Gaussian noise with mean of zero and variance of 1, B0Representing white gaussian noise omega0ω (k) represents a non-random bounded perturbation signal, B1An input matrix representing a non-random bounded perturbation signal ω;
(2.2) after discretization, the observation equation of the ith sensor is as follows:
yi(k)＝C2；ix(k)+D1；iω0(k)+Diω(k) (2)
where k denotes the discretization time, x denotes the estimation object, C2；iAn observation matrix, ω, representing an estimated object of the i-th sensor0Denotes white Gaussian noise with mean 0 and variance 1, D1；iWhite Gaussian noise omega representing the ith sensor0ω is a non-random bounded perturbation signal, DiAn observation matrix representing non-random bounded perturbation signals of an ith sensor;
(2.3) output equation of the discretized system:
Figure FDA0002809639160000012
where k denotes the discretization time, z0Representing the output of the system, x representing the object of estimation, C1,C0An output matrix representing the estimation object x;
3) assuming that there are N nodes, each with a filter and a sensor, defining η to represent the set of all nodes, i.e., { 1.· N }; in distributed filtering, for the ith filter, the ith filter can not only receive an observed value y from the own sensori(k) And can receive the observed value of its neighbor sensor
Figure FDA0002809639160000021
Definition etaiSet representing the reception of neighbor observations by the ith nodeThat is to say have
Figure FDA0002809639160000022
Definition JiIs the set of the ith node itself and all its neighbors, i.e., Ji＝{i}∪ηi(ii) a Definition of
Figure FDA0002809639160000023
A set of all observations representing an ith node;
Figure FDA0002809639160000024
a set of all observation matrices representing the ith node;
Figure FDA0002809639160000025
a set of observation matrices representing all white gaussian noise for the ith node;
Figure FDA0002809639160000026
a set of observation matrices representing all non-random bounded perturbation signals of the ith node;
in the indoor positioning technology, data transmission is performed through a wireless sensor network, for an ith node, the observed value of a neighbor node can be received through the wireless sensor network, but disturbance can be received in the transmission process, so that data are lost; assuming that all observed values of the ith node receive m disturbances of independent and uniformly distributed random processes in the transmission process, recording as:
Figure FDA0002809639160000027
where k represents the discretized time,
Figure FDA0002809639160000028
representing all the data actually received by the ith node,
Figure FDA0002809639160000029
all neighbor node observations representing the ith node
Figure FDA00028096391600000210
Whether data transmitted to the ith node is lost;
designing a filter for each node:
Figure FDA00028096391600000211
where k denotes the current discretization time, k +1 denotes the next discretization time, a denotes the state transition matrix of the estimation object x,
Figure FDA00028096391600000212
an estimated value, L, representing an estimated object xiRepresenting the filter gain of the i-th node, C1，C0An output matrix representing the estimated object x,
Figure FDA00028096391600000213
a set of all observation matrices representing the ith node,
Figure FDA00028096391600000214
indicating the data actually received by the ith node,
Figure FDA0002809639160000031
all neighbor node observations representing the ith node
Figure FDA0002809639160000032
Whether data transmitted to the ith node is lost;
the effect of the filter is to make the estimate
Figure FDA0002809639160000033
Proximity system output z, z0FromThe real-time high-precision estimation of an estimation object x, namely a moving object is realized;
4) providing a system error model, further providing the system error model based on worst non-disturbance, designing and solving the gain of the filter through an iterative algorithm, and specifically comprising the following steps:
(4.1) obtaining an error system model through (1) (2) (3) (5):
Figure FDA0002809639160000034
where k denotes the current discrete time, k +1 denotes the next discrete time, ex；iRepresenting the estimated object x and the corresponding estimated value
Figure FDA0002809639160000035
Difference of (e)iRepresenting the system output z and the corresponding estimate
Figure FDA0002809639160000036
Difference of (e)0；iRepresenting system output z0And corresponding estimated value
Figure FDA0002809639160000037
A represents a state transition matrix of the estimation object, LiIndicating the gain of the filter that needs to be designed,
Figure FDA0002809639160000038
a set of all observation matrices representing the ith node;
Figure FDA0002809639160000039
a set of observation matrices representing all white gaussian noise for the ith node;
Figure FDA00028096391600000310
set of observation matrices, B, representing all the non-random bounded perturbation signals of the ith node0Is Gaussian white noise omega0Input matrix of, B1An input matrix of non-random bounded perturbation signals omega, omega0Representing white Gaussian noise with mean 0 and variance 1, omega being a non-random bounded perturbation signal, C1，C0An output matrix representing the estimation object x;
(4.2) defining the worst non-random bounded perturbation signal omega of the ith node based on an error systemi(k):
ωi(k)＝Wiex；i(k) (7)
Substituting (7) into an equation (6), and further obtaining a system error model under worst non-random disturbance:
Figure FDA0002809639160000041
(4.3) when k is 0, the intermediate matrix W is alignediAnd filter gain LiGiving an initial value, i.e.
Wi(0)，Li(0) (9)
(4.4) error-based System, from H∞The worst non-random bounded disturbance signal omega is obtained by the filtering algorithmi：
Figure FDA0002809639160000042
Therefore:
Wi＝γ-2Δi -1(B1 TP1；iAL；i+D1；i TDμ；iAΩ；i) (11)
where k denotes the current discrete time, ωiRepresenting a non-random bounded perturbation signal, gamma being H representing a preset∞The parameter, superscript "-1" denotes the inverse of the matrix, superscript "T" denotes the transpose of the matrix, B1An input matrix representing a non-random bounded perturbation signal omega,
Figure FDA0002809639160000043
set of observation matrices, D, representing all white Gaussian noises of the ith nodeμ；iRepresents DΦ；iExpectation of (D)Φ；iAn observed value y of the node itself representing the ith nodei(k) Whether data of (2) is lost, C1，C0An output matrix representing an estimated object x, AL；i、P1；i、AΩ；iAnd ΔiAre all intermediate matrices, ex；iRepresenting the estimated object x and the corresponding estimated value
Figure FDA00028096391600000410
A difference of (d);
(4.5) based on the system error model under the worst non-random disturbance, passing through H2The filter algorithm obtains the filter gain LiExpression (c):
Figure FDA0002809639160000044
wherein the superscript "-1" represents the inverse of the matrix, the superscript "T" represents the transpose of the matrix,
Figure FDA0002809639160000045
a set of all observation matrices representing the ith node,
Figure FDA0002809639160000046
a set of observation matrices representing all white gaussian noise for the ith node,
Figure FDA0002809639160000047
a set of observation matrices representing all non-random bounded perturbation signals of the ith node,
Figure FDA0002809639160000048
to represent
Figure FDA0002809639160000049
Expectation of (A), B0Is Gaussian white noise omega0Input matrix of, B1Input matrix, P, for non-random bounded perturbation signal omega2；i、Wi、AW；iAnd ΛiAre all intermediate matrices; a. theWRepresents all intermediate matrices AW；iA set of (a);
(4.6) when k is 1, it is obtained from formula (11) (12):
Wi(1)＝γ-2Δi -1(B1 TP1；i(0)AL；i+D1 TDμ；iAΩ；i) (13)
Figure FDA0002809639160000051
where A represents a state transition matrix of an estimation object, and γ represents a preset H∞The parameter, superscript "-1" denotes the inverse of the matrix, superscript "T" denotes the transpose of the matrix, LiIndicating the gain of the filter that needs to be designed,
Figure FDA0002809639160000052
to represent
Figure FDA0002809639160000053
In the expectation that the position of the target is not changed,
Figure FDA0002809639160000054
a set of all observation matrices representing the ith node,
Figure FDA0002809639160000055
set of observation matrices, B, representing all the non-random bounded perturbation signals of the ith node0Is Gaussian white noise omega0Input matrix of, B1Input matrix, C, for non-random bounded perturbation signal omega1，C0Representing the output of the estimation object xOut of matrix, AL；i、AΩ；i、Δi、Λi、AW；i、P1；iAnd P2；iAre all intermediate matrices; p1；i(0)，P2；i(0) When k is 0, P1；iAnd P2；iAn initial value assigned;
(4.7) intermediate matrix P1；iThe following equation is satisfied:
Figure FDA0002809639160000056
thus obtaining an intermediate matrix P1；i(1)：
Figure FDA0002809639160000057
Where k denotes the current discretization time, k +1 denotes the next discretization time, a denotes a state transition matrix of the estimation object, and γ denotes a preset H∞The parameter, superscript "-1" denotes the inverse of the matrix, superscript "T" denotes the transpose of the matrix, LiIndicating the gain of the filter that needs to be designed,
Figure FDA0002809639160000058
to represent
Figure FDA0002809639160000059
In the expectation that the position of the target is not changed,
Figure FDA00028096391600000510
a set of all observation matrices representing the ith node,
Figure FDA00028096391600000511
set of observation matrices, B, representing all the non-random bounded perturbation signals of the ith node1Input matrix, C, for non-random bounded perturbation signal omega1An output matrix representing an estimated object x, AL；i、Γi、AΩ；i、ΔiAnd P1；iAre all intermediate matrices;
(4.8) obtaining an intermediate matrix P2；i(1)；
Figure FDA0002809639160000061
Thus obtaining an intermediate matrix P2；i(1)：
Figure FDA0002809639160000062
Where k denotes the current discretization time, k +1 denotes the next discretization time, a denotes a state transition matrix of the estimation object, and γ denotes a preset H∞The parameter, superscript "-1" denotes the inverse of the matrix, superscript "T" denotes the transpose of the matrix, LiIndicating the gain of the filter that needs to be designed,
Figure FDA0002809639160000063
to represent
Figure FDA0002809639160000064
In the expectation that the position of the target is not changed,
Figure FDA0002809639160000065
a set of all observation matrices representing the ith node,
Figure FDA0002809639160000066
set of observation matrices, B, representing all the non-random bounded perturbation signals of the ith node1Input matrix for non-random bounded perturbation signal omega, AW；i、Wi、ΛiAnd P1；iAre all intermediate matrices;
(4.9) repeating the step (4.6), the step (4.7) and the step (4.8);
if k equals T time, the matrix P1(T) and matrix P1(T-1) two-norm of differenceLess than a given error, yields:
P1；i＝P1；i(T)＝P1；i(T-1) (19)
similarly, if k equals T, the matrix P2(T) and matrix P2(T-1) the two-norm difference is less than a given error, resulting in:
P2；i＝P2；i(T)＝P2；i(T-1) (20)
wherein, P1；iAnd P2；iAre all intermediate matrices;
(4.10) combining the intermediate matrices P2；iSubstituting formula (12) to obtain filter gain matrix L of ith nodei(ii) a Thus, the filter equation (12) realizes real-time high-precision estimation of the estimation object x.
CN201811415999.0A 2018-11-26 2018-11-26 Indoor positioning method based on distributed hybrid filtering Active CN109282820B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811415999.0A CN109282820B (en) 2018-11-26 2018-11-26 Indoor positioning method based on distributed hybrid filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811415999.0A CN109282820B (en) 2018-11-26 2018-11-26 Indoor positioning method based on distributed hybrid filtering

Publications (2)

Publication Number Publication Date
CN109282820A CN109282820A (en) 2019-01-29
CN109282820B true CN109282820B (en) 2021-05-11

Family

ID=65173088

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811415999.0A Active CN109282820B (en) 2018-11-26 2018-11-26 Indoor positioning method based on distributed hybrid filtering

Country Status (1)

Country Link
CN (1) CN109282820B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115442762B (en) * 2022-08-22 2024-05-03 浙江工业大学 Target tracking method based on distributed consistency filtering of wireless sensor network

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102589550A (en) * 2012-01-12 2012-07-18 山东轻工业学院 Method and system for realizing integrated navigation and accurate positioning by applying federal H-infinity filter
CN102622520B (en) * 2012-03-14 2015-08-19 北京航空航天大学 A kind of distributed multimode type estimation fusion method of maneuvering target tracking
US9599698B2 (en) * 2014-12-02 2017-03-21 Intel Corporation Enhanced positioning system using hybrid filter
CN107255795B (en) * 2017-06-13 2020-01-07 山东大学 Indoor mobile robot positioning method and device based on EKF/EFIR hybrid filtering
CN108413923B (en) * 2018-01-22 2020-07-24 浙江工业大学 Vehicle roll angle and pitch angle estimation method based on robust hybrid filtering

Also Published As

Publication number Publication date
CN109282820A (en) 2019-01-29

Similar Documents

Publication Publication Date Title
CN110849369B (en) Robot tracking method, device, equipment and computer readable storage medium
CN111536967B (en) EKF-based multi-sensor fusion greenhouse inspection robot tracking method
CN107677272B (en) AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering
CN108896047B (en) Distributed sensor network collaborative fusion and sensor position correction method
CN104318072B (en) QKF-MMF (Quantitative Kalman Filtering-Multi Method Fusion) based multi-sensor quantitative fusion method
Dehnavi et al. Three dimensional target tracking via underwater acoustic wireless sensor network
CN107727097B (en) Information fusion method and device based on airborne distributed position and attitude measurement system
CN107941211A (en) Multielement fusion and positioning method, device and electronic equipment based on Two-orders
CN109115228B (en) Target positioning method based on weighted least square volume Kalman filtering
CN109282819B (en) Ultra-wideband positioning method based on distributed hybrid filtering
Zhang et al. Tracking mobile robot in indoor wireless sensor networks
CN109282820B (en) Indoor positioning method based on distributed hybrid filtering
Sibley et al. A sliding window filter for incremental SLAM
Sahawneh et al. Factor graphs-based multi-robot cooperative localization: A study of shared information influence on optimization accuracy and consistency
Kong et al. Hybrid indoor positioning method of BLE and monocular VINS based smartphone
CN111928851B (en) TMA technology-based multi-autonomous underwater robot cluster collaborative navigation method
JP2011516825A (en) Object tracking method in three-dimensional space using acoustic sensor based on particle filter
Suseenthiran et al. Indoor positioning utilizing bluetooth low energy RSSI on LoRa system
CN116929343A (en) Pose estimation method, related equipment and storage medium
CN115755009A (en) Multi-underwater robot distributed cooperative positioning method and system
Li et al. Robust cooperative navigation for AUVs using the student's t distribution
CN115291168A (en) Underwater target cooperative positioning method and system based on maximum consistency
CN111216146B (en) Two-part consistency quantitative control method suitable for networked robot system
Fan et al. A modified tetrahedron shape measure and its application for 3D trilateration localization in mobile cluster networks
Ortiz et al. A framework for a relative real-time tracking system based on ultra-wideband technology

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