一种基于IMU和RFID信息融合的室内目标快速跟踪方法

摘要

本发明提供一种基于IMU和RFID信息融合的室内目标快速跟踪方法，根据RFID的采样周期与阈值的比较结果选择目标追踪的方法，若RFID的采样周期小于阈值，则通过不敏卡尔曼滤波器对RFID测量数据进行坐标的估计，获得机动目标的坐标；如果RFID的采样周期大于或者等于阈值，则将两次估计到的目标坐标间的距离作为估算距离，利用估算距离和航向角数据估计目标在各个采样间隔内的运动坐标；还判断RFID测量系统中是否还有未处理的原始测量数据，如果有，则再次进行上述的估计过程；如果没有，则按基于IMU数据的姿态解算方法对目标的运动进行补充计算。实现了对室内目标的快速跟踪，提高了跟踪精度，其估计结果不会随着时间的推移造成误差的累积。

说明书

技术领域

本发明涉及目标跟踪技术领域，尤其涉及一种基于IMU和RFID信息融合的室内目标快速跟踪方法。

背景技术

随着科技的发展，室内跟踪定位技术成为人们关注的热点问题。例如，在大型医院、机场、火车站等室内场所中如何感知自身所处的位置并到达自己的目的地、在大型仓库中如何跟踪移动货物的位置以实现货物移动的自动化、公共场所中服务型机器人在运动过程中对自身位置的感知等。

人们通常使用GPS定位等方式获取室外目标的坐标位置，然而这些技术对室内运动目标位置的确定时误差很大。常用的室内机动目标运动的跟踪技术包括Wi-Fi技术、射频识别定位技术(RFID，Radio Frequency Identification)、惯性导航技术、光跟踪定位技术、传感器网络定位技术、红外线定位技术等。但这些技术的发展还处于起步阶段，有些对室内目标的跟踪精度不高，难以达到实际应用的要求，有些精度稍高的成本却更高难以投入实用。

RFID即射频标签(Radio frequency identification)，是室内定位与跟踪系统的主要部件。RFID系统由射频标签和读取器组成，射频标签内存有该标签的标识信息。当带有该标签的目标移动至读取器附件时，读取器可获得该标签的标识信息，从而辨识出某个目标，同时获得该标签与读取器的距离信息，进而可对目标进行定位或跟踪。目前，关于RFID系统的目标定位专利主要有一种基于有源RFID的室内定位方法(CN201110101432.8)、基于无线射频识别技术的钢铁成品智能仓储系统(CN201210093992.8)、车站人员定位系统(CN201010256882.X)、一种用于RFID室内定位系统的定位方法(CN200910040571.7)、基于RFID技术的室内定位系统(CN200810027885.9)，这些专利都是获得目标的定位信息，其统一特征是假设目标为静止不动的，利用射频原理，得到带有标签的目标的大致位置。

对于运动的目标，上述方法具有以下缺陷：1.因为RFID只能获得与读取器的距离信息，因此要想得到准确位置数据，必须依靠多个读取器获得的某一个目标的距离信息，利用各读取器之间的几何关系才能得到目标的位置，这给系统计算增加了复杂度；2.上述方法都是假设目标是静止的，这不符合运动目标的实际情况，运动目标的位移、速度及加速度满足运动学的基本约束关系，假设目标是静止的就完全抛弃了该关系，造成约束关系的减少，直接导致运动目标轨迹估计不准确。3.当目标运动过程中，出现与RFID距离过远或者因为其他因素导致没有采集到运动目标相关数据时，会影响定位的准确性。

获得目标实时轨迹在很多跟踪系统中十分常用，例如室内场馆的行人跟踪、智能交通等领域，因此，上述发明并不适用于需要获得运动目标实时轨迹的RFID室内目标跟踪系统。

发明内容

基于此，本发明的目的在于提供一种基于IMU和RFID信息融合的室内目标快速跟踪方法，其实现了对室内目标的快速跟踪，提高了跟踪精度。

该方法基于不敏Kalman滤波器(Unsented Kalman Filters,简写为UKF)，利用RFID系统获得的目标到读取器的距离信息，根据室内运动目标的动力学特性，获得运动轨迹的估计。在没有RFID测量数据的地方，则利用IMU获得的姿态解算方法进行信息补充，实现室内目标的快速跟踪。

