CN108120438B - Indoor target rapid tracking method based on IMU and RFID information fusion - Google Patents

Abstract

The invention provides an indoor target rapid tracking method based on IMU and RFID information fusion, which is characterized in that a target tracking method is selected according to a comparison result of a sampling period and a threshold value of RFID, if the sampling period of the RFID is smaller than the threshold value, coordinates of RFID measurement data are estimated through an insensitive Kalman filter, and coordinates of a maneuvering target are obtained; if the sampling period of the RFID is larger than or equal to the threshold value, the distance between the target coordinates estimated twice is used as an estimated distance, and the motion coordinates of the target in each sampling interval are estimated by using the estimated distance and the course angle data; whether unprocessed original measurement data exist in the RFID measurement system is also judged, and if the unprocessed original measurement data exist in the RFID measurement system, the estimation process is carried out again; and if not, performing supplementary calculation on the motion of the target according to an attitude calculation method based on the IMU data. The indoor target can be quickly tracked, the tracking precision is improved, and the estimation result cannot cause error accumulation along with the time.

Description

Indoor target rapid tracking method based on IMU and RFID information fusion

Technical Field

The invention relates to the technical field of target tracking, in particular to an indoor target rapid tracking method based on IMU and RFID information fusion.

Background

With the development of science and technology, the indoor tracking and positioning technology becomes a hot spot of people's attention. For example, how to sense the position of the robot and reach the destination in an indoor place such as a large hospital, an airport, a train station, etc., how to track the position of moving goods in a large warehouse to realize automation of moving goods, how to sense the position of the robot during movement in a public place, etc.

People usually use GPS positioning and other methods to acquire the coordinate position of an outdoor target, however, the technology has great error in determining the position of an indoor moving target. Common tracking technologies for indoor maneuvering target movement include Wi-Fi technology, Radio Frequency Identification (RFID) technology, inertial navigation technology, optical tracking and positioning technology, sensor network positioning technology, infrared positioning technology, and the like. However, the development of the technologies is still in a starting stage, some indoor targets are not high in tracking accuracy and difficult to meet the requirements of practical application, and some indoor targets are higher in accuracy and higher in cost and difficult to put into practical use.

RFID, Radio frequency identification (Radio frequency tag), is a major component of an indoor positioning and tracking system. The RFID system is composed of a radio frequency tag and a reader, and identification information of the tag is stored in the radio frequency tag. When the target with the tag moves to the reader accessory, the reader can obtain the identification information of the tag so as to identify a certain target, and simultaneously obtain the distance information between the tag and the reader so as to position or track the target. At present, the target location patents related to the RFID system mainly include an indoor location method (CN201110101432.8) based on active RFID, a finished steel product intelligent warehousing system (CN201210093992.8) based on radio frequency identification technology, a station personnel location system (cn201010256882.x), a location method (CN200910040571.7) for the RFID indoor location system, and an indoor location system (CN200810027885.9) based on RFID technology, which all obtain location information of a target, and have the unified feature that the target is assumed to be stationary, and the approximate position of the target with a tag is obtained by using a radio frequency principle.

For moving objects, the above method has the following drawbacks: 1. because the RFID can only obtain the distance information with the reader, in order to obtain accurate position data, the position of a target can be obtained by using the geometric relationship between the readers depending on the distance information of a certain target obtained by a plurality of readers, which increases the complexity of system calculation; 2. the above methods assume that the target is stationary, which is not in line with the actual situation of the moving target, the displacement, velocity and acceleration of the moving target satisfy the basic constraint relationship of kinematics, and if the target is stationary, the relationship is completely abandoned, which results in the reduction of the constraint relationship, and directly results in inaccurate estimation of the trajectory of the moving target. 3. When the distance between the target and the RFID is too far or data related to the moving target is not collected due to other factors during the moving process of the target, the positioning accuracy is affected.

The method for obtaining the real-time target track is quite common in a plurality of tracking systems, such as the fields of pedestrian tracking, intelligent transportation and the like in indoor venues, so the method is not suitable for the RFID indoor target tracking system needing to obtain the real-time target track of the moving target.

Disclosure of Invention

Based on the above, the invention aims to provide an indoor target rapid tracking method based on IMU and RFID information fusion, which realizes rapid tracking of an indoor target and improves tracking precision.

The method is based on an insensitive Kalman Filter (UKF), utilizes the distance information from a target to a reader obtained by an RFID system, and obtains the estimation of a motion trail according to the dynamic characteristics of an indoor motion target. And in places without RFID measurement data, information supplement is carried out by utilizing an attitude calculation method obtained by the IMU, and the indoor target is quickly tracked.

The basic idea for realizing the purpose of the invention is as follows: firstly, judging whether the interval between two times of measurement data from an RFID reader is smaller than a threshold value (the threshold value is taken to be 0.18 second), if the interval is smaller than the threshold value, estimating the coordinates by adopting a nonlinear Kalman filtering method based on the RFID measurement data, and storing the estimated coordinates of the maneuvering target; if the interval is larger than the threshold value, the distance between the target coordinates estimated twice is used as the estimated distance in the attitude calculation method, the attitude angle change condition of the target in the period of time is estimated by utilizing IMU measurement data, the drift characteristic of gyroscope measurement data is considered, the attitude angle data is delayed for 6 seconds, then the motion coordinates of the target in the period of time are estimated by utilizing the distance and course angle data, and the obtained position coordinates are stored. After the data is stored, whether the measurement data from the RFID reader exists needs to be judged, and if the measurement data exists, the estimation process is carried out again; if not, whether the measurement data from the IMU exist is judged, if the measurement data exist, the motion of the target is subjected to supplementary calculation according to an attitude calculation method based on the IMU data, the measurement data of the accelerometer are adopted for distance calculation, and if the measurement data do not exist, the algorithm is ended. The algorithm has the advantages that: and (3) track estimation is carried out on the moving target by utilizing a Kalman filter based on the RFID data, and the estimation result does not cause accumulation of errors along with the time. When no RFID measurement data exists, the motion of the target is tracked by adopting an attitude calculation method of inertial sensor data, and the tracking information at the moment is used as supplement of target motion information.

In order to achieve the purpose, the technical scheme of the invention is as follows:

an indoor target rapid tracking method based on IMU and RFID information fusion comprises the following steps:

step 1: selecting a target tracking method according to a comparison result of the sampling period of the RFID and a threshold value, and if the sampling period of the RFID is smaller than the threshold value, estimating coordinates of RFID measurement data through an insensitive Kalman filter to obtain the coordinates of a maneuvering target; if the sampling period of the RFID is larger than or equal to the threshold value, estimating coordinates of the RFID measured data through an insensitive Kalman filter, and taking the distance between the coordinates estimated twice as an estimated distance d₁And estimating the course angle by selecting gyroscope data of the IMU based on

The coordinates of the maneuvering target at each moment are calculated by the mathematical relation;

step 2: judging whether unprocessed original measurement data exist in the RFID measurement system or not, and if yes, returning to the step 1; if not, judging whether measurement data from the IMU exist, if so, estimating a course angle by using gyroscope data of the IMU, and calculating the displacement d of the maneuvering target in each sampling interval by using the accelerometer data of the IMU through Newton's law of motion₂According to

The coordinates of the maneuvering target at each moment are calculated by the mathematical relation; if not, the algorithm is ended;

wherein, Δ x is the variation of the horizontal coordinate x of the target in the tracking area, Δ y is the variation of the vertical coordinate y of the target in the tracking area,

is the heading angle the target is turning.

On the basis of the scheme, the invention can be further improved as follows.

Further, in step 1, if the sampling period of the RFID is less than the threshold, the coordinates of the maneuvering target are obtained by estimating the coordinates of the RFID measurement data through the insensitive kalman filter, including:

step 1.1: initializing a target motion state and system self-adaptive parameters;

