CN108120438A - A kind of indoor objects fast tracking method merged based on IMU and RFID information - Google Patents

Abstract

The present invention provides an indoor target fast tracking method based on the fusion of IMU and RFID information. The target tracking method is selected according to the comparison result between the RFID sampling period and the threshold value. If the RFID sampling period is less than the threshold value, the insensitive Kalman filter is used Estimate the coordinates of the RFID measurement data to obtain the coordinates of the maneuvering target; if the sampling period of the RFID is greater than or equal to the threshold, the distance between the two estimated target coordinates is used as the estimated distance, and the estimated distance and heading angle data are used to estimate the target The motion coordinates in each sampling interval; it is also judged whether there are unprocessed raw measurement data in the RFID measurement system, and if so, the above-mentioned estimation process is performed again; if not, the attitude calculation method based on IMU data is used. The motion of the target is supplemented by calculations. The fast tracking of indoor targets is realized, the tracking accuracy is improved, and the estimation results will not cause error accumulation over time.

Description

A Fast Tracking Method for Indoor Targets 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 fast tracking method based on IMU and RFID information fusion.

Background technique

With the development of science and technology, indoor tracking and positioning technology has become a hot issue that people pay attention to. For example, how to perceive your own location and reach your destination in indoor places such as large hospitals, airports, and railway stations; how to track the location of moving goods in large warehouses to realize the automation of goods movement; The perception of the robot's own position during the movement, etc.

People usually use methods such as GPS positioning to obtain the coordinates of outdoor targets. However, these technologies have large errors in determining the positions of indoor moving targets. Commonly used indoor mobile target tracking technologies include Wi-Fi technology, Radio Frequency Identification (RFID, Radio Frequency Identification), inertial navigation technology, optical tracking and positioning technology, sensor network positioning technology, infrared positioning technology, etc. However, the development of these technologies is still in its infancy. Some of them have low tracking accuracy for indoor targets and are difficult to meet the requirements of practical applications.

RFID, or Radio frequency identification, is the main component of an indoor positioning and tracking system. The RFID system consists of a radio frequency tag and a reader, and the radio frequency tag stores the identification information of the tag. When the target with the tag moves to the vicinity of the reader, the reader can obtain the identification information of the tag, thereby identifying a certain target, and at the same time obtain the distance information between the tag and the reader, and then can carry out the target locate or track. At present, the target positioning patents of the RFID system mainly include an indoor positioning method based on active RFID (CN201110101432.8), an intelligent storage system for steel products based on radio frequency identification technology (CN201210093992.8), and a station personnel positioning system (CN201010256882. X), a positioning method for RFID indoor positioning system (CN200910040571.7), indoor positioning system based on RFID technology (CN200810027885.9), these patents are to obtain the positioning information of the target, and its unified feature is to assume that the target is Stationary, use the radio frequency principle to get the approximate location of the tagged target.

For moving targets, the above method has the following defects: 1. Because RFID can only obtain distance information with the reader, so in order to obtain accurate position data, it must rely on the distance information of a certain target obtained by multiple readers. The geometric relationship between the readers can get the position of the target, which increases the complexity of the system calculation; 2. The above methods all assume that the target is stationary, which does not conform to the actual situation of the moving target. The displacement of the moving target, Velocity and acceleration satisfy the basic constraint relationship of kinematics. Assuming that the target is stationary, the relationship is completely discarded, resulting in the reduction of the constraint relationship, which directly leads to inaccurate estimation of the trajectory of the moving target. 3. When the target is moving, the distance from the RFID is too far away or the data related to the moving target is not collected due to other factors, which will affect the accuracy of positioning.

Obtaining real-time trajectories of targets is very common in many tracking systems, such as pedestrian tracking in indoor venues, intelligent transportation and other fields. Therefore, the above invention is not suitable for RFID indoor target tracking systems that need to obtain real-time trajectories of moving targets.

Contents of the invention

Based on this, the object of the present invention is to provide a fast tracking method for indoor targets based on IMU and RFID information fusion, which realizes fast tracking of indoor targets and improves tracking accuracy.

The method is based on the Unsented Kalman Filters (UKF for short), uses the distance information from the target to the reader obtained by the RFID system, and obtains the estimation of the trajectory according to the dynamic characteristics of the indoor moving target. In places where there is no RFID measurement data, the attitude calculation method obtained by the IMU is used to supplement information to achieve fast tracking of indoor targets.

The basic train of thought of realizing the purpose of the present invention is: at first judge whether the interval between two measurement data from RFID reader is less than threshold value (take threshold value as 0.18 second), if interval is less than threshold value, then take based on the non-linearity of RFID measurement data The Kalman filter method estimates the coordinates and stores the estimated coordinates of the maneuvering target; if the interval is greater than the threshold, the distance between the two estimated target coordinates is used as the estimated distance in the attitude calculation method, and the IMU is used to The measurement data estimates the attitude angle change of the target during this period and takes into account the drift characteristics of the gyroscope measurement data. The attitude angle data is delayed for 6 seconds, and then the distance and heading angle data are used to estimate the target during this time. The motion coordinates within and store the obtained position coordinates. After the data is stored, it is necessary to determine whether there is any measurement data from the RFID reader. If so, perform the above estimation process again; if not, determine whether there is measurement data from the IMU. If there is measurement data, press The attitude calculation method based on IMU data supplements the calculation of the target’s motion. At this time, the distance calculation should use the measurement data of the accelerometer. If there is no measurement data from the IMU, the algorithm ends. The advantage of this algorithm is that the Kalman filter is used to estimate the trajectory of the moving target based on the RFID data, and the estimation result will not cause error accumulation over time. When there is no RFID measurement data, the attitude calculation method of inertial sensor data is used to track the movement of the target, and the tracking information at this time is used as a supplement to the movement information of the target.

To achieve the above object, the technical scheme of the present invention is as follows:

A fast tracking method for indoor targets based on IMU and RFID information fusion, comprising the following steps:

Step 1: Select the target tracking method according to the comparison result between the sampling period of the RFID and the threshold value. If the sampling period of the RFID is less than the threshold value, estimate the coordinates of the RFID measurement data through an insensitive Kalman filter to obtain the coordinates of the maneuvering target; If the sampling period of the RFID is greater than or equal to the threshold, the coordinates of the RFID measurement data are estimated through the insensitive Kalman filter, and the distance between the two estimated coordinates is used as the estimated distance d ₁ , and the gyroscope of the IMU is selected Estimate heading angle from data, according to Calculate the coordinates of the maneuvering target at each moment using the mathematical relational formula;

Step 2: Determine whether there are unprocessed raw measurement data in the RFID measurement system, if so, return to step 1; if not, determine whether there is still measurement data from the IMU, if so, use the gyroscope data of the IMU to estimate heading angle, and use the accelerometer data of the IMU to calculate the displacement d ₂ of the maneuvering target in each sampling interval by Newton's law of motion, according to Calculate the coordinates of the maneuvering target at each moment by the mathematical relational expression; if not, the algorithm ends;

Among them, Δx is the variation of the horizontal coordinate x of the target in the tracking area, and Δy is the variation of the longitudinal coordinate y of the target in the tracking area, The heading angle turned by the target.

On the basis of the above solution, the present invention can also be improved as follows.

Further, in step 1, if the sampling period of the RFID is less than the threshold value, the coordinates of the RFID measurement data are estimated through an insensitive Kalman filter to obtain the coordinates of the maneuvering target, including:

Step 1.1: Target motion state and system adaptive parameter initialization;

Step 1.2: Establish a motion model with system adaptive parameters and an RFID measurement model;

Step 1.3: Predict the target state according to the established motion model with system adaptive parameters, and obtain the predicted value of the target state at the next time point;

