一种基于简化Sage-Husa自适应滤波的室内目标跟踪方法与流程

本发明属于室内跟踪领域，具体涉及一种基于简化sage-husa自适应滤波的室内目标跟踪方法。

背景技术：

在现今这个时代中，无线网络发展迅速，人们对于生活和工作的各方面需求也越来越高，室内定位与跟踪服务以其广泛的实用性和功能性越来越被人们所需要。其中由于wi-fi设备在大型公共场所、办公室或者家庭中都很常见，所以基于wi-fi的室内定位与跟踪是现今研究的重点。wi-fi环境下常见的研究都是基于信号接收强度(rssi)，但是跟踪效果不理想。可以从无线网卡中获得的信道状态信息(csi)是更精细的特征，对环境的感知更加敏感，本文就采用信道状态信息的幅度为特征值，进行室内跟踪算法的设计。现有的跟踪滤波算法在实际应用中存在着收敛速度慢、易发散、收敛精度低等问题。比如变化大的观测数据容易引起扩展类卡尔曼滤波的发散现象，锚节点低密度时容易引起粒子滤波的粒子退化、样本贫化现象；并且当目标处于复杂且无规律的运动状态时，单一的滤波技术由于误差累计问题难以进行长时间高精度的动态跟踪。因此，现有滤波算法仍需进一步改进。

对于移动跟踪系统，利用时间序列上的多次测量来迭代减少定位误差是一种通用的做法。针对无源跟踪系统，研究者提出了几种解决方案。例如，chenz利用卡尔曼滤波器分别使用基于变量的rti和基于子空间变量的射电断层扫描的定位结果来跟踪单个目标。此外，他们将该方案扩展到跟踪多人。与卡尔曼滤波不同，跟踪多人的问题被表述为数据分配问题(dap)，并通过最小化dap的总成本来解决。标准卡尔曼滤波算法要求准确的系统模型和噪声统计特性，在实际应用中很难达到，最终可能导致滤波发散。sage-husa自适应卡尔曼滤波算法可以实时估计噪声统计特性，但会降低滤波实时性，增加算法的复杂程度，实际应用中效果并不理想。vasisht提出一种对sage-husa自适应卡尔曼滤波算法的改进方法，但是其公式复杂，计算量大，不易在实际应用中实现。wuc提出一种简化的sage-husa算法，只对对滤波响大的测量噪声协方差阵进行在线估计，但每个采样周期都对进行估计，未能降低算法的复杂度，无法保证滤波的实时性。cais结合协方差匹配技术对sage-husa算法进行了改进，有效防止了滤波发散并提高了实时性，但由于此方法对系统模型不确定性的鲁棒性差，并且在系统稳定后对突变状态失去跟踪能力所以在实际应用中效果并非特别理想。

技术实现要素：

本发明的目的在于提供一种基于简化的sage-husa自适应滤波的室内目标跟踪算法，其能够解决目标运动状态发生突变时跟踪效果差的问题。

一种基于简化sage-husa自适应滤波的跟踪算法，具体包括以下步骤：

步骤1、在普通系统方程的基础上建立本跟踪系统的系统方程；

步骤2、预测阶段：在不利用观测值的前提下，目标估计当前时刻状态；

步骤3、更新阶段：利用观测值更新估计状态；

步骤4、噪声估计阶段：利用sage-husa自适应滤波的时变噪声统计估值器对系统噪声统计特征和实时修正；

步骤5、至此一轮迭代完毕，令k＝k-1，本轮中估计的和用于下一轮的计算中，重复步骤2至步骤4，直至结束跟踪。

所述一种基于简化sage-husa自适应滤波的室内目标跟踪算法，普通系统方程为：

xk＝fkxk-1+bkuk+wk

式中，xk是状态向量，描述位置，速度，航向等信息，uk代表了所有控制输入向量，包括转向角，油门设置，制动力等，fk是状态转移矩阵，它表示k-1时刻的每个系统状态参数对k时刻的系统状态的影响(例如，k-1时刻的位置和速度都影响k时刻的位置)，bk是控制输入矩阵，其将相量uk中的每个控制输入参数应用于状态向量上，wk代表包含状态向量中每个参数的过程噪声向量，假设wk服从一个由协方差矩阵qk给出的零均值多元高斯分布，观测模型为：

