抗差自适应容积卡尔曼滤波在UWB室内定位中的应用

高 嵩，宋佳鹏，房 穹，张熙为

抗差自适应容积卡尔曼滤波在UWB室内定位中的应用

高 嵩，宋佳鹏，房 穹，张熙为

（辽宁工程技术大学 测绘与地理科学学院，辽宁 阜新 123000）

针对超宽带（UWB）测距异常值、传统滤波方法中动力学模型不精准、状态向量误差协方差阵非正定等问题，提出一种基于奇异值分解的抗差自适应容积卡尔曼滤波算法，并将其应用于UWB室内定位中：以标准容积卡尔曼滤波（CKF）算法为基础，利用残差向量构造抗差因子消除观测异常值对定位解的影响；利用自适应因子对整体模型误差进行实时调整和修正以提高滤波精度；同时用奇异值分解代替乔莱斯基（Cholesky）分解以进一步提高滤波的稳定性。实验结果表明，所提算法相比传统的扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）、CKF算法，能够进一步提高UWB系统的定位精度和抗干扰能力，定位最大误差由1.5 m降至0.3 m，均方根误差小于0.05 m。

超宽带（UWB）定位；奇异值分解；容积卡尔曼滤波；测距异常值；系统噪声

0 引言

随着中国经济的转型升级，特别是中国制造2025国家战略的实施以来，目标定位技术在诸多领域成为研究的热点，人们对高精度定位技术的需求愈发强烈，希望其可以加快国民经济建设的步伐并助力产业结构的升级。在良好的室外环境下，全球卫星导航系统（global navigation satellite system，GNSS）可提供连续高精度定位结果；但在复杂的室内环境下，由于GNSS信号受遮挡以及强烈的多路径效应影响，GNSS的室内定位精度显著下降甚至无法定位。因此需要考虑采用其他定位技术实现室内定位。在众多室内定位技术中，超宽带（ultra-wide band，UWB）由于其测距精度高、抗多路径性能强等优点，逐渐成为室内定位技术的研究热点。

虽然UWB凭借其带宽大、时间分辨率高的优点可以获得高达厘米级的测距精度，但在实际的应用过程中将不可避免地产生测量误差[1]。UWB测距误差主要为标准时间偏差和非视距（non-line of sight，NLOS）误差。标准时间偏差一般容易改正，可通过改正函数对其进行校正[2]。针对NLOS误差的处理，主要分为抗差滤波处理方法和根据数据信号特征鉴别方法2类[3]。经过标准时间偏差改正和NLOS误差削弱后的UWB测距值的精度得到很大的提高；但UWB测距值中还会存在一些测量值误差，这种误差在滤波定位解算过程中表现为观测值异常，会严重影响滤波解算结果的精度与稳定。针对观测值异常的问题，有学者将抗差估计理论与滤波算法相结合，提出了抗差卡尔曼滤波、抗差扩展卡尔曼滤波、抗差无迹卡尔曼滤波等滤波算法[4-5]，这些算法通过对异常值降权处理抑制其对滤波解算结果的影响，能大幅度提高滤波解算结果的精度。此外在实际的运动过程中，载体的运动模型一般难以用精确的数学模型构建，导致动力学模型与载体的真实运动模型之间存在误差，这会使滤波器的动态性能受到较大影响[6]。为了有效控制动力学模型误差的影响，有学者提出了自适应无迹卡尔曼滤波和自适应容积卡尔曼滤波[7-8]，这些滤波算法通过自适应因子控制动态扰动异常，能提高滤波算法的精度和稳定性。本文在充分吸收抗差滤波、自适应滤波和容积卡尔曼滤波各自优点的基础上，同时顾及在实际运算过程中，由于计算舍入误差、系统噪声的先验统计信息不准确等原因导致状态向量的误差协方差阵失去对称正定性的问题[9-10]，利用基于奇异值分解的抗差自适应容积卡尔曼滤波（singular value decomposition adaptively robust cubature Kalman filter，SVD-ARCKF）算法进行UWB定位解算。

1 UWB定位系统的动力学模型和量测模型

UWB定位模式主要有基于接收信号强度、基于信号到达时间、基于信号到达时间差、基于信号到达角度和信号双向传播时间（round-trip time，RTT）模式[11-12]。RTT测距不需要基准站和流动站时间同步[13]，因此本文选用RTT测距模式。

在UWB的定位中，一般以UWB流动站天线的相位中心点的位置和速度信息作为滤波的状态向量。则UWB定位系统的动力学模型为

