CN114577210A - 基于地图信息矩阵的跨区域检测算法 - Google Patents

Abstract

本发明提供一种基于地图信息矩阵的跨区域检测算法，在保证将错误跨区域的粒子检测出来的基础上，提高算法的运算速度。针对基于粒子滤波的地图匹配算法在粒子更新过程中的错误跨区域现象，提出了一种跨区域检测算法。针对算法计算量大的问题，改进了地图建模过程，利用栅格化地图得到地图信息矩阵。本发明利用MATLAB仿真软件进行了仿真分析和实测验证。通过定量分析，与传统跨立实验法穿门检测算法进行对比，证明了基于地图信息矩阵的穿门检测算法的准确性与快速性；通过定性分析，与未加入跨区域检测的地图匹配算法进行对比，证明了本发明提出的跨区域检测算法的有效性。

Description

基于地图信息矩阵的跨区域检测算法

技术领域

本发明涉及一种基于地图信息矩阵的跨区域检测算法，属于地图匹配的技术领域，尤其涉及一种基于粒子滤波的地图匹配算法领域。

背景技术

近年来，随着物联网产业的发展，基于位置的服务正日趋成为人们社会生活中的重要组成部分。作为一种经济简单的定位方法，室内地图数据不仅可以将行人所处的位置直观的显示在地图上，还不需要其他任何辅助设施或仪器就能对定位结果具有天然的约束力。地图匹配技术将定位数据和地图数据进行有效融合，利用其它室内定位技术得到初始定位结果，利用地图数据对该定位结果进行修正，从而获得更准确的定位坐标，是一种辅助定位算法。传统地图匹配技术通常分为三类，包括基于几何的地图匹配技术、基于拓扑结构的地图匹配技术以及基于粒子滤波的高级匹配技术。现阶段的室内地图匹配技术多使用基于粒子滤波的地图匹配技术，该技术充分利用了状态的更新过程与室内地图的墙壁信息，具有较高的定位精度。所以，基于粒子滤波的地图匹配技术一直是地图匹配领域重要的研究方向之一。

在基于粒子滤波的地图匹配技术中，由于室内门等连接区域是可通行的，在粒子状态的更新过程中，总是有部分粒子错误的通过门进入到其他区域，导致接下来的更新误差增大。因此，研究准确的跨区域检测算法从而将本不该穿过门的粒子检测出来并剔除掉，可以提高地图匹配算法的定位精度。根据算法实现的功能，跨区域检测算法可主要分为穿门检测和跨区域判断两步。穿门检测是通过判断粒子轨迹与门线段是否相交来判断粒子的更新过程是否通过门，是跨区域检测最重要的一步。判断两线段相交的经典算法是跨立实验法，该方法思路清晰、便于理解，但是在运用到基于粒子滤波的地图匹配技术中时，其计算量比较大，导致实时性不高。另外，在地图匹配技术中，地图数据是实现匹配算法的先决条件，地图数据的准确性严重影响着地图匹配技术的精度。设计合理的地图信息获取方法得到室内地图数据是必要的。

发明内容

本发明的目的是提供一种基于地图信息矩阵的跨区域检测算法，在保证将错误跨区域的粒子检测出来的基础上，提高算法的运算速度。

本发明的目的是这样实现的：包括以下步骤：

(1)根据室内地图建立地图信息矩阵；

(2)进行粒子滤波的状态更新，得到第i个粒子k-1时刻位置

和k时刻位置

(3)循环检测粒子轨迹在地图信息矩阵里的元素值，判断粒子轨迹是否穿过门；

(4)计算粒子轨迹与门之间的夹角；

(5)与预设夹角阈值比较，正确判断粒子是否跨区域；

(6)将跨区域检测算法加入到粒子滤波算法中的权重更新部分，完成滤波。

可选的，所述步骤(1)的具体过程为：

假设室内地图的总长度为L，宽度为W，选定的网格宽度为l，该地图总共可划分为m×n个网格，即该地图信息矩阵总共有m行n列，其中m,n可由下式求得：

