针对测距信号SLAM的矢量粒子滤波方法技术

本发明专利技术公开了一种针对测距信号SLAM的矢量粒子滤波方法，包括以下步骤：针对N个测距信号运行N组粒子滤波器，估计信号源位置和用户定位；粒子滤波器进行滤波估计，并依据用户和信号源的运动模型更新每个粒子的值；计算每组粒子滤波器所有粒子的暂态权重值；分别计算每组粒子滤波器中用户定位状态与信号源位置状态的暂态均值和暂态协方差；计算每组粒子滤波器的辅助均值和辅助协方差；将每组粒子滤波器进行联合权重更新：每组粒子滤波器计算相应的用户定位与信号源位置的本地状态量和本地协方差；对每组粒子滤波器进行重采样；融合估计N组滤波器，输出用户定位的状态量和协方差。本发明专利技术可以降低粒子采样难度，提高算法实时性，适用于实际工程运用。适用于实际工程运用。适用于实际工程运用。

【技术实现步骤摘要】
针对测距信号SLAM的矢量粒子滤波方法


[0001]本专利技术属于定位导航
，具体涉及一种针对测距信号SLAM的矢量粒子滤波方法。

技术介绍

[0002]即时定位与构图(SLAM)是一种进行用户定位并估计周围环境特征的应用，基于测距信号的SLAM是一种重要应用环境，通常采用粒子滤波技术实现。在实际导航定位场景下，用户可能接收到多个测距信号，给粒子滤波的粒子采样带来困难，需要大量的粒子表示用户定位和信号源位置，直接影响了SLAM系统的实时性。

技术实现思路

