CN112800889A - Target tracking method based on distributed matrix weighting and Gaussian filtering fusion - Google Patents

Target tracking method based on distributed matrix weighting and Gaussian filtering fusion Download PDF

Info

Publication number
CN112800889A
CN112800889A CN202110060141.2A CN202110060141A CN112800889A CN 112800889 A CN112800889 A CN 112800889A CN 202110060141 A CN202110060141 A CN 202110060141A CN 112800889 A CN112800889 A CN 112800889A
Authority
CN
China
Prior art keywords
covariance
estimation
calculating
local
error
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110060141.2A
Other languages
Chinese (zh)
Other versions
CN112800889B (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 CN202110060141.2A priority Critical patent/CN112800889B/en
Publication of CN112800889A publication Critical patent/CN112800889A/en
Application granted granted Critical
Publication of CN112800889B publication Critical patent/CN112800889B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2218/00—Aspects of pattern recognition specially adapted for signal processing
    • 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/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques

Abstract

A target tracking method based on distributed matrix weighting fusion Gaussian filtering is characterized in that Gaussian filtering is used for local state estimation, then statistical linear regression is used for approximating error cross covariance between local estimation, finally, an optimization parameter under a maximum likelihood criterion is obtained by solving an optimization problem, and an error cross covariance matrix is adjusted. The invention reduces the performance loss caused by the linearization error in the statistical linear regression and improves the target tracking precision.

Description

Target tracking method based on distributed matrix weighting and Gaussian filtering fusion
Technical Field
The invention belongs to the field of moving target tracking, and particularly relates to a target tracking method based on distributed matrix weighting and Gaussian filtering.
Background
The target tracking is a basic problem in the fields of military and civil use, and plays an important role in the fields of military national defense, urban traffic, family service and the like. In recent years, communication technology and microelectronic technology are rapidly developed, wireless sensor networks are widely applied to positioning and tracking of moving targets, and the requirement of people on target tracking accuracy is higher and higher.
In moving object tracking, nonlinear filtering problems are often involved. Gaussian filtering is a kind of nonlinear filtering method, and is widely used in practical systems. However, only a single sensor is considered in gaussian filtering, and the estimation accuracy often cannot meet the requirement, so that data of a plurality of sensors needs to be fused to obtain estimation with higher accuracy. Although the global optimal estimation can be achieved by centralized fusion, the robustness and reliability are poor compared with distributed fusion due to the error of sensor information and the complexity of calculation. The matrix weighted fusion is an optimal weighted fusion criterion in the sense of minimum mean square error, but it needs to know the cross covariance between local filters, and in the case of a linear system, the cross covariance can be deduced as an analytic solution by using Kalman filtering. However, in a nonlinear system, the state of the system and the measurement equation are complicated, the error cross-covariance between local filters cannot be obtained, and the classical fusion criterion is not applicable.
Disclosure of Invention
In order to solve the problem that the error covariance between local filters cannot be obtained when the existing nonlinear moving target tracking method is fused, the invention provides a target tracking method based on distributed matrix weighting fusion Gaussian filtering, and the target tracking precision and robustness are improved.
In order to achieve the purpose, the invention provides the following technical scheme:
a target tracking method based on a distributed matrix weighting fusion Gaussian filter comprises the following steps:
step 1: establishing a nonlinear system state space model and a measurement model, wherein the process comprises the following steps:
1.1 establishing a System State model
xk+1＝f(xk)+wk (1)
Wherein xkSystem state at time k, f (x)k)∈RnIs an arbitrary non-linear vector function, wkAs covariance of QkWhite gaussian noise of (1);
1.2 building a system measurement model
Figure BDA0002902058200000021
Where i is the observation station number,
Figure BDA0002902058200000022
the measured value of the observation station i at the moment k +1,
Figure BDA0002902058200000023
for any non-linear function of the vector,
Figure BDA0002902058200000024
is covariance of
Figure BDA0002902058200000025
White gaussian noise of (1);
step 2: calculating local Gaussian filter estimation and covariance by the following process:
2.1 initialization x0|0And P0|0，k＝0；
2.2 definition
Figure BDA0002902058200000026
Where g (x) is an arbitrary non-linear function,
Figure BDA0002902058200000027
is mean value of
Figure BDA0002902058200000028
Normal distribution with covariance P, L system state dimension, S Chislesky decomposition with P, ej∈RL×1Row 1 for j and row 0 for other row;
2.3 calculating State prediction values
Figure BDA0002902058200000031
And state prediction covariance
Figure BDA0002902058200000032
Figure BDA0002902058200000033
Figure BDA0002902058200000034
2.4 calculating State estimation
Figure BDA0002902058200000035
And state estimation covariance
Figure BDA0002902058200000036
Figure BDA0002902058200000037
Figure BDA0002902058200000038
Wherein
Figure BDA0002902058200000039
Figure BDA00029020582000000310
Figure BDA00029020582000000311
Figure BDA00029020582000000312
And step 3: calculating the local estimation cross covariance and the fusion estimation, and the process is as follows:
3.1 calculating linearization parameters
Figure BDA00029020582000000313
And
Figure BDA00029020582000000314
Figure BDA00029020582000000315
Figure BDA00029020582000000316
wherein
Figure BDA00029020582000000317
3.2 computing local error cross-covariance
Figure BDA00029020582000000318
Figure BDA0002902058200000041
3.3 computing fusion estimates
Figure BDA0002902058200000042
Figure RE-GDA0002996429770000043
Wherein the optimized parameter lambda is based on the maximum likelihood criterion of the measurementkThe optimization problem is solved and obtained;
first, define
Figure BDA0002902058200000044
Solving for
Figure BDA0002902058200000045
In the step 3, the error cross covariance between the local estimates is approximated by using a statistical linear regression method, the optimization parameters under the maximum likelihood criterion are obtained by solving the optimization problem, and the error cross covariance matrix is adjusted, so that the performance loss caused by the linearization error in the statistical linear regression is reduced.
The invention has the following beneficial effects: the invention provides a target tracking method based on distributed matrix weighting fusion Gaussian filtering. The method first uses Gaussian filtering to estimate the local state, and then uses a statistical linear regression method to approximate the error cross covariance between the local estimates. And finally, obtaining an optimized parameter under a maximum likelihood criterion by solving an optimization problem, adjusting an error cross covariance matrix, reducing performance loss caused by a linearization error in statistical linear regression, and improving target tracking precision.
Drawings
Fig. 1 is a schematic diagram of a robot target tracking system.
FIG. 2 is a control flow chart of the present invention.
Fig. 3 is a trajectory and tracking estimation of a mobile robot.
Fig. 4 is a diagram illustrating accumulated errors.
Detailed Description
The invention is further described below with reference to the accompanying drawings.
Referring to fig. 1 to 4, a target tracking method based on a distributed matrix weighted fusion gaussian filter includes the following steps:
step 1: establishing a nonlinear system state space model and a measurement model, wherein the process comprises the following steps:
1.1 establishing a System State model
xk+1＝f(xk)+wk (1)
Wherein xkSystem state at time k, f (x)k)∈RnIs an arbitrary non-linear vector function, wkAs covariance of QkWhite gaussian noise of (1);
1.2 building a system measurement model
Figure BDA0002902058200000051
Where i is the observation station number,
Figure BDA0002902058200000052
the measured value of the observation station i at the moment k +1,
Figure BDA0002902058200000053
for any non-linear function of the vector,
Figure BDA0002902058200000054
is covariance of
Figure BDA0002902058200000055
White gaussian noise of (1);
step 2: calculating local Gaussian filter estimation and covariance by the following process:
2.1 initialization x0|0And P0|0，k＝0；
2.2 definition
Figure BDA0002902058200000061
Where g (x) is an arbitrary non-linear function,
Figure BDA0002902058200000062
is mean value of
Figure BDA0002902058200000063
Normal distribution with covariance P, L system state dimension, S Chislesky decomposition with P, ejRow 1 for j and row 0 for other row;
2.3 calculating State prediction values
Figure BDA0002902058200000064
And state prediction covariance
Figure BDA0002902058200000065
Figure BDA0002902058200000066
Figure BDA0002902058200000067
2.4 calculating State estimation
Figure BDA0002902058200000068
And state estimation covariance
Figure BDA0002902058200000069
Figure BDA00029020582000000610
Figure BDA00029020582000000611
Wherein
Figure BDA00029020582000000612
Figure BDA00029020582000000613
Figure BDA00029020582000000614
Figure BDA00029020582000000615
And step 3: calculating the local estimation cross covariance and the fusion estimation, and the process is as follows:
3.1 calculating linearization parameters
Figure BDA00029020582000000616
And
Figure BDA00029020582000000617
Figure BDA0002902058200000071
Figure BDA0002902058200000072
wherein
Figure BDA0002902058200000073
3.2 computing local error cross-covariance
Figure BDA0002902058200000074
Figure BDA0002902058200000075
3.3 computing fusion estimates
Figure BDA0002902058200000076
Figure RE-GDA0002996429770000077
Wherein the optimized parameter lambda is based on the maximum likelihood criterion of the measurementkThe optimization problem is solved and obtained;
first, define
Figure BDA0002902058200000078
Solving for
Figure BDA0002902058200000079
To verify the effectiveness of the method designed by the present invention, the following example was used for verification.
As shown in fig. 1, the robot is in a wireless sensor network, and the robot is tracked by a distributed fusion estimation method, and a motion model of the robot is shown as (19):
Figure BDA0002902058200000081
wherein s isx,k，sy,kIs the position coordinate of the robot at time k, thetakIs the direction of the robot at time k, vl,k，vr,kThe speed of the left and right wheels of the robot, and d is the distance from the left and right wheels to the center of the robot;
considering the system process noise, the model is rewritten as:
Figure BDA0002902058200000082
wherein
Figure BDA0002902058200000083
The measurement equation of the system is shown in equation (21):
Figure BDA0002902058200000084
wherein (a)i,bi) Is the sensor location.
The target tracking algorithm of the present invention is simulated as follows, and the parameters are set as follows: Δ t ═ 0.5, Qk＝diag(0.01,0.01,(π/180)2)，
Figure BDA0002902058200000085
uv＝5， uw＝0.125，(a1,b1)＝(0,0)，(a2,b2)＝(200,0)，(a3,b3)＝(200,200)， (a4,b4)＝(0,200)。
Defining the accumulated error of the system as shown in equation (22)
Figure BDA0002902058200000091
The results are shown in fig. 3-4, fig. 3 is a track and tracking estimation of the mobile robot, and fig. 4 is a schematic diagram of accumulated errors, which shows that the method provided by the present invention has a good tracking estimation effect.
While the foregoing has described a preferred embodiment of the invention, it will be appreciated that the invention is not limited to the embodiment described, but is capable of numerous modifications without departing from the basic spirit and scope of the invention as set out in the appended claims.