式中，[·]是向上取整符号。这是因为地图是从0开始，而矩阵索引位置是从1开始。假设某个位置通过上式求解得到该位置位于地图信息矩阵的第i行第j列，则其位置在矩阵中可表示为(i,j),i＝1,2,...,m,j＝1,2,...,n。根据该网格表示的地图信息，可以将地图数据转换成m行n列的地图信息矩阵Indoor。当所处位置(i,j)是墙壁时，Indoor(i,j)＝0；当所处位置(i,j)是桌椅等室内布局时，Indoor(i,j)＝1；当所处位置(i,j)是人们日常活动区域时，Indoor(i,j)＝2；当所处位置(i,j)是门等连接区域时，Indoor(i,j)＝3，这样就可以将地图数据以数字的形式存储在地图信息矩阵里。

可选的，所述步骤(3)的具体过程为：

假设k-1时刻第i个粒子的坐标为

则其在地图信息矩阵Indoor中所在的行列为：

粒子的状态更新后，在k时刻第i个粒子的坐标为

其在地图信息矩阵Indoor中所在的行列为：

根据k-1时刻和k时刻第i个粒子的相对位置关系，可以将整个区域划分为2个坐标轴与4个区域共六个部分，各部分循环检测如下：

(1)若

即k-1与k时刻坐标连线为x轴，令

若存在

则第i个粒子在k-1时刻到k时刻的更新过程中通过门，需求粒子轨迹与门的夹角；

(2)若

即k-1与k时刻坐标连线为y轴，，令

若存在

则第i个粒子在k-1时刻到k时刻的更新过程中通过门，需求粒子轨迹与门的夹角；

(3)令斜率

若|a|＜1且

此时进行列循环遍历。若始末位置网格为3，即

或

则第i个粒子通过门，需求粒子轨迹与门的夹角。若不为3，令

则

当y方向发生变化时，相应的x方向变化Δx＝Δy/a，则

令p₁＝[x₁/l]，q₁＝[y₁/l]，p₂＝[x₂/l]，q₂＝[y₂/l]，若存在Indoor(p₁,q₁)＝3或Indoor(p₂,q₂)＝3，则说明第i个粒子在k-1时刻到k时刻的更新过程中通过门，需求粒子轨迹与门的夹角；

(4)令斜率

若|a|＜1且

此时进行列循环遍历。若始末位置网格为3，即

或

则第i个粒子通过门，需求粒子轨迹与门的夹角。若不为3，令

则

当y方向发生变化时，相应的x方向变化Δx＝Δy/a，则

令p₁＝[x₁/l]，q₁＝[y₁/l]，p₂＝[x₂/l]，q₂＝[y₂/l]，若存在Indoor(p₁,q₁)＝3或Indoor(p₂,q₂)＝3，则说明第i个粒子在k-1时刻到k时刻的更新过程中通过门，需求粒子轨迹与门的夹角；

(5)令斜率

若|a|≥1且

此时进行行循环遍历。若始末位置网格为3，即

或

则第i个粒子通过门，需求粒子轨迹与门的夹角。若不为3，令

则

当x方向发生变化时，相应的y方向变化Δy＝Δx·a，则

令p₁＝[x₁/l]，q₁＝[y₁/l]，p₂＝[x₂/l]，q₂＝[y₂/l]，若存在Indoor(p₁,q₁)＝3或Indoor(p₂,q₂)＝3，则说明第i个粒子在k-1时刻到k时刻的更新过程中通过门，需求粒子轨迹与门的夹角；

(6)令斜率

若|a|≥1且

此时进行行循环遍历。若始末位置网格为3，即

或

则第i个粒子通过门，需求粒子轨迹与门的夹角。若不为3，令

则

当x方向发生变化时，相应的y方向变化Δy＝Δx·a，则

令p₁＝[x₁/l]，q₁＝[y₁/l]，p₂＝[x₂/l]，q₂＝[y₂/l]，若存在Indoor(p₁,q₁)＝3或Indoor(p₂,q₂)＝3，则说明第i个粒子在k-1时刻到k时刻的更新过程中通过门，需求粒子轨迹与门的夹角；

可选的，所述步骤(4)的具体过程为：

检测到粒子通过门后，需要正确判断粒子是否跨区域。若轨迹确实跨区域，则粒子有效；若轨迹没有跨区域，则该粒子需要剔除。具体思路是根据通过门的粒子轨迹与门之间的夹角大小，设置合适的夹角阈值判断粒子是否跨区域。跨区域判断转换为求k-1时刻与k时刻粒子的坐标连线与门的夹角，并与预设的夹角阈值做比较。而粒子轨迹与门之间的夹角可抽象为两条线段之间的夹角。两条线段之间的夹角可分为以下三种情况：