UWB的量测模型为

2 基于SVD-ARCKF的UWB定位算法

该算法以标准容积卡尔曼滤波（cubature Kalman filter，CKF）算法为基础，利用残差向量构造抗差因子，消除测距异常值对定位解的影响，利用自适应因子对整体模型误差进行实时调整和修正以提高滤波精度，同时用奇异值分解（singular value decomposition，SVD）代替Cholesky分解以进一步提高滤波的稳定性，具体实现步骤为：

3）时间更新。通过UWB定位系统的动力学模型计算传播容积点

5）量测更新。由UWB的量测模型计算容积点

计算量测估计值

计算预测残差

常用的误差判别统计量有方差分量统计量和预测残差统计量等，常用的抗差矩阵构建法有Huber法及Andrew法等[15]。针对UWB定位系统模型中可能出现观测量个数小于状态向量维数的情况，本文选择利用预测残差值构建统计量，并利用Huber法建立等价权矩阵以确保等价权阵对角线不为零。

基于预测残差构建的统计量为

抗差因子构造的等式为

7）对状态向量预测值协方差进行修正。由于建立精准的UWB定位系统模型比较困难，且系统噪声的先验统计信息也常是未知或不准确的，这些因素都会导致动力学模型出现误差。动力学模型误差通常影响整体的参数估计效果[16]，因此本文构造一个自适应因子对UWB的定位系统模型进行整体的调节和修正。具体实现步骤为

8）状态更新。滤波增益矩阵计算如下式

3 实验与结果分析

为了验证所提的SVD-ARCKF算法在UWB定位解算中的有效性，并评估其性能，本文在室内场景下对UWB定位算法进行了实验验证，并和现有的扩展卡尔曼滤波（extended Kalman filter，EKF）、无迹卡尔曼滤波（unscented kalman filter，UKF）、CKF定位算法进行对比分析。

图1 定位场景

图2 实验平台

图3 EKF定位轨迹

图4 UKF定位轨迹

图5 CKF定位轨迹

图6 SVD-ARCKF定位轨迹

图8 y方向误差

表1 室内实验中4种滤波方法的误差统计信息 m

从上述结果分析可以得到：

1）4种滤波定位方法的整体轨迹都比较符合流动站的运动轨迹，但在实验过程中，小车有一小段时间在房间的柱子中间穿过，此时UWB脉冲信号受到柱子的遮挡产生了NLOS误差，平面轨迹在拐角处出现了持续较大的偏差。

2）EKF方法定位的精度较差，稳定性难以得到保证，当UWB信号受到NLOS误差影响时，其定位精度迅速降低。由于本文只采用15个参考检核点对不同方法的定位精度进行检验，部分误差较大区域没有设置参考检核点，因此EKF方法的实际定位精度要低于表1中的统计结果。

3）UKF方法与CKF方法的定位精度大致相同，但都比EKF方法的定位精度高，这是因为利用UKF和CKF方法进行定位时不需要对UWB的量测模型进行线性化，不会产生截断误差。

4 结束语

UWB以其距离分辨率高和抗干扰能力强等特点，在室内定位中优势明显，很多研究团队已取得了一定的研究成果，但在UWB测距异常值、传统滤波方法中动力学模型不精准、状态向量误差协方差阵非正定等问题仍没有得到很好的解决；因此本文提出SVD-ARCKF算法，并将其应用于UWB室内定位中。利用实验对所提算法进行验证，实验结果表明本文所提算法相比传统的EKF、UKF、CKF算法更能提高UWB系统的定位精度和抗干扰能力。

[1] 闫大禹, 宋伟, 王旭丹, 等. 国内室内定位技术发展现状综述[J]. 导航定位学报, 2019, 7(4): 5-12.

[2] CAZZORLA A, DE A G, MOSCHITTA A, et al. A 5. 6-GHz UWB position measurement system[J]. IEEE Transactions on Instrumentation and Measurement, 2013, 62(3): 675-683.

[3] 盛坤鹏, 王坚, 李晨辉, 等. 非视距误差改正的超宽带定位模型研究[J]. 测绘科学, 2021, 46(3): 40-47.

[4] EINICKE G A, WHITE L B. Robust extended Kalman filtering[J]. IEEE Transactions on Signal Processing, 1999, 47(9): 2596-2599.

[5] 张且且, 赵龙, 周建华. 抗差自适应分步滤波算法在PPP/INS组合导航中的应用[J]. 导航定位与授时, 2020, 7(5): 73-81.