Claims (2)

1. A target tracking method based on distributed matrix weighting and Gaussian filtering is characterized by comprising the following steps:
step 1: establishing a nonlinear system state space model and a measurement model, wherein the process comprises the following steps:
1.1 establishing a System State model
xk+1＝f(xk)+wk (1)
Wherein xkSystem state at time k, f (x)k)∈RnIs an arbitrary non-linear vector function, wkAs covariance of QkWhite gaussian noise of (1);
1.2 building a system measurement model
Figure FDA0002902058190000011
Where i is the observation station number,
Figure FDA0002902058190000012
the measured value of the observation station i at the moment k +1,
Figure FDA0002902058190000013
for any non-linear function of the vector,
Figure FDA0002902058190000014
is covariance of
Figure FDA0002902058190000015
White gaussian noise of (1);
step 2: calculating local Gaussian filter estimation and covariance by the following process:
2.1 initialization x0|0And P0|0，k＝0；
2.2 definition
Figure FDA0002902058190000016
Where g (x) is an arbitrary non-linear function,
Figure FDA0002902058190000017
is mean value of
Figure FDA0002902058190000018
Normal distribution with covariance P, L system state dimension, S Chislesky decomposition with P, ej∈RL×1Row 1 for j and row 0 for other row;
2.3 calculating State prediction values
Figure FDA0002902058190000019
And state prediction covariance
Figure FDA00029020581900000110
Figure FDA00029020581900000111
Figure FDA0002902058190000021
2.4 calculating State estimation
Figure FDA0002902058190000022
And state estimation covariance
Figure FDA0002902058190000023
Figure FDA0002902058190000024
Figure FDA0002902058190000025
Wherein
Figure FDA0002902058190000026
Figure FDA0002902058190000027
Figure FDA0002902058190000028
Figure FDA0002902058190000029
And step 3: calculating the local estimation cross covariance and the fusion estimation, and the process is as follows:
3.1 calculating linearization parameters
Figure FDA00029020581900000210
And
Figure FDA00029020581900000211
Figure FDA00029020581900000212
Figure FDA00029020581900000213
wherein
Figure FDA00029020581900000214
3.2 computing local error cross-covariance
Figure FDA00029020581900000215
Figure FDA00029020581900000216
3.3 computing fusion estimates
Figure FDA00029020581900000217
Figure FDA0002902058190000031
Wherein the optimized parameter lambda is based on the maximum likelihood criterion of the measurementkThe optimization problem is solved and obtained;
first, define
Figure FDA0002902058190000032
Solving for
Figure FDA0002902058190000033
2. The method as claimed in claim 1, wherein in step 3, the error cross-covariance between local estimates is approximated by a statistical linear regression method, and the optimization parameters under the maximum likelihood criterion are obtained by solving the optimization problem, and the error cross-covariance matrix is adjusted to reduce the performance loss caused by the linearization error in the statistical linear regression.
CN202110060141.2A 2021-01-18 2021-01-18 Target tracking method based on distributed matrix weighted fusion Gaussian filtering Active CN112800889B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110060141.2A CN112800889B (en) 2021-01-18 2021-01-18 Target tracking method based on distributed matrix weighted fusion Gaussian filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110060141.2A CN112800889B (en) 2021-01-18 2021-01-18 Target tracking method based on distributed matrix weighted fusion Gaussian filtering

Publications (2)

Publication Number Publication Date
CN112800889A true CN112800889A (en) 2021-05-14
CN112800889B CN112800889B (en) 2024-02-23

Family

ID=75809939

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110060141.2A Active CN112800889B (en) 2021-01-18 2021-01-18 Target tracking method based on distributed matrix weighted fusion Gaussian filtering

Country Status (1)

Country Link
CN (1) CN112800889B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113334388A (en) * 2021-07-08 2021-09-03 清华大学 Robot kinematics calibration method and calibration device based on local linear regression

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106950562A (en) * 2017-03-30 2017-07-14 电子科技大学 A kind of state fusion method for tracking target based on predicted value measurement conversion
CN107390199A (en) * 2017-09-20 2017-11-24 哈尔滨工业大学（威海） A kind of radar maneuvering target tracking waveform design method
CN108983215A (en) * 2018-05-25 2018-12-11 哈尔滨工程大学 A kind of method for tracking target based on maximum cross-correlation entropy adaptively without mark particle filter
CN110426689A (en) * 2019-07-02 2019-11-08 中国航空工业集团公司雷华电子技术研究所 A kind of airborne multi-platform Multi-sensor systematic error registration Algorithm based on EM-CKS

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106950562A (en) * 2017-03-30 2017-07-14 电子科技大学 A kind of state fusion method for tracking target based on predicted value measurement conversion
CN107390199A (en) * 2017-09-20 2017-11-24 哈尔滨工业大学（威海） A kind of radar maneuvering target tracking waveform design method
CN108983215A (en) * 2018-05-25 2018-12-11 哈尔滨工程大学 A kind of method for tracking target based on maximum cross-correlation entropy adaptively without mark particle filter
CN110426689A (en) * 2019-07-02 2019-11-08 中国航空工业集团公司雷华电子技术研究所 A kind of airborne multi-platform Multi-sensor systematic error registration Algorithm based on EM-CKS

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113334388A (en) * 2021-07-08 2021-09-03 清华大学 Robot kinematics calibration method and calibration device based on local linear regression
CN113334388B (en) * 2021-07-08 2022-12-02 清华大学 Robot kinematics calibration method and calibration device based on local linear regression

Also Published As

Publication number Publication date
CN112800889B (en) 2024-02-23

Similar Documents

Publication Publication Date Title
CN108802707B (en) Improved Kalman filtering method for target tracking
CN110244715B (en) Multi-mobile-robot high-precision cooperative tracking method based on ultra wide band technology
CN103925925B (en) A kind of real-time high-precision position calculation method for multipoint location system
CN111178385B (en) Target tracking method for robust online multi-sensor fusion
CN113074739B (en) UWB/INS fusion positioning method based on dynamic robust volume Kalman
CN107315171B (en) Radar networking target state and system error joint estimation algorithm
CN108120438B (en) Indoor target rapid tracking method based on IMU and RFID information fusion
CN110926485B (en) Mobile robot positioning method and system based on linear features
CN110031798A (en) A kind of indoor objects tracking based on simplified Sage-Husa adaptive-filtering
CN104936147A (en) Positioning method based on building layout constraint under complex indoor environment
CN111982102A (en) BP-EKF-based UWB-IMU positioning method in complex environment
CN106780567A (en) A kind of immune particle filter extension method for tracking target of Fusion of Color and histogram of gradients
CN112367614A (en) LSTM-based Wi-Fi and geomagnetic field fusion indoor positioning algorithm
CN112800889B (en) Target tracking method based on distributed matrix weighted fusion Gaussian filtering
CN114236480A (en) Airborne platform sensor system error registration algorithm
CN110501686A (en) A kind of method for estimating state based on NEW ADAPTIVE high-order Unscented kalman filtering
CN112034713B (en) Method and system for estimating optimal state of moving target in non-ideal network environment
CN113324544A (en) Indoor mobile robot co-location method based on UWB/IMU (ultra wide band/inertial measurement unit) of graph optimization
Tu et al. 3-D TDOA/AOA location based on Extended Kalman Filter
CN109375159B (en) Pure orientation weighting constraint total least square positioning method
CN107966697B (en) Moving target tracking method based on progressive unscented Kalman
CN111107070A (en) Target tracking method under DoS attack
Zhang et al. Analysis of the NLOS channel environment of TDOA multiple algorithms
CN113189578B (en) Extended target tracking method
CN113438596B (en) Beidou and 5G fusion-oriented millimeter wave low-delay beamforming method

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