zk＝hkxk+vk

式中zk是观测向量，hk是将状态向量参数映射到观测域的变换矩阵，vk是观测噪声，它与wk一样是高斯白噪声，wk和vk的统计特征如下：

式中，δkj为krinecker-δ函数：

所述一种基于简化sage-husa自适应滤波的室内目标跟踪算法，步骤1中建立本跟踪系统的系统方程具体包括以下步骤：

步骤1.1、在跟踪系统中，假设目标以恒定的速度行走，且不存在外界输入，所以忽略对uk和bk的讨论，因此，目标在k到k+1的时间间隔δt内的运动能用一个具有随机加速度的固定速度来描述，将状态向量xk定义为xk＝[pxk，pyk，vxk，vyk]^t，其中(pxk，pyk)和(vxk，vyk)是目标的坐标和速度，δt内目标在x轴方向上的位移与速度用下式描述：

步骤1.2、系统模型为xk+1＝fkxk+γkwk，则

假设在任意时间间隔δt内的加速度wk为高斯随机噪声，且wk的方差是设γkwk的协方差矩阵为qk，所以

步骤1.3、观测模型为：

zk＝hkxk+vk

则

zk为观测值，把定位的结果作为观测值不断输入到跟踪系统中。将观测噪声vk定义为其中和分别是x和y轴方向的观测噪声，假设它们不相关；从文献^[49]中可以看出，在使用sage-husa滤波时不可能同时估计系统噪声与观测噪声的统计特性，只能固定其中一个而估计另外一个。在本文中，认为目标在室内行走或转弯时会发生随机加速度，这个加速度需要实时估计，而观测噪声的统计特性可以通过定位误差来求得。所以相比于传统做法——以经验确定vk的统计特性的做法，在本文中，将定位阶段获得的沿x轴和y轴方向的定位误差的均值和方差作为观测噪声的统计特征，更加贴合实际情况。

所述一种基于简化sage-husa自适应滤波的室内目标跟踪算法，步骤2具体包括以下步骤：

步骤2.1、状态一步预测方程：

步骤2.2、用估计误差协方差矩阵pk|k-1表示本轮预测的不确定程度：

步骤2.3、新息方程：

步骤2.4、利用遗忘因子b计算中间变量dk，0＜b＜1：

所述一种基于简化sage-husa自适应滤波的室内目标跟踪算法，步骤3具体包括以下步骤：

步骤3.1、更新卡尔曼增益矩阵，其中通过pk|k-1间接的影响kk：

步骤3.2、估计目标位置：

步骤3.3、更新预测误差协方差矩阵：

pk＝(i-kkhk)pk|k-1。

所述一种基于简化sage-husa自适应滤波的室内目标跟踪算法，步骤4具体包括以下步骤：

步骤4.1、；利用sage-husa自适应滤波的时变噪声统计估值器重新估计

步骤4.2、利用sage-husa自适应滤波的时变噪声统计估值器重新估计

本发的有益效果在于：

相比于传统卡尔曼滤波，当目标的运动状态不确定或者运动状态发生突变时，本发明提出的基于简化的sage-husa自适应滤波的室内目标跟踪算法可以实时校正系统噪声，使得系统方程更加符合实际情况，提升滤波精度并且抑制滤波发散。

附图说明

图1为本发明的流程图。

具体实施方式

下面结合附图对本发明作进一步描述。

在实际应用中，随着观测数量的增加，可能会由于系统方程、观测方程中的某些参数设置不正确或者递推的过程中误差不断累积使滤波逐渐失去准确估计的作用，也就是滤波发散。为了应对上述发散的种种原因，本发明使用一种基于简化sage-husa滤波的追踪算法来抑制发散，从而完成追踪。

在实际应用中，有时并不能完全确定系统噪声qk和观测噪声rk，甚至连状态转移矩阵和观测矩阵都不能确定，从而使得滤波发散。这种情况一旦发生，要控制卡尔曼增益矩阵kk来保证滤波结果不变，所以应该首先对qk或者rk进行估计，然后反馈给增益矩阵kk，对已经变化了的qk或者rk进行估计后反馈调节kk的过程就是系统自适应调节过程。sage-husa自适应滤波就是在卡尔曼滤波的基础上，引入了时变噪声统计估值器，所以它可以不断校正系统噪声和观测噪声的均值与方差，以减小滤波误差。

