CN114994601A - Generalized Kalman filtering positioning method and system based on distance measurement - Google Patents
Generalized Kalman filtering positioning method and system based on distance measurement Download PDFInfo
- Publication number
- CN114994601A CN114994601A CN202210621595.7A CN202210621595A CN114994601A CN 114994601 A CN114994601 A CN 114994601A CN 202210621595 A CN202210621595 A CN 202210621595A CN 114994601 A CN114994601 A CN 114994601A
- Authority
- CN
- China
- Prior art keywords
- equation
- measurement
- distance measurement
- measurement equation
- generalized
- 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.)
- Pending
Links
- 238000005259 measurement Methods 0.000 title claims abstract description 84
- 238000001914 filtration Methods 0.000 title claims abstract description 40
- 238000000034 method Methods 0.000 title claims description 18
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 32
- 230000003044 adaptive effect Effects 0.000 claims abstract description 21
- 238000004364 calculation method Methods 0.000 claims abstract description 6
- 230000008859 change Effects 0.000 claims abstract description 6
- 238000000691 measurement method Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 description 8
- 230000008569 process Effects 0.000 description 6
- 238000005094 computer simulation Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000012886 linear function Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 230000000875 corresponding effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000005358 geomagnetic field Effects 0.000 description 1
- 230000002401 inhibitory effect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000005180 public health Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0252—Radio frequency fingerprinting
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Pure & Applied Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Computational Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Operations Research (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
The invention provides a generalized Kalman filtering positioning system based on distance measurement, which comprises a distance measurement calculation module and is characterized by also comprising: the finite order nonzero derivative measurement equation generation module is used for adjusting and changing the measurement equation to enable the Taylor series expansion of the nonlinear measurement equation to only contain finite terms; the self-adaptive noise mean value estimation module estimates and adjusts the measurement noise mean value in the changed measurement equation by using a self-adaptive algorithm; and the generalized Kalman filtering position resolving module is used for resolving the target position by applying the generalized Kalman filtering principle based on a system state equation and a measurement equation after adjustment and change and combining with an adaptive algorithm. And adjusting the changed measurement equation, wherein the partial derivatives above the third order are all zero, and the majority of the second partial derivatives are zero. Since the partial derivatives above the third order are all zero, all the terms above the third order of the Taylor series are zero, even if the deviation between the estimated value and the nominal value is large, the divergence of the Kalman filter cannot be easily caused.
Description
Technical Field
The invention relates to the technical field of wireless positioning navigation, in particular to a method and a system for positioning by using a wide-sense Kalman filtering based on distance measurement.
Background
With the continuous development of modern society, the urbanization process is accelerated, the number of large buildings is increased, more than 80% of people are in indoor environments (including underground, mines, tunnels and the like), catering, shopping and subway traffic become very important components in the life of people, and the demand of people on indoor position service is rapidly increased due to the changes. The fields of public safety, production safety, emergency rescue, public health, internet of things, special crowd monitoring, large-scale venue management, smart city construction and the like all need to use accurate indoor positioning information, so that the indoor navigation and location service industry has trillions of market spaces.
At present, the indoor positioning technology is various in types, and can be divided into two categories according to the size of a signal coverage area: the method comprises the following steps of indoor positioning of local areas, such as Wireless Local Area Network (WLAN), radio frequency tag (RFID), Zigbee (Zigbee), Bluetooth (BT), Ultra Wide Band (UWB), geomagnetic field intensity, infrared positioning, light tracking positioning, computer vision positioning, ultrasonic positioning and the like. Secondly, wide area indoor positioning, such as an assisted GPS (a-GPS) based mobile communication network, Pseudolite (Pseudolite), terrestrial digital communication and broadcast network positioning system, and the like. The bluetooth positioning technology has achieved wide coverage of networks due to its advantages of low cost, low power consumption and high positioning accuracy, and is widely applied to indoor positioning scenes.
The Bluetooth 5.0 is released, so that the power consumption is reduced, the positioning accuracy is greatly improved, and the positioning-Internet of things equipment enables indoor positioning to be more hot. The mode that bluetooth iBeacon was fixed a position mainly has two kinds: based on received Signal Strength rssi (received Signal Strength indication) and based on location fingerprints, or a combination of both. The biggest problem of positioning fingerprint based is that the manpower cost and time cost for acquiring fingerprint data in the early stage are very high, and the database is difficult to maintain. And if a new base station is added to a market or other modifications are made, the original fingerprint data may not be suitable any more. Currently, the mainstream bluetooth positioning technology in the market adopts a distance measurement triangulation algorithm and combines a distance measurement method based on a received signal strength RSSI value, namely, the distance between two bluetooth devices is calculated through the RSSI value and an indoor electromagnetic wave propagation loss model. The method is simple and easy to implement, and has the defects that the RSSI value of the Bluetooth signal is greatly influenced by the environment, the serious abnormal fluctuation of the ranging result is caused by signal attenuation and multi-path effect caused by the shielding of a moving human body on electromagnetic waves, and the positioning error is obviously increased. In fact, all indoor positioning technologies (such as wireless local area networks (WIFI), zigbee, radio frequency tags, etc.) that implement triangulation based on RSSI ranging suffer from the same problems.
Kalman filtering is an algorithm for performing optimal estimation on the system state by using a linear system state equation and inputting and outputting observation data through the system. The optimal estimation can also be seen as a filtering process, since the observed data includes the effects of noise and interference in the system. Kalman filtering is a linear minimum variance estimation, and describes the law of dynamic changes of the estimated quantity using a kinetic equation, i.e. a state equation. The basic equations of the Kalman filtering include a state equation describing the dynamic change rule of the estimated quantity and a measurement equation reflecting the relation between the observed quantity and the estimated quantity. In the standard kalman filter theory, both the equation of state and the measurement equation are linear equations (sets), and the kalman filter can output the optimal estimate of the system state. Therefore, the kalman filter is essentially a linear optimal estimation and cannot be directly applied to the estimation of a nonlinear system. By nonlinear system is meant an equation of state and an equation of measure, at least one of which is a nonlinear equation (set).
The state equation of a more general nonlinear continuous system can be expressed as (qinyong, flood axe, wangtaihua, 1998):
the measurement equation is expressed as:
Z(t)＝h[X(t)，t]+v(t)
where x (t) is the state quantity to be estimated, z (t) is the observed quantity (measured value), w (t) and v (t) are zero-mean white noises that are not correlated with each other, and g (t) is some deterministic function. f [ X (t), t ] is the system function, h [ X (t), t ] is the observation function; both are deterministic linear or non-linear functions, and at least one is a non-linear function.
Mathematical models of physical systems encountered in engineering practice tend to be non-linear. For example: inertial navigation systems on airplanes and ships, guidance systems for missiles, doppler navigation systems, satellite navigation systems, and many other industrial control systems, etc., are generally nonlinear systems. In order to solve the state estimation problem of a nonlinear system by applying the Kalman filtering theory, a generalized Kalman filtering algorithm is proposed. The basic idea is to first surround a certain nominal state X u (t) expanding the nonlinear system function and the observation function into a taylor series:
then, the deviation X (t) -X is assumed n (t) is sufficiently small in absolute value, ignoring the taylor series high order terms, and taking an approximation of one order:
the system after the approximation processing is a linear system, both the state equation and the measurement equation are linear equations (sets), and a standard Kalman filtering theory can be applied to obtain a suboptimal estimation of the state, namely a generalized Kalman filtering estimation.
From the above analysis, it can be seen that the generalized kalman filter ignores higher-order terms above the quadratic term of the taylor series. The effectiveness of the generalized kalman filter and the estimation error depend on whether the influence of the higher order terms is really negligible. High-order terms of Taylor series of system functions and observation functions in state equation and measurement equation
Are all high-order derivatives
With the higher power of deviation [ X (t) -X n (t)] m The product of (a). Although the generalized Kalman Filter Algorithm employs the optimal state estimate as the nominal state, i.e.
A smaller deviation can be obtained. However, in practical engineering application, it is difficult to ensure that the generalized kalman filter can always keep a sufficiently small deviation in the operation process in the face of any application scenario. The existing generalized kalman filter algorithm has no control over the magnitude of the high-order derivative in the taylor series, another aspect of the problem, and whether the absolute value of the high-order derivative is small enough is an unknown. Therefore, neglecting the truncation error introduced by the high-order term brings hidden danger to the stable operation of the generalized Kalman filter. That is, the generalized kalman filter requires that the system function and the observation function are smooth enough and weak non-linear, and if the function has strong non-linearity, a large state estimation error is caused, and the generalized kalman filter loses effectiveness.
Taking a bluetooth indoor two-dimensional positioning scene as an example, assuming that bluetooth beacons are arranged at N known positions, indoor positioning navigation is provided for people or assets (robots) carrying bluetooth devices. Bluetooth device whose position coordinate (x, y) is unknown and position coordinate (x) i ，y i ) The true distance between the ith beacons of (1) is:
the measurement equation is as follows:
due to the fact that
Is a non-linear (vector) function, so the measurement equation is a non-linear equation (set). According to the existing generalized Kalman filtering theory, Taylor series expansion needs to be carried out on a measurement equation and a first-order approximate value is taken. It is clear that,
the higher order partial derivatives for x (t), y (t) exist and are not zero. Therefore h [ X (t), t]Higher partial derivatives of X (t)
Also present and not zero. The magnitude of the higher order partial derivative is not controlled, and it is completely unknown whether its absolute value is small enough.
Disclosure of Invention
The invention provides a generalized Kalman filtering positioning method and system based on distance measurement, which can remarkably reduce the nonlinearity of a measurement equation by changing the measurement equation.
The technical problem to be solved by the invention is realized by the following technical scheme:
the invention provides a generalized Kalman filtering positioning system based on distance measurement, which comprises a distance measurement calculation module and is characterized by also comprising:
the finite order nonzero derivative measurement equation generation module is used for adjusting and changing the measurement equation to enable the Taylor series expansion of the nonlinear measurement equation to only contain finite terms;
the self-adaptive noise mean value estimation module estimates and adjusts the measurement noise mean value in the changed measurement process by using a self-adaptive algorithm;
and the generalized Kalman filtering position resolving module is used for resolving the target position by applying the generalized Kalman filtering principle based on a system state equation and a measurement equation after adjustment and change and combining with an adaptive algorithm.
Preferably, the adjusting the modified metrology equation is performed by simultaneously squaring both sides of the original metrology equation.
Preferably, the adaptive algorithm is SAGE-HUSA adaptive algorithm.
Preferably, the distance measurement calculation module performs bluetooth RSSI ranging.
A generalized Kalman filtering positioning method based on distance measurement comprises the steps of calculating and measuring the distance between a target to be positioned and each beacon, and is characterized by further comprising the following steps:
adjusting and changing the measurement equation to ensure that the Taylor series expansion of the nonlinear measurement equation only contains a finite term;
estimating the measurement noise mean value in the measurement equation after the adjustment and the change by using a self-adaptive algorithm;
based on a system state equation, adjusting the changed measurement equation, and combining with a self-adaptive algorithm, the generalized Kalman filtering principle is used for resolving the target position.
Preferably, the adjusting and changing measurement equation is to perform a square operation on both sides of the original measurement equation simultaneously.
Preferably, the adaptive algorithm is SAGE-HUSA adaptive algorithm.
Preferably, the distance measurement method is bluetooth RSSI ranging.
The invention has the beneficial effects that: the changed measurement equation is adjusted, the partial derivatives above the third order are all zero, the second partial derivatives are mostly zero, and the small amount (2) of the second partial derivatives is equal to 2 (smaller). Therefore, the expansion of the measurement equation (set) into a taylor series and the first approximation only neglects a few quadratic terms of the taylor series. In the generalized Kalman filtering algorithm, the optimal state estimation is adopted as a nominal state, and a leading strip with smaller deviation can be obtainedUnder the condition, the Taylor series first-order approximation of the linear measurement equation and the nonlinear state equation of the minimum high-order derivative generalized Kalman filtering technical scheme can describe the system quite accurately. Since the partial derivatives above the third order are all zero, it is ensured that the terms above the third order of the Taylor order number are all zero, even if the deviation between the estimated value and the nominal value is zero
Large, and does not easily cause kalman filter divergence.
Drawings
FIG. 1 is a finite order non-zero derivative adaptive generalized Kalman filtering process of the present invention;
FIG. 2 is a computer simulation verification scheme for the finite order non-zero derivative adaptive generalized Kalman filtering algorithm of the present invention;
FIG. 3 is a target position solution error obtained by computer simulation of the finite-order non-zero derivative adaptive generalized Kalman filtering algorithm of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is further described in detail with reference to the following embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
In order to avoid the problem that the Kalman estimation precision is reduced due to overlarge truncation error caused by neglecting a high-order term, a measurement equation is adjusted; the nonlinearity of the measurement equation/observation function is reduced, and the first-order approximation of the Taylor series is ensured to still be capable of accurately describing the system.
The reason why the high-order derivative of the bluetooth indoor two-dimensional positioning scene measurement equation is always not zero is that the h [ x (t), t ] function is a root form, i.e. a power function with an exponent of 0.5:
if the 0.5 power exponent is removed by squaring, the result is a polynomial containing only positive integer exponentials, the higher derivative of which must be zero. That is, the two sides of the aforementioned distance equation are squared simultaneously to obtain
d i_tr 2 ＝(x-x i ) 2 +(y-y i ) 2
Distance observation
n i (t) is zero-mean white noise. Distance measurement between the target device and different beacons is also an independent process, so no matter t j And t k Whether it is a different time, n i (t j ) And n l (t k ) Are all random variables that are independent of each other.
Due to the square of the distance observation
It can be known that
D i (t)＝d i 2 (t)＝d i_tr 2 (t)+v i (t)
Wherein
Mean value of the above noise
E[v i (t)]＝E[n i 2 (t)]
Covariance
Variance (variance)
From this, v is i (t) white noise with non-zero mean.
After the two sides of the distance equation are squared simultaneously, the two sides are processed by item shifting, and the following results are obtained:
d i_tr 2 -(x i 2 +y i 2 )＝-2xx i -2yy i +(X 2 +y 2 )
order to
v i (t)＝m i (t)+vv i (t) m i (t)＝E[v i (t)]
vv i (t) is zero-mean white noise, the variance of which
In the mean value m i (t) and its derivatives as the state quantities to be estimated, the new measurement equation:
z i (t)＝D i (t)-(x i 2 +y i 2 )＝d i_tr 2 (t)+v i (t)-(x i 2 +y i 2 )
＝-2xx i -2yy i +(x 2 +y 2 )+m i (t)+vv i (t)
the new measurement equation is
Namely, it is
Z(t)＝h[X(t)]+m(t)+vv(t)
m(t)＝[m 1 (t)，m 2 (t)，…，m N (t)] T
vv(t)＝[vv 1 (t)，vv 2 (t)，…，vv N (t)] T
Where T as superscript denotes the matrix transpose. Quantity of state
Then the
Wherein
w(t)＝[0，0，w x (t)，w y (t)] T
The equation of state is a linear equation (set) whose first approximation of the taylor series is the equation of state itself. The measurement equation (set) consists of N equations which determine the functional part
h[X(t)，t]＝[h 1 [X(t)，t]，h 2 [X(t)，t]，…，h N [X(t)，t]] T
h i [X(t)，t]＝x 2 (t)+y 2 (t)-2x(t)x i -2y(t)y i
According to the generalized Kalman filtering theory, the measurement equation (set) needs to be expanded into Taylor series and subjected to a first approximation treatment. The Taylor series expansion of the measurement equation(s) is the function of the observation h [ X (t), t [ ]]Is essentially each member function h of the Taylor series expansion 1 [X(t)，t]To h N [X(t)，t]Respectively performing Taylor series expansion on the components, and performing a member function h i [X(t)，t]Is a scalar multivariate function.
For the convenience of mathematical derivation, will
Is abbreviated as
Its non-zero first partial derivative
Its non-zero second partial derivative
Due to h i [X(t)，t]Partial derivatives above the third order are all zero, i.e.
The basic equation of a continuous system after a first order approximation of the taylor series is (qinyong, zhanghou, wangtaiwa, 1998):
δZ(t)＝H(t)δX(t)+m(t)+vv(t)
wherein, the first and the second end of the pipe are connected with each other,
w(t)＝[0，0，w x (t)，w y (t)] T
vv(t)＝[vv 1 (t)，vv 2 (t)，...，vv N (t)] T
w x (t)、w y (t)W m (t) and v i And (t)1 is more than or equal to i and less than or equal to N, which are uncorrelated zero-mean white noises.
E[w(t)w T (τ)]＝qδ(t-τ)
E[vv(t)vv T (τ)]＝rδ(t-τ)
Where δ (·) is the impulse function, q is the variance intensity matrix of w (t), and r is the variance intensity matrix of v (t).
By
h[X(t)，t]＝[h 1 [X(t)，t]，h 2 [X(t)，t]，…，h N [X(t)，t]] T
h i [X(t)，t]＝x 2 (t)+y 2 (t)-2x(t)x i -2y(t)y i
It can be known that
While
Therefore, it is
According to the linear system theory, the discretization form of the system equation is:
wherein
Let the filter period T be T k -t k-1 Sufficiently short, F (t) can be approximately regarded as a constant matrix,
F(t)≈F(t k-1 ) t k-1 ≤t＜t k
obtaining a one-step transfer matrix
Namely that
F k ＝F(t k )
Approximation of
To obtain
Wherein I is a unit matrix.
Measuring array
Therefore, the basic equation of the Kalman filtering after discretization processing can be obtained
δX k ＝Φ k，k-1 δX k-1 +W k-1
δZ k ＝H k δX k +m k +V k
It can be known that W k Is also zero mean and
wherein
The numerical algorithm for its values is as follows:
M 1 ＝q
M i+1 ＝F k M i +(F k M i ) T i≥1 F k ＝F(t k )
equation of measurement
δZ(t k )＝H(t k )δX(t k )+m(t k )+vv(t k )
And discrete forms corresponding thereto
δZ k ＝H k δX k +m k +V k
It can be known that
E[V k V j T ]＝E[vv(t k )vv T (t j )]＝rδ(t k -t j )＝R k δ k，j
R k ＝r
Due to the fact that
And is
If n is i (t) satisfies the Gaussian distribution, i.e. n i (t k ) Is zero mean white Gaussian noise
Thus, it is possible to provide
Kalman filtering equation of deviation
The adaptive filter is a filtering method with the function of inhibiting the divergence of a Kalman filter, and in the filtering calculation, on one hand, the predicted value is continuously corrected by using measurement, and meanwhile, unknown or uncertain system model parameters and noise statistical parameters are estimated or corrected. Estimation of statistical properties of measured noise according to SAGE-HUSA adaptive filtering principles (Lining, Tourette, Zhang Yongg, 2014)
d＝(1-b)/1-b K )
Where K is the total number of filter samples, 0< b < 1.
Filter gain array
One-step transfer variance matrix
Estimating mean square error matrix
Discrete nonlinear generalized Kalman filtering equation
Fig. 1 is a finite-order non-zero derivative adaptive generalized kalman filter flow of the present embodiment. Firstly, the distance between the device to be positioned and a beacon with a known position is calculated by using RSSI or signal arrival time, and an original observed quantity is generated. And removing the non-positive integer power exponent in the original observation quantity equation by utilizing power operation, so that the determined function part in the new observation quantity equation is a polynomial. And estimating the noise mean value in the new measurement equation by using SAGE-HUSA adaptive algorithm. Under the condition that the state equation is linear, the Taylor series expansion of the nonlinear measurement equation does not have more than three-order terms, and the first-order approximation of the Taylor series accurately describes the system characteristics. And finally, implementing generalized Kalman filtering based on the new measurement equation and the state equation.
Fig. 2 is a computer simulation verification scheme for the finite-order non-zero derivative adaptive generalized kalman filter algorithm of the present embodiment. According to the finite-order nonzero derivative self-adaptive generalized Kalman filter equation derived in the previous section, an MATLAB program implementation algorithm scheme is compiled, and a computer simulation test is performed. As shown in fig. 2, the experimental setup was to distribute 6 bluetooth beacons (star points in the figure) evenly along a circumference with a radius of 1000 meters. The travel route of the moving object to be positioned is shown in a red track. The two-dimensional coordinates of the initial position of the moving object are (500, -500), the initial velocity and the initial acceleration are both zero, and the acceleration is a random quantity thereafter.
Fig. 3 is a target position solution error obtained by performing computer simulation on the finite-order non-zero derivative adaptive generalized kalman filter algorithm according to the present embodiment. The MATLAB program performs a two-dimensional position estimation on the moving object. Fig. 3 shows an estimation error of the two-dimensional position estimation in the X-axis direction, an estimation error in the Y-axis direction, and a two-dimensional positioning error, respectively, from top to bottom. The kalman filter filtering initial values (position, velocity, acceleration, etc.) are all set to zero. As can be seen from FIG. 3, the finite-order non-zero derivative adaptive wide-sense Kalman filtering algorithm is fast in convergence, runs stably and works stably, and the theoretical design of the filter is proved to be basically correct and effective.
The technical scheme of the invention can be widely applied to any indoor and outdoor positioning systems based on ranging and triangulation, such as wireless local area networks, radio frequency tags, Zigbee (Zigbee), ultra-wideband radio and the like, Beidou, GPS (A-GPS), pseudolites, terrestrial digital communication and broadcast network positioning systems and the like, and is not limited to Bluetooth positioning equipment for RSSI ranging.
Claims (8)
1. The generalized Kalman filtering positioning system based on distance measurement comprises a distance measurement calculation module and is characterized by further comprising:
the finite order nonzero derivative measurement equation generation module is used for adjusting and changing the measurement equation to enable the Taylor series expansion of the nonlinear measurement equation to only contain finite terms;
the self-adaptive noise mean value estimation module estimates and adjusts the measurement noise mean value in the changed measurement equation by using a self-adaptive algorithm;
and the generalized Kalman filtering position resolving module is used for resolving the target position by applying the generalized Kalman filtering principle based on a system state equation and a measurement equation after adjustment and change and combining with an adaptive algorithm.
2. The generalized kalman filter positioning system based on distance measurement according to claim 1, wherein: the adjusting and changing measurement equation is to perform a square operation on both sides of the original measurement equation simultaneously.
3. The generalized kalman filter positioning system based on distance measurement according to claim 1, wherein: the adaptive algorithm is SAGE-HUSA adaptive algorithm.
4. The generalized kalman filter positioning system based on distance measurement according to claim 1, wherein: the distance measurement calculation module performs bluetooth RSSI ranging.
5. A generalized kalman filter positioning method based on distance measurement, operating on the generalized kalman filter positioning system based on distance measurement according to any one of claims 1 to 4, comprising calculating and measuring the distance between the target to be positioned and each beacon, further comprising:
adjusting and changing the measurement equation to ensure that the Taylor series expansion of the nonlinear measurement equation only contains finite terms;
estimating the measurement noise mean value in the measurement equation after the adjustment and the change by using a self-adaptive algorithm;
based on a system state equation, adjusting the changed measurement equation and combining with a self-adaptive algorithm, the target position is solved by applying the generalized Kalman filtering principle.
6. The generalized Kalman filter positioning method based on distance measurement according to claim 5, characterized in that: the adjusting and changing measurement equation is to perform a square operation on both sides of the original measurement equation simultaneously.
7. The generalized Kalman filter positioning method based on distance measurement according to claim 5, characterized in that: the adaptive algorithm is SAGE-HUSA adaptive algorithm.
8. The generalized Kalman filter positioning method based on distance measurement according to claim 5, characterized in that: the distance measurement method is bluetooth RSSI ranging.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210621595.7A CN114994601A (en) | 2022-06-02 | 2022-06-02 | Generalized Kalman filtering positioning method and system based on distance measurement |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210621595.7A CN114994601A (en) | 2022-06-02 | 2022-06-02 | Generalized Kalman filtering positioning method and system based on distance measurement |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114994601A true CN114994601A (en) | 2022-09-02 |
Family
ID=83031179
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210621595.7A Pending CN114994601A (en) | 2022-06-02 | 2022-06-02 | Generalized Kalman filtering positioning method and system based on distance measurement |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114994601A (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20070213933A1 (en) * | 2006-03-08 | 2007-09-13 | Honeywell International Inc. | Methods and systems for implementing an iterated extended Kalman filter within a navigation system |
| KR20150053311A (en) * | 2013-11-07 | 2015-05-18 | 목포대학교산학협력단 | Effective Indoor Positioning Method in Wireless Sensor Network |
| CN105676181A (en) * | 2016-01-15 | 2016-06-15 | 浙江大学 | Underwater moving target extended Kalman filtering tracking method based on distributed sensor energy ratios |
| CN106646356A (en) * | 2016-11-23 | 2017-05-10 | 西安电子科技大学 | Nonlinear system state estimation method based on Kalman filtering positioning |
| CN106931966A (en) * | 2017-02-24 | 2017-07-07 | 西北工业大学 | A kind of Combinated navigation method based on the fitting of Taylor's high-order remainder |
| US20200132775A1 (en) * | 2017-06-14 | 2020-04-30 | Mitsubishi Electric Corporation | State estimation device |
| AU2021100689A4 (en) * | 2021-02-03 | 2021-04-22 | Tongji University | GNSS (Global Navigation Satellite System) Dynamic Kalman Filter Method In Autonomous Cooperation |
| CN112731372A (en) * | 2020-12-21 | 2021-04-30 | 杭州电子科技大学 | State estimation method based on additive latent variable extended Vickerman filtering |
-
2022
- 2022-06-02 CN CN202210621595.7A patent/CN114994601A/en active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20070213933A1 (en) * | 2006-03-08 | 2007-09-13 | Honeywell International Inc. | Methods and systems for implementing an iterated extended Kalman filter within a navigation system |
| KR20150053311A (en) * | 2013-11-07 | 2015-05-18 | 목포대학교산학협력단 | Effective Indoor Positioning Method in Wireless Sensor Network |
| CN105676181A (en) * | 2016-01-15 | 2016-06-15 | 浙江大学 | Underwater moving target extended Kalman filtering tracking method based on distributed sensor energy ratios |
| CN106646356A (en) * | 2016-11-23 | 2017-05-10 | 西安电子科技大学 | Nonlinear system state estimation method based on Kalman filtering positioning |
| CN106931966A (en) * | 2017-02-24 | 2017-07-07 | 西北工业大学 | A kind of Combinated navigation method based on the fitting of Taylor's high-order remainder |
| US20200132775A1 (en) * | 2017-06-14 | 2020-04-30 | Mitsubishi Electric Corporation | State estimation device |
| CN112731372A (en) * | 2020-12-21 | 2021-04-30 | 杭州电子科技大学 | State estimation method based on additive latent variable extended Vickerman filtering |
| AU2021100689A4 (en) * | 2021-02-03 | 2021-04-22 | Tongji University | GNSS (Global Navigation Satellite System) Dynamic Kalman Filter Method In Autonomous Cooperation |
Non-Patent Citations (4)
| Title |
|---|
| KONATOWSKI, S 等: "A comparison of estimation accuracy by the use of KF, EKF & UKF filters", COMPUTATIONAL METHODS AND EXPERIMENTAL MEASUREMENTS XIII, vol. 46, 1 January 2007 (2007-01-01), pages 779 - 789 * |
| 杨鹏生 等: "改进扩展卡尔曼滤波算法的目标跟踪算法", 计算机工程与应用, vol. 52, no. 05, 17 March 2014 (2014-03-17), pages 71 - 74 * |
| 熊奎允 等: "基于无迹卡尔曼滤波器的雷达测距算法研究", 软件导刊, vol. 16, no. 01, 31 January 2017 (2017-01-31), pages 32 - 34 * |
| 胡雄飞 等: "基于UD迭代扩展卡尔曼滤波的纯方位机动目标定位与跟踪", 指挥控制与仿真, vol. 37, no. 06, 15 December 2015 (2015-12-15), pages 66 - 70 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN101191832B (en) | Wireless sensor network node position finding process based on range measurement | |
| KR101240629B1 (en) | Detecting and localization method of unknown signal using aircraft with ads-b system | |
| CN112533163B (en) | Indoor positioning method based on NB-IoT (NB-IoT) improved fusion ultra-wideband and Bluetooth | |
| Ni et al. | UWB indoor positioning application based on Kalman filter and 3-D TOA localization algorithm | |
| Daiya et al. | Experimental analysis of RSSI for distance and position estimation | |
| WO2006088599A1 (en) | Systems and methods for positioning using multipath signals | |
| Dong | A low-cost NLOS identification and mitigation method for UWB ranging in static and dynamic environments | |
| Chen et al. | An indoor positioning technique based on fuzzy logic | |
| Zhang et al. | Three-dimensional localization algorithm of WSN nodes based on RSSI-TOA and single mobile anchor node | |
| Cheng et al. | Real time indoor positioning system for smart grid based on uwb and artificial intelligence techniques | |
| Song et al. | Fingerprinting localization method based on toa and particle filtering for mines | |
| Zhu et al. | EKF localization based on TDOA/RSS in underground mines using UWB ranging | |
| Pu et al. | Fingerprint-based localization performance analysis: From the perspectives of signal measurement and positioning algorithm | |
| Liu et al. | Lora RTT ranging characterization and indoor positioning system | |
| Alavijeh et al. | Localization improvement in wireless sensor networks using a new statistical channel model | |
| Peneda et al. | Trilateration for indoors positioning within the framework of wireless communications | |
| Tarrio et al. | An RSS localization method based on parametric channel models | |
| Jose et al. | Taylor series method in TDOA approach for indoor positioning system. | |
| Wu et al. | Motion parameter capturing of multiple mobile targets in robotic sensor networks | |
| Gao et al. | An improved localization method in cyber-social environments with obstacles | |
| CN114994601A (en) | Generalized Kalman filtering positioning method and system based on distance measurement | |
| Ismail et al. | Comparison of wireless sensor node localisation between trilateration and multi-lateration methods using rssi | |
| Wang et al. | Gaussian filtered rssi-based indoor localization in wlan using bootstrap filter | |
| KR101459915B1 (en) | Method of Localization | |
| Li et al. | Performance Analysis of Collaborative Positioning Method in Automated Car Driving. |
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 |