step 1.2: establishing a motion model and an RFID (radio frequency identification) measurement model with system self-adaptive parameters;

step 1.3: predicting the target state according to the established motion model with the system self-adaptive parameters to obtain a target state predicted value of the next time point;

step 1.4: calculating a target state measurement predicted value according to the RFID measurement model and the target state predicted value;

step 1.5: calculating the gain of the insensitive Kalman filter according to the target state predicted value and the target state measurement predicted value;

step 1.6: reading the original measured value z of the target state at the next time point acquired by the RFID monitoring system_n(t_i)；

Step 1.7: calculating a target state correction value according to the target state original measurement value, the target state measurement prediction value and the filter gain:

wherein, X (t)_i) Is a target state correction value, t_iFor the sampling instant, N is 1,2, … N (t)_i)，N(t_i) Is t_iThe number of RFID readers which acquire distance data between the RFID readers and the target at any moment; k_n(t_i) Is t_iFilter gain, z, of the nth reader/writer at time_n(t_i) For the nth reader at t_iThe measurement data of the time of day,

is t_iAn estimate of the measurement of the time of day;

step 1.8: predicting a value based on a target state

And target stateCorrection value X (t)_i) Calculating a target state estimation value and a target state covariance estimation value;

the calculation formula of the target state estimation value is as follows:

wherein the content of the first and second substances,

represents t_iA target state estimate at a time;

is shown at t_i-1Time of day prediction t_iTarget state prediction value of the moment; x (t)_i) Is a target state correction value, t_iIs the sampling time;

the target state covariance estimate is calculated as follows:

wherein, P (t)_i|t_i) Represents t_iTarget state covariance estimate at time, P (t)_i|t_i-1) Is shown at t_i-1Time of day prediction t_iTarget state covariance predicted value at time, K_n(t_i) Is t_iThe filter gain of the nth reader-writer at the moment; s_n(t_i) The covariance matrix of the nth reader-writer is obtained;

step 1.9: updating the adaptive parameters of the system by using the target state predicted value, and further updating the motion model in the step 1.3;

step 1.10: the estimated coordinates of the maneuvering target are stored.

Further, the specific process of initializing the target motion state and the system adaptive parameters in step 1.1 is as follows:

step 1.1.1: setting an initial value of a target motion state

The vector is a 6-dimensional all-0 column vector, and the dimension is the dimension of a state vector in the system model;

step 1.1.2 setting initial values α of system adaptive parameters_η＝α₀And

where η is x, y, x representing the lateral coordinate of the target in the tracking area, y representing the longitudinal coordinate of the target in the tracking area, α_ηThe acceleration maneuvering frequency of the transverse coordinate and the longitudinal coordinate is represented,

the acceleration variance of the transverse coordinate and the longitudinal coordinate is represented;

step 1.1.3: setting initial value of system acceleration component

Wherein η is x, y, x represents the lateral coordinate of the target in the tracking area, and y represents the longitudinal coordinate of the target in the tracking area;

step 1.1.4: setting the scale parameters k of the system to be-3, x of the system to be 0.01 and o of the system to be 2;

step 1.1.5: setting the initial value of the cross-correlation parameter of the acceleration one step as r_0,η(1) 0, the initial value of the acceleration autocorrelation parameter is r_0,η(0)＝0。

Further, the step 1.2 of establishing a motion model and an RFID measurement model with system adaptive parameters includes:

step 1.2.1: the motion characteristics of the object are described using the following equation:

wherein the content of the first and second substances,

is a 6-dimensional state column vector, x (t)_i),

Respectively transverse coordinate displacement, velocity and acceleration, y (t)_i),

Respectively longitudinal coordinate displacement, velocity and acceleration; x (t)_i) Is t_iState vector of the time target, t_iIs the sampling time; a (t)_i-1) Is t_i-1A state transition matrix of a time; u (t)_i-1) Is t_i-1A control matrix of time instants; w is a_d(t_i-1) Is process noise, with a mean of 0 and a variance of Q; a (t)_i-1)、U(t_i-1) And Q comprises system adaptive parameters;

step 1.2.2: establishing the following target measurement equation of the RFID system:

z_n(t_i)＝h_n[x(t_i)]+v_n(t_i)

wherein z is_n(t_i) Target obtained for RFID monitoring system at t_iMeasured value of time of day, h_n[x(t_i)]For the measurement equation, x (t)_i) Is t_iA state vector of the time target; v. of_n(t_i) White noise is measured for the Gaussian of the nth reader in the RFID monitoring system and correlated with the process noise w_d(t_i-1) Independently of one another, d_n(t_i) Is targeted at t_iThe true distance (unknown), x (t), from the location at which the time is located to the nth RFID reader_i)、y(t_i) Are each t_iThe horizontal and vertical coordinates of the target position at the moment; x is the number of_n(0)、y_n(0) Respectively, the abscissa and ordinate of the nth RFID reader.

Further, the step 1.3 of predicting the target state according to the established motion model with the system adaptive parameters includes:

step 1.3.1: calculating sigma point state values and weight values required by the insensitive Kalman filter according to the following formula;

wherein x is⁽ⁱ⁾Extended state value for the ith sigma point, including

i＝0,1,…2N_xIs the state value of the ith sigma point, N_xIs the dimension of the system state;

the process noise analog for the ith sigma point,

measuring noise analog quantity for the ith sigma point;

is shown at t_i-1Target state estimate at time, P (t)_i-1|t_i-1) Is shown at t_i-a target state covariance estimate at time 1,

is the process noise w (t)_i-1) An all-0 matrix of the same dimension,

is related to the measurement noise v (t)_i-1) All 0 matrices of the same dimension;

i＝0,1,…2N_xestimating a weight value for the state of the ith sigma point;

i＝0,1,…2N_xestimating a variance weight for the state of the ith sigma point;

to represent

The ith column, kappa, chi and omicron of the mean square error matrix are scale parameters;

step 1.3.2: and calculating the state predicted value of each sigma point according to the motion model and the target state initial value, wherein the calculation formula is as follows,

wherein the content of the first and second substances,

the state predicted value of the ith sigma point is obtained;

is the state value at the ith sigma point,

reconstructing the influence of process noise on state prediction for the process noise analog quantity of the ith sigma point; a (t)_i-1) Is t_i-1A state transition matrix of a time; u (t)_i-1) Is t_i-1A control matrix of time instants;

from time 0 to t_i-1The mean value of the acceleration at that moment;

step 1.3.3: calculating weighted values of all sigma point state predicted values, namely target state predicted values, wherein the calculation formula is as follows:

wherein the content of the first and second substances,

is shown at t_i-1Time of day prediction t_iTarget State prediction value at time, t_iFor the sampling moment, | represents a conditional operator;

the state predicted value of the ith sigma point is obtained;

estimating a weight value for the state of the ith sigma point;

step 1.3.4: calculating the target state covariance predicted value according to the following formula

Wherein, P (t)_i|t_i-1) Is shown at t_i-1Time of day prediction t_iPredicting the covariance value of the target state at the moment;

the state predicted value of the ith sigma point is obtained;

is shown at t_i-1Time of day prediction t_iTarget state prediction value of the moment;

estimating a variance weight for the state of the ith sigma point; q (t)_i-1) Is the process noise covariance.

Further, the step 1.4 of calculating the target state measurement predicted value according to the RFID measurement model and the target state predicted value includes:

step 1.4.1: and calculating the measurement predicted value of each sigma point according to the state predicted value of each sigma point, wherein the calculation formula is as follows:

wherein the content of the first and second substances,

measuring and predicting value of ith sigma point;

the state predicted value of the ith sigma point is obtained;

predicting the state of each sigma point

Substituting the result obtained by the measurement equation;

reconstructing the influence of the measurement noise on the measurement prediction for the measurement noise analog quantity of the ith sigma point;