(1)当门表示的线段平行于X轴时，此时：

a.当轨迹线段斜率不存在，即y_k＝y_k-1时，此时夹角θ＝90°；

b.当轨迹线段斜率k₁＝(y_k-1-y_k)/(x_k-1-x_k)≥0时，此时夹角θ＝arctan(k₁)；

c.当轨迹线段斜率k₁＝(y_k-1-y_k)/(x_k-1-x_k)＜0时，此时夹角θ＝arctan|k₁|。

(2)当门表示的线段垂直于X轴时，此时：

a.当轨迹线段斜率不存在，即y_k＝y_k-1时，此时夹角θ＝0°；

b.当轨迹线段斜率k₁＝(y_k-1-y_k)/(x_k-1-x_k)≥0时，此时夹角θ＝90-arctan(k₁)；

c.当轨迹线段斜率k₁＝(y_k-1-y_k)/(x_k-1-x_k)＜0时，此时夹角θ＝90-arctan|k₁|。

(3)当门表示的线段斜率存在且为k₂时，此时：

a.当轨迹线段斜率不存在，即y_k＝y_k-1时，此时夹角θ＝90-arctan|k₂|；

b.当轨迹线段斜率为k₁时，此时夹角θ＝arctan|(k₁-k₂)/(1+k₁·k₂)|。

可选的，所述步骤(6)的具体过程为：

在完成粒子的状态更新后，没有错误的通过门进入到其他区域的粒子是合理的。若出现错误的跨区域，认为粒子的定位结果不合理，需要将该粒子的权重置为0。否则权重置为1。跨区域权重可表示如下:

粒子的最终权重可用粒子其他权重与跨区域权重的乘积表示，将权重值为0的粒子剔除后，需要对该权重进行归一化操作，得到新的权重：

得到归一化处理的粒子权重后，可以用该权重加权求和得到状态的估计值，从而获得最终的定位结果。状态的估计方程如下：

与现有技术相比，本发明的有益效果是：(1)本发明设计简单，算法思路清晰；(2)本发明在保证正确检测出穿门粒子的基础上，降低了算法的计算量；(3)本发明可以抑制基于粒子滤波的地图匹配算法在粒子更新过程中的错误跨区域现象，使得粒子分布更加合理。

附图说明

图1为本发明基于地图信息矩阵的跨区域检测算法流程图；

图2(a)-(b)为地图信息矩阵的建立过程，图2(a)为室内房间简图，图2(b)网格化处理后的图；

图3为粒子轨迹与门的相对位置关系；

图4(a)-(b)为循环遍历示意图，图4(a)为|k|≤1遍历示意图，图4(b)为|k|＞1遍历示意图；

图5为粒子轨迹与门的夹角关系；

图6(a)-(c)分别为θ＝30°、θ＝45°、θ＝60°时粒子可通行范围示意图；

图7为仿真场景设置；

图8为本发明跨区域检测算法中穿门检测与传统检测方法准确性对比图(仿真)；

图9为本发明跨区域检测算法中穿门检测与传统检测方法运行时间对比图(仿真)；

图10为本发明跨区域检测算法中不同夹角阈值的结果对比；

图11为实测场景设置；

图12为UWB实测轨迹；

图13为基于粒子滤波的地图匹配算法的粒子分布图(实测)；

图14为加入本发明跨区域检测算法后粒子分布图(实测)。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

图1是本发明流程图，具体步骤包括：

(1)根据室内地图建立地图信息矩阵；

(2)进行粒子滤波的状态更新，得到第i个粒子k-1时刻位置

和k时刻位置

并转换为地图信息矩阵的行列值

(3)循环检测粒子轨迹在地图信息矩阵里的元素值判断粒子轨迹是否穿过门；

(4)若检测到粒子轨迹通过门，计算粒子轨迹与门之间的夹角θ；

(5)与预设夹角阈值比较，若θ＞45°，粒子跨区域，否则粒子无效；

(6)将跨区域检测算法加入到粒子滤波算法中的权重更新部分，完成滤波。