Step 1.4: Calculate the target state measurement prediction value according to the RFID measurement model and the target state prediction value;

Step 1.5: Calculate the insensitive Kalman filter gain according to the target state prediction value and the target state measurement prediction value;

Step 1.6: Read the original measured value z _n (t _i ) of the target state at the next time point obtained by the RFID monitoring system;

Step 1.7: Calculate the target state correction value based on the target state raw measurement value, target state measurement prediction value and filter gain:

Among them, X(t _i ) is the correction value of the target state, t _i is the sampling time, n=1,2,...N(t _i ), N(t _i ) is the RFID reading of the acquisition and target distance data at the time t _i The number of readers; K _n (t _i ) is the filter gain at time t _i , z _n (t _i ) is the measurement data of the nth reader at time t _i , is the estimated value of the measurement at time t _i ;

Step 1.8: Predict value based on target state and target state correction value X(t _i ) to calculate target state estimated value and target state covariance estimated value;

The calculation formula of target state estimate is as follows:

in, Indicates the estimated value of the target state at time t _i ; Indicates the predicted value of the target state at the time t _{i -1} when predicting the target state; X(t _i ) is the target state correction value, and t _i is the sampling time;

The calculation formula of target state covariance estimate is as follows:

Among them, P(t _i |t _i ) represents the estimated value of the target state covariance at time t _i , and P(t _i |t _i-1 ) represents the predicted value of the target state covariance at time t _i at time t _i-1 , K _n (t _i ) is the filter gain of the nth reader at time t _i ; S _n (t _i ) is the covariance matrix of the nth reader;

Step 1.9: Utilize the predicted value of the target state to update the adaptive parameters of the system, and then update the motion model in step 1.3;

Step 1.10: Store the estimated coordinates of the maneuvering target.

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

Step 1.1.1: Set the initial value of the target motion state is a 6-dimensional vector of all 0 columns, and its dimension is the dimension of the state vector in the system model;

Step 1.1.2: Set the initial value of the system adaptive parameter α _η = α ₀ and Wherein η=x, y, x represents the horizontal coordinate of target in tracking area, and y represents the vertical coordinate of target in tracking area, and α _η represents the acceleration maneuvering frequency of horizontal coordinate, vertical coordinate, Represents the acceleration variance of the horizontal and vertical coordinates;

Step 1.1.3: Set the initial value of the acceleration component of the system Wherein η=x, y, x represents the transverse coordinates of the target in the tracking area, and y represents the longitudinal coordinates of the target in the tracking area;

Step 1.1.4: Set the scale parameters of the system κ=-3, χ=0.01, ο=2;

Step 1.1.5: Set the initial value of the cross-correlation parameter of the previous step of acceleration to r _0,η (1)=0, and the initial value of the auto-correlation parameter of the acceleration step to r _0,η (0)=0.

Further, in step 1.2, a motion model and an RFID measurement model with system adaptive parameters are established, including:

Step 1.2.1: Use the following formula to describe the motion characteristics of the target:

in, is a 6-dimensional state column vector, are the lateral coordinate displacement, velocity and acceleration, respectively, are the longitudinal coordinate displacement, velocity and acceleration respectively; x(t _i ) is the state vector of the target at time t _i , and t _i is the sampling time; A(t _i-1 ) is the state transition matrix at time t _i-1 ; U( t _i-1 ) is the control matrix at time t _i-1 ; w _d (t _i-1 ) is the process noise, its mean value is 0, and its variance is Q; A(t _i-1 ), U(t _i-1 ) and Q include system adaptive parameters;

Step 1.2.2: Establish the following RFID system target measurement equation:

z _n (t _i )=h _n [x(t _i )]+v _n (t _i )

Among them, z _n (t _i ) is the measured value of the target at time t _i obtained by the RFID monitoring system, h _n [x(t _i )] is the measurement equation, and x(t _i ) is the state vector of the target at time t _i ; v _n (t _i ) is the Gaussian measurement white noise of the nth reader in the RFID monitoring system, and is independent of the process noise w _d (t _i-1 ), d _n (t _i ) is the target at time t _i The real distance (unknown) from the position to the nth RFID reader, x(t _i ), y(t _i ) are the abscissa and ordinate of the target position at time t _i respectively; x _n (0), y _n (0) are the abscissa and ordinate of the nth RFID reader respectively.

Further, in step 1.3, the target state is predicted according to the established motion model with system adaptive parameters, including:

Step 1.3.1: Calculate the sigma point state value and its weight required by the insensitive Kalman filter according to the following formula;

Among them, x ⁽ⁱ⁾ is the extended state value of the ith sigma point, including i=0,1,...2N _x is the state value of the i-th sigma point, and N _x is the dimension of the system state; is the process noise analog quantity of the ith sigma point, is the measurement noise analog quantity of the ith sigma point; Represents the estimated value of the target state at time t _i-1 , P(t _i-1 |t _i-1 ) represents the estimated value of covariance of the target state at time t _i-1 , is an all-0 matrix with the same dimension as the process noise w(t _i-1 ), is an all-0 matrix with the same dimension as the measurement noise v(t _i-1 ); i=0,1,...2N _x , which is the state estimation weight of the i-th sigma point; i=0,1,…2N _x , is the state estimation variance weight of the i-th sigma point; express The ith column of the mean square error matrix, κ, χ, ο are scale parameters;

Step 1.3.2: Calculate the state prediction value of each sigma point according to the motion model and the initial value of the target state, the calculation formula is as follows,

in, is the state prediction value of the i-th sigma point; i=0,1,…2N _x , which is the state value of the i-th sigma point, is the process noise analog quantity at the i-th sigma point to reconstruct the influence of process noise on state prediction; A(t _i-1 ) is the state transition matrix at time t _i-1 ; U(t _i-1 ) is t The control matrix at time _i-1 ; is the mean value of acceleration from time 0 to time t _i-1 ;

Step 1.3.3: Calculate the weighted value of all sigma point state prediction values, that is, the target state prediction value, the calculation formula is as follows:

in, Represents the predicted value of the target state at time t _i-1 , _{t i is} the sampling time, and | represents the conditional operator; is the state prediction value of the i-th sigma point; i=0,1,...2N _x , which is the state estimation weight of the i-th sigma point;

Step 1.3.4: Calculate the target state covariance prediction value, the calculation formula is as follows

Among them, P(t _i |t _{i-1 )} represents the predicted value of the target state covariance at time t _i at time t i-1; is the state prediction value of the i-th sigma point; Indicates the predicted value of the target state at time t _i -1 at time t _i ; i=0,1,...2N _x , is the state estimation variance weight of the ith sigma point; Q(t _i -1) is the process noise covariance.

Further, in step 1.4, calculate the target state measurement prediction value according to the RFID measurement model and the target state prediction value, including:

Step 1.4.1: Calculate the measurement prediction value of each sigma point according to the state prediction value of each sigma point, the calculation formula is as follows:

in, is the measured predicted value of the ith sigma point; is the state prediction value of the i-th sigma point; For the state prediction value of each sigma point The results obtained by substituting into the measurement equation; is the measurement noise analog quantity of the i-th sigma point to reconstruct the influence of measurement noise on measurement prediction;

Step 1.4.2: Calculate the weighted value of the measured predicted value of all sigma points, which is the measured predicted value of the target state. The calculation formula is as follows:

in, Measure the predicted value for the target state; is the measured predicted value of the ith sigma point; i=0,1,...2N _x , is the state estimation weight of the i-th sigma point; n=1,2,...N(t _i ), N(t _i ) is the target measurement value that can be obtained at time t _i The number of RFID readers.

Further, in step 1.5, the insensitive Kalman filter gain is calculated according to the predicted value of the target state and the measured predicted value of the target state, including:

Step 1.5.1: Calculate the covariance of the target state measurement prediction value, the calculation formula is as follows:

in, is the measured predicted value of the ith sigma point; Measure the predicted value for the target state; i=0,1,...2N _x , is the state estimation variance weight of the i-th sigma point; R _n (t _i ) is the measurement variance of the n-th reader;

Step 1.5.2: Calculate the cross-covariance of the predicted value of the target state and the predicted value of the target state measurement, the calculation formula is as follows:

Among them, P _xz,n (t _i ) is the cross-covariance matrix; i=0,1,…2N _x , is the state estimation variance weight of the i-th sigma point; is the state prediction value of the i-th sigma point; Indicates the predicted value of the target state at time t _i -1 at time t _i ; is the measured predicted value of the ith sigma point; measure the predicted value for the target;

Step 1.5.3: Calculate the filter gain based on the covariance in step 1.5.1 and the cross-covariance in step 1.5.2, the calculation formula is as follows:

Among them, K _n (t _i ) is the filter gain of the nth RFID reader at time t _i , t _i is the sampling time; P _zz,n (t _i ) is the cross-covariance matrix, Represents the inverse of the cross covariance matrix; S _n (t _i ) is the covariance matrix, n=1,2,...N(t _i ), N(t _i ) is the RFID reading that can obtain the target measurement value at time t _i number of devices.

Further, the calculation of step 1.9 is as follows:

Pick The third row and the sixth row of the target state prediction value that step 1.4 obtains, i.e. the horizontal and vertical axis acceleration prediction values, η=x, y represent the horizontal and vertical coordinates of the tracking area respectively;

1) When i≤4, update the system adaptive parameters as follows:

when hour,

when hour,

if Can take any small positive number;

The maneuvering frequency is set to α ₀ unchanged; where η=x, y respectively represent the horizontal and vertical acceleration variance values of the tracking area, a _{M, η} is a positive number, the range is between 1 and 100, a _{-M, η} is the same absolute value as a _{M, η} negative number;

2) When i>4, update the system adaptive parameters as follows;

Among them, r _{i, η} (1) is the cross-correlation parameter before the acceleration step at t _i moment, r _{i, η} (0) is the acceleration autocorrelation parameter at t _i moment, η=x, y, and x represents the acceleration in the tracking area Horizontal coordinate, y represents the vertical coordinate in the tracking area; Acceleration prediction value of the horizontal and vertical axes, r _{i-1, η} (1) is the cross-correlation parameter of the previous step of acceleration at the time t _i-1, r _{i-1, η} (0) is the autocorrelation of the acceleration at the time t _i-1 parameters, βi _{, η} and is the calculation intermediate variable at time t _i , is the acceleration variance at time t _i , α _{i, η} is the maneuvering frequency at time t _i , th _i =t _i -t _i-1 is the time interval between two measurement data.

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 through an insensitive Kalman filter, and the distance between the two estimated coordinates is used as the estimated distance d ₁ , and Choose to use the gyroscope data of the IMU to estimate the heading angle, according to Calculate the coordinates of the maneuvering target at each moment using the mathematical relational formula; the specific implementation steps are:

Step 1.1': Estimate the coordinates of the RFID measurement data through an insensitive Kalman filter, and use the distance between the two estimated coordinates as the estimated distance d ₁ ;

Step 1.2': Use the IMU measurement data to estimate the attitude angle change of the target in each sampling interval and take into account the drift characteristics of the gyroscope measurement data, and make a 6-second delay for the attitude angle data;

Step 1.3': Obtain the angular velocity information of the gyroscope in the IMU in each sampling interval, and list the differential equations about the quaternion according to the angular velocity information of the gyroscope;

Real-time quaternion q ₀ , q ₁ , q ₂ , q ₃ are obtained by solving the above quaternion differential equation, and the obtained q ₀ , q ₁ , q ₂ , q ₃ are put into the following formula to obtain the attitude matrix

in, represents a quaternion product, Indicates the angular velocity of the b system relative to the n system, ω _nbx , ω _nby , ω _nbz are the output values of the gyroscope;

Step 1.4': update the quaternion by using the fourth-order Runge-Kutta method;

h is the attitude update cycle;

Step 1.5': Update the attitude matrix according to the updated quaternion, and then calculate the attitude angle according to the updated attitude matrix:

θ _main = arcsinC ₃₂

θ = θ _main

The definition domain of the pitch angle is (-90°, 90°), the definition domain of the roll angle is (-180°, 180°), and the definition domain of the heading angle is (0°, 360°). The θ in the formula _main , gamma _{main and} The pitch angle, roll angle and heading angle calculated by the attitude matrix respectively, θ, γ and Indicates the angle value transformed into the domain;

Step 1.6': According to Calculate the coordinates of the maneuvering target at each moment using the mathematical relational formula;

Step 1.7': Store the calculated coordinates of the maneuvering target.

Further, in step 2, it is judged whether there is any measurement data from the IMU, if so, the gyroscope data of the IMU is used to estimate the heading angle, and the accelerometer data of the IMU is used to calculate the displacement of the moving target in each sampling interval by Newton's law of motion d ₂ , according to Calculate the coordinates of the maneuvering target at each moment using the mathematical relational formula; the specific implementation steps are:

Step 2.1: Calculate the displacement d ₂ of the target using the measurement data of the accelerometer;

Step 2.2: Obtain the real-time quaternion q ₀ , q ₁ , q ₂ , q ₃ by solving the following quaternion differential equation:

in, represents a quaternion product, Indicates the angular velocity of the b system relative to the n system, ω _nbx , ω _nby , ω _nbz are the output values of the gyroscope;

Step 2.3: Put the obtained q ₀ , q ₁ , q ₂ , and q ₃ into the following formula to obtain the attitude matrix

Step 2.4: Use the fourth-order Runge-Kutta method to update the quaternion, the calculation formula is as follows:

h is the attitude update cycle;

Step 2.5: Update the attitude matrix according to the updated quaternion, and then solve the attitude angle according to the updated attitude matrix:

θ _main = arcsinC ₃₂

θ = θ _main

The definition domain of the pitch angle is (-90°, 90°), the definition domain of the roll angle is (-180°, 180°), and the definition domain of the heading angle is (0°, 360°). The θ in the formula _main , gamma _{main and} The pitch angle, roll angle and heading angle calculated by the attitude matrix respectively, θ, γ and Indicates the angle value transformed into the domain;

Step 2.6: Use the obtained displacement d ₂ and heading angle according to Calculate the coordinates of the maneuvering target at each moment using the mathematical relational formula;

Step 2.7: Store the calculated coordinates of the maneuvering target.

The beneficial effects of the present invention are:

The multi-sensor fusion indoor fast tracking method based on IMU and RFID measurement data of the present invention uses the Kalman filter to estimate the trajectory of the moving target based on the RFID data, and the estimation result will not cause error accumulation over time; without RFID measurement When the data is collected, the attitude calculation method of the inertial sensor data is used to track the movement of the target. At this time, the tracking information is used as a supplement to the target movement information to reconstruct the movement trajectory of the target; the fast tracking of the indoor target is realized, and the tracking is improved. Accuracy, especially suitable for RFID indoor target tracking systems that need to obtain real-time trajectories of moving targets. The optimization of the tracking method further improves the tracking accuracy.

Description of drawings

FIG. 1 is a flow chart 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 ways

In order to make the purpose, technical solution and advantages of the present invention clearer, the multi-sensor fusion indoor fast tracking method based on IMU and RFID measurement data of the present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described here are only used to explain the present invention, not to limit the present invention.

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

