自定义算法过滤定位点，实现高德轨迹纠偏

自定义算法过滤定位点，实现高德轨迹纠偏

示例Demo：https://github.com/ShihooWang/TraceByAmap

一、背景

在做户外出行轨迹记录的时候，往往因为基站切换，导致定位点偏差过大，于是需要纠偏。高德地图提供了最新的纠偏算法：

然而，该API只适合将记录的行车轨迹点进行抽稀、纠偏操作，将轨迹匹配到道路上，提供平滑的绘制效果，并计算行驶里程（地图SDK V3.4.0以上支持）；也可以通过结合高德定位帮助您记录真实行车轨迹（地图SDK V5.1.0版本以上支持）。换言之，不适合操场，公园等地方。
效果是这样的：

经过几次的测试，发现这个API不适合我的场景。

二、需求

如下图所示 我需要在实际测试中将下图中红色异常点滤掉，只留取蓝色正常点。
这个轨迹来源于高德地图demo：Amap3DDemo。
我就用它的轨迹来模拟跳点，进行测试。

算法过程：

1. 我们的计算过程滞后运动过程，在经过点检验之后将有效点加入我们的距离计算过程。
2. 有效点的判断是根据与权重点的距离进行判断的。
3. 权重点的获取是通过前一个权威点与新的轨迹点根据一定权重得到的（一开始权威点选为首个轨迹点）。
4. 轨迹的运行过程中，有效权重点只有一个，代表的是一段有效轨迹的稳定点，如果出现疑似偏移点的点，则再生成一个，代表新的这个疑似偏移点为开始的点的权重，如果这个新的疑似偏移点之后的5个点都没有偏移，则说明这段轨迹有效，则加入有效轨迹点中并更新权重点诶该点。
5. 权重点代表着一段轨迹的最终稳定点，当它代表的轨迹点数小于5个则表明那段点可能是偏移点。则要被干掉。

这里选取的权重值为0.8：
W1 = P1 * 0.2 + P2 * 0.8

三、纠偏模型对异常情况的处理

图中的网格是定位间隔时间 2S 。假定连续的5个点的偏移值允许在误差范围内，则认定这5个点均为有效点。
误差范围可以根据不同的运动模式，选择不同的范围。比如走路 5m/s,跑步 10m/s， 骑行30m/s，登山 5m/s。

1.起始点有偏移

起始点为偏差过大的点，需要舍弃掉。

P1、P2为不可接受偏移点，但是在起始不知道，当p3出现之后，发现距离有点大，则为p3生成新的权重点w2，连续判断5个点没有偏差，说明这几个点的轨迹是正确的则采用，将w1代表的点干掉，并将w1更新到w2这里。

2.起始点无偏移，过程中有一个点偏移

红色位置P4，距离P3和P5均超过允许偏移值

通过权重点，可以将P4点滤掉，轨迹修复到P5正常点
这里有一个跳点p4，判定为可疑跳点之后，我们给他一个新的权重点w2,在接下来判断中发现p5相对于w2来说也是跳点，则把w2代表的点干掉，然后重新让他代表p5。在接下来的5次定位稳定，则将权重点更新为w1。

3.未采集到数据，造成的数据分割

中间一段数据缺失，整个数据分成两段。

所有的点均为正常点。

4.随机产生的偏移（偏移点少于连续5个）

随机出现P6、P7两个跳点

算法舍弃掉P6、P7两个点，从P8重新开始计算。

四、判断偏移点算法流程图

需要注意判断偏移值得时，需要考虑时间问题，这里每个定位点都使用本地时间戳。

五、代码实现

下面代码中使用的是高德定位，定