[0003]本专利技术的目的在于提供一种针对测距信号SLAM的矢量粒子滤波方法，以降低粒子采样难度，提高系统的实时性能。
[0004]为实现上述目的，本专利技术提供了一种针对测距信号SLAM的矢量粒子滤波方法，包括以下步骤：
[0005]S1：针对N个测距信号运行N组粒子滤波器，以估计信号源位置和用户定位；
[0006]S2：粒子滤波器进行滤波估计，并依据用户和信号源的运动模型更新每个粒子的值；
[0007]S3：计算每组粒子滤波器所有粒子的暂态权重值；
[0008]S4：基于步骤S3的暂态权重值分别计算每组粒子滤波器中用户定位状态与信号源位置状态的暂态均值和暂态协方差；
[0009]S5：基于步骤S4中用户定位状态的暂态均值和暂态协方差，计算每组粒子滤波器的辅助均值和辅助协方差；
[0010]S6：基于步骤S4中用户定位状态的暂态均值和暂态协方差、步骤S5的辅助均值和辅助协方差，将每组粒子滤波器进行联合权重更新：
[0011]S7：基于步骤S6的联合权重，每组粒子滤波器计算相应的用户定位与信号源位置的本地状态量和本地协方差；
[0012]S8：对每组粒子滤波器进行重采样；
[0013]S9：基于步骤S7计算的用户定位的本地状态量和本地协方差，融合估计N组滤波器，输出用户定位的状态量和协方差。
[0014]作为本专利技术的进一步改进，所述步骤S1中，以t时刻第i个粒子滤波器为例，其第n个粒子包含其中代表用户的定位状态量，代表第i个信号源的位置状态量，代表该粒子的权重，i＝1,2,...,N。
[0015]作为本专利技术的进一步改进，所述步骤S3中，以t时刻第i个粒子滤波器第n个粒子的
暂态权重值为例，其计算方式为
[0016][0017]其中，为该粒子的暂态观测协方差，为t时刻第i个测距信号，为基于计算的第i个测距信号估计值。
[0018]作为本专利技术的进一步改进，所述步骤S4中，以t时刻第i个粒子滤波器为例，其用户定位状态的暂态均值为暂态协方差为信号源位置状态的暂态均值为暂态协方差为其中，K
i
为第i个粒子滤波器的粒子数量。
[0019]作为本专利技术的进一步改进，所述步骤S5中，以t时刻第i个粒子滤波器为例，其辅助均值为辅助协方差为其中，下标\i代表相应的变量排除了第i个测距信号，Z
\i
表示用户接收到的除第i个测距信号之外的所有测距信号集合，为步骤S4所计算的第j个粒子滤波器的用户定位状态的暂态协方差；为第j个粒子滤波器的权重。
[0020]作为本专利技术的进一步改进，所述步骤S6中，以t时刻第i个粒子滤波器第n个粒子的联合权重更新为例，更新后的联合权重为
[0021]其中，为该粒子的辅助观测协方差，
[0022]作为本专利技术的进一步改进，所述步骤S7中，以t时刻第i个粒子滤波器为例，用户定
位的本地状态量为本地协方差为信号源位置的本地状态量为本地协方差为
[0023]作为本专利技术的进一步改进，所述步骤S9中，以t时刻为例，输出的用户定位状态量为协方差为其中，为第i个粒子滤波的权重，通过公式计算获得，符号[]l,l
代表相应矩阵的第l行l列元素。
[0024]本专利技术的有益效果为：与现有技术相比，本专利技术将传统的高维粒子滤波矢量化为一组低维粒子滤波器，并通过每组粒子滤波器的辅助均值和辅助协方差实现联合权重更新，可以降低粒子采样难度，提高算法实时性，适用于实际工程运用。
附图说明
[0025]图1是本专利技术的总体方法流程图。
[0026]图2是单个粒子滤波器的流程图。
具体实施方式
[0027]为了使本专利技术的目的、技术方案和优点更加清楚，下面结合附图和具体实施例对本专利技术进行详细描述。
[0028]如图1与图2所示，本专利技术提供了一种针对测距信号SLAM的矢量粒子滤波方法，包括以下步骤：
[0029]S1：针对N个测距信号运行N组粒子滤波器，以估计相应的信号源位置和用户定位；
[0030]S2：粒子滤波器进行滤波估计，并依据用户和信号源的运动模型更新每个粒子的值；
[0031]S3：计算每组粒子滤波器所有粒子的暂态权重值；
[0032]S4：基于步骤S3的暂态权重值分别计算每组粒子滤波器中用户定位状态与信号源位置状态的暂态均值和暂态协方差；
[0033]S5：基于步骤S4中用户定位状态的暂态均值和暂态协方差，计算每组粒子滤波器的辅助均值和辅助协方差；
[0034]S6：基于步骤S4中用户定位状态的暂态均值和暂态协方差、步骤S5的辅助均值和辅助协方差，将每组粒子滤波器进行联合权重更新：
[0035]S7：基于步骤S6的联合权重，每组粒子滤波器计算相应的用户定位与信号源位置的本地状态量和本地协方差；
[0036]S8：对每组粒子滤波器进行重采样；
[0037]S9：基于步骤S7计算的用户定位的本地状态量和本地协方差，融合估计N组滤波器，输出用户定位的状态量和协方差。
[0038]以下将对步骤S1
‑
S9进行举例说明。
[0039]步骤S1中，以t时刻第i个粒子滤波器为例，其第n个粒子包含其中代表用户的定位状态量，代表第i个信号源的位置状态量，代表该粒子的权重，i＝1,2,...,N。
[0040]步骤S3中，以t时刻第i个粒子滤波器第n个粒子的暂态权重值为例，其计算方式为
[0041][0042]其中，为该粒子的暂态观测协方差，为t时刻第i个测距信号，为基于计算的第i个测距信号估计值。
[0043]步骤S4中，以t时刻第i个粒子滤波器为例，其用户定位状态的暂态均值为暂态协方差为信号源位置状态的暂态均值为暂态协方差为其中，K
i
为第i个粒子滤波器的粒子数量。
[0044]步骤S5中，以t时刻第i个粒子滤波器为例，其辅助均值为辅助协方差为其中，下标\i代表相应的变量排除了第i个测距信号，Z
\i
表示用户接收到的除第i个测距信号之外的所有测距信号集合，为步骤S4所计算的第j个粒子滤波器的用户定位状态的暂态协方差；为第j个粒子滤波器的权重。
[0045]步骤S6中，以t时刻第i个粒子滤波器第n个粒子的联合权重更新为例，更新后的联合权重为
[0046]其中，为该粒子的辅助观测协方本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种针对测距信号SLAM的矢量粒子滤波方法，其特征在于，包括以下步骤：S1：针对N个测距信号运行N组粒子滤波器，以估计信号源位置和用户定位；S2：粒子滤波器进行滤波估计，并依据用户和信号源的运动模型更新每个粒子的值；S3：计算每组粒子滤波器所有粒子的暂态权重值；S4：基于步骤S3的暂态权重值分别计算每组粒子滤波器中用户定位状态与信号源位置状态的暂态均值和暂态协方差；S5：基于步骤S4中用户定位状态的暂态均值和暂态协方差，计算每组粒子滤波器的辅助均值和辅助协方差；S6：基于步骤S4中用户定位状态的暂态均值和暂态协方差、步骤S5的辅助均值和辅助协方差，将每组粒子滤波器进行联合权重更新：S7：基于步骤S6的联合权重，每组粒子滤波器计算相应的用户定位与信号源位置的本地状态量和本地协方差；S8：对每组粒子滤波器进行重采样；S9：基于步骤S7计算的用户定位的本地状态量和本地协方差，融合估计N组滤波器，输出用户定位的状态量和协方差。2.根据权利要求1所述的针对测距信号SLAM的矢量粒子滤波方法，其特征在于：所述步骤S1中，以t时刻第i个粒子滤波器为例，其第n个粒子包含其中代表用户的定位状态量，代表第i个信号源的位置状态量，代表该粒子的权重，i＝1,2,...,N。3.根据权利要求2所述的针对测距信号SLAM的矢量粒子滤波方法，其特征在于：所述步骤S3中，以t时刻第i个粒子滤波器第n个粒子的暂态权重值为例，其计算方式为其中，为该粒子的暂态观测协方差，为t时刻第i个测距信号，为基于计算的第i个测距信号估计值。4....

【专利技术属性】
技术研发人员：熊骏，熊师洵，王凯，陆音，丁飞，
申请(专利权)人：南京邮电大学，
类型：发明
国别省市：