step 1.4.2: calculating weighted values of the measurement predicted values of all sigma points, namely the target state measurement predicted value, wherein the calculation formula is as follows:

wherein the content of the first and second substances,

measuring a predicted value for a target state;

measuring and predicting value of ith sigma point;

estimating a weight value for the state of the ith sigma point; n is 1,2, … N (t)_i)，N(t_i) Is t_iThe number of RFID readers that can obtain the target measurement value at the time.

Further, in step 1.5, calculating the insensitive kalman filter gain according to the target state predicted value and the target state measurement predicted value, includes:

step 1.5.1: and (3) calculating the covariance of the target state measurement predicted value, wherein the calculation formula is as follows:

wherein the content of the first and second substances,

measuring and predicting value of ith sigma point;

measuring a predicted value for a target state;

i＝0,1,…2N_xestimating a variance weight for the state of the ith sigma point; r_n(t_i) The measured variance for the nth reader;

step 1.5.2: calculating the cross covariance of the target state predicted value and the target state measurement predicted value, wherein the calculation formula is as follows:

wherein, P_xz,n(t_i) Is a cross covariance matrix;

estimating a variance weight for the state of the ith sigma point;

the state predicted value of the ith sigma point is obtained;

is shown at t_i-1Time of day prediction t_iTarget state prediction value of the moment;

measuring and predicting value of ith sigma point;

measuring a predicted value for the target;

step 1.5.3: the filter gain is calculated from the covariance in step 1.5.1 and the cross covariance in step 1.5.2, the calculation formula is as follows:

wherein, K_n(t_i) Is t_iFilter gain, t, of the nth RFID reader at time_iIs the sampling time; p_zz,n(t_i) In the form of a cross-covariance matrix,

represents the inverse of the cross covariance matrix; s_n(t_i) Is a covariance matrix, N is 1,2, … N (t)_i)，N(t_i) Is t_iThe number of RFID readers that can obtain the target measurement value at the time.

Further, the calculation of step 1.9 is as follows:

get

The third and sixth rows of the target state predicted values obtained in step 1.4, that is, the lateral and vertical axis acceleration predicted values, η ═ x, y respectively represent the lateral and vertical coordinates of the tracking area;

1) when i is less than or equal to 4, updating the system self-adaptive parameters as follows:

when in use

When the temperature of the water is higher than the set temperature,

when in use

When the temperature of the water is higher than the set temperature,

if it is not

Any small positive number can be taken;

maneuvering frequency set to α₀The change is not changed; wherein

η x, y respectively represent lateral and longitudinal acceleration variance values of the tracking area, i.e. when η x,

representing the lateral acceleration variance of the tracking area, when η y,

representing a longitudinal acceleration variance of the tracking area; a is_M,ηIs a positive number ranging from 1 to 100, a-_M,ηIs a and_M,ηnegative numbers with the same absolute value;

2) when i is greater than 4, updating the system adaptive parameters in the following manner;

wherein r is_i,η(1) Is t_iAcceleration previous step cross-correlation parameter of time, r_i,η(0) Is t_iTime acceleration autocorrelation parameters, η ═ x, y, x denoting lateral coordinates within the tracking area and y denoting longitudinal coordinates within the tracking area;

predicted values of lateral and longitudinal axial acceleration, r_i-1,η(1) Is t_i-1Acceleration previous step cross-correlation parameter of time, r_i-1,η(0) Is t_i-1Time of day acceleration autocorrelation parameter, β_i,ηAnd

is t_iThe intermediate variable of the calculation of the time of day,

is t_iVariance of acceleration at time α_i,ηIs t_iFrequency of manoeuvres of time, th_i＝t_i-t_i-1The time interval between two measurements.

Further, in step 1, if the sampling period of the RFID is greater than or equal to the threshold, the coordinates of the RFID measurement data are estimated by the insensitive kalman filter, and the distance between the two estimated coordinates is used as the estimated distance d₁And estimating the course angle by selecting gyroscope data of the IMU based on

The coordinates of the maneuvering target at each moment are calculated by the mathematical relation; the concrete implementation steps are as follows:

step 1.1': estimating coordinates of RFID measurement data through an insensitive Kalman filter, and taking the distance between two estimated coordinates as an estimated distance d₁；

Step 1.2': estimating the attitude angle change condition of the target in each sampling interval by utilizing IMU (inertial measurement Unit) measurement data, and delaying the attitude angle data for 6 seconds by taking the drift characteristic of gyroscope measurement data into consideration;

step 1.3': obtaining angular velocity information of a gyroscope in each sampling interval IMU, and listing a quaternion differential equation according to the angular velocity information of the gyroscope;

obtaining real-time quaternion q by solving the quaternion differential equation₀，q₁，q₂，q₃Q to be obtained₀，q₁，q₂，q₃Carry into the following formula to get the attitude matrix

Wherein the content of the first and second substances,

which represents the product of the quaternion numbers,

denotes the angular velocity, ω, of b with respect to n_nbx、ω_nby、ω_nbzIs the output value of the gyroscope;

step 1.4': updating the quaternion by using a 4-order Runge Kutta method;

h is a posture updating period;

step 1.5': updating an attitude matrix according to the updated quaternion, and solving according to the updated attitude matrix to obtain an attitude angle as follows:

θ_{master and slave}＝arcsinC₃₂

θ＝θ_{Master and slave}

Wherein the pitch angle is defined as (-90 degrees and 90 degrees), the roll angle is defined as (-180 degrees and 180 degrees), the course angle is defined as (0 degrees and 360 degrees), and theta in the formula_{Master and slave}、γ_{Master and slave}And

the pitch angle, roll angle and course angle, theta, gamma and

representing the angle value converted into the defined domain;

step 1.6': according to

The coordinates of the maneuvering target at each moment are calculated by the mathematical relation;

step 1.7': and storing the calculated coordinates of the maneuvering target.

Further, judging whether measurement data from the IMU exist or not in the step 2, if so, estimating a course angle by using gyroscope data of the IMU, and calculating the displacement d of the maneuvering target in each sampling interval by using accelerometer data of the IMU according to the Newton's law of motion₂According to

The coordinates of the maneuvering target at each moment are calculated by the mathematical relation; the concrete implementation steps are as follows:

step 2.1: calculating the displacement d of the target using the measurement data of the accelerometer₂；

Step 2.2: obtaining a real-time quaternion q by solving a quaternion differential equation₀，q₁，q₂，q₃：

Wherein the content of the first and second substances,

which represents the product of the quaternion numbers,

denotes the angular velocity, ω, of b with respect to n_nbx、ω_nby、ω_nbzIs the output value of the gyroscope;

step 2.3: q to be obtained₀，q₁，q₂，q₃Carry into the following formula to get the attitude matrix

Step 2.4: and updating the quaternion by adopting a fourth-order Runge Kutta method, wherein the calculation formula is as follows:

h is a posture updating period;

step 2.5: updating an attitude matrix according to the updated quaternion, and solving according to the updated attitude matrix to obtain an attitude angle as follows:

θ_{master and slave}＝arcsinC₃₂

θ＝θ_{Master and slave}

Wherein the pitch angle is defined as (-90 degrees, 90 degrees), and the transverse rolling is performedThe angle is defined as (-180 DEG, 180 DEG), the heading angle is defined as (0 DEG, 360 DEG), and theta is expressed in the formula_{Master and slave}、γ_{Master and slave}And

the pitch angle, roll angle and course angle, theta, gamma and

representing the angle value converted into the defined domain;

step 2.6: using the determined displacement d₂And course angle

According to

The coordinates of the maneuvering target at each moment are calculated by the mathematical relation;

step 2.7: and storing the calculated coordinates of the maneuvering target.

The invention has the beneficial effects that:

according to the multi-sensor fusion indoor rapid tracking method based on IMU and RFID measurement data, a Kalman filter is used for carrying out track estimation on a moving target based on RFID data, and errors cannot be accumulated along with the lapse of time in an estimation result; when no RFID measurement data exists, tracking the motion of the target by adopting an attitude calculation method of inertial sensor data, and reconstructing the motion track of the target by taking the tracking information as the supplement of target motion information; the method and the system realize the rapid tracking of the indoor target, improve the tracking precision and are particularly suitable for the RFID indoor target tracking system needing to obtain the real-time track of the moving target. And the tracking precision is further improved by optimizing the tracking method.

Drawings

Fig. 1 is a flowchart of a multi-sensor fusion indoor fast tracking method based on IMU and RFID measurement data according to an embodiment of the present invention.

Detailed Description

In order to make the objects, technical solutions and advantages of the present invention more clearly understood, the following describes in detail a multi-sensor fusion indoor fast tracking method based on IMU and RFID measurement data according to the present invention with reference to the accompanying drawings and 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.

Referring to fig. 1, the multi-sensor fusion indoor fast tracking method based on IMU and RFID measurement data according to an embodiment of the present invention includes the following steps:

step 1: selecting a target tracking method according to a comparison result of the sampling period of the RFID and a threshold, if the sampling period of the RFID is smaller than the threshold, estimating coordinates of RFID measurement data through an insensitive Kalman filter to obtain the coordinates (position information of a carrier) of a maneuvering target; if the sampling period of the RFID is larger than or equal to the threshold value, estimating coordinates of the RFID measured data through an insensitive Kalman filter, and taking the distance between the coordinates estimated twice as an estimated distance d₁And estimating the course angle by selecting gyroscope data of the IMU based on

The coordinates of the maneuvering target (carrier) at each moment are calculated by the mathematical relation;

step 2: judging whether unprocessed original measurement data exist in the RFID measurement system or not, and if yes, returning to the step 1; if not, judging whether measurement data from the IMU exist, if so, estimating a course angle by using gyroscope data of the IMU, and calculating the displacement d of the maneuvering target in each sampling interval by using the accelerometer data of the IMU through Newton's law of motion₂According to

The coordinates of the maneuvering target at each moment are calculated by the mathematical relation; if not, the algorithm is ended;

wherein Δ x represents a change amount Δ y of a lateral coordinate x of the object within the tracking area represents a change of a longitudinal coordinate y of the object within the tracking areaThe amount of the compound (A) is,

is the heading angle the target is turning. In this embodiment, the threshold may be 0.18 seconds.

As a preferable mode, if the sampling period of the RFID is less than the threshold in step 1, performing coordinate estimation on the RFID measurement data through an insensitive kalman filter to obtain coordinates of the maneuvering target, including:

step 1.1: initializing a target motion state and system self-adaptive parameters;

step 1.2: establishing a motion model and an RFID (radio frequency identification) measurement model with system self-adaptive parameters;

step 1.3: predicting the target state according to the established motion model with the system self-adaptive parameters to obtain a target state predicted value of the next time point;

step 1.4: calculating a target state measurement predicted value according to the RFID measurement model and the target state predicted value;

step 1.5: calculating the gain of the insensitive Kalman filter according to the target state predicted value and the target state measurement predicted value;

step 1.6: reading the original measured value z of the target state at the next time point acquired by the RFID monitoring system_n(t_i)；

Step 1.7: calculating a target state correction value according to the target state original measurement value, the target state measurement prediction value and the filter gain:

wherein, X (t)_i) Is a target state correction value, t_iFor the sampling instant, N is 1,2, … N (t)_i)，N(t_i) Is t_iThe number of RFID readers which acquire distance data between the RFID readers and the target at any moment; k_n(t_i) Is t_iTime of day filter gain, z_n(t_i) For the nth reader at t_iThe measurement data of the time of day,

is t_iAn estimate of the measurement of the time of day;

step 1.8: predicting a value based on a target state

And a target state correction value X (t)_i) Calculating a target state estimation value and a target state covariance estimation value;

the calculation formula of the target state estimation value is as follows:

wherein the content of the first and second substances,

represents t_iA target state estimate at a time;

is shown at t_i-1Time of day prediction t_iTarget state prediction value of the moment; x (t)_i) Is a target state correction value, t_iIs the sampling time;

the target state covariance estimate is calculated as follows:

wherein, P (t)_i|t_i) Represents t_iTarget state covariance estimate at time, P (t)_i|t_i-1) Is shown at t_i-1Time of day prediction t_iTarget state covariance predicted value at time, K_n(t_i) Is t_iThe filter gain of the nth reader-writer at the moment; s_n(t_i) The covariance matrix of the nth reader-writer is obtained;

step 1.9: updating the adaptive parameters of the system by using the target state predicted value, and further updating the motion model in the step 2.3;

step 1.10: the estimated coordinates of the maneuvering target are stored.

Further, the specific process of initializing the target motion state and the system adaptive parameters in step 1.1 is as follows:

step 1.1.1: setting an initial value of a target motion state

The vector is a 6-dimensional all-0 column vector, and the dimension is the dimension of a state vector in the system model;

step 1.1.2 setting initial values α of system adaptive parameters_η＝α₀And

where η is x, y, x representing the lateral coordinate of the target in the tracking area, y representing the longitudinal coordinate of the target in the tracking area, α_ηThe acceleration maneuvering frequency of the transverse coordinate and the longitudinal coordinate is represented,

representing the variance of acceleration in the lateral and longitudinal coordinates, α in this embodiment₀＝1/20、

Step 1.1.3: setting initial value of system acceleration component

In this example

Taking 0, wherein η ═ x, y, x denotes the lateral coordinate of the target in the tracking area, and y denotes the longitudinal coordinate of the target in the tracking area;

step 1.1.4: setting the scale parameters k of the system to be-3, x of the system to be 0.01 and o of the system to be 2;

step 1.1.5: setting the initial value of the cross-correlation parameter of the acceleration one step as r_0,η(1) 0, the initial value of the acceleration autocorrelation parameter is r_0,η(0)＝0。

Further, the step 1.2 of establishing a motion model and an RFID measurement model with system adaptive parameters includes:

step 1.2.1: the motion characteristics of the object (motion model formula) are described using the following equation:

wherein the content of the first and second substances,

is a 6-dimensional state column vector, x (t)_i),

Respectively transverse coordinate displacement, velocity and acceleration, y (t)_i),

Respectively longitudinal coordinate displacement, velocity and acceleration; x (t)_i) Is t_iState vector of the time target, t_iIs the sampling time; a (t)_i-1) Is t_i-1A state transition matrix of a time; u (t)_i-1) Is t_i-1A control matrix of time instants; w is a_d(t_i-1) Is process noise, with a mean of 0 and a variance of Q; a (t)_i-1)、U(t_i-1) And Q comprises system adaptive parameters;

is a state transition matrix;

is a control matrix;

from time 0 to t_i-1The mean value of the acceleration of the target at the moment; w (t)_i-1)＝[w_x(t_i-1) w_y(t_i-1)]^TIs process noise with a mean of 0 and a variance of

Let η be x, y, x represents the horizontal coordinate of the target in the tracking area, y represents the vertical coordinate of the target in the tracking area, wherein the values of the corresponding matrix are:

wherein th_i＝t_i-t_i-1The time interval for measuring data twice;

step 1.2.2: establishing the following target measurement equation of the RFID system:

z_n(t_i)＝h_n[x(t_i)]+v_n(t_i)

wherein z is_n(t_i) Target obtained for RFID monitoring system at t_iMeasured value of time of day, h_n[x(t_i)]For the measurement equation, x (t)_i) Is t_iA state vector of the time target; v. of_n(t_i) White noise is measured for the Gaussian of the nth reader in the RFID monitoring system and correlated with the process noise w_d(t_i-1) Independent of each other, the measured variance thereof satisfies