Step 1: Select the target tracking method according to the comparison result between the sampling period of the RFID and the threshold value. If the sampling period of the RFID is less than the threshold value, estimate the coordinates of the RFID measurement data through an insensitive Kalman filter to obtain the coordinates of the maneuvering target ( location information of the carrier); 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 an insensitive Kalman filter, and the distance between the two estimated coordinates is used as the estimated distance d ₁ , and Choose to use the gyroscope data of the IMU to estimate the heading angle, according to Calculate the coordinates of the maneuvering target (carrier) at each moment using the mathematical relational formula;

Step 2: Determine whether there are unprocessed raw measurement data in the RFID measurement system, if so, return to step 1; if not, determine whether there is still measurement data from the IMU, if so, use the gyroscope data of the IMU to estimate heading angle, and use the accelerometer data of the IMU to calculate the displacement d ₂ of the maneuvering target in each sampling interval by Newton's law of motion, according to Calculate the coordinates of the maneuvering target at each moment by the mathematical relational expression; if not, the algorithm ends;

Among them, Δx represents the variation of the horizontal coordinate x of the target in the tracking area, and Δy represents the variation of the vertical coordinate y of the target in the tracking area, The heading angle turned by the target. In this embodiment, the threshold may be set to 0.18 seconds.

As a preferred method, if the sampling period of the RFID is less than the threshold in step 1, the coordinates of the RFID measurement data are estimated by an insensitive Kalman filter to obtain the coordinates of the maneuvering target, including:

Step 1.1: Target motion state and system adaptive parameter initialization;

Step 1.2: Establish a motion model with system adaptive parameters and an RFID measurement model;

Step 1.3: Predict the target state according to the established motion model with system adaptive parameters, and obtain the predicted value of the target state at the next time point;

Step 1.4: Calculate the target state measurement prediction value according to the RFID measurement model and the target state prediction value;

Step 1.5: Calculate the insensitive Kalman filter gain according to the target state prediction value and the target state measurement prediction value;

Step 1.6: Read the original measurement value zn(ti) of the target state at the next time point obtained by the RFID monitoring system;

Step 1.7: Calculate the target state correction value based on the target state raw measurement value, target state measurement prediction value and filter gain:

Among them, X(t _i ) is the correction value of the target state, t _i is the sampling time, n=1,2,...N(t _i ), N(t _i ) is the RFID reading of the acquisition and target distance data at the time t _i The number of readers; K _n (t _i ) is the filter gain at time t _i , z _n (t _i ) is the measurement data of the nth reader at time t _i , is the estimated value of the measurement at time t _i ;

Step 1.8: Predict value based on target state and target state correction value X(t _i ) to calculate target state estimated value and target state covariance estimated value;

The calculation formula of target state estimate is as follows:

in, Indicates the estimated value of the target state at time t _i ; Indicates the predicted value of the target state at the time t _{i -1} when predicting the target state; X(t _i ) is the target state correction value, and t _i is the sampling time;

The calculation formula of target state covariance estimate is as follows:

Among them, P(t _i |t _i ) represents the estimated value of the target state covariance at time t _i , and P(t _i |t _i-1 ) represents the predicted value of the target state covariance at time t _i at time t _i-1 , K _n (t _i ) is the filter gain of the nth reader at time t _i ; S _n (t _i ) is the covariance matrix of the nth reader;

Step 1.9: Utilize the predicted value of the target state to update the adaptive parameters of the system, and then update the motion model in step 2.3;

Step 1.10: Store the estimated coordinates of the maneuvering target.

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

Step 1.1.1: Set the initial value of the target motion state is a 6-dimensional vector of all 0 columns, and its dimension is the dimension of the state vector in the system model;

Step 1.1.2: Set the initial value of the system adaptive parameter α _η = α ₀ and Wherein η=x, y, x represents the horizontal coordinate of target in tracking area, and y represents the vertical coordinate of target in tracking area, and α _η represents the acceleration maneuvering frequency of horizontal coordinate, vertical coordinate, Indicates the acceleration variance of the horizontal and vertical coordinates, in this embodiment α ₀ =1/20,

Step 1.1.3: Set the initial value of the acceleration component of the system In this example Get 0, where η=x, y, x represents the horizontal coordinates of the target in the tracking area, and y represents the longitudinal coordinates of the target in the tracking area;

Step 1.1.4: Set the scale parameters of the system κ=-3, χ=0.01, ο=2;

Step 1.1.5: Set the initial value of the cross-correlation parameter of the previous step of acceleration to r _0,η (1)=0, and the initial value of the auto-correlation parameter of the acceleration step to r _0,η (0)=0.

Further, in step 1.2, a motion model and an RFID measurement model with system adaptive parameters are established, including:

Step 1.2.1: Use the following formula to describe the motion characteristics of the target (motion model formula):

in, is a 6-dimensional state column vector, are the lateral coordinate displacement, velocity and acceleration, respectively, are the longitudinal coordinate displacement, velocity and acceleration respectively; x(t _i ) is the state vector of the target at time t _i , and t _i is the sampling time; A(t _i-1 ) is the state transition matrix at time t _i-1 ; U( t _i-1 ) is the control matrix at time t _i-1 ; w _d (t _i-1 ) is the process noise, its mean value is 0, and its variance is Q; A(t _i-1 ), U(t _i-1 ) and Q include system adaptive parameters;

is the state transition matrix; is the control matrix;

is the average acceleration value of the target from time 0 to time t _i-1 ; w(t _i-1 )=[w _x (t _i-1 ) w _y (t _i-1 )] ^T is the process noise, and its mean value is 0 , with a variance of Assume that the horizontal and vertical process noises are independent of each other; let η=x, y, x represents the horizontal coordinates of the target in the tracking area, and y represents the vertical coordinates of the target in the tracking area, where the value of the corresponding matrix is:

Wherein th _i =t _i -t _i-1 is the time interval between two measurement data;

Step 1.2.2: Establish the following RFID system target measurement equation:

z _n (t _i )=h _n [x(t _i )]+v _n (t _i )

Among them, z _n (t _i ) is the measured value of the target at time t _i obtained by the RFID monitoring system, h _n [x(t _i )] is the measurement equation, and x(t _i ) is the state vector of the target at time t _i ; v _n (t _i ) is the Gaussian measurement white noise of the nth reader in the RFID monitoring system, which is independent of the process noise w _d (t _i-1 ), and its measurement variance satisfies σ _p and γ are the parameters of radio frequency measurement. Experiments show that σ _p is taken as 4dB, and γ can be taken as any positive number in the interval from 1.6 to 6.5; d _n (t _i ) is the position of the target at time t _i to the nth The real distance (unknown) of the RFID reader, x(t _i ), y(t _i ) are the horizontal and vertical coordinates of the target position at time t _i respectively; x _n (0), y _n (0) are the nth The abscissa and ordinate of each RFID reader, n=1, 2, ... N(t _i ), N(t _i ) is the number of RFID readers capable of obtaining the target measurement value at time t _i .

Further, in step 1.3, the target state is predicted according to the established motion model with system adaptive parameters, including:

Step 1.3.1: Calculate the sigma point state value and its weight required by the insensitive Kalman filter according to the following formula;

Among them, x ⁽ⁱ⁾ is the extended state value of the ith sigma point, including i=0,1,...2N _x is the state value of the i-th sigma point, and N _x is the dimension of the system state; is the process noise analog quantity of the ith sigma point, is the measurement noise analog quantity of the ith sigma point; Represents the estimated value of the target state at time t _i -1, P(t _i-1 |t _i-1 ) represents the estimated value of covariance of the target state at time t _i-1 , is an all-0 matrix with the same dimension as the process noise w(t _i-1 ), is an all-0 matrix with the same dimension as the measurement noise v(t _i-1 ); i=0,1,...2N _x , which is the state estimation weight of the i-th sigma point; i=0,1,...2N _x , is the state estimation variance weight of the i-th sigma point, and N _x is the dimension of the system state; express The ith column of the mean square error matrix, κ, χ, ο are scale parameters;