本发明是基于地图信息矩阵的跨区域检测算法，在保证将错误跨区域的粒子检测出来的基础上，提高了算法的运算速度。针对基于粒子滤波的地图匹配算法在粒子更新过程中的错误跨区域现象，提出了一种跨区域检测算法。针对算法计算量大的问题，改进了地图建模过程，利用栅格化地图得到地图信息矩阵。本发明利用MATLAB仿真软件进行了仿真分析和实测验证。通过定量分析，与传统跨立实验法穿门检测算法进行对比，证明了基于地图信息矩阵的穿门检测算法的准确性与快速性；通过定性分析，与未加入跨区域检测的地图匹配算法进行对比，证明了本发明提出的跨区域检测算法的有效性。

本发明实施例用来解释本发明，而不是对本发明进行限制，在发明的精神和权利要求的保护范围内，对本发明做出的任何修改和改变，都落入本发明的保护范围。

一、对本发明进行仿真数据验证：

为验证本发明的跨区域检测算法性能，使用仿真数据进行了验证。图7为仿真场景设置，其中假设房间大小为10m×10m，网格长度为0.1m，包括24条墙壁线段、4个门。仿真过程中粒子数量为500，假设k时刻的位置坐标为(7.05,7.05)，如图7中红点所示，k-1时刻的粒子均匀分布在地图上，如图7中蓝点所示。

图8是本发明跨区域检测算法中穿门检测与传统检测方法的准确性对比图，通过设置标志位flag来表示穿门检测算法是否检测到粒子通过门。若flag＝0，表示第i个粒子在k-1时刻到k时刻的转移过程中没有通过门，该粒子未跨区域；若flag＝1，说明第i个粒子在k-1时刻到k时刻的更新过程中通过门，需要求粒子轨迹与门的夹角。仿真结果表明，两种方法的穿门检测结果一致，证明了基于地图信息矩阵的穿门检测算法的准确性。

图9为本发明跨区域检测算法中穿门检测与传统检测方法运行时间对比图，在仿真环境不变的条件下，改变仿真过程中粒子的数量，从而得到两种算法在不同粒子数下进行穿门检测所消耗的时间。仿真结果表明，基于地图信息矩阵的穿门检测算法的运行时间小于传统跨立实验法进行穿门检测，在门数较多的时候优势更大。综合对比可知，基于地图信息矩阵的穿门检测算法优于传统跨立实验穿门检测算法。

图10为本发明跨区域检测算法中不同夹角阈值的结果对比，在跨区域检测算法中，合理的夹角阈值可以提高算法的准确性，夹角阈值的选择需要综合考虑轨迹跨区域和不跨区域两种情况，即综合考虑图7所示仿真环境中4个门附近的跨区域情况。仿真过程中夹角阈值分别设置为30°、45°、60°三种情况，并进行了多次试验。结果表明，当粒子轨迹不跨区域时，随着夹角阈值的增大，跨区域检测算法的准确率也增大。当粒子轨迹跨区域时，随着夹角阈值的增大，跨区域检测算法的准确率减小。综合考虑后，选择45°作为跨区域检测算法的角度阈值。

二、对本发明进行实测数据验证：

图11为实测场景设置，实测实验地点为哈尔滨工程大学61号楼4003房间内。利用UWB技术得到初始定位结果，选择4003房间的东南角为坐标原点，X轴正方向指向北，Y轴正方向指向西，如图中深红色箭头方向。房间内共部署四个基站，四个基站的位置坐标分别为：S₁＝[0.2 0.4]，S₂＝[7.74 0.17]，S₃＝[7.6 11.75]，S₄＝[0.5 11.75]。在4003房间空旷区域搭建了房间简图，共有8条墙壁，4个门，墙壁信息如图中黑色线所示。行走轨迹如图中红色箭头所示。UWB的实测轨迹如图12所示。

图13是传统基于粒子滤波的地图匹配算法的粒子分布图，由于粒子更新过程中认为室内门等连接区域是可通行的，因此在粒子的更新过程中，总有部分粒子错误的通过门进入到其他区域。图14是加入本发明跨区域检测算法后粒子分布图，引入门对粒子的更新进行约束，将错误跨区域的粒子剔除后，粒子的分布更加合理。实测验证是定性分析，只能在直观上说明本发明的跨区域检测算法可将错误跨区域的粒子剔除掉，得到更为合理的粒子分布。