实现本发明的目的的基本思路是：首先判断来自RFID阅读器的两次测量数据之间的间隔是否小于阈值(取阈值为0.18秒)，如果间隔小于阈值，则采取基于RFID测量数据的非线性卡尔曼滤波方法进行坐标的估计，并将估计到的机动目标的坐标存储下来；如果间隔大于阈值，则将两次估计到的目标坐标间的距离作为姿态解算方法中的估算距离，利用IMU测量数据估算目标在这段时间内的姿态角变化情况并考虑到陀螺仪测量数据的漂移特性，将姿态角数据做一个6秒的延迟，然后利用距离和航向角的数据估计目标在这段时间内的运动坐标，并将得到的位置坐标进行存储。数据被存储后需要判断是否还有来自RFID阅读器的测量数据，如果有，则再次进行上述的估计过程；如果没有，还要判断是否有来自IMU的测量数据，如果有测量数据，则应该按基于IMU数据的姿态解算方法对目标的运动进行补充计算，此时的距离计算应采用加速度计的测量数据，如果也没有来自IMU的测量数据，则算法结束。该算法的优势在于：基于RFID数据利用Kalman滤波器对运动目标进行轨迹估计，估计结果不会随着时间的推移造成误差的累积。在没有RFID测量数据时，采用惯性传感器数据的姿态解算方法对目标的运动进行跟踪，此时的跟踪信息作为目标运动信息的补充。

为实现上述目的，本发明的技术方案如下：

一种基于IMU和RFID信息融合的室内目标快速跟踪方法，包括以下步骤：

步骤1：根据RFID的采样周期与阈值的比较结果选择目标追踪的方法，若RFID的采样周期小于阈值，则通过不敏卡尔曼滤波器对RFID测量数据进行坐标的估计，获得机动目标的坐标；如果RFID的采样周期大于或者等于阈值，则通过不敏卡尔曼滤波器对RFID测量数据进行坐标的估计，以两次估计到的坐标间的距离作为估算距离d₁，并选择用IMU的陀螺仪数据估算航向角，根据的数学关系式推算出机动目标在各个时刻的坐标；

步骤2：判断RFID测量系统中是否还有未处理的原始测量数据，如果有，则返回步骤1；如果没有，则判断是否还有来自IMU的测量数据，若是，则用IMU的陀螺仪数据估算航向角，并采用IMU的加速度计数据由牛顿运动定律计算出机动目标在各个采样间隔内的位移d₂，根据的数学关系式推算出机动目标在各个时刻的坐标；若否，则算法结束；

其中，Δx为目标在跟踪区域内的横向坐标x的变化量，Δy为目标在跟踪区域内的纵向坐标y的变化量，为目标转过的航向角。

在上述方案的基础上，本发明还可以做如下改进。

进一步地，步骤1中若RFID的采样周期小于阈值，则通过不敏卡尔曼滤波器对RFID测量数据进行坐标的估计，获得机动目标的坐标，包括：

步骤1.1：目标运动状态和系统自适应参数初始化；

步骤1.2：建立具有系统自适应参数的运动模型及RFID测量模型；

步骤1.3：根据建立的具有系统自适应参数的运动模型对目标状态进行预测，得到下一时间点的目标状态预测值；

步骤1.4：根据RFID测量模型和目标状态预测值计算目标状态测量预测值；

步骤1.5：根据目标状态预测值和目标状态测量预测值计算不敏卡尔曼滤波器增益；

步骤1.6：读取RFID监控系统获取的下一时间点目标状态原始测量值z_n(t_i)；

步骤1.7：根据目标状态原始测量值、目标状态测量预测值和滤波器增益计算目标状态修正值：

其中，X(t_i)为目标状态修正值，t_i为采样时刻，n＝1,2,…N(t_i)，N(t_i)为t_i时刻的获得与目标距离数据的RFID读取器的个数；K_n(t_i)为t_i时刻滤波器增益，z_n(t_i)为第n个读取器在t_i时刻的测量数据，为t_i时刻的测量的估计值；

步骤1.8：根据目标状态预测值和目标状态修正值X(t_i)计算目标状态估计值及目标状态协方差估计值；

目标状态估计值的计算公式如下：

其中，表示t_i时刻的目标状态估计值；表示在t_i-1时刻时预测t_i时刻的目标状态预测值；X(t_i)为目标状态修正值，t_i为采样时刻；

目标状态协方差估计值的计算公式如下：

其中，P(t_i|t_i)表示t_i时刻的目标状态协方差估计值，P(t_i|t_i-1)表示在t_i-1时刻预测t_i时刻的目标状态协方差预测值，K_n(t_i)为t_i时刻第n个读写器的滤波器增益；S_n(t_i)为第n个读写器的协方差矩阵；

步骤1.9：利用目标状态预测值更新系统自适应参数，进而更新步骤1.3中的运动模型；

步骤1.10：将估计到的机动目标的坐标存储下来。

进一步地，步骤1.1中目标运动状态和系统自适应参数初始化的具体过程如下：

步骤1.1.1：设置目标运动状态的初始值为6维全0列向量，维数为系统模型中状态向量的维数；

步骤1.1.2：设置系统自适应参数的初始值α_η＝α₀和其中η＝x,y，x表示目标在跟踪区域的横向坐标，y表示目标在跟踪区域内的纵向坐标，α_η表示横向坐标、纵向坐标的加速度机动频率，表示横向坐标、纵向坐标的加速度方差；

步骤1.1.3：设置系统加速度分量的初始值其中η＝x,y，x表示目标在跟踪区域的横向坐标，y表示目标在跟踪区域内的纵向坐标；

