自定义算法过滤定位点，实现高德轨迹纠偏
示例Demo：https://github.com/ShihooWang/TraceByAmap
一、背景
在做户外出行轨迹记录的时候，往往因为基站切换，导致定位点偏差过大，于是需要纠偏。高德地图提供了最新的纠偏算法：
然而，该API只适合将记录的行车轨迹点进行抽稀、纠偏操作，将轨迹匹配到道路上，提供平滑的绘制效果，并计算行驶里程（地图SDK V3.4.0以上支持）；也可以通过结合高德定位帮助您记录真实行车轨迹（地图SDK V5.1.0版本以上支持）。换言之，不适合操场，公园等地方。
效果是这样的：
经过几次的测试，发现这个API不适合我的场景。
二、需求
如下图所示 我需要在实际测试中将下图中红色异常点滤掉，只留取蓝色正常点。
这个轨迹来源于高德地图demo：Amap3DDemo。
我就用它的轨迹来模拟跳点，进行测试。
算法过程：
- 我们的计算过程滞后运动过程，在经过点检验之后将有效点加入我们的距离计算过程。
- 有效点的判断是根据与权重点的距离进行判断的。
- 权重点的获取是通过前一个权威点与新的轨迹点根据一定权重得到的（一开始权威点选为首个轨迹点）。
- 轨迹的运行过程中，有效权重点只有一个，代表的是一段有效轨迹的稳定点，如果出现疑似偏移点的点，则再生成一个，代表新的这个疑似偏移点为开始的点的权重，如果这个新的疑似偏移点之后的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重新开始计算。
四、判断偏移点算法流程图
需要注意判断偏移值得时，需要考虑时间问题，这里每个定位点都使用本地时间戳。
五、代码实现
下面代码中使用的是高德定位，定位间隔2秒，AMapLocationClientOption属性设置如下：
private AMapLocationClientOption getDefaultOption(){
AMapLocationClientOption mOption = new AMapLocationClientOption();
mOption.setLocationMode(AMapLocationClientOption.AMapLocationMode.Hight_Accuracy);//可选，设置定位模式，可选的模式有高精度、仅设备、仅网络。默认为高精度模式
mOption.setGpsFirst(false);//可选，设置是否gps优先，只有在单次定位高精度定位模式下有效有效。默认关闭
mOption.setHttpTimeOut(10*1000);//可选，设置网络请求超时时间。默认为30秒。在仅设备模式下无效
mOption.setInterval(2000);//可选，设置定位间隔。默认为2秒
mOption.setNeedAddress(false);//可选，设置是否返回逆地理地址信息。默认是true
mOption.setOnceLocation(false);//可选，设置是否单次定位。默认是false
mOption.setOnceLocationLatest(false);//可选，设置是否等待wifi刷新，默认为false.如果设置为true,会自动变为单次定位，持续定位时不要使用
AMapLocationClientOption.setLocationProtocol(AMapLocationClientOption.AMapLocationProtocol.HTTP);//可选， 设置网络请求的协议。可选HTTP或者HTTPS。默认为HTTP
mOption.setSensorEnable(false);//可选，设置是否使用传感器。默认是false
mOption.setWifiScan(true); //可选，设置是否开启wifi扫描。默认为true，如果设置为false会同时停止主动刷新，停止以后完全依赖于系统刷新，定位位置可能存在误差
mOption.setLocationCacheEnable(false); //可选，设置是否使用缓存定位，默认为true
return mOption;
}
private BPLocationLatLng getLocation(AMapLocation aMapLocation,long currentStamp){
BPLocationLatLng locationLatLng = new BPLocationLatLng();
locationLatLng.latitude = aMapLocation.getLatitude();
locationLatLng.longitude = aMapLocation.getLongitude();
locationLatLng.altitude = (float) aMapLocation.getAltitude();
locationLatLng.timestamp = currentStamp;
locationLatLng.mapAccuracy = (int)( aMapLocation.getAccuracy()+0.5f);
locationLatLng.mapSatellites = aMapLocation.getSatellites();
return locationLatLng;
}
public class BPLocationLatLng implements Serializable {
public double latitude;
public double longitude;
public long timestamp;
public float altitude;
public int mapAccuracy;
public int mapSatellites;
}
纠偏算法：
private int SPORT_MAX_SPEED = 10; // 最大运动时长
private int isFirst = 0; // 是否是第一次定位点
private BPLocationLatLng weight1; // 权重点1
private BPLocationLatLng weight2; // 权重点2
private List<BPLocationLatLng> w1TempList = new ArrayList<>(); // w1的临时定位点集合
private List<BPLocationLatLng> w2TempList = new ArrayList<>(); // w2的临时定位点集合
private int w1Count = 0; // 统计w1Count所统计过的点数
/**
* 判断偏移点算法
* @param aMapLocation 高德定位返回
* @return 自定义稳定点的集合
*/
private List<BPLocationLatLng> filterPos(AMapLocation aMapLocation) {
long currentStamp = System.currentTimeMillis()/1000;
// String filterString = currentStamp +" :";
try {
// 获取的第一个定位点不进行过滤
if (isFirst < 2) {
isFirst ++;
if (isFirst < 2){
// filterString += "第一个定位点 : （容易漂移）不记录，不更新";
// 测试记录
weight1 = getLocation(aMapLocation,currentStamp);
return null;
}
float distance = AMapUtils.calculateLineDistance(
new LatLng(weight1.latitude, weight1.longitude),
new LatLng(aMapLocation.getLatitude(), aMapLocation.getLongitude()));
weight1 = getLocation(aMapLocation,currentStamp);
// filterString += "第二个定位点 : 与第一个点的距离："+distance+", 重新开始记录，不更新 ,";
// 将得到的第一个点存储入w1的缓存集合
final BPLocationLatLng traceLocation = getLocation(aMapLocation,currentStamp);
w1TempList.add(traceLocation);
w1Count++;
notifyFirstLocation(traceLocation);
return null;
} else {
// 开始运动后，只针对GPS定位点且当前可用卫星数量>=8时才有效
if (aMapLocation.getLocationType() == AMapLocation.LOCATION_TYPE_GPS && aMapLocation.getSatellites()>=8){
// filterString += "非第一次" + " : ";
// // 过滤静止时的偏点，在静止时速度小于1米就算做静止状态
if (weight2 == null) {
// filterString += "weight2=null" + " : ";
// 计算w1与当前定位点p1的时间差并得到最大偏移距离D
long offsetTime = currentStamp - weight1.timestamp;
if (offsetTime == 0){
return null;
}
long MaxDistance = offsetTime * SPORT_MAX_SPEED;
float distance = AMapUtils.calculateLineDistance(
new LatLng(weight1.latitude, weight1.longitude),
new LatLng(aMapLocation.getLatitude(), aMapLocation.getLongitude()));
// filterString += "distance=" + distance + ", MaxDistance=" + MaxDistance + " : ";
if (distance > MaxDistance) {
// filterString += " distance>MaxDistance" + " 当前点 距离大: 设置w2位新的点，并添加到w2TempList";
// 将设置w2位新的点，并存储入w2临时缓存
weight2 = getLocation(aMapLocation,currentStamp);
w2TempList.add(weight2);
return null;
} else {
// filterString += " distance<MaxDistance" + " 当前点 距离小 : 添加到w1TempList";
// 将p1加入到做坐标集合w1TempList
BPLocationLatLng traceLocation = getLocation(aMapLocation,currentStamp);
w1TempList.add(traceLocation);
w1Count++;
// 更新w1权值点
weight1.latitude = weight1.latitude * 0.2 + aMapLocation.getLatitude() * 0.8;
weight1.longitude = weight1.longitude * 0.2 + aMapLocation.getLongitude() * 0.8;
weight1.timestamp = currentStamp;
// if (w1TempList.size() > 3) {
// filterString += "d1TempList.size() > 3" + " : 更新";
// // 将w1TempList中的数据放入finalList，并将w1TempList清空
// mListPoint.addAll(w1TempList);
// w1TempList.clear();
// return true;
// } else {
// filterString += "d1TempList.size() < 3" + " : 不更新";
// return false;
// }
if (w1Count > 3){
// filterString += " : 更新";
List<BPLocationLatLng> list = new ArrayList<>(w1TempList);
w1TempList.clear();
return list;
}else {
// filterString += " w1Count<3: 不更新";
return null;
}
}
} else {
// filterString += "weight2 != null" + " : ";
// 计算w2与当前定位点p1的时间差并得到最大偏移距离D
long offsetTimes = currentStamp - weight2.timestamp;
long MaxDistance = offsetTimes * SPORT_MAX_SPEED;
float distance = AMapUtils.calculateLineDistance(
new LatLng(weight2.latitude, weight2.longitude),
new LatLng(aMapLocation.getLatitude(), aMapLocation.getLongitude()));
// filterString += "distance = " + distance + ",MaxDistance = " + MaxDistance + " : ";
if (distance > MaxDistance) {
// filterString += "当前点 距离大: weight2 更新";
w2TempList.clear();
// 将设置w2位新的点，并存储入w2临时缓存
weight2 = getLocation(aMapLocation,currentStamp);
w2TempList.add(weight2);
return null;
} else {
// filterString += "当前点 距离小: 添加到w2TempList";
// 将p1加入到做坐标集合w2TempList
BPLocationLatLng traceLocation = getLocation(aMapLocation,currentStamp);
w2TempList.add(traceLocation);
// 更新w2权值点
weight2.latitude = weight2.latitude * 0.2 + aMapLocation.getLatitude() * 0.8;
weight2.longitude = (weight2.longitude * 0.2 + aMapLocation.getLongitude() * 0.8);
weight2.timestamp = currentStamp;
if (w2TempList.size() > 4) {
List<BPLocationLatLng> list = new ArrayList<>();
// 判断w1所代表的定位点数是否>4,小于说明w1之前的点为从一开始就有偏移的点
if (w1Count > 4) {
// filterString += "w1Count > 4" + "计算增加W1";
list.addAll(w1TempList);
} else {
// filterString += "w1Count < 4" + "计算丢弃W1";
w1TempList.clear();
}
// filterString += "w2TempList.size() > 4" + " : 更新到偏移点";
// 将w2TempList集合中数据放入finalList中
list.addAll(w2TempList);
// 1、清空w2TempList集合 2、更新w1的权值点为w2的值 3、将w2置为null
w2TempList.clear();
weight1 = weight2;
weight2 = null;
return list;
} else {
// filterString += "w2TempList.size() < 4";
return null;
}
}
}
}else {
LogUtil.wshLog().d("定位成功" + ", 类型："+aMapLocation.getLocationType() + " ----- 丢弃当前定位点 ---");
return null;
}
}
} finally {
// StringBuilder sb = new StringBuilder();
// if(aMapLocation.getErrorCode() == 0){
// sb.append("定位成功" + ", 类型："+aMapLocation.getLocationType()+"，");
// sb.append("定位时间: ").append(AmapLLLLUtils.formatUTC(aMapLocation.getTime(), "yyyy-MM-dd HH:mm:ss")).append(", ");
// } else {
// //定位失败
// sb.append("定位失败" + ", ");
// sb.append("错误码:").append(aMapLocation.getErrorCode()).append(", ");
// sb.append("错误信息:").append(aMapLocation.getErrorInfo()).append(", ");
// sb.append("错误描述:").append(aMapLocation.getLocationDetail()).append(", ");
// }
// sb.append("---定位质量报告").append(": ");
// sb.append("WIFI开关：").append(aMapLocation.getLocationQualityReport().isWifiAble() ? "开启":"关闭").append(", ");
// sb.append("GPS状态：").append(getGPSStatusString(aMapLocation.getLocationQualityReport().getGPSStatus())).append(", ");
// sb.append("GPS星数：").append(aMapLocation.getLocationQualityReport().getGPSSatellites()).append(", ");
// sb.append("网络类型：").append(aMapLocation.getLocationQualityReport().getNetworkType()).append(", ");
// sb.append("网络耗时：").append(aMapLocation.getLocationQualityReport().getNetUseTime()).append(", ");
// filterString += ",------ "+sb.toString();
// BPOutdoorLocationLogUtils.handleLogData(String.valueOf(startStamp),filterString);
// LogUtil.wshLog().d(filterString);
}
}
测试时，可以将注释打开，打印Log输出。
六、测试、分析数据
借助高德demo中现有的定位点，手动模拟跳点，实现异常数据。
步骤：
- 在算法的每个关键步骤和分支判断加入记录，并保持到本地方便后续分析。
- 通过分析步骤之后确保算法大概的正确性。
- 通过定时任务模拟运动过程，重现轨迹运行过程，这时候因为有了真实数据，我们就可以将文件存储的结果，变成log打印来分析。
- 通过改变真实数据的各个值去模拟我们可能遇到的偏移点。（自定义生成跳点）
查看示例详情，请移步到：https://github.com/ShihooWang/TraceByAmap
欢迎交流~
432
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