σ_pAnd gamma is the parameter of radio frequency measurement, and the experiment shows that sigma_pTaking 4dB, and gamma can be any positive number in the interval of 1.6 to 6.5; d_n(t_i) Is targeted at t_iThe true distance (unknown), x (t), from the location at which the time is located to the nth RFID reader_i)、y(t_i) Are each t_iThe horizontal and vertical coordinates of the target position at the moment; x is the number of_n(0)、y_n(0) Respectively, the abscissa and ordinate of the nth RFID reader, where N is 1,2, … N (t)_i)，N(t_i) Is t_iThe number of RFID readers that can obtain the target measurement value at the time.

Further, the step 1.3 of predicting the target state according to the established motion model with the system adaptive parameters includes:

step 1.3.1: calculating sigma point state values and weight values required by the insensitive Kalman filter according to the following formula;

wherein x is⁽ⁱ⁾Extended state value for the ith sigma point, including

Is the state value of the ith sigma point, N_xIs the dimension of the system state;

the process noise analog for the ith sigma point,

measuring noise analog quantity for the ith sigma point;

is shown at t_i-1Target state estimate at time, P (t)_i-1|t_i-1) Is shown at t_i-1The target state covariance estimate for the time of day,

is the process noise w (t)_i-1) An all-0 matrix of the same dimension,

is related to the measurement noise v (t)_i-1) All 0 matrices of the same dimension;

i＝0,1,…2N_xestimating a weight value for the state of the ith sigma point;

estimate variance weight, N, for the state of the ith sigma point_xIs the dimension of the system state;

to represent

The ith column, kappa, chi and omicron of the mean square error matrix are scale parameters;

step 1.3.2: and calculating the state predicted value of each sigma point according to the motion model and the target state initial value, wherein the calculation formula is as follows,

wherein the content of the first and second substances,

the state predicted value of the ith sigma point is obtained;

is the state value at the ith sigma point,

for the process noise analog at the ith sigma point, to reconstruct the effect of process noise on state prediction, N_xIs the dimension of the system state; a (t)_i-1) Is t_i-1A state transition matrix of a time; u (t)_i-1) Is t_i-₁A control matrix of time instants;

from time 0 to t_i-1The mean value of the acceleration at that moment;

step 1.3.3: calculating weighted values of all sigma point state predicted values, namely target state predicted values, wherein the calculation formula is as follows:

wherein the content of the first and second substances,

is shown at t_i-1Time of day prediction t_iTarget State prediction value at time, t_iFor the sampling moment, | represents a conditional operator;

the state predicted value of the ith sigma point is obtained;

estimate weights, N, for the state of the ith sigma point_xIs the dimension of the system state;

step 1.3.4: calculating the target state covariance predicted value according to the following formula

Wherein, P (t)_i|t_i-1) Is shown at t_i-1Time of day prediction t_iPredicting the covariance value of the target state at the moment;

the state predicted value of the ith sigma point is obtained;

is shown at t_i-1Time of day prediction t_iTarget state prediction value of the moment;

estimating a variance weight for the state of the ith sigma point; n is a radical of_xIs the dimension of the system state; q (t)_i-1) Is the process noise covariance.

Further, the step 1.4 of calculating the target state measurement predicted value according to the RFID measurement model and the target state predicted value includes:

step 1.4.1: and calculating the measurement predicted value of each sigma point according to the state predicted value of each sigma point, wherein the calculation formula is as follows:

wherein the content of the first and second substances,

measuring and predicting value of ith sigma point;

the state predicted value of the ith sigma point is obtained;

predicting the state of each sigma point

Substituting the result obtained by the measurement equation; n is 1,2, … N (t)_i)，N(t_i) Is t_iThe number of RFID readers capable of obtaining target measured values at any moment;

reconstructing the influence of the measurement noise on the measurement prediction for the measurement noise analog quantity of the ith sigma point;

step 1.4.2: calculating weighted values of the measurement predicted values of all sigma points, namely the target state measurement predicted value, wherein the calculation formula is as follows:

wherein the content of the first and second substances,

measuring a predicted value for a target state;

measuring and predicting value of ith sigma point;

i＝0,1,…2N_xestimate the weight, N, for the state at the ith sigma point_xIs aThe dimension of the system state; n is 1,2, … N (t)_i)，N(t_i) Is t_iThe number of RFID readers that can obtain the target measurement value at the time.

Further, in step 1.5, calculating the insensitive kalman filter gain according to the target state predicted value and the target state measurement predicted value, includes:

step 1.5.1: and (3) calculating the covariance of the target state measurement predicted value, wherein the calculation formula is as follows:

wherein the content of the first and second substances,

measuring and predicting value of ith sigma point;

measuring a predicted value for a target state;

estimating a variance weight for the state of the ith sigma point; n is a radical of_xIs the dimension of the system state; r_n(t_i) The measured variance for the nth reader; n is 1,2, … N (t)_i)，N(t_i) Is t_iThe number of RFID readers capable of obtaining target measured values at any moment;

step 1.5.2: calculating the cross covariance of the target state predicted value and the target state measurement predicted value, wherein the calculation formula is as follows:

wherein, P_xz,n(t_i) Is a cross covariance matrix;

estimating a variance weight for the state of the ith sigma point;

the state predicted value of the ith sigma point is obtained;

is shown at t_i-1Time of day prediction t_iTarget state prediction value of the moment;

measuring and predicting value of ith sigma point;

measuring a predicted value for the target;

step 1.5.3: the filter gain is calculated from the covariance in step 2.5.1 and the cross covariance in step 2.5.2, the calculation formula is as follows:

wherein, K_n(t_i) Is t_iFilter gain, t, of the nth RFID reader at time_iIs the sampling time; p_zz,n(t_i) In the form of a cross-covariance matrix,

represents the inverse of the cross covariance matrix; s_n(t_i) Is a covariance matrix, N is 1,2, … N (t)_i)，N(t_i) Is t_iThe number of RFID readers that can obtain the target measurement value at the time.

Further, the calculation of step 1.9 is as follows:

get

The third and sixth rows of the target state predicted values obtained in step 1.4, i.e., the lateral and vertical axis acceleration predicted values, η ═ x, y respectively represent the rootHorizontal and vertical coordinates of the tracking area;

1) when i is less than or equal to 4, updating the system self-adaptive parameters as follows:

when in use

When the temperature of the water is higher than the set temperature,

when in use

When the temperature of the water is higher than the set temperature,

if it is not

Any small positive number can be taken;

maneuvering frequency set to α₀The change is not changed; wherein

η x, y respectively represent lateral and longitudinal acceleration variance values of the tracking area, i.e. when η x,

representing the lateral acceleration variance of the tracking area, when η y,

representing a longitudinal acceleration variance of the tracking area; a is_M,ηIs a positive number ranging from 1 to 100, a-_M,ηIs a and_M,ηnegative numbers with the same absolute value;

2) when i is greater than 4, updating the system adaptive parameters in the following manner;

wherein r is_i,η(1) Is t_iAcceleration previous step cross-correlation parameter of time, r_i,η(0) Is t_iTime acceleration autocorrelation parameters, η ═ x, y, x denoting lateral coordinates within the tracking area and y denoting longitudinal coordinates within the tracking area;

predicted values of lateral and longitudinal axial acceleration, r_i-1,η(1) Is t_i-1Acceleration previous step cross-correlation parameter of time, r_i-1,η(0) Is t_i-1Time of day acceleration autocorrelation parameter, β_i,ηAnd

is t_iThe intermediate variable of the calculation of the time of day,

is t_iVariance of acceleration at time α_i,ηIs t_iFrequency of manoeuvres of time, th_i＝t_i-t_i-1The time interval between two measurements.