步骤1.1.4：设置系统的尺度参数κ＝-3，χ＝0.01，ο＝2；

步骤1.1.5：设置加速度前一步互相关参数初始值为r_0,η(1)＝0，向加速度自相关参数初值为r_0,η(0)＝0。

进一步地，步骤1.2中建立具有系统自适应参数的运动模型及RFID测量模型，包括：

步骤1.2.1：利用下式描述目标的运动特征：

其中，为6维状态列向量，分别是横向坐标位移、速度和加速度，分别是纵向坐标位移、速度和加速度；x(t_i)为t_i时刻目标的状态向量，t_i为采样时刻；A(t_i-1)为t_i-1时刻的状态转移矩阵；U(t_i-1)为t_i-1时刻的控制矩阵；w_d(t_i-1)为过程噪声，其均值为0，方差为Q；A(t_i-1)、U(t_i-1)和Q中包含系统自适应参数；

步骤1.2.2：建立如下RFID系统目标测量方程：

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

其中，z_n(t_i)为RFID监控系统获取的目标在t_i时刻的测量值，h_n[x(t_i)]为测量方程，x(t_i)为t_i时刻目标的状态向量；v_n(t_i)为RFID监控系统中第n个读取器的高斯测量白噪声，且与过程噪声w_d(t_i-1)相互独立，d_n(t_i)为目标在t_i时刻所处的位置到第n个RFID读取器的真实距离(未知)，x(t_i)、y(t_i)分别为t_i时刻目标位置的横、纵坐标；x_n(0)、y_n(0)分别为第n个RFID读取器的横、纵坐标。

进一步地，步骤1.3中根据建立的具有系统自适应参数的运动模型对目标状态进行预测，包括：

步骤1.3.1：按照下式计算不敏卡尔曼滤波器所需的sigma点状态值及其权值；

其中，x⁽ⁱ⁾为第i个sigma点的扩展状态值，其中包括i＝0,1,…2N_x，为第i个sigma点的状态值,N_x是系统状态的维数；为第i个sigma点的过程噪声模拟量，为第i个sigma点的测量噪声模拟量；表示在t_i-1时刻的目标状态估计值,P(t_i-1|t_i-1)表示在t_i-1时刻的目标状态协方差估计值,是与过程噪声w(t_i-1)维数相同的全0矩阵,是与测量噪声v(t_i-1)维数相同的全0矩阵；i＝0,1,…2N_x，为第i个sigma点的状态估计权值；i＝0,1,…2N_x，为第i个sigma点的状态估计方差权值；表示均方差矩阵的第i列，κ、χ、ο为尺度参数；

步骤1.3.2：根据运动模型和目标状态初始值计算每一个sigma点的状态预测值，计算公式如下，

其中，为第i个sigma点的状态预测值；i＝0,1,…2N_x，为第i个sigma点的状态值,为第i个sigma点的过程噪声模拟量，来重构过程噪声对状态预测的影响；A(t_i-1)为t_i-1时刻的状态转移矩阵；U(t_i-1)为t_i-1时刻的控制矩阵；为从0时刻至t_i-1时刻的加速度均值；

步骤1.3.3：计算所有sigma点状态预测值的加权值，即目标状态预测值，计算公式如下：

其中，表示在t_i-1时刻预测t_i时刻的目标状态预测值，t_i为采样时刻，|表示条件操作符；为第i个sigma点的状态预测值；i＝0,1,…2N_x，为第i个sigma点的状态估计权值；

步骤1.3.4：计算目标状态协方差预测值，计算公式如下

其中，P(t_i|t_i-1)表示在t_i-1时刻预测t_i时刻的目标状态协方差预测值；为第i个sigma点的状态预测值；表示在t_i-1时刻预测t_i时刻的目标状态预测值；i＝0,1,…2N_x，为第i个sigma点的状态估计方差权值；Q(t_i-1)为过程噪声协方差。

进一步地，步骤1.4中根据RFID测量模型和目标状态预测值计算目标状态测量预测值，包括：

步骤1.4.1：根据每个sigma点的状态预测值计算每个sigma点的测量预测值，计算公式如下：

其中，为第i个sigma点的测量预测值；为第i个sigma点的状态预测值；为将每个sigma点的状态预测值代入测量方程得到的结果；为第i个sigma点的测量噪声模拟量，来重构测量噪声对测量预测的影响；

步骤1.4.2：计算所有sigma点的测量预测值的加权值，即为目标状态测量预测值，计算公式如下：

其中，为目标状态测量预测值；为第i个sigma点的测量预测值；i＝0,1,…2N_x，为第i个sigma点的状态估计权值；n＝1,2,…N(t_i)，N(t_i)为t_i时刻能够获得目标测量值的RFID读取器的个数。

进一步地，步骤1.5中根据目标状态预测值和目标状态测量预测值计算不敏卡尔曼滤波器增益，包括：

步骤1.5.1：计算目标状态测量预测值的协方差，计算公式如下：