Step 1.3.2: Calculate the state prediction value of each sigma point according to the motion model and the initial value of the target state, the calculation formula is as follows,

in, is the state prediction value of the i-th sigma point; i=0,1,…2N _x , which is the state value of the i-th sigma point, is the process noise analog quantity at the i-th sigma point to reconstruct the influence of process noise on state prediction, N _x is the dimension of the system state; A(t _i-1 ) is the state transition matrix at time t _i-1 ; U(t _i-1 ) is the control matrix at time t _i - ₁ ; is the mean value of acceleration from time 0 to time t _i-1 ;

Step 1.3.3: Calculate the weighted value of all sigma point state prediction values, that is, the target state prediction value, the calculation formula is as follows:

in, Represents the predicted value of the target state at time t _i-1 , _{t i is} the sampling time, and | represents the conditional operator; is the state prediction value of the i-th sigma point; i=0,1,...2N _x , is the state estimation weight of the i-th sigma point, and N _x is the dimension of the system state;

Step 1.3.4: Calculate the target state covariance prediction value, the calculation formula is as follows

Among them, P(t _i |t _{i-1 )} represents the predicted value of the target state covariance at time t _i at time t i-1; is the state prediction value of the i-th sigma point; Indicates the predicted value of the target state at time t _i-1 at time t _i ; i=0,1,...2N _x , is the state estimation variance weight of the ith sigma point; N _x is the dimension of the system state; Q(t _i-1 ) is the process noise covariance.

Further, in step 1.4, calculate the target state measurement prediction value according to the RFID measurement model and the target state prediction value, including:

Step 1.4.1: Calculate the measurement prediction value of each sigma point according to the state prediction value of each sigma point, the calculation formula is as follows:

in, is the measured predicted value of the ith sigma point; is the state prediction value of the i-th sigma point; For the state prediction value of each sigma point The result obtained by substituting the measurement equation; n=1,2,...N(t _i ), N(t _i ) is the number of RFID readers that can obtain the target measurement value at t _i moment; is the measurement noise analog quantity of the i-th sigma point to reconstruct the influence of measurement noise on measurement prediction;

Step 1.4.2: Calculate the weighted value of the measured predicted value of all sigma points, which is the measured predicted value of the target state. The calculation formula is as follows:

in, Measure the predicted value for the target state; is the measured predicted value of the ith sigma point; i=0,1,...2N _x , is the state estimation weight of the i-th sigma point, N _x is the dimension of the system state; n=1,2,...N(t _i ), N(t _i ) is The number of RFID readers that can obtain the target measurement value at time t _i .

Further, in step 1.5, the insensitive Kalman filter gain is calculated according to the predicted value of the target state and the measured predicted value of the target state, including:

Step 1.5.1: Calculate the covariance of the target state measurement prediction value, the calculation formula is as follows:

in, is the measured predicted value of the ith sigma point; Measure the predicted value for the target state; i=0,1,...2N _x , is the state estimation variance weight of the i-th sigma point; N _x is the dimension of the system state; R _n (t _i ) is the measurement variance of the n-th reader; n =1,2,...N(t _i ), N(t _i ) is the number of RFID readers that can obtain the target measurement value at time t _i ;

Step 1.5.2: Calculate the cross-covariance of the predicted value of the target state and the predicted value of the target state measurement, the calculation formula is as follows:

Among them, P _xz,n (t _i ) is the cross-covariance matrix; i=0,1,...2N _x , is the state estimation variance weight of the i-th sigma point; is the state prediction value of the i-th sigma point; Indicates the predicted value of the target state at time t _i at time t _i-1 ; is the measured predicted value of the ith sigma point; measure the predicted value for the target;

Step 1.5.3: Calculate the filter gain based on the covariance in step 2.5.1 and the cross-covariance in step 2.5.2, the calculation formula is as follows:

Among them, K _n (t _i ) is the filter gain of the nth RFID reader at time t _i , t _i is the sampling time; P _zz,n (t _i ) is the cross-covariance matrix, Represents the inverse of the cross-covariance matrix; S _n (t _i ) is the covariance matrix, n=1,2,...N(t _i ), N(t _i ) is the RFID reading that can obtain the target measurement value at time t _i number of devices.

Further, the calculation of step 1.9 is as follows:

Pick The third row and the sixth row of the target state prediction value that step 1.4 obtains, i.e. the horizontal and vertical axis acceleration prediction values, η=x, y represent the horizontal and vertical coordinates of the tracking area respectively;

1) When i≤4, update the system adaptive parameters as follows:

when Time,

when Time,

if Can take any small positive number;

The maneuvering frequency is set to α ₀ unchanged; where η=x, y respectively represent the horizontal and vertical acceleration variance values of the tracking area, a _{M, η} is a positive number, the range is between 1 and 100, a _{-M, η} is the same absolute value as a _{M, η} negative number;

2) When i>4, update the system adaptive parameters as follows;

Among them, r _{i, η} (1) is the cross-correlation parameter of the previous step of the acceleration at the time t _i , r _{i, η} (0) is the autocorrelation parameter of the acceleration at the time t _i , and η=x, y, and x represents the acceleration in the tracking area Horizontal coordinate, y represents the vertical coordinate in the tracking area; Acceleration prediction value of the horizontal and vertical axes, r _{i-1, η} (1) is the cross-correlation parameter of the previous step of acceleration at the time t _i-1, r _{i-1, η} (0) is the autocorrelation of the acceleration at the time t _i-1 parameters, βi _{, η} and is the calculation intermediate variable at time t _i , is the acceleration variance at time t _i , α _{i, η} is the maneuvering frequency at time t _i , th _i =t _i -t _i-1 is the time interval between two measurement data.

As a preferred method, 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 an insensitive Kalman filter, and the distance between the two estimated coordinates is used as the estimated distance d ₁ , and choose to use the gyroscope data of the IMU to estimate the heading angle, according to Calculate the coordinates of the maneuvering target at each moment using the mathematical relational formula; the specific implementation steps are:

Step 1.1': Estimate the coordinates of the RFID measurement data through an insensitive Kalman filter, and use the distance between the two estimated coordinates as the estimated distance d ₁ ;

Step 1.2': Use the IMU measurement data to estimate the attitude angle change of the target in each sampling interval and take into account the drift characteristics of the gyroscope measurement data, and make a 6-second delay for the attitude angle data;

Step 1.3': Obtain the angular velocity information of the gyroscope in the IMU in each sampling interval, and list the differential equations about the quaternion according to the angular velocity information of the gyroscope;

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

Real-time quaternion q ₀ , q ₁ , q ₂ , q ₃ are obtained by solving the above quaternion differential equation, and the obtained q ₀ , q ₁ , q ₂ , q ₃ are put into the following formula to obtain (b to n Attitude matrix of the system) Attitude matrix

in, represents a quaternion product, Indicates the angular velocity of the b system relative to the n system, ω _nbx , ω _nby , ω _nbz are the output values of the gyroscope;

Step 1.4': update the quaternion by using the fourth-order Runge-Kutta method;

h is the attitude update cycle;

Step 1.5': Update the attitude matrix according to the updated quaternion, and then calculate the attitude angle according to the updated attitude matrix:

θ _main = arcsinC ₃₂

θ = θ _main