综上，为解决现有基于粒子滤波的地图匹配算法在粒子更新过程中粒子的错误跨区域问题，本发明提出了一种基于地图信息矩阵的跨区域检测算法，在保证将错误跨区域的粒子检测出来的基础上，提高算法的运算速度。针对基于粒子滤波的地图匹配算法在粒子更新过程中的错误跨区域现象，提出了一种跨区域检测算法。针对算法计算量大的问题，改进了地图建模过程，利用栅格化地图得到地图信息矩阵。本发明利用MATLAB仿真软件进行了仿真分析和实测验证。通过定量分析，与传统跨立实验法穿门检测算法进行对比，证明了基于地图信息矩阵的穿门检测算法的准确性与快速性；通过定性分析，与未加入跨区域检测的地图匹配算法进行对比，证明了本发明提出的跨区域检测算法的有效性。仿真结果表明，相比于现有的地图匹配算法，本发明可以进一步提高粒子分布的合理性，对进一步提高基于粒子滤波的地图匹配算法定位精度具有一定的参考意义。

Claims (5)

1.基于地图信息矩阵的跨区域检测算法，其特征在于，步骤如下：

步骤一：根据室内地图建立地图信息矩阵；

步骤二：进行粒子滤波的状态更新，得到第i个粒子k-1时刻位置

和k时刻位置

步骤三：循环检测粒子轨迹在地图信息矩阵里的元素值，判断粒子轨迹是否穿过门；

步骤四：计算粒子轨迹与门之间的夹角；

步骤五：与预设夹角阈值比较，正确判断粒子是否跨区域；

步骤六：将跨区域检测算法加入到粒子滤波算法中的权重更新部分，完成滤波。

2.根据权利要求1所述的基于地图信息矩阵的跨区域检测算法，其特征在于，步骤一具体包括：假设室内地图的总长度为L，宽度为W，选定的网格宽度为l，该地图总共可划分为m×n个网格，即该地图信息矩阵总共有m行n列，其中m,n可由下式求得：

式中，[·]是向上取整符号，某个位置通过上式求解得到该位置位于地图信息矩阵的第i行第j列，则其位置在矩阵中可表示为(i,j),i＝1,2,...,m,j＝1,2,...,n；根据该网格表示的地图信息，将地图数据转换成m行n列的地图信息矩阵Indoor；当所处位置(i,j)是墙壁时，Indoor(i,j)＝0；当所处位置(i,j)是桌椅等室内布局时，Indoor(i,j)＝1；当所处位置(i,j)是人们日常活动区域时，Indoor(i,j)＝2；当所处位置(i,j)是门等连接区域时，Indoor(i,j)＝3，将地图数据以数字的形式存储在地图信息矩阵里。

3.根据权利要求1所述的基于地图信息矩阵的跨区域检测算法，其特征在于，步骤三具体包括：假设k-1时刻第i个粒子的坐标为

则其在地图信息矩阵Indoor中所在的行列值为：

粒子的状态更新后，在k时刻第i个粒子的坐标为

其在地图信息矩阵Indoor中所在的行列值为：

根据k-1时刻和k时刻第i个粒子的相对位置关系，可以将整个区域划分为2个坐标轴与4个区域共六个部分，各部分循环检测如下：

(1)若

即k-1与k时刻坐标连线为x轴，令

若存在

则第i个粒子在k-1时刻到k时刻的更新过程中通过门，需要求粒子轨迹与门的夹角；

(2)若

即k-1与k时刻坐标连线为y轴，，令

若存在

则第i个粒子在k-1时刻到k时刻的更新过程中通过门，需要求粒子轨迹与门的夹角；

(3)令斜率

若|a|＜1且

此时进行列循环遍历；若始末位置网格为3，即

或

则第i个粒子通过门，需要求粒子轨迹与门的夹角；若不为3，令

则

当y方向发生变化时，相应的x方向变化Δx＝Δy/a，则

令p₁＝[x₁/l]，q₁＝[y₁/l]，p₂＝[x₂/l]，q₂＝[y₂/l]，若存在Indoor(p₁,q₁)＝3或Indoor(p₂,q₂)＝3，则说明第i个粒子在k-1时刻到k时刻的更新过程中通过门，需要求粒子轨迹与门的夹角；

(4)令斜率

若|a|＜1且

此时进行列循环遍历；若始末位置网格为3，即

或