其中，为第i个sigma点的测量预测值；为目标状态测量预测值；i＝0,1,…2N_x，为第i个sigma点的状态估计方差权值；R_n(t_i)为第n个读取器的测量方差；

步骤1.5.2：计算目标状态预测值与目标状态测量预测值的交叉协方差，计算公式如下：

其中，P_xz,n(t_i)为交叉协方差矩阵；i＝0,1,…2N_x，为第i个sigma点的状态估计方差权值；为第i个sigma点的状态预测值；表示在t_i-1时刻预测t_i时刻的目标状态预测值；为第i个sigma点的测量预测值；为目标测量预测值；

步骤1.5.3：根据步骤1.5.1中的协方差和步骤1.5.2中的交叉协方差计算滤波器增益，计算公式如下：

其中，K_n(t_i)为t_i时刻第n个RFID读取器的滤波器增益，t_i为采样时刻；P_zz,n(t_i)为交叉协方差矩阵，表示交叉协方差矩阵的逆；S_n(t_i)为协方差矩阵，n＝1,2,…N(t_i)，N(t_i)为t_i时刻能够获得目标测量值的RFID读取器的个数。

进一步地，步骤1.9的计算如下：

取为步骤1.4得到的目标状态预测值的第三行及第六行，即横、纵轴加速度预测值，η＝x,y分别表示跟踪区域的横、纵向坐标；

1)当i≤4时，按照如下方式更新系统自适应参数：

当时，

当时，

如果可取任意小的正数；

机动频率设置为α₀不变；其中η＝x,y分别表示跟踪区域的横、纵向加速度方差值，a_M,η为一正数，范围在1到100之间，a_-M,η为与a_M,η绝对值相同的负数；

2)当i＞4时，按照如下方式更新系统自适应参数；

其中，r_i,η(1)为t_i时刻的加速度前一步互相关参数，r_i,η(0)为t_i时刻向加速度自相关参数，η＝x,y，x表示跟踪区域内的横向坐标，y表示跟踪区域内的纵向坐标；横、纵轴加速度预测值，r_i-1,η(1)为t_i-1时刻的加速度前一步互相关参数，r_i-1,η(0)为t_i-1时刻向加速度自相关参数，β_i,η和为t_i时刻的计算中间变量，为t_i时刻的加速度方差，α_i,η为t_i时刻的机动频率，th_i＝t_i-t_i-1为两次测量数据的时间间隔。

进一步地，步骤1中如果RFID的采样周期大于或者等于阈值，则通过不敏卡尔曼滤波器对RFID测量数据进行坐标的估计，以两次估计到的坐标间的距离作为估算距离d₁，并选择用IMU的陀螺仪数据估算航向角，根据的数学关系式推算出机动目标在各个时刻的坐标；具体实现步骤为：

步骤1.1′：通过不敏卡尔曼滤波器对RFID测量数据进行坐标的估计，以两次估计到的坐标间的距离作为估算距离d₁；

步骤1.2′：利用IMU测量数据估算目标在各个采样间隔内的姿态角变化情况并考虑到陀螺仪测量数据的漂移特性，将姿态角数据做一个6秒的延迟；

步骤1.3′：获得各个采样间隔内IMU中陀螺仪的角速度信息，根据陀螺仪的角速度信息列出关于四元数微分方程；

通过求解以上四元数微分方程得到实时的四元数q₀，q₁，q₂，q₃，将获得的q₀，q₁，q₂，q₃带入下式得到姿态矩阵

其中，表示四元数乘积，表示b系相对n系的角速度，ω_nbx、ω_nby、ω_nbz为陀螺仪的输出值；

步骤1.4′：利用4阶龙格库塔法对四元数进行更新；

h为姿态更新周期；

步骤1.5′：根据更新后的四元数更新姿态矩阵，再根据更新后的姿态矩阵求解得到姿态角为：

θ_主＝arcsinC₃₂

θ＝θ_主

其中俯仰角的定义域为(-90°,90°)，橫滚角的定义域为(-180°,180°)，航向角的定义域为(0°,360°)，公式中的θ_主、γ_主和分别是由姿态矩阵计算得到的俯仰角、横滚角和航向角，θ、γ和表示转换到定义域内的角度值；

步骤1.6′：根据的数学关系式推算出机动目标在各个时刻的坐标；

步骤1.7′：将计算得到的机动目标的坐标存储。

进一步地，步骤2中判断是否还有来自IMU的测量数据，若是，则用IMU的陀螺仪数据估算航向角，并采用IMU的加速度计数据由牛顿运动定律计算机动目标在各个采样间隔内的位移d₂，根据的数学关系式推算出机动目标在各个时刻的坐标；具体实现步骤为：

步骤2.1：采用加速度计的测量数据计算目标的位移d₂；

步骤2.2：通过求解如下四元数微分方程得到实时的四元数q₀，q₁，q₂，q₃：

其中，表示四元数乘积，表示b系相对n系的角速度，ω_nbx、ω_nby、ω_nbz为陀螺仪的输出值；