The definition domain of the pitch angle is (-90°, 90°), the definition domain of the roll angle is (-180°, 180°), and the definition domain of the heading angle is (0°, 360°). The θ _main , γ _{main and} The pitch angle, roll angle and heading angle calculated by the attitude matrix respectively, θ, γ and Represents the angle value transformed into the domain. After the quaternion is updated, the updated attitude matrix can be obtained, and the attitude angle can be obtained from the attitude matrix to obtain the updated attitude angle.

Step 1.6': According to The coordinates of the maneuvering target at _each moment are deduced from the mathematical relationship of The amount of change in the longitudinal coordinate y within the tracking area, The heading angle turned by the target.

Step 1.7': Store the calculated coordinates of the maneuvering target.

As a preferred method, in step 2, it is judged whether there is any measurement data from the IMU, and if so, the gyroscope data of the IMU is used to estimate the heading angle, and the accelerometer data of the IMU is used to calculate the moving target at each sampling point by Newton's law of motion. The displacement d ₂ within the interval, according to The coordinates of the maneuvering target at each moment are calculated by the mathematical relationship, including:

Step 2.1: Calculate the displacement d ₂ of the target by using the measurement data of the accelerometer;

Step 2.2: Obtain the real-time quaternion q ₀ , q ₁ , q ₂ , q ₃ by solving the following quaternion differential equation:

According to the measurement data of the gyroscope, the following differential equations for quaternion update are listed;

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

Step 2.3: Put the obtained q ₀ , q ₁ , q ₂ , and q ₃ into the following formula to obtain the attitude matrix (the attitude matrix of the b-system to the n-system)

Step 2.4: Use the fourth-order Runge-Kutta method to update the quaternion, the calculation formula is as follows

h is the attitude update cycle;

Step 2.5: Update the attitude matrix according to the updated quaternion, and then solve the attitude angle according to the updated attitude matrix:

θ _main = arcsinC ₃₂

θ = θ _main

in order to accurately determine The true value of θ and γ, the definition domain of the attitude angle is defined below, where the definition domain of the pitch angle is (-90°, 90°), and the definition domain of the roll angle is (-180°, 180°) , the domain of heading angle is (0°,360°), the θ _main , γ _{main and} The pitch angle, roll angle and heading angle calculated by the attitude matrix respectively, θ, γ and Represents the angle value transformed into the domain; the attitude matrix is The functions of θ and γ, so the attitude angle can be obtained by solving the equation of the attitude matrix.

Step 2.6: Use the calculated displacement d ₂ and heading angle according to The coordinates of the maneuvering target at each moment are calculated by the mathematical relationship, Δx is the variation of the horizontal coordinate x of the target in the tracking area, and Δy is the variation of the longitudinal coordinate y of the target in the tracking area, The heading angle turned by the target.

Step 2.7: Store the calculated coordinates of the maneuvering target.

The multi-sensor fusion indoor fast tracking method based on the IMU and RFID measurement data of the above embodiments uses the Kalman filter to estimate the trajectory of the moving target based on the RFID data, and the estimation result will not cause error accumulation over time; When RFID measures data, the attitude calculation method of inertial sensor data is used to track the movement of the target. At this time, the tracking information is used as a supplement to the target movement information to reconstruct the trajectory of the target; it realizes the fast tracking of indoor targets and improves It improves the tracking accuracy, and is especially suitable for RFID indoor target tracking systems that need to obtain real-time trajectories of moving targets. The optimization of the tracking method further improves the tracking accuracy.

It should be noted that, in the case of no conflict, the above embodiments and the features in the embodiments can be combined with each other.

The above-mentioned embodiments only express several implementation modes of the present invention, and the descriptions thereof are relatively specific and detailed, but should not be construed as limiting the patent scope of the present invention. It should be noted that those skilled in the art can make several modifications and improvements without departing from the concept of the present invention, and these all belong to the protection 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 estimating the distance between the two estimated coordinatesDistance as the estimated distance d₁And estimating the course angle by selecting gyroscope data of the IMU based onThe 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 toThe 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:

<mrow> <mi>X</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>n</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mi>N</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> </mrow> </munderover> <msub> <mi>K</mi> <mi>n</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>{</mo> <msub> <mi>z</mi> <mi>n</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mover> <mi>z</mi> <mo>^</mo> </mover> <mi>n</mi> </msub> <mo>&amp;rsqb;</mo> <mo>}</mo> </mrow>

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 stateAnd 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:

<mrow> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>|</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>|</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <mi>X</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> </mrow>

wherein,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:

<mrow> <mi>P</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>|</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mi>P</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>|</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>-</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>n</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mi>N</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> </mrow> </munderover> <msub> <mi>K</mi> <mi>n</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <msub> <mi>S</mi> <mi>n</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <msubsup> <mi>K</mi> <mi>n</mi> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> </mrow>

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_iFilter of nth reader-writer at momentA filter gain; 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 stateThe 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 value alpha of system self-adaptive parameter_η＝α₀Andwhere η is x, y, x represents the lateral coordinate of the target in the tracking area, y represents 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 componentwhere η is x, y, x representing the lateral coordinate of the target in the tracking area, and y representing the longitudinal coordinate of the target in the tracking area;

step 1.1.4: setting a scale parameter k of the system to be-3, χ to be 0.01, and omicron 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, initial value of auto-correlation parameter of acceleration 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:

<mrow> <mi>x</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mi>A</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>x</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <mi>U</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mover> <mi>a</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>w</mi> <mi>d</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow>

wherein,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)