则第i个粒子通过门，需要求粒子轨迹与门的夹角；若不为3，令

则

当y方向发生变化时，相应的x方向变化Δx＝Δy/a，则

令p₁＝[x₁/l]，q₁＝[y₁/l]，p₂＝[x₂/l]，q₂＝[y₂/l]，若存在Indoor(p₁,q₁)＝3或Indoor(p₂,q₂)＝3，则说明第i个粒子在k-1时刻到k时刻的更新过程中通过门，需要求粒子轨迹与门的夹角；

(5)令斜率

若|a|≥1且

此时进行行循环遍历；若始末位置网格为3，即

或

则第i个粒子通过门，需要求粒子轨迹与门的夹角；若不为3，令

则

当x方向发生变化时，相应的y方向变化Δy＝Δx·a，则

令p₁＝[x₁/l]，q₁＝[y₁/l]，p₂＝[x₂/l]，q₂＝[y₂/l]，若存在Indoor(p₁,q₁)＝3或Indoor(p₂,q₂)＝3，则说明第i个粒子在k-1时刻到k时刻的更新过程中通过门，需要求粒子轨迹与门的夹角；

(6)令斜率

若|a|≥1且

此时进行行循环遍历；若始末位置网格为3，即

或

则第i个粒子通过门，需要求粒子轨迹与门的夹角；若不为3，令

则

当x方向发生变化时，相应的y方向变化Δy＝Δx·a，则

令p₁＝[x₁/l]，q₁＝[y₁/l]，p₂＝[x₂/l]，q₂＝[y₂/l]，若存在Indoor(p₁,q₁)＝3或Indoor(p₂,q₂)＝3，则说明第i个粒子在k-1时刻到k时刻的更新过程中通过门，需要求粒子轨迹与门的夹角。

4.根据权利要求1所述的基于地图信息矩阵的跨区域检测算法，其特征在于，步骤四具体包括：检测到粒子通过门后，需要正确判断粒子是否跨区域；若轨迹确实跨区域，则粒子有效；若轨迹没有跨区域，则该粒子需要剔除；跨区域判断转换为求k-1时刻与k时刻粒子的坐标连线与门的夹角，并与预设的夹角阈值做比较；而粒子轨迹与门之间的夹角可抽象为两条线段之间的夹角；两条线段之间的夹角可分为以下三种情况：

(1)当门表示的线段平行于X轴时，此时：

a.当轨迹线段斜率不存在，即y_k＝y_k-1时，此时夹角θ＝90°；

b.当轨迹线段斜率k₁＝(y_k-1-y_k)/(x_k-1-x_k)≥0时，此时夹角θ＝arctan(k₁)；

c.当轨迹线段斜率k₁＝(y_k-1-y_k)/(x_k-1-x_k)＜0时，此时夹角θ＝arctan|k₁|；

(2)当门表示的线段垂直于X轴时，此时：

a.当轨迹线段斜率不存在，即y_k＝y_k-1时，此时夹角θ＝0°；

b.当轨迹线段斜率k₁＝(y_k-1-y_k)/(x_k-1-x_k)≥0时，此时夹角θ＝90-arctan(k₁)；

c.当轨迹线段斜率k₁＝(y_k-1-y_k)/(x_k-1-x_k)＜0时，此时夹角θ＝90-arctan|k₁|；

(3)当门表示的线段斜率存在且为k₂时，此时：

a.当轨迹线段斜率不存在，即y_k＝y_k-1时，此时夹角θ＝90-arctan|k₂|；

b.当轨迹线段斜率为k₁时，此时夹角θ＝arctan|(k₁-k₂)/(1+k₁·k₂)|。

5.根据权利要求1所述的基于地图信息矩阵的跨区域检测算法，其特征在于，步骤六具体包括：在完成粒子的状态更新后，没有错误的通过门进入到其他区域的粒子是合理的；若出现错误的跨区域，认为粒子的定位结果不合理，需要将该粒子的权重置为0‘’否则权重置为1；跨区域权重表示如下:

粒子的最终权重可用粒子其他权重与跨区域权重的乘积表示，将权重值为0的粒子剔除后，需要对该权重进行归一化操作，得到新的权重：

得到归一化处理的粒子权重后，用该权重加权求和得到状态的估计值，获得最终的定位结果，状态的估计方程如下：

Images