步骤2.3：将获得的q₀，q₁，q₂，q₃带入下式得到姿态矩阵

步骤2.4：采用四阶龙格库塔法对四元数进行更新，计算公式如下：

h为姿态更新周期；

步骤2.5：根据更新后的四元数更新姿态矩阵，再根据更新后的姿态矩阵求解得到姿态角为：

θ_主＝arcsinC₃₂

θ＝θ_主

其中俯仰角的定义域为(-90°,90°)，橫滚角的定义域为(-180°,180°)，航向角的定义域为(0°,360°)，公式中的θ_主、γ_主和分别是由姿态矩阵计算得到的俯仰角、横滚角和航向角，θ、γ和表示转换到定义域内的角度值；

步骤2.6：利用求得的位移d₂以及航向角根据的数学关系式推算出机动目标在各个时刻的坐标；

步骤2.7：将计算得到的机动目标的坐标存储。

本发明的有益效果是：

本发明的基于IMU和RFID测量数据的多传感器融合室内快速跟踪方法，基于RFID数据利用Kalman滤波器对运动目标进行轨迹估计，估计结果不会随着时间的推移造成误差的累积；在没有RFID测量数据时，采用惯性传感器数据的姿态解算方法对目标的运动进行跟踪，此时的跟踪信息作为目标运动信息的补充，重构目标的运动轨迹；实现了对室内目标的快速跟踪，提高了跟踪精度，尤其适用于需要获得运动目标实时轨迹的RFID室内目标跟踪系统。对跟踪方法的优化，进一步提高了跟踪精度。

附图说明

图1为本发明一实施例的基于IMU和RFID测量数据的多传感器融合室内快速跟踪方法的流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例对本发明的基于IMU和RFID测量数据的多传感器融合室内快速跟踪方法进行进一步详细说明。应当理解，此处所描述的具体实施例仅用于解释本发明，并不用于限定本发明。

参照图1，本发明一实施例的基于IMU和RFID测量数据的多传感器融合室内快速跟踪方法，包括以下步骤：

步骤1：根据RFID的采样周期与阈值的比较结果选择目标追踪的方法，若RFID的采样周期小于阈值，则通过不敏卡尔曼滤波器对RFID测量数据进行坐标的估计，获得机动目标的坐标(载体的位置信息)；如果RFID的采样周期大于或者等于阈值，则通过不敏卡尔曼滤波器对RFID测量数据进行坐标的估计，以两次估计到的坐标间的距离作为估算距离d₁，并选择用IMU的陀螺仪数据估算航向角，根据的数学关系式推算出机动目标(载体)在各个时刻的坐标；

步骤2：判断RFID测量系统中是否还有未处理的原始测量数据，如果有，则返回步骤1；如果没有，则判断是否还有来自IMU的测量数据，若是，则用IMU的陀螺仪数据估算航向角，并采用IMU的加速度计数据由牛顿运动定律计算出机动目标在各个采样间隔内的位移d₂，根据的数学关系式推算出机动目标在各个时刻的坐标；若否，则算法结束；

其中，Δx表示目标在跟踪区域内的横向坐标x的变化量Δy表示目标在跟踪区域内的纵向坐标y的变化量，为目标转过的航向角。本实施例中，阈值可取为0.18秒。

作为一种可优选方式，步骤1中若RFID的采样周期小于阈值，则通过不敏卡尔曼滤波器对RFID测量数据进行坐标的估计，获得机动目标的坐标，包括：

步骤1.1：目标运动状态和系统自适应参数初始化；

步骤1.2：建立具有系统自适应参数的运动模型及RFID测量模型；

步骤1.3：根据建立的具有系统自适应参数的运动模型对目标状态进行预测，得到下一时间点的目标状态预测值；

步骤1.4：根据RFID测量模型和目标状态预测值计算目标状态测量预测值；

步骤1.5：根据目标状态预测值和目标状态测量预测值计算不敏卡尔曼滤波器增益；

步骤1.6：读取RFID监控系统获取的下一时间点目标状态原始测量值zn(ti)；

步骤1.7：根据目标状态原始测量值、目标状态测量预测值和滤波器增益计算目标状态修正值：

其中，X(t_i)为目标状态修正值，t_i为采样时刻，n＝1,2,…N(t_i)，N(t_i)为t_i时刻的获得与目标距离数据的RFID读取器的个数；K_n(t_i)为t_i时刻滤波器增益，z_n(t_i)为第n个读取器在t_i时刻的测量数据，为t_i时刻的测量的估计值；

步骤1.8：根据目标状态预测值和目标状态修正值X(t_i)计算目标状态估计值及目标状态协方差估计值；

目标状态估计值的计算公式如下：

其中，表示t_i时刻的目标状态估计值；表示在t_i-1时刻时预测t_i时刻的目标状态预测值；X(t_i)为目标状态修正值，t_i为采样时刻；

目标状态协方差估计值的计算公式如下：