如附图1所示，为本发明流程图，具体包括以下步骤：

步骤1、在普通系统方程的基础上建立本跟踪系统的系统方程；

步骤2、预测阶段：在不利用观测值的前提下，目标估计当前时刻状态；

步骤3、更新阶段：利用观测值更新估计状态。

步骤4、噪声估计阶段：利用sage-husa自适应滤波的时变噪声统计估值器对系统噪声统计特征和实时修正；

步骤5、至此一轮迭代完毕，令k＝k-1，本轮中估计的和用于下一轮的计算中，重复步骤2至步骤4。

所述一种基于简化sage-husa自适应滤波的室内目标跟踪算法，普通系统方程为：

xk＝fkxk-1+bkuk+wk

式中，xk是状态向量，描述位置，速度，航向等信息，uk代表了所有控制输入向量，包括转向角，油门设置，制动力等，fk是状态转移矩阵，它表示k-1时刻的每个系统状态参数对k时刻的系统状态的影响(例如，k-1时刻的位置和速度都影响k时刻的位置)，bk是控制输入矩阵，其将相量uk中的每个控制输入参数应用于状态向量上，wk代表包含状态向量中每个参数的过程噪声向量，假设wk服从一个由协方差矩阵qk给出的零均值多元高斯分布，观测模型为：

zk＝hkxk+vk

式中zk是观测向量，hk是将状态向量参数映射到观测域的变换矩阵，vk是观测噪声，它与wk一样是高斯白噪声，wk和vk的统计特征如下：

式中，δkj为krinecker-δ函数：

所述一种基于简化sage-husa自适应滤波的室内目标跟踪算法，步骤1中建立本跟踪系统的系统方程具体包括以下步骤：

步骤1.1、在跟踪系统中，假设目标以恒定的速度行走，且不存在外界输入，所以忽略对uk和bk的讨论，因此，目标在k到k+1的时间间隔δt内的运动能用一个具有随机加速度的固定速度来描述，将状态向量xk定义为xk＝[pxk，pyk，vxk，vyk]^t，其中(pxk，pyk)和(vxk，vyk)是目标的坐标和速度，δt内目标在x轴方向上的位移与速度用下式描述：

步骤1.2、系统模型为xk+1＝fkxk+γkwk，则

假设在任意时间间隔δt内的加速度wk为高斯随机噪声，且wk的方差是设γkwk的协方差矩阵为qk，所以

步骤1.3、观测模型为：

zk＝hkxk+vk

则

zk为观测值，把定位的结果作为观测值不断输入到跟踪系统中。将观测噪声vk定义为其中和分别是x和y轴方向的观测噪声，假设它们不相关；从文献^[49]中可以看出，在使用sage-husa滤波时不可能同时估计系统噪声与观测噪声的统计特性，只能固定其中一个而估计另外一个。在本文中，认为目标在室内行走或转弯时会发生随机加速度，这个加速度需要实时估计，而观测噪声的统计特性可以通过定位误差来求得。所以相比于传统做法——以经验确定vk的统计特性的做法，在本文中，将定位阶段获得的沿x轴和y轴方向的定位误差的均值和方差作为观测噪声的统计特征，更加贴合实际情况。

所述一种基于简化sage-husa自适应滤波的室内目标跟踪算法，步骤2具体包括以下步骤：

步骤2.1、状态一步预测方程：

步骤2.2、用估计误差协方差矩阵pk|k-1表示本轮预测的不确定程度：

步骤2.3、新息方程：

步骤2.4、利用遗忘因子b计算中间变量dk，0＜b＜1：

所述一种基于简化sage-husa自适应滤波的室内目标跟踪算法，步骤3具体包括以下步骤：

步骤3.1、更新卡尔曼增益矩阵，其中通过pk|k-1间接的影响kk：

步骤3.2、估计目标位置：

步骤3.3、更新预测误差协方差矩阵：

pk＝(i-kkhk)pk|k-1。

所述一种基于简化sage-husa自适应滤波的室内目标跟踪算法，步骤4具体包括以下步骤：

步骤4.1、；利用sage-husa自适应滤波的时变噪声统计估值器重新估计

步骤4.2、利用sage-husa自适应滤波的时变噪声统计估值器重新估计