[6] YANG Y, GAO W. An optimal adaptive Kalman filter[J]. Journal of Geodesy, 2006 , 80(4): 177-183.

[7] GAO Z, MU D, GAO S, et al. Adaptive unscented Kalman filter based on maximum posterior and random weighting[J]. Aerospace Science and Technology, 2017, 71: 12-24.

[8] WANG Z, QIU H, QIAN G. Adaptive robust cubature Kalman filtering for satellite attitude estimation[J]. Chinese Journal of Aeronautics, 2018, 31(4): 806-819.

[9] 高社生, 王建超, 焦雅林. 自适应SVD-UKF算法及在组合导航的应用[J]. 中国惯性技术学报, 2010, 18(6): 737-742.

[10] 李兆铭, 杨文革, 丁丹, 等. 基于SVD的多终端实时定轨自适应鲁棒CKF算法[J]. 仪器仪表学报, 2016 , 37(3): 490-496.

[11] 王卿, 蔚保国. 基于非合作室内环境的超宽带无中心组网定位[J]. 河北科技大学学报, 2021, 42(1) : 8-14.

[12] 董家志. 基于UWB的室内定位与跟踪算法研究[D]. 成都：电子科技大学, 2015: 9-12.

[13] 徐爱功, 刘韬, 隋心, 等. UWB/INS紧组合的室内定位定姿方法[J]. 导航定位学报, 2017, 5(2): 14-19.

[14] 谢强, 唐和生, 邸元. SVD-Unscented卡尔曼滤波的非线性结构系统识别[J]. 应用力学学报, 2008(1) : 57-61, 181.

[15] 杨元喜, 任夏, 许艳. 自适应抗差滤波理论及应用的主要进展[J]. 导航定位学报, 2013, 1(1) : 9-15.

[16] YANG Y, GAO W. An optimal adaptive Kalman filter[J]. Journal of Geodesy, 2006, 80(4): 177-183.

[17] ER M J, WU S, LU J, et al. Face recognition with radial basis function (RBF) neural networks[J]. IEEE Transactions on Neural Networks, 2002, 13(3): 697-710.

Application of adaptively robust cubature Kalman filter in UWB indoor location

GAO Song, SONG Jiapeng, FANG Qiong, ZHANG Xiwei

（School of Geomatics, Liaoning Technical University, Fuxin, Liaoning 123000, China）

Aiming at the problems of ultra-wide band (UWB) ranging outliers, the inaccuracy of dynamic model in traditional filtering methods, and the non-positive definite state vector error covariance matrix, a robust adaptive volume Kalman filter algorithm based on singular value decomposition was proposed and applied to UWB indoor positioning. Based on the standard cubature Kalman filter (CKF) algorithm, the residual vector is used to construct the robust factor to eliminate the influence of the observed outliers on the positioning solution, and the adaptive factor is used to adjust and correct the overall model error in real time to improve the filtering accuracy. At the same time, the singular value decomposition is used to replace Cholesky decomposition to further improve the filtering stability. Experimental results show that compared with traditional extended Kalman filter (EKF), unscented Kalman filter (UKF) and CKF algorithms, the proposed algorithm can improve the positioning accuracy and anti-jamming ability of UWB system better. The maximum positioning error is reduced from 1.5 m to 0.3 m, and the root mean square error is less than 0.05 m.

ultra-wide band (UWB) positioning; singular value decomposition; cubature Kalman filter (CKF); ranging outliers; system noise

P228

A

2095-4999(2023)01-0142-06

高嵩，宋佳鹏，房穹，等. 抗差自适应容积卡尔曼滤波在UWB室内定位中的应用[J].导航定位学报, 2023, 11(1): 142-147.（GAO Song, SONG Jiapeng, FANG Qiong,et al. Application of adaptively robust cubature Kalman filter in UWB indoor location[J]. Journal of Navigation and Positioning, 2023, 11(1): 142-147.）DOI:10.16547/j.cnki.10-1096.20230121.

2021-08-05

国家自然科学基金项目（42074012）；辽宁省重点研发计划项目（2020JH2/10100044）；辽宁省自然科学基金计划指导计划项目（2019-ZD-0051）；辽宁省教育厅基础研究项目（LJ2020JCL016）。

高嵩（2000—），女，辽宁沈阳人，本科生，研究方向为室内外一体化定位。

张熙为（1998—），男，辽宁阜新人，硕士研究生，研究方向为室内外导航定位。