其中，P(t_i|t_i)表示t_i时刻的目标状态协方差估计值，P(t_i|t_i-1)表示在t_i-1时刻预测t_i时刻的目标状态协方差预测值，K_n(t_i)为t_i时刻第n个读写器的滤波器增益；S_n(t_i)为第n个读写器的协方差矩阵；

步骤1.9：利用目标状态预测值更新系统自适应参数，进而更新步骤2.3中的运动模型；

步骤1.10：将估计到的机动目标的坐标存储下来。

进一步地，步骤1.1中目标运动状态和系统自适应参数初始化的具体过程如下：

步骤1.1.1：设置目标运动状态的初始值为6维全0列向量，维数为系统模型中状态向量的维数；

步骤1.1.2：设置系统自适应参数的初始值α_η＝α₀和其中η＝x,y，x表示目标在跟踪区域的横向坐标，y表示目标在跟踪区域内的纵向坐标，α_η表示横向坐标、纵向坐标的加速度机动频率，表示横向坐标、纵向坐标的加速度方差，本实施例中α₀＝1/20、

步骤1.1.3：设置系统加速度分量的初始值本实施例中取0，其中η＝x,y，x表示目标在跟踪区域的横向坐标，y表示目标在跟踪区域内的纵向坐标；

步骤1.1.4：设置系统的尺度参数κ＝-3，χ＝0.01，ο＝2；

步骤1.1.5：设置加速度前一步互相关参数初始值为r_0,η(1)＝0，向加速度自相关参数初值为r_0,η(0)＝0。

进一步地，步骤1.2中建立具有系统自适应参数的运动模型及RFID测量模型，包括：

步骤1.2.1：利用下式描述目标的运动特征(运动模型公式)：

其中，为6维状态列向量，分别是横向坐标位移、速度和加速度，分别是纵向坐标位移、速度和加速度；x(t_i)为t_i时刻目标的状态向量，t_i为采样时刻；A(t_i-1)为t_i-1时刻的状态转移矩阵；U(t_i-1)为t_i-1时刻的控制矩阵；w_d(t_i-1)为过程噪声，其均值为0，方差为Q；A(t_i-1)、U(t_i-1)和Q中包含系统自适应参数；

为状态转移矩阵；为控制矩阵；

为0时刻开始至t_i-1时刻目标的加速度均值；w(t_i-1)＝[w_x(t_i-1)>y(t_i-1)]^T为过程噪声，其均值为0，方差为假设横向和纵向过程噪声相互独立；设η＝x,y，x表示目标在跟踪区域的横向坐标，y表示目标在跟踪区域内的纵向坐标，其中相应矩阵的取值为：

其中th_i＝t_i-t_i-1为两次测量数据的时间间隔；

步骤1.2.2：建立如下RFID系统目标测量方程：

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

其中，z_n(t_i)为RFID监控系统获取的目标在t_i时刻的测量值，h_n[x(t_i)]为测量方程，x(t_i)为t_i时刻目标的状态向量；v_n(t_i)为RFID监控系统中第n个读取器的高斯测量白噪声，且与过程噪声w_d(t_i-1)相互独立，其测量方差满足σ_p和γ为射频测量的参数,实验表明σ_p取为4dB，γ可取为1.6到6.5区间的任意正数；d_n(t_i)为目标在t_i时刻所处的位置到第n个RFID读取器的真实距离(未知)，x(t_i)、y(t_i)分别为t_i时刻目标位置的横、纵坐标；x_n(0)、y_n(0)分别为第n个RFID读取器的横、纵坐标，n＝1,2,…N(t_i)，N(t_i)为t_i时刻能够获得目标测量值的RFID读取器的个数。

进一步地，步骤1.3中根据建立的具有系统自适应参数的运动模型对目标状态进行预测，包括：

步骤1.3.1：按照下式计算不敏卡尔曼滤波器所需的sigma点状态值及其权值；

其中，x⁽ⁱ⁾为第i个sigma点的扩展状态值，其中包括i＝0,1,…2N_x，为第i个sigma点的状态值,N_x是系统状态的维数；为第i个sigma点的过程噪声模拟量，为第i个sigma点的测量噪声模拟量；表示在t_i-1时刻的目标状态估计值,P(t_i-1|t_i-1)表示在t_i-1时刻的目标状态协方差估计值,是与过程噪声w(t_i-1)维数相同的全0矩阵,是与测量噪声v(t_i-1)维数相同的全0矩阵；i＝0,1,…2N_x，为第i个sigma点的状态估计权值；i＝0,1,…2N_x，为第i个sigma点的状态估计方差权值，N_x是系统状态的维数；表示均方差矩阵的第i列，κ、χ、ο为尺度参数；

步骤1.3.2：根据运动模型和目标状态初始值计算每一个sigma点的状态预测值，计算公式如下，