<mrow> <msub> <mi>h</mi> <mi>n</mi> </msub> <mo>&amp;lsqb;</mo> <mi>x</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <mo>=</mo> <msub> <mi>d</mi> <mi>n</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <msqrt> <mrow> <msup> <mrow> <mo>(</mo> <mi>x</mi> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> <mo>-</mo> <msub> <mi>x</mi> <mi>n</mi> </msub> <mo>(</mo> <mn>0</mn> <mo>)</mo> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <mi>y</mi> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> <mo>-</mo> <msub> <mi>y</mi> <mi>n</mi> </msub> <mo>(</mo> <mn>0</mn> <mo>)</mo> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> </mrow>

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.

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;

<mrow> <msup> <mi>x</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msup> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msubsup> <mi>x</mi> <mi>x</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> </mtd> <mtd> <msubsup> <mi>x</mi> <mi>w</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> </mtd> <mtd> <msubsup> <mi>x</mi> <mi>v</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>x</mi> <mi>x</mi> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>|</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <msub> <mi>N</mi> <mi>w</mi> </msub> </msub> </mtd> <mtd> <msub> <mn>0</mn> <msub> <mi>N</mi> <mi>v</mi> </msub> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> <mo>;</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>x</mi> <mi>x</mi> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> </msubsup> <mo>+</mo> <msub> <mrow> <mo>(</mo> <msqrt> <mrow> <mfrac> <msub> <mi>N</mi> <mi>x</mi> </msub> <mrow> <mn>1</mn> <mo>-</mo> <msubsup> <mi>W</mi> <mi>x</mi> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> </msubsup> </mrow> </mfrac> <mi>P</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>|</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> <mo>)</mo> </mrow> </mrow> </msqrt> <mo>)</mo> </mrow> <mi>i</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mn>......</mn> <mo>,</mo> <msub> <mi>N</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>x</mi> <mi>x</mi> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> </msubsup> <mo>-</mo> <msub> <mrow> <mo>(</mo> <msqrt> <mrow> <mfrac> <msub> <mi>N</mi> <mi>x</mi> </msub> <mrow> <mn>1</mn> <mo>-</mo> <msubsup> <mi>W</mi> <mi>x</mi> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> </msubsup> </mrow> </mfrac> <mi>P</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>|</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow> <mo>)</mo> </mrow> </mrow> </msqrt> <mo>)</mo> </mrow> <mi>i</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <msub> <mi>N</mi> <mi>x</mi> </msub> <mo>+</mo> <mn>1</mn> <mo>,</mo> <msub> <mi>N</mi> <mi>x</mi> </msub> <mo>+</mo> <mn>2</mn> <mo>,</mo> <mn>......</mn> <mo>,</mo> <mn>2</mn> <msub> <mi>N</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>W</mi> <mi>x</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mfrac> <mi>&amp;kappa;</mi> <mrow> <msub> <mi>N</mi> <mi>x</mi> </msub> <mo>+</mo> <mi>&amp;kappa;</mi> </mrow> </mfrac> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> <mo>;</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>N</mi> <mi>x</mi> </msub> <mo>+</mo> <mi>&amp;kappa;</mi> <mo>)</mo> </mrow> </mrow> </mfrac> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mn>......</mn> <mo>,</mo> <msub> <mi>N</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>N</mi> <mi>x</mi> </msub> <mo>+</mo> <mi>&amp;kappa;</mi> <mo>)</mo> </mrow> </mrow> </mfrac> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <msub> <mi>N</mi> <mi>x</mi> </msub> <mo>+</mo> <mn>1</mn> <mo>,</mo> <msub> <mi>N</mi> <mi>x</mi> </msub> <mo>+</mo> <mn>2</mn> <mo>,</mo> <mn>......</mn> <mo>,</mo> <mn>2</mn> <msub> <mi>N</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>W</mi> <mi>p</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mn>1</mn> <mo>-</mo> <msup> <mi>&amp;chi;</mi> <mn>2</mn> </msup> <mo>+</mo> <mi>o</mi> <mo>+</mo> <mfrac> <mi>&amp;kappa;</mi> <mrow> <msub> <mi>N</mi> <mi>x</mi> </msub> <mo>+</mo> <mi>&amp;kappa;</mi> </mrow> </mfrac> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> <mo>;</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>N</mi> <mi>x</mi> </msub> <mo>+</mo> <mi>&amp;kappa;</mi> <mo>)</mo> </mrow> </mrow> </mfrac> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mn>......</mn> <mo>,</mo> <msub> <mi>N</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>N</mi> <mi>x</mi> </msub> <mo>+</mo> <mi>&amp;kappa;</mi> <mo>)</mo> </mrow> </mrow> </mfrac> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <msub> <mi>N</mi> <mi>x</mi> </msub> <mo>+</mo> <mn>1</mn> <mo>,</mo> <msub> <mi>N</mi> <mi>x</mi> </msub> <mo>+</mo> <mn>2</mn> <mo>,</mo> <mn>......</mn> <mo>,</mo> <mn>2</mn> <msub> <mi>N</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> </mfenced>

wherein x is⁽ⁱ⁾Extended state value for the ith sigma point, includingi＝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_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;i＝0,1,…2N_xstate estimation for ith sigma pointA weight value;i＝0,1,…2N_xestimating a variance weight for the state of the ith sigma point;to representP(t_i-1|t_i-1) 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,

<mrow> <msup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msup> <mo>=</mo> <mi>A</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msubsup> <mi>x</mi> <mi>x</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>+</mo> <mi>U</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mover> <mi>a</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>x</mi> <mi>w</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>0</mn> <mo>,</mo> <mn>1</mn> <mo>,</mo> <mn>...2</mn> <msub> <mi>N</mi> <mi>x</mi> </msub> </mrow>

wherein,the state predicted value of the ith sigma point is obtained;i＝0,1,…2N_xthe state value of 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:

<mrow> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>|</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <msub> <mi>N</mi> <mi>x</mi> </msub> </mrow> </munderover> <msup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msup> <msubsup> <mi>W</mi> <mi>x</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> </mrow>

wherein,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;i＝0,1,…2N_xestimating 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

<mrow> <mi>P</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>|</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <msub> <mi>N</mi> <mi>x</mi> </msub> </mrow> </munderover> <msubsup> <mi>W</mi> <mi>p</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>{</mo> <msup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msup> <mo>-</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>|</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>}</mo> <msup> <mrow> <mo>{</mo> <msup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msup> <mo>-</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>|</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>}</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <mi>Q</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow>

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;i＝0,1,…2N_xestimating 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:

<mrow> <msubsup> <mover> <mi>z</mi> <mo>^</mo> </mover> <mi>n</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <msub> <mi>h</mi> <mi>n</mi> </msub> <mo>&amp;lsqb;</mo> <msup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msup> <mo>&amp;rsqb;</mo> <mo>+</mo> <msubsup> <mi>x</mi> <mi>v</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> </mrow>

wherein,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 pointSubstituting 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:

<mrow> <msub> <mover> <mi>z</mi> <mo>^</mo> </mover> <mi>n</mi> </msub> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <msub> <mi>N</mi> <mi>x</mi> </msub> </mrow> </munderover> <msubsup> <mover> <mi>z</mi> <mo>^</mo> </mover> <mi>n</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <msubsup> <mi>W</mi> <mi>x</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> </mrow>

wherein,measuring a predicted value for a target state;measuring and predicting value of ith sigma point;i＝0,1,…2N_xestimating 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:

<mrow> <msub> <mi>S</mi> <mi>n</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <msub> <mi>N</mi> <mi>x</mi> </msub> </mrow> </munderover> <msubsup> <mi>W</mi> <mi>p</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>{</mo> <msubsup> <mover> <mi>z</mi> <mo>^</mo> </mover> <mi>n</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>-</mo> <msub> <mover> <mi>z</mi> <mo>^</mo> </mover> <mi>n</mi> </msub> <mo>}</mo> <msup> <mrow> <mo>{</mo> <msubsup> <mover> <mi>z</mi> <mo>^</mo> </mover> <mi>n</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>-</mo> <msub> <mover> <mi>z</mi> <mo>^</mo> </mover> <mi>n</mi> </msub> <mo>}</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>R</mi> <mi>n</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> </mrow>

wherein,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:

<mrow> <msub> <mi>P</mi> <mrow> <mi>x</mi> <mi>z</mi> <mo>,</mo> <mi>n</mi> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <msub> <mi>N</mi> <mi>x</mi> </msub> </mrow> </munderover> <msubsup> <mi>W</mi> <mi>p</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>{</mo> <msup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msup> <mo>-</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>|</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>}</mo> <msup> <mrow> <mo>{</mo> <msubsup> <mover> <mi>z</mi> <mo>^</mo> </mover> <mi>n</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>-</mo> <msub> <mover> <mi>z</mi> <mo>^</mo> </mover> <mi>n</mi> </msub> <mo>}</mo> </mrow> <mi>T</mi> </msup> </mrow>

wherein, P_xz,n(t_i) Is a cross covariance matrix;i＝0,1,…2N_xestimating 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:

<mrow> <msub> <mi>K</mi> <mi>n</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>S</mi> <mi>n</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <msubsup> <mi>P</mi> <mrow> <mi>x</mi> <mi>z</mi> <mo>,</mo> <mi>n</mi> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> </mrow>

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:

getthe third row and the sixth row of the target state predicted value obtained in step 1.4 are horizontal and vertical axis acceleration predicted values, η ═ x, y respectively represent horizontal 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 useWhen the temperature of the water is higher than the set temperature,

when in useWhen the temperature of the water is higher than the set temperature,

if it is notAny small positive number can be taken;

with the motive frequency set to α₀The change is not changed; whereinη ═ x, y denote the lateral and longitudinal acceleration variance values of the tracking area, respectively, a_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;

<mrow> <msub> <mi>r</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>r</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>+</mo> <mfrac> <mn>1</mn> <mi>i</mi> </mfrac> <mo>&amp;lsqb;</mo> <msub> <mover> <mi>a</mi> <mo>^</mo> </mover> <mi>&amp;eta;</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <msub> <mover> <mi>a</mi> <mo>^</mo> </mover> <mi>&amp;eta;</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>r</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> </mrow>