As a preferable mode, in step 1, if the sampling period of the RFID is greater than or equal to the threshold, the coordinates of the RFID measurement data are estimated by the insensitive kalman filter, and the distance between the two estimated coordinates is used as the estimated distance d₁And estimating the course angle by selecting gyroscope data of the IMU based on

The coordinates of the maneuvering target at each moment are calculated by the mathematical relation; the concrete implementation steps are as follows:

step 1.1': estimating coordinates of RFID measurement data through an insensitive Kalman filter, and taking the distance between two estimated coordinates as an estimated distance d₁；

Step 1.2': estimating the attitude angle change condition of the target in each sampling interval by utilizing IMU (inertial measurement Unit) measurement data, and delaying the attitude angle data for 6 seconds by taking the drift characteristic of gyroscope measurement data into consideration;

step 1.3': obtaining angular velocity information of a gyroscope in each sampling interval IMU, and listing a quaternion differential equation according to the angular velocity information of the gyroscope;

the quaternion differential equation is further written in matrix form as:

obtaining real-time quaternion q by solving the quaternion differential equation₀，q₁，q₂，q₃Q to be obtained₀，q₁，q₂，q₃The attitude matrix (b-system to n-system) is obtained by substituting the following formula

Wherein the content of the first and second substances,

which represents the product of the quaternion numbers,

denotes the angular velocity, ω, of b with respect to n_nbx、ω_nby、ω_nbzIs the output value of the gyroscope;

step 1.4': updating the quaternion by using a 4-order Runge Kutta method;

h is a posture updating period;

step 1.5': updating an attitude matrix according to the updated quaternion, and solving according to the updated attitude matrix to obtain an attitude angle as follows:

θ_{master and slave}＝arcsinC₃₂

θ＝θ_{Master and slave}

Wherein the pitch angle is defined as (-90 °,90 °), the roll angle is defined as (-180 °,180 °), and the heading angle is defined as (0 °,360 °). Theta in the formula_{Master and slave}、γ_{Master and slave}And

the pitch angle, roll angle and course angle, theta, gamma and

indicating the angle value converted into the defined domain. After the quaternion is updated, an updated attitude matrix can be obtained, and the attitude angle can be obtained by the attitude matrix, so that the updated attitude angle can be obtained.

Step 1.6': according to

The coordinates of the maneuvering target at each moment are calculated by the mathematical relation; wherein d is₁Representing the distance between the coordinates estimated twice in step 2.1, Δ x is the variation of the transverse coordinate x of the target in the tracking area, Δ y is the variation of the longitudinal coordinate y of the target in the tracking area,

is the heading angle the target is turning.

Step 1.7': and storing the calculated coordinates of the maneuvering target.

As a preferable mode, in the step 2, whether measurement data from the IMU exist is judged, if yes, the heading angle is estimated by using gyroscope data of the IMU, and the displacement d of the maneuvering target in each sampling interval is calculated by using the accelerometer data of the IMU according to the newton's law of motion₂According to

The method for calculating the coordinates of the maneuvering target at each moment comprises the following steps:

step 2.1: calculating the displacement d of the target using the measurement data of the accelerometer₂；

Step 2.2: obtaining a real-time quaternion q by solving a quaternion differential equation₀，q₁，q₂，q₃：

The following differential equations for quaternion updates are listed from the gyroscope measurement data;

since the state estimator q is a quaternion, the quaternion differential equation is further written in matrix form as:

step 2.3: q to be obtained₀，q₁，q₂，q₃The attitude matrix (b-system to n-system) is obtained by substituting the following formula

Step 2.4: the quaternion is updated by adopting a four-order Runge Kutta method, and the calculation formula is as follows

h is a posture updating period;

step 2.5: updating an attitude matrix according to the updated quaternion, and solving according to the updated attitude matrix to obtain an attitude angle as follows:

θ_{master and slave}＝arcsinC₃₂

θ＝θ_{Master and slave}

For accurate determination

The truth values of theta and gamma are defined as the definition domain of the attitude angle, wherein the definition domain of the pitch angle is (-90 degrees and 90 degrees), the definition domain of the roll angle is (-180 degrees and 180 degrees), the definition domain of the heading angle is (0 degrees and 360 degrees), and theta in the formula_{Master and slave}、γ_{Master and slave}And

the pitch angle, roll angle and course angle, theta, gamma and

representing the angle value converted into the defined domain; the attitude matrix is

Theta, gamma, and thus the attitude angle can be obtained by solving the equation of the attitude matrix.

Step 2.6: using the determined displacement d₂And course angle

According to

The coordinate of the maneuvering target at each moment is calculated by the mathematical relation of the coordinate system, delta x is the variation of the transverse coordinate x of the target in the tracking area, delta y is the variation of the longitudinal coordinate y of the target in the tracking area,

is the heading angle the target is turning.

Step 2.7: and storing the calculated coordinates of the maneuvering target.

According to the multi-sensor fusion indoor rapid tracking method based on IMU and RFID measurement data in each embodiment, the track of the moving target is estimated by using a Kalman filter based on RFID data, and the estimation result cannot cause error accumulation along with the time; when no RFID measurement data exists, tracking the motion of the target by adopting an attitude calculation method of inertial sensor data, and reconstructing the motion track of the target by taking the tracking information as the supplement of target motion information; the method and the system realize the rapid tracking of the indoor target, improve the tracking precision and are particularly suitable for the RFID indoor target tracking system needing to obtain the real-time track of the moving target. And the tracking precision is further improved by optimizing the tracking method.

It should be noted that the features of the above embodiments and examples may be combined with each other without conflict.

The above-mentioned embodiments only express several embodiments of the present invention, and the description thereof is more specific and detailed, but not construed as limiting the scope of the present invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the inventive concept, which falls within the scope of the present invention.

Claims (10)

1. An indoor target rapid tracking method based on IMU and RFID information fusion is characterized by comprising the following steps:

step 1: selecting a target tracking method according to a comparison result of the sampling period of the RFID and a threshold value, and if the sampling period of the RFID is smaller than the threshold value, estimating coordinates of RFID measurement data through an insensitive Kalman filter to obtain the coordinates of a maneuvering target; if the sampling period of the RFID is larger than or equal to the threshold value, estimating coordinates of the RFID measured data through an insensitive Kalman filter, and taking the distance between the coordinates estimated twice as an estimated distance d₁And selecting the gyroscope using IMUEstimating a course angle from the spirometer data

The coordinates of the maneuvering target at each moment are calculated by the mathematical relation;

step 2: judging whether unprocessed original measurement data exist in the RFID measurement system or not, and if yes, returning to the step 1; if not, judging whether measurement data from the IMU exist, if so, estimating a course angle by using gyroscope data of the IMU, and calculating the displacement d of the maneuvering target in each sampling interval by using the accelerometer data of the IMU through Newton's law of motion₂According to

The coordinates of the maneuvering target at each moment are calculated by the mathematical relation; if not, the algorithm is ended;

wherein, Δ x is the variation of the horizontal coordinate x of the target in the tracking area, Δ y is the variation of the vertical coordinate y of the target in the tracking area,

is the heading angle the target is turning.

2. The IMU and RFID information fusion-based indoor target fast tracking method of claim 1, wherein in step 1, if the sampling period of the RFID is less than a threshold, the RFID measurement data is subjected to coordinate estimation through an insensitive Kalman filter to obtain the coordinates of the maneuvering target, and the method comprises the following steps:

step 1.1: initializing a target motion state and system self-adaptive parameters;

step 1.2: establishing a motion model and an RFID (radio frequency identification) measurement model with system self-adaptive parameters;

step 1.3: predicting the target state according to the established motion model with the system self-adaptive parameters to obtain a target state predicted value of the next time point;

step 1.4: calculating a target state measurement predicted value according to the RFID measurement model and the target state predicted value;

step 1.5: calculating the gain of the insensitive Kalman filter according to the target state predicted value and the target state measurement predicted value;

step 1.6: reading the original measured value z of the target state at the next time point acquired by the RFID monitoring system_n(t_i)；

Step 1.7: calculating a target state correction value according to the target state original measurement value, the target state measurement prediction value and the filter gain:

wherein, X (t)_i) Is a target state correction value, t_iFor the sampling instant, N is 1,2, … N (t)_i)，N(t_i) Is t_iThe number of RFID readers which acquire distance data between the RFID readers and the target at any moment; k_n(t_i) Is t_iFilter gain, z, of the nth reader/writer at time_n(t_i) For the nth reader at t_iThe measurement data of the time of day,

is t_iAn estimate of the measurement of the time of day;

step 1.8: predicting a value based on a target state

And a target state correction value X (t)_i) Calculating a target state estimation value and a target state covariance estimation value;

the calculation formula of the target state estimation value is as follows:

wherein the content of the first and second substances,

represents t_iA target state estimate at a time;

is shown at t_i-1Time of day prediction t_iTarget state prediction value of the moment; x (t)_i) Is a target state correction value, t_iIs the sampling time;

the target state covariance estimate is calculated as follows:

wherein, P (t)_i|t_i) Represents t_iTarget state covariance estimate at time, P (t)_i|t_i-1) Is shown at t_i-1Time of day prediction t_iTarget state covariance predicted value at time, K_n(t_i) Is t_iThe filter gain of the nth reader-writer at the moment; s_n(t_i) The covariance matrix of the nth reader-writer is obtained;

step 1.9: updating the adaptive parameters of the system by using the target state predicted value, and further updating the motion model in the step 1.3;

step 1.10: the estimated coordinates of the maneuvering target are stored.

3. The IMU and RFID information fusion-based indoor target fast tracking method according to claim 2, characterized in that the specific process of target motion state and system adaptive parameter initialization in step 1.1 is as follows:

step 1.1.1: setting an initial value of a target motion state

The vector is a 6-dimensional all-0 column vector, and the dimension is the dimension of a state vector in the system model;

step 1.1.2 setting initial values α of system adaptive parameters_η＝α₀And

where η is x, y, x representing the lateral coordinate of the target in the tracking area, y representing the longitudinal coordinate of the target in the tracking area, α_ηThe acceleration maneuvering frequency of the transverse coordinate and the longitudinal coordinate is represented,

the acceleration variance of the transverse coordinate and the longitudinal coordinate is represented;

step 1.1.3: setting initial value of system acceleration component

Wherein η is x, y, x represents the lateral coordinate of the target in the tracking area, and y represents the longitudinal coordinate of the target in the tracking area;

step 1.1.4: setting the scale parameters k of the system to be-3, x of the system to be 0.01 and o of the system to be 2;

step 1.1.5: setting the initial value of the cross-correlation parameter of the acceleration one step as r_0,η(1) 0, the initial value of the acceleration autocorrelation parameter is r_0,η(0)＝0。

4. The IMU and RFID information fusion-based indoor target fast tracking method according to claim 3, wherein the establishing of the motion model and the RFID measurement model with system adaptive parameters in step 1.2 comprises:

step 1.2.1: the motion characteristics of the object are described using the following equation:

wherein the content of the first and second substances,

is a 6-dimensional state column vector, x (t)_i),

Respectively, transverse coordinate displacementSpeed and acceleration, y (t)_i),

Respectively longitudinal coordinate displacement, velocity and acceleration; x (t)_i) Is t_iState vector of the time target, t_iIs the sampling time; a (t)_i-1) Is t_i-1A state transition matrix of a time; u (t)_i-1) Is t_i-1A control matrix of time instants;

from time 0 to t_i-1The mean value of the acceleration at that moment; w is a_d(t_i-1) Is process noise, with a mean of 0 and a variance of Q; a (t)_i-1)、U(t_i-1) And Q comprises system adaptive parameters;

step 1.2.2: establishing the following target measurement equation of the RFID system:

z_n(t_i)＝h_n[x(t_i)]+v_n(t_i)

wherein z is_n(t_i) Target obtained for RFID monitoring system at t_iMeasured value of time of day, h_n[x(t_i)]For the measurement equation, x (t)_i) Is t_iA state vector of the time target; v. of_n(t_i) White noise is measured for the Gaussian of the nth reader in the RFID monitoring system and correlated with the process noise w_d(t_i-1) Independently of one another, d_n(t_i) Is targeted at t_iThe true distance, x (t), from the location at which the time is located to the nth RFID reader_i)、y(t_i) Are each t_iThe horizontal and vertical coordinates of the target position at the moment; x is the number of_n(0)、y_n(0) Respectively, the abscissa and ordinate of the nth RFID reader.

5. The IMU and RFID information fusion-based indoor target rapid tracking method according to claim 2, wherein the predicting of the target state according to the established motion model with system adaptive parameters in step 1.3 comprises:

step 1.3.1: calculating sigma point state values and weight values required by the insensitive Kalman filter according to the following formula;

wherein x is⁽ⁱ⁾Extended state value for the ith sigma point, including

Is the state value of the ith sigma point, N_xIs the dimension of the system state;

the process noise analog for the ith sigma point,

measuring noise analog quantity for the ith sigma point;

is shown at t_i-1Target state estimate at time, P (t)_i-1|t_i-1) Is shown at t_i-1The target state covariance estimate for the time of day,

is the process noise w_d(t_i-1) An all-0 matrix of the same dimension,

is related to the measurement noise v (t)_i-1) All 0 matrices of the same dimension;

estimating a weight value for the state of the ith sigma point;

estimating a variance weight for the state of the ith sigma point;

to represent

The ith column, kappa, chi and omicron of the mean square error matrix are scale parameters;

step 1.3.2: and calculating the state predicted value of each sigma point according to the motion model and the target state initial value, wherein the calculation formula is as follows,

wherein the content of the first and second substances,

the state predicted value of the ith sigma point is obtained;

is the state value at the ith sigma point,

reconstructing the influence of process noise on state prediction for the process noise analog quantity of the ith sigma point; a (t)_i-1) Is t_i-1A state transition matrix of a time; u (t)_i-1) Is t_i-1A control matrix of time instants;

from time 0 to t_i-1The mean value of the acceleration at that moment;

step 1.3.3: calculating weighted values of all sigma point state predicted values, namely target state predicted values, wherein the calculation formula is as follows:

wherein the content of the first and second substances,

is shown at t_i-1Time of day prediction t_iTarget State prediction value at time, t_iFor the sampling moment, | represents a conditional operator;

the state predicted value of the ith sigma point is obtained;

estimating a weight value for the state of the ith sigma point;

step 1.3.4: calculating the target state covariance predicted value according to the following formula

Wherein, P (t)_i|t_i-1) Is shown at t_i-1Time of day prediction t_iPredicting the covariance value of the target state at the moment;

the state predicted value of the ith sigma point is obtained;

is shown at t_i-1Time of day prediction t_iPurpose of timeA target state prediction value;

estimating a variance weight for the state of the ith sigma point; q (t)_i-1) Is the process noise covariance.

6. The indoor target fast tracking method based on IMU and RFID information fusion of claim 2, wherein the step 1.4 of calculating the target state measurement predicted value according to the RFID measurement model and the target state predicted value comprises:

step 1.4.1: and calculating the measurement predicted value of each sigma point according to the state predicted value of each sigma point, wherein the calculation formula is as follows:

wherein the content of the first and second substances,

measuring and predicting value of ith sigma point;

the state predicted value of the ith sigma point is obtained;

predicting the state of each sigma point

Substituting the result obtained by the measurement equation;

reconstructing the influence of the measurement noise on the measurement prediction for the measurement noise analog quantity of the ith sigma point;

step 1.4.2: calculating weighted values of the measurement predicted values of all sigma points, namely the target state measurement predicted value, wherein the calculation formula is as follows:

wherein the content of the first and second substances,

measuring a predicted value for a target state;

measuring and predicting value of ith sigma point;

estimating a weight value for the state of the ith sigma point; n is 1,2, … N (t)_i)，N(t_i) Is t_iThe number of RFID readers that can obtain the target measurement value at the time.

7. The IMU and RFID information fusion-based indoor target fast tracking method of claim 2, wherein the step 1.5 of calculating the insensitive Kalman filter gain according to the target state prediction value and the target state measurement prediction value comprises:

step 1.5.1: and (3) calculating the covariance of the target state measurement predicted value, wherein the calculation formula is as follows:

wherein the content of the first and second substances,

measuring and predicting value of ith sigma point;

measuring a predicted value for a target state;

estimating a variance weight for the state of the ith sigma point; r_n(t_i) The measured variance for the nth reader;

step 1.5.2: calculating the cross covariance of the target state predicted value and the target state measurement predicted value, wherein the calculation formula is as follows:

wherein, P_xz,n(t_i) Is a cross covariance matrix;

estimating a variance weight for the state of the ith sigma point;

the state predicted value of the ith sigma point is obtained;

is shown at t_i-1Time of day prediction t_iTarget state prediction value of the moment;

measuring and predicting value of ith sigma point;

measuring a predicted value for the target;

step 1.5.3: the filter gain is calculated from the covariance in step 1.5.1 and the cross covariance in step 1.5.2, the calculation formula is as follows:

wherein, K_n(t_i) Is t_iFilter gain, t, of the nth RFID reader at time_iIs the sampling time; p_zz,n(t_i) In the form of a cross-covariance matrix,

represents the inverse of the cross covariance matrix; s_n(t_i) Is a covariance matrix, N is 1,2, … N (t)_i)，N(t_i) Is t_iThe number of RFID readers that can obtain the target measurement value at the time.

8. The IMU and RFID information fusion based indoor target fast tracking method according to claim 2, characterized in that the calculation of step 1.9 is as follows:

get

The third and sixth rows of the target state predicted values obtained in step 1.4, that is, the lateral and vertical axis acceleration predicted values, η ═ x, y respectively represent the lateral and vertical coordinates of the tracking area;

1) when i is less than or equal to 4, updating the system self-adaptive parameters as follows:

when in use

When the temperature of the water is higher than the set temperature,

when in use

When the temperature of the water is higher than the set temperature,

if it is not

Any small positive number can be taken;

maneuvering frequency set to α₀The change is not changed; wherein

η x, y respectively represent lateral and longitudinal acceleration variance values of the tracking area, i.e. when η x,

representing the lateral acceleration variance of the tracking area, when η y,

representing a longitudinal acceleration variance of the tracking area; a is_M,ηIs a positive number ranging from 1 to 100, a-_M,ηIs a and_M,ηnegative numbers with the same absolute value;

2) when i is greater than 4, updating the system adaptive parameters in the following manner;

wherein r is_i,η(1) Is t_iAcceleration previous step cross-correlation parameter of timeNumber r_i,η(0) Is t_iTime acceleration autocorrelation parameters, η ═ x, y, x denoting lateral coordinates within the tracking area and y denoting longitudinal coordinates within the tracking area;

predicted values of lateral and longitudinal axial acceleration, r_i-1,η(1) Is t_i-1Acceleration previous step cross-correlation parameter of time, r_i-1,η(0) Is t_i-1Time of day acceleration autocorrelation parameter, β_i,ηAnd

is t_iThe intermediate variable of the calculation of the time of day,

is t_iVariance of acceleration at time α_i,ηIs t_iFrequency of manoeuvres of time, th_i＝t_i-t_i-1The time interval between two measurements.

9. The IMU and RFID information fusion-based indoor target fast tracking method according to any one of claims 1-8, wherein if the sampling period of RFID is greater than or equal to the threshold in step 1, the RFID measurement data is subjected to coordinate estimation through an insensitive Kalman filter, and the distance between the two estimated coordinates is used as the estimated distance d₁And estimating the course angle by selecting gyroscope data of the IMU based on

The coordinates of the maneuvering target at each moment are calculated by the mathematical relation; the concrete implementation steps are as follows:

step 1.1': estimating coordinates of RFID measurement data through an insensitive Kalman filter, and taking the distance between two estimated coordinates as an estimated distance d₁；

Step 1.2': estimating the attitude angle change condition of the target in each sampling interval by utilizing IMU (inertial measurement Unit) measurement data, and delaying the attitude angle data for 6 seconds by taking the drift characteristic of gyroscope measurement data into consideration;

step 1.3': obtaining angular velocity information of a gyroscope in each sampling interval IMU, and listing a quaternion differential equation according to the angular velocity information of the gyroscope;

obtaining real-time quaternion q by solving the quaternion differential equation₀，q₁，q₂，q₃Q to be obtained₀，q₁，q₂，q₃Carry into the following formula to get the attitude matrix

Wherein the content of the first and second substances,

which represents the product of the quaternion numbers,

denotes the angular velocity, ω, of b with respect to n_nbx、ω_nby、ω_nbzIs the output value of the gyroscope;

step 1.4': updating the quaternion by using a 4-order Runge Kutta method;

h is a posture updating period;

step 1.5': updating an attitude matrix according to the updated quaternion, and solving according to the updated attitude matrix to obtain an attitude angle as follows:

θ_{master and slave}＝arcsin C₃₂

θ＝θ_{Master and slave}

Wherein the pitch angle is defined as (-90 degrees and 90 degrees), the roll angle is defined as (-180 degrees and 180 degrees), the course angle is defined as (0 degrees and 360 degrees), and theta in the formula_{Master and slave}、γ_{Master and slave}And

the pitch angle, roll angle and course angle, theta, gamma and

representing the angle value converted into the defined domain;

step 1.6': according to

To calculate the maneuvering targetCoordinates at each time;

step 1.7': and storing the calculated coordinates of the maneuvering target.

10. The IMU and RFID information fusion-based indoor target fast tracking method as claimed in any one of claims 1-8, wherein in step 2, it is determined whether there is any measurement data from the IMU, if yes, the heading angle is estimated by using gyroscope data of the IMU, and the displacement d of the maneuvering target in each sampling interval is calculated by Newton's law of motion using accelerometer data of the IMU₂According to

The coordinates of the maneuvering target at each moment are calculated by the mathematical relation; the concrete implementation steps are as follows:

step 2.1: calculating the displacement d of the target using the measurement data of the accelerometer₂；

Step 2.2: obtaining a real-time quaternion q by solving a quaternion differential equation₀，q₁，q₂，q₃：

Wherein the content of the first and second substances,

which represents the product of the quaternion numbers,

denotes the angular velocity, ω, of b with respect to n_nbx、ω_nby、ω_nbzBeing gyroscopesThe output value of (d);

step 2.3: q to be obtained₀，q₁，q₂，q₃Carry into the following formula to get the attitude matrix

Step 2.4: and updating the quaternion by adopting a fourth-order Runge Kutta method, wherein the calculation formula is as follows:

h is a posture updating period;

step 2.5: updating an attitude matrix according to the updated quaternion, and solving according to the updated attitude matrix to obtain an attitude angle as follows:

θ_{master and slave}＝arcsinC₃₂

θ＝θ_{Master and slave}

Wherein the pitch angle is defined as (-90 degrees and 90 degrees), the roll angle is defined as (-180 degrees and 180 degrees), the course angle is defined as (0 degrees and 360 degrees), and theta in the formula_{Master and slave}、γ_{Master and slave}And

the pitch angle, roll angle and course angle, theta, gamma and

representing the angle value converted into the defined domain;

step 2.6: using the determined displacement d₂And course angle

According to

The coordinates of the maneuvering target at each moment are calculated by the mathematical relation;

step 2.7: and storing the calculated coordinates of the maneuvering target.

Images