其中，为第i个sigma点的状态预测值；i＝0,1,…2N_x，为第i个sigma点的状态值,为第i个sigma点的过程噪声模拟量，来重构过程噪声对状态预测的影响，N_x是系统状态的维数；A(t_i-1)为t_i-1时刻的状态转移矩阵；U(t_i-1)为t_i-₁时刻的控制矩阵；为从0时刻至t_i-1时刻的加速度均值；

步骤1.3.3：计算所有sigma点状态预测值的加权值，即目标状态预测值，计算公式如下：

其中，表示在t_i-1时刻预测t_i时刻的目标状态预测值，t_i为采样时刻，|表示条件操作符；为第i个sigma点的状态预测值；i＝0,1,…2N_x，为第i个sigma点的状态估计权值，N_x是系统状态的维数；

步骤1.3.4：计算目标状态协方差预测值，计算公式如下

其中，P(t_i|t_i-1)表示在t_i-1时刻预测t_i时刻的目标状态协方差预测值；为第i个sigma点的状态预测值；表示在t_i-1时刻预测t_i时刻的目标状态预测值；i＝0,1,…2N_x，为第i个sigma点的状态估计方差权值；N_x是系统状态的维数；Q(t_i-1)为过程噪声协方差。

进一步地，步骤1.4中根据RFID测量模型和目标状态预测值计算目标状态测量预测值，包括：

步骤1.4.1：根据每个sigma点的状态预测值计算每个sigma点的测量预测值，计算公式如下：

其中，为第i个sigma点的测量预测值；为第i个sigma点的状态预测值；为将每个sigma点的状态预测值代入测量方程得到的结果；n＝1,2,…N(t_i)，N(t_i)为t_i时刻能够获得目标测量值的RFID读取器的个数；为第i个sigma点的测量噪声模拟量，来重构测量噪声对测量预测的影响；

步骤1.4.2：计算所有sigma点的测量预测值的加权值，即为目标状态测量预测值，计算公式如下：

其中，为目标状态测量预测值；为第i个sigma点的测量预测值；i＝0,1,…2N_x，为第i个sigma点的状态估计权值，N_x是系统状态的维数；n＝1,2,…N(t_i)，N(t_i)为t_i时刻能够获得目标测量值的RFID读取器的个数。

进一步地，步骤1.5中根据目标状态预测值和目标状态测量预测值计算不敏卡尔曼滤波器增益，包括：

步骤1.5.1：计算目标状态测量预测值的协方差，计算公式如下：

其中，为第i个sigma点的测量预测值；为目标状态测量预测值；i＝0,1,…2N_x，为第i个sigma点的状态估计方差权值；N_x是系统状态的维数；R_n(t_i)为第n个读取器的测量方差；n＝1,2,…N(t_i)，N(t_i)为t_i时刻能够获得目标测量值的RFID读取器的个数；

步骤1.5.2：计算目标状态预测值与目标状态测量预测值的交叉协方差，计算公式如下：

其中，P_xz,n(t_i)为交叉协方差矩阵；i＝0,1,…2N_x，为第i个sigma点的状态估计方差权值；为第i个sigma点的状态预测值；表示在t_i-1时刻预测t_i时刻的目标状态预测值；为第i个sigma点的测量预测值；为目标测量预测值；

步骤1.5.3：根据步骤2.5.1中的协方差和步骤2.5.2中的交叉协方差计算滤波器增益，计算公式如下：

其中，K_n(t_i)为t_i时刻第n个RFID读取器的滤波器增益，t_i为采样时刻；P_zz,n(t_i)为交叉协方差矩阵，表示交叉协方差矩阵的逆；S_n(t_i)为协方差矩阵，n＝1,2,…N(t_i)，N(t_i)为t_i时刻能够获得目标测量值的RFID读取器的个数。

进一步地，步骤1.9的计算如下：

取为步骤1.4得到的目标状态预测值的第三行及第六行，即横、纵轴加速度预测值，η＝x,y分别表示跟踪区域的横、纵向坐标；

1)当i≤4时，按照如下方式更新系统自适应参数：

当时,

当时,

如果可取任意小的正数；

机动频率设置为α₀不变；其中η＝x,y分别表示跟踪区域的横、纵向加速度方差值，a_M,η为一正数，范围在1到100之间，a_-M,η为与a_M,η绝对值相同的负数；

2)当i＞4时，按照如下方式更新系统自适应参数；

其中，r_i,η(1)为t_i时刻的加速度前一步互相关参数，r_i,η(0)为t_i时刻向加速度自相关参数，η＝x,y，x表示跟踪区域内的横向坐标，y表示跟踪区域内的纵向坐标；横、纵轴加速度预测值，r_i-1,η(1)为t_i-1时刻的加速度前一步互相关参数，r_i-1,η(0)为t_i-1时刻向加速度自相关参数，β_i,η和为t_i时刻的计算中间变量，为t_i时刻的加速度方差，α_i,η为t_i时刻的机动频率，th_i＝t_i-t_i-1为两次测量数据的时间间隔。

作为一种可优选方式，步骤1中如果RFID的采样周期大于或者等于阈值，则通过不敏卡尔曼滤波器对RFID测量数据进行坐标的估计，以两次估计到的坐标间的距离作为估算距离d₁，并选择用IMU的陀螺仪数据估算航向角，根据的数学关系式推算出机动目标在各个时刻的坐标；具体实现步骤为：

步骤1.1′：通过不敏卡尔曼滤波器对RFID测量数据进行坐标的估计，以两次估计到的坐标间的距离作为估算距离d₁；

步骤1.2′：利用IMU测量数据估算目标在各个采样间隔内的姿态角变化情况并考虑到陀螺仪测量数据的漂移特性，将姿态角数据做一个6秒的延迟；

步骤1.3′：获得各个采样间隔内IMU中陀螺仪的角速度信息，根据陀螺仪的角速度信息列出关于四元数微分方程；

四元数微分方程进一步写成矩阵形式为：

通过求解以上四元数微分方程得到实时的四元数q₀，q₁，q₂，q₃，将获得的q₀，q₁，q₂，q₃带入下式得到(b系到n系的姿态矩阵)姿态矩阵

其中，表示四元数乘积，表示b系相对n系的角速度，ω_nbx、ω_nby、ω_nbz为陀螺仪的输出值；

步骤1.4′：利用4阶龙格库塔法对四元数进行更新；

h为姿态更新周期；

步骤1.5′：根据更新后的四元数更新姿态矩阵，再根据更新后的姿态矩阵求解得到姿态角为：

θ_主＝arcsinC₃₂

θ＝θ_主

其中俯仰角的定义域为(-90°,90°)，橫滚角的定义域为(-180°,180°)，航向角的定义域为(0°,360°)。公式中的θ_主、γ_主和分别是由姿态矩阵计算得到的俯仰角、横滚角和航向角，θ、γ和表示转换到定义域内的角度值。四元数更新之后，便可得到更新后的姿态矩阵，而姿态角可由姿态矩阵求得，从而得到更新后的姿态角。

步骤1.6′：根据的数学关系式推算出机动目标在各个时刻的坐标；其中d₁表示步骤2.1中两次估计到的坐标间的距离，Δx为目标在跟踪区域内的横向坐标x的变化量，Δy为目标在跟踪区域内的纵向坐标y的变化量，为目标转过的航向角。

步骤1.7′：将计算得到的机动目标的坐标存储。

作为一种可优选方式，步骤2中判断是否还有来自IMU的测量数据，若是，则用IMU的陀螺仪数据估算航向角，并采用IMU的加速度计数据由牛顿运动定律计算机动目标在各个采样间隔内的位移d₂，根据的数学关系式推算出机动目标在各个时刻的坐标，包括：

步骤2.1：采用加速度计的测量数据计算目标的位移d₂；

步骤2.2：通过求解如下四元数微分方程得到实时的四元数q₀，q₁，q₂，q₃：

根据陀螺仪的测量数据列出如下关于四元数更新的微分方程；

由于状态估计量q为四元数，四元数微分方程进一步写成矩阵形式为：

步骤2.3：将获得的q₀，q₁，q₂，q₃带入下式得到(b系到n系的姿态矩阵)姿态矩阵

步骤2.4：采用四阶龙格库塔法对四元数进行更新，计算公式如下

h为姿态更新周期；

步骤2.5：根据更新后的四元数更新姿态矩阵，再根据更新后的姿态矩阵求解得到姿态角为：

θ_主＝arcsinC₃₂

θ＝θ_主

为了准确的确定θ、γ的真值，下面对姿态角的定义域做了定义，其中俯仰角的定义域为(-90°,90°)，橫滚角的定义域为(-180°,180°)，航向角的定义域为(0°,360°)，公式中的θ_主、γ_主和分别是由姿态矩阵计算得到的俯仰角、横滚角和航向角，θ、γ和表示转换到定义域内的角度值；姿态矩阵是θ、γ的函数，因此姿态角可以通过解姿态矩阵的方程得。

步骤2.6：利用求得的位移d₂以及航向角根据的数学关系式推算出机动目标在各个时刻的坐标，Δx为目标在跟踪区域内的横向坐标x的变化量，Δy为目标在跟踪区域内的纵向坐标y的变化量，为目标转过的航向角。

步骤2.7：将计算得到的机动目标的坐标存储。

以上各实施例的基于IMU和RFID测量数据的多传感器融合室内快速跟踪方法，基于RFID数据利用Kalman滤波器对运动目标进行轨迹估计，估计结果不会随着时间的推移造成误差的累积；在没有RFID测量数据时，采用惯性传感器数据的姿态解算方法对目标的运动进行跟踪，此时的跟踪信息作为目标运动信息的补充，重构目标的运动轨迹；实现了对室内目标的快速跟踪，提高了跟踪精度，尤其适用于需要获得运动目标实时轨迹的RFID室内目标跟踪系统。对跟踪方法的优化，进一步提高了跟踪精度。

需要说明的是，在不冲突的情况下，以上各实施例及实施例中的特征可以相互组合。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。