<mrow> <msub> <mi>r</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>r</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>+</mo> <mfrac> <mn>1</mn> <mi>i</mi> </mfrac> <mo>&amp;lsqb;</mo> <msub> <mover> <mi>a</mi> <mo>^</mo> </mover> <mi>&amp;eta;</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <msub> <mover> <mi>a</mi> <mo>^</mo> </mover> <mi>&amp;eta;</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>r</mi> <mrow> <mi>i</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> </mrow>

<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;beta;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <mi>r</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>r</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> </mrow> </mfrac> </mrow> </mtd> <mtd> <mrow> <msub> <msubsup> <mi>&amp;delta;</mi> <mrow> <mi>a</mi> <mi>w</mi> </mrow> <mn>2</mn> </msubsup> <mrow> <mi>i</mi> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>r</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>&amp;beta;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <msub> <mi>r</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced>

<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <msubsup> <mi>&amp;delta;</mi> <mi>a</mi> <mn>2</mn> </msubsup> <mrow> <mi>i</mi> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <msubsup> <mi>&amp;delta;</mi> <mrow> <mi>a</mi> <mi>w</mi> </mrow> <mn>2</mn> </msubsup> <mrow> <mi>i</mi> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> </mrow> <mrow> <mn>1</mn> <mo>-</mo> <msubsup> <mi>&amp;beta;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> <mn>2</mn> </msubsup> </mrow> </mfrac> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;alpha;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <mi>ln&amp;beta;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>&amp;eta;</mi> </mrow> </msub> </mrow> <mrow> <mo>-</mo> <msub> <mi>th</mi> <mi>i</mi> </msub> </mrow> </mfrac> </mrow> </mtd> </mtr> </mtable> </mfenced>

wherein r is_i,η(1) Is t_iAcceleration previous step cross-correlation parameter of time, r_i,η(0) Is t_ithe moment-to-moment acceleration autocorrelation parameter, η ═ x, y, x represents a transverse coordinate in the tracking area, and y represents a longitudinal coordinate in 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,ηAndis 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 onThe 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;

<mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mi>q</mi> <mo>&amp;CircleTimes;</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> </mrow>

<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mover> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> <mtr> <mtd> <mover> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> <mtr> <mtd> <mover> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> <mtr> <mtd> <mover> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>x</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>y</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>z</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>x</mi> </mrow> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>z</mi> </mrow> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>y</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>y</mi> </mrow> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>z</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>x</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>z</mi> </mrow> </msub> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>y</mi> </mrow> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>x</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>

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

<mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>q</mi> <mn>0</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>q</mi> <mn>0</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>q</mi> <mn>0</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>C</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>13</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>C</mi> <mn>21</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>22</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>23</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>C</mi> <mn>31</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>32</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>33</mn> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>

Wherein,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;

<mfenced open = "" close = "}"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>K</mi> <mn>1</mn> </msub> <mo>=</mo> <msub> <mi>&amp;Gamma;</mi> <mi>b</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>q</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>K</mi> <mn>2</mn> </msub> <mo>=</mo> <msub> <mi>&amp;Gamma;</mi> <mi>b</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>+</mo> <mfrac> <mi>h</mi> <mn>2</mn> </mfrac> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>+</mo> <mfrac> <mi>h</mi> <mn>2</mn> </mfrac> <msub> <mi>K</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>K</mi> <mn>3</mn> </msub> <mo>=</mo> <msub> <mi>&amp;Gamma;</mi> <mi>b</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>+</mo> <mfrac> <mi>h</mi> <mn>2</mn> </mfrac> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>+</mo> <mfrac> <mi>h</mi> <mn>2</mn> </mfrac> <msub> <mi>K</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>K</mi> <mn>4</mn> </msub> <mo>=</mo> <msub> <mi>&amp;Gamma;</mi> <mi>b</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>+</mo> <msub> <mi>hK</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced>

<mrow> <mi>q</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>q</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mfrac> <mi>h</mi> <mn>6</mn> </mfrac> <mrow> <mo>(</mo> <msub> <mi>K</mi> <mn>1</mn> </msub> <mo>+</mo> <mn>2</mn> <msub> <mi>K</mi> <mn>2</mn> </msub> <mo>+</mo> <mn>3</mn> <msub> <mi>K</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>K</mi> <mn>4</mn> </msub> <mo>)</mo> </mrow> </mrow>

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}Andthe pitch angle, roll angle and course angle, theta, gamma andrepresenting the angle value converted into the defined domain;

step 1.6': according toThe 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.

10. The IMU and RF based according to any of claims 1-8The method for quickly tracking the indoor target fused with the ID information is characterized in that whether measurement data from the IMU exist or not is judged in the step 2, if the measurement data from the IMU exist, a course 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 adopting 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₃：

<mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mi>q</mi> <mo>&amp;CircleTimes;</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> </mrow>

<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mover> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> <mtr> <mtd> <mover> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> <mtr> <mtd> <mover> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> <mtr> <mtd> <mover> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>&amp;CenterDot;</mo> </mover> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>x</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>y</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>z</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>x</mi> </mrow> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>z</mi> </mrow> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>y</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>y</mi> </mrow> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>z</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>x</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>z</mi> </mrow> </msub> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>y</mi> </mrow> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>x</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>

Wherein,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

<mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>q</mi> <mn>0</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>q</mi> <mn>0</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>q</mi> <mn>0</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>C</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>13</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>C</mi> <mn>21</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>22</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>23</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>C</mi> <mn>31</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>32</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>33</mn> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>

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

<mfenced open = "" close = "}"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>K</mi> <mn>1</mn> </msub> <mo>=</mo> <msub> <mi>&amp;Gamma;</mi> <mi>b</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>q</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>K</mi> <mn>2</mn> </msub> <mo>=</mo> <msub> <mi>&amp;Gamma;</mi> <mi>b</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>+</mo> <mfrac> <mi>h</mi> <mn>2</mn> </mfrac> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>+</mo> <mfrac> <mi>h</mi> <mn>2</mn> </mfrac> <msub> <mi>K</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>K</mi> <mn>3</mn> </msub> <mo>=</mo> <msub> <mi>&amp;Gamma;</mi> <mi>b</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>+</mo> <mfrac> <mi>h</mi> <mn>2</mn> </mfrac> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>+</mo> <mfrac> <mi>h</mi> <mn>2</mn> </mfrac> <msub> <mi>K</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>K</mi> <mn>4</mn> </msub> <mo>=</mo> <msub> <mi>&amp;Gamma;</mi> <mi>b</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>q</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>+</mo> <msub> <mi>hK</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced>

<mrow> <mi>q</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>q</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mfrac> <mi>h</mi> <mn>6</mn> </mfrac> <mrow> <mo>(</mo> <msub> <mi>K</mi> <mn>1</mn> </msub> <mo>+</mo> <mn>2</mn> <msub> <mi>K</mi> <mn>2</mn> </msub> <mo>+</mo> <mn>3</mn> <msub> <mi>K</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>K</mi> <mn>4</mn> </msub> <mo>)</mo> </mrow> </mrow>

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 definition range of the pitch angle is (-90 degrees and 90 degrees), the definition range of the roll angle is (-180 degrees and 180 degrees), the definition range of the course angle is (0 degrees and 360 degrees), and the formula is shown in the specificationTheta of_{Master and slave}、γ_{Master and slave}Andthe pitch angle, roll angle and course angle, theta, gamma andrepresenting the angle value converted into the defined domain;

step 2.6: using the determined displacement d₂And course angleAccording toThe 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.