轨迹平滑优化算法：
去噪 -> 滤波 -> 抽稀
-
去噪
轨迹去噪，删除垂距大于mNoiseThreshholdm的点 -
滤波
轨迹多点滤波，卡尔曼滤波 -
抽稀
抽稀算法，删除垂距小于mThreshhold的点
Amap PathSmoothTool 源码
import android.util.Log;
import com.amap.api.maps.AMapUtils;
import com.amap.api.maps.model.LatLng;
import java.util.ArrayList;
import java.util.List;
/**
* 轨迹优化工具类 Android
* <p>
* 使用方法：
* <p>
* PathSmoothTool pathSmoothTool = new PathSmoothTool();
* pathSmoothTool.setIntensity(2);//设置滤波强度，默认3
* List<LatLng> mList = LatpathSmoothTool.kalmanFilterPath(list);
*/
public class PathSmoothTool {
private int mIntensity = 3;
private float mThreshhold = 0.3f;
private float mNoiseThreshhold = 10;
public PathSmoothTool(){
}
public int getIntensity() {
return mIntensity;
}
public void setIntensity(int mIntensity) {
this.mIntensity = mIntensity;
}
public float getThreshhold() {
return mThreshhold;
}
public void setThreshhold(float mThreshhold) {
this.mThreshhold = mThreshhold;
}
public void setNoiseThreshhold(float mnoiseThreshhold) {
this.mNoiseThreshhold = mnoiseThreshhold;
}
/**
* 轨迹平滑优化
* @param originlist 原始轨迹list,list.size大于2
* @return 优化后轨迹list
*/
public List<LatLng> pathOptimize(List<LatLng> originlist){
List<LatLng> list = removeNoisePoint(originlist);//去噪
List<LatLng> afterList = kalmanFilterPath(list,mIntensity);//滤波
List<LatLng> pathoptimizeList = reducerVerticalThreshold(afterList,mThreshhold);//抽稀
// Log.i("MY","originlist: "+originlist.size());
// Log.i("MY","list: "+list.size());
// Log.i("MY","afterList: "+afterList.size());
// Log.i("MY","pathoptimizeList: "+pathoptimizeList.size());
return pathoptimizeList;
}
/**
* 轨迹线路滤波
* @param originlist 原始轨迹list,list.size大于2
* @return 滤波处理后的轨迹list
*/
public List<LatLng> kalmanFilterPath(List<LatLng> originlist) {
return kalmanFilterPath(originlist,mIntensity);
}
/**
* 轨迹去噪，删除垂距大于20m的点
* @param originlist 原始轨迹list,list.size大于2
* @return
*/
public List<LatLng> removeNoisePoint(List<LatLng> originlist){
return reduceNoisePoint(originlist,mNoiseThreshhold);
}
/**
* 单点滤波
* @param lastLoc 上次定位点坐标
* @param curLoc 本次定位点坐标
* @return 滤波后本次定位点坐标值
*/
public LatLng kalmanFilterPoint(LatLng lastLoc, LatLng curLoc) {
return kalmanFilterPoint(lastLoc,curLoc,mIntensity);
}
/**
* 轨迹抽稀
* @param inPoints 待抽稀的轨迹list，至少包含两个点，删除垂距小于mThreshhold的点
* @return 抽稀后的轨迹list
*/
public List<LatLng> reducerVerticalThreshold(List<LatLng> inPoints) {
return reducerVerticalThreshold(inPoints,mThreshhold);
}
/********************************************************************************************************/
/**
* 轨迹线路滤波
* @param originlist 原始轨迹list,list.size大于2
* @param intensity 滤波强度（1—5）
* @return
*/
private List<LatLng> kalmanFilterPath(List<LatLng> originlist,int intensity) {
List<LatLng> kalmanFilterList = new ArrayList<LatLng>();
if (originlist == null || originlist.size() <= 2)
return kalmanFilterList;
initial();//初始化滤波参数
LatLng latLng = null;
LatLng lastLoc = originlist.get(0);
kalmanFilterList.add(lastLoc);
for (int i = 1; i < originlist.size(); i++) {
LatLng curLoc = originlist.get(i);
latLng = kalmanFilterPoint(lastLoc,curLoc,intensity);
if (latLng != null) {
kalmanFilterList.add(latLng);
lastLoc = latLng;
}
}
return kalmanFilterList;
}
/**
* 单点滤波
* @param lastLoc 上次定位点坐标
* @param curLoc 本次定位点坐标
* @param intensity 滤波强度（1—5）
* @return 滤波后本次定位点坐标值
*/
private LatLng kalmanFilterPoint(LatLng lastLoc, LatLng curLoc, int intensity) {
if (pdelt_x == 0 || pdelt_y == 0 ){
initial();
}
LatLng kalmanLatlng = null;
if (lastLoc == null || curLoc == null){
return kalmanLatlng;
}
if (intensity < 1){
intensity = 1;
} else if (intensity > 5){
intensity = 5;
}
for (int j = 0; j < intensity; j++){
kalmanLatlng = kalmanFilter(lastLoc.longitude,curLoc.longitude,lastLoc.latitude,curLoc.latitude);
curLoc = kalmanLatlng;
}
return kalmanLatlng;
}
/***************************卡尔曼滤波开始********************************/
private double lastLocation_x; //上次位置
private double currentLocation_x;//这次位置
private double lastLocation_y; //上次位置
private double currentLocation_y;//这次位置
private double estimate_x; //修正后数据
private double estimate_y; //修正后数据
private double pdelt_x; //自预估偏差
private double pdelt_y; //自预估偏差
private double mdelt_x; //上次模型偏差
private double mdelt_y; //上次模型偏差
private double gauss_x; //高斯噪音偏差
private double gauss_y; //高斯噪音偏差
private double kalmanGain_x; //卡尔曼增益
private double kalmanGain_y; //卡尔曼增益
private double m_R= 0;
private double m_Q= 0;
//初始模型
private void initial(){
pdelt_x = 0.001;
pdelt_y = 0.001;
// mdelt_x = 0;
// mdelt_y = 0;
mdelt_x = 5.698402909980532E-4;
mdelt_y = 5.698402909980532E-4;
}
private LatLng kalmanFilter(double oldValue_x, double value_x, double oldValue_y, double value_y){
lastLocation_x = oldValue_x;
currentLocation_x= value_x;
gauss_x = Math.sqrt(pdelt_x * pdelt_x + mdelt_x * mdelt_x)+m_Q; //计算高斯噪音偏差
kalmanGain_x = Math.sqrt((gauss_x * gauss_x)/(gauss_x * gauss_x + pdelt_x * pdelt_x)) +m_R; //计算卡尔曼增益
estimate_x = kalmanGain_x * (currentLocation_x - lastLocation_x) + lastLocation_x; //修正定位点
mdelt_x = Math.sqrt((1-kalmanGain_x) * gauss_x *gauss_x); //修正模型偏差
lastLocation_y = oldValue_y;
currentLocation_y = value_y;
gauss_y = Math.sqrt(pdelt_y * pdelt_y + mdelt_y * mdelt_y)+m_Q; //计算高斯噪音偏差
kalmanGain_y = Math.sqrt((gauss_y * gauss_y)/(gauss_y * gauss_y + pdelt_y * pdelt_y)) +m_R; //计算卡尔曼增益
estimate_y = kalmanGain_y * (currentLocation_y - lastLocation_y) + lastLocation_y; //修正定位点
mdelt_y = Math.sqrt((1-kalmanGain_y) * gauss_y * gauss_y); //修正模型偏差
LatLng latlng = new LatLng(estimate_y,estimate_x);
return latlng;
}
/***************************卡尔曼滤波结束**********************************/
/***************************抽稀算法*************************************/
private List<LatLng> reducerVerticalThreshold(List<LatLng> inPoints,
float threshHold) {
if (inPoints == null) {
return null;
}
if (inPoints.size() <= 2) {
return inPoints;
}
List<LatLng> ret = new ArrayList<LatLng>();
for (int i = 0; i < inPoints.size(); i++) {
LatLng pre = getLastLocation(ret);
LatLng cur = inPoints.get(i);
if (pre == null || i == inPoints.size() - 1) {
ret.add(cur);
continue;
}
LatLng next = inPoints.get(i + 1);
double distance = calculateDistanceFromPoint(cur, pre, next);
if (distance > threshHold){
ret.add(cur);
}
}
return ret;
}
private static LatLng getLastLocation(List<LatLng> oneGraspList) {
if (oneGraspList == null || oneGraspList.size() == 0) {
return null;
}
int locListSize = oneGraspList.size();
LatLng lastLocation = oneGraspList.get(locListSize - 1);
return lastLocation;
}
/**
* 计算当前点到线的垂线距离
* @param p 当前点
* @param lineBegin 线的起点
* @param lineEnd 线的终点
*
*/
private static double calculateDistanceFromPoint(LatLng p, LatLng lineBegin,
LatLng lineEnd) {
double A = p.longitude - lineBegin.longitude;
double B = p.latitude - lineBegin.latitude;
double C = lineEnd.longitude - lineBegin.longitude;
double D = lineEnd.latitude - lineBegin.latitude;
double dot = A * C + B * D;
double len_sq = C * C + D * D;
double param = dot / len_sq;
double xx, yy;
if (param < 0 || (lineBegin.longitude == lineEnd.longitude
&& lineBegin.latitude == lineEnd.latitude)) {
xx = lineBegin.longitude;
yy = lineBegin.latitude;
// return -1;
} else if (param > 1) {
xx = lineEnd.longitude;
yy = lineEnd.latitude;
// return -1;
} else {
xx = lineBegin.longitude + param * C;
yy = lineBegin.latitude + param * D;
}
return AMapUtils.calculateLineDistance(p,new LatLng(yy,xx));
}
/***************************抽稀算法结束*********************************/
private List<LatLng> reduceNoisePoint(List<LatLng> inPoints, float threshHold) {
if (inPoints == null) {
return null;
}
if (inPoints.size() <= 2) {
return inPoints;
}
List<LatLng> ret = new ArrayList<LatLng>();
for (int i = 0; i < inPoints.size(); i++) {
LatLng pre = getLastLocation(ret);
LatLng cur = inPoints.get(i);
if (pre == null || i == inPoints.size() - 1) {
ret.add(cur);
continue;
}
LatLng next = inPoints.get(i + 1);
double distance = calculateDistanceFromPoint(cur, pre, next);
if (distance < threshHold){
ret.add(cur);
}
}
return ret;
}
}
web js实现：
function LatLng(var1, var3, var5 = true) {
if (!var5) {
return {
"latitude": var1,
"longitude": var3
}
} else {
let longitude = null, latitude = null
if (-180.0 <= var3 && var3 < 180.0) {
longitude = var3;
} else {
longitude = ((var3 - 180.0) % 360.0 + 360.0) % 360.0 - 180.0;
}
if (var1 < -90.0 || var1 > 90.0) {
console.error("非法坐标值")
throw new Error('非法坐标值')
}
latitude = Math.max(-90.0, Math.min(90.0, var1));
return {
"latitude": latitude,
"longitude": longitude
}
}
}
let mIntensity = 5;
let mThreshhold = 0.3;
let mNoiseThreshhold = 10;
/**
* 轨迹平滑优化
* @param originlist 原始轨迹list,list.size大于2
* @return 优化后轨迹list
*/
function pathOptimize(originlist) {
let list = removeNoisePoint(originlist);//去噪
let afterList = kalmanFilterPath(list, mIntensity);//滤波
let pathoptimizeList = reducerVerticalThreshold(afterList, mThreshhold);//抽稀
// Log.i("MY","originlist: "+originlist.size());
// Log.i("MY","list: "+list.size());
// Log.i("MY","afterList: "+afterList.size());
// Log.i("MY","pathoptimizeList: "+pathoptimizeList.size());
return pathoptimizeList;
}
/**
* 轨迹去噪，删除垂距大于20m的点
* @param originlist 原始轨迹list,list.size大于2
* @return
*/
function removeNoisePoint(originlist) {
return reduceNoisePoint(originlist, mNoiseThreshhold);
}
function reduceNoisePoint(inPoints, threshHold) {
if (inPoints == null) {
return null;
}
if (inPoints.length <= 2) {
return inPoints;
}
let ret = []
for (let i = 0; i < inPoints.length; i++) {
let pre = getLastLocation(ret);
let cur = inPoints[i];
if (pre == null || i == inPoints.length - 1) {
ret.push(cur);
continue;
}
let next = inPoints[i + 1];
let distance = calculateDistanceFromPoint(cur, pre, next);
if (distance < threshHold) {
ret.push(cur);
}
}
return ret;
}
function getLastLocation(oneGraspList) {
if (oneGraspList == null || oneGraspList.length == 0) {
return null;
}
let locListSize = oneGraspList.length;
let lastLocation = oneGraspList[locListSize - 1];
return lastLocation;
}
/**
* 计算当前点到线的垂线距离
* @param p 当前点
* @param lineBegin 线的起点
* @param lineEnd 线的终点
*
*/
function calculateDistanceFromPoint(p, lineBegin, lineEnd) {
let A = p.longitude - lineBegin.longitude;
let B = p.latitude - lineBegin.latitude;
let C = lineEnd.longitude - lineBegin.longitude;
let D = lineEnd.latitude - lineBegin.latitude;
let dot = A * C + B * D;
let len_sq = C * C + D * D;
let param = dot / len_sq;
let xx, yy;
if (param < 0 || (lineBegin.longitude == lineEnd.longitude
&& lineBegin.latitude == lineEnd.latitude)) {
xx = lineBegin.longitude;
yy = lineBegin.latitude;
// return -1;
} else if (param > 1) {
xx = lineEnd.longitude;
yy = lineEnd.latitude;
// return -1;
} else {
xx = lineBegin.longitude + param * C;
yy = lineBegin.latitude + param * D;
}
return calculateLineDistance(p, LatLng(yy, xx));
}
function calculateLineDistance(var0, var1) {
if (var0 != null && var1 != null) {
try {
let var2 = var0.longitude;
let var4 = var0.latitude;
let var6 = var1.longitude;
let var8 = var1.latitude;
var2 *= 0.01745329251994329;
var4 *= 0.01745329251994329;
var6 *= 0.01745329251994329;
var8 *= 0.01745329251994329;
let var10 = Math.sin(var2);
let var12 = Math.sin(var4);
let var14 = Math.cos(var2);
let var16 = Math.cos(var4);
let var18 = Math.sin(var6);
let var20 = Math.sin(var8);
let var22 = Math.cos(var6);
let var24 = Math.cos(var8);
let var28 = [];
let var29 = [];
var28[0] = var16 * var14;
var28[1] = var16 * var10;
var28[2] = var12;
var29[0] = var24 * var22;
var29[1] = var24 * var18;
var29[2] = var20;
return (Math.asin(Math.sqrt((var28[0] - var29[0]) * (var28[0] - var29[0]) + (var28[1] - var29[1]) * (var28[1] - var29[1]) + (var28[2] - var29[2]) * (var28[2] - var29[2])) / 2.0) * 1.27420015798544E7);
} catch (var26) {
console.error(var26)
return 0.0;
}
} else {
console.error("非法坐标值");
return 0.0;
}
}
/***************************卡尔曼滤波开始********************************/
let lastLocation_x; //上次位置
let currentLocation_x;//这次位置
let lastLocation_y; //上次位置
let currentLocation_y;//这次位置
let estimate_x; //修正后数据
let estimate_y; //修正后数据
let pdelt_x; //自预估偏差
let pdelt_y; //自预估偏差
let mdelt_x; //上次模型偏差
let mdelt_y; //上次模型偏差
let gauss_x; //高斯噪音偏差
let gauss_y; //高斯噪音偏差
let kalmanGain_x; //卡尔曼增益
let kalmanGain_y; //卡尔曼增益
let m_R = 0;
let m_Q = 0;
//初始模型
function initial() {
pdelt_x = 0.001;
pdelt_y = 0.001;
// mdelt_x = 0;
// mdelt_y = 0;
mdelt_x = 5.698402909980532E-4;
mdelt_y = 5.698402909980532E-4;
}
/**
* 轨迹线路滤波
* @param originlist 原始轨迹list,list.size大于2
* @param intensity 滤波强度（1—5）
* @return
*/
function kalmanFilterPath(originlist, intensity) {
let kalmanFilterList = []
if (originlist == null || originlist.length <= 2)
return kalmanFilterList;
initial();//初始化滤波参数
let latLng = null;
let lastLoc = originlist[0];
kalmanFilterList.push(lastLoc);
for (let i = 1; i < originlist.length; i++) {
let curLoc = originlist[i];
latLng = kalmanFilterPoint(lastLoc, curLoc, intensity);
if (latLng != null) {
kalmanFilterList.push(latLng);
lastLoc = latLng;
}
}
return kalmanFilterList;
}
/**
* 单点滤波
* @param lastLoc 上次定位点坐标
* @param curLoc 本次定位点坐标
* @param intensity 滤波强度（1—5）
* @return 滤波后本次定位点坐标值
*/
function kalmanFilterPoint(lastLoc, curLoc, intensity) {
if (pdelt_x == 0 || pdelt_y == 0) {
initial();
}
let kalmanLatlng = null;
if (lastLoc == null || curLoc == null) {
return kalmanLatlng;
}
if (intensity < 1) {
intensity = 1;
} else if (intensity > 5) {
intensity = 5;
}
for (let j = 0; j < intensity; j++) {
kalmanLatlng = kalmanFilter(lastLoc.longitude, curLoc.longitude, lastLoc.latitude, curLoc.latitude);
curLoc = kalmanLatlng;
}
return kalmanLatlng;
}
function kalmanFilter(oldValue_x, value_x, oldValue_y, value_y) {
lastLocation_x = oldValue_x;
currentLocation_x = value_x;
gauss_x = Math.sqrt(pdelt_x * pdelt_x + mdelt_x * mdelt_x) + m_Q; //计算高斯噪音偏差
kalmanGain_x = Math.sqrt((gauss_x * gauss_x) / (gauss_x * gauss_x + pdelt_x * pdelt_x)) + m_R; //计算卡尔曼增益
estimate_x = kalmanGain_x * (currentLocation_x - lastLocation_x) + lastLocation_x; //修正定位点
mdelt_x = Math.sqrt((1 - kalmanGain_x) * gauss_x * gauss_x); //修正模型偏差
lastLocation_y = oldValue_y;
currentLocation_y = value_y;
gauss_y = Math.sqrt(pdelt_y * pdelt_y + mdelt_y * mdelt_y) + m_Q; //计算高斯噪音偏差
kalmanGain_y = Math.sqrt((gauss_y * gauss_y) / (gauss_y * gauss_y + pdelt_y * pdelt_y)) + m_R; //计算卡尔曼增益
estimate_y = kalmanGain_y * (currentLocation_y - lastLocation_y) + lastLocation_y; //修正定位点
mdelt_y = Math.sqrt((1 - kalmanGain_y) * gauss_y * gauss_y); //修正模型偏差
let latlng = LatLng(estimate_y, estimate_x);
return latlng;
}
function reducerVerticalThreshold(inPoints, threshHold) {
if (inPoints == null) {
return null;
}
if (inPoints.length <= 2) {
return inPoints;
}
let ret = []
for (let i = 0; i < inPoints.length; i++) {
let pre = getLastLocation(ret);
let cur = inPoints[i];
if (pre == null || i == inPoints.length - 1) {
ret.push(cur);
continue;
}
let next = inPoints[i + 1];
let distance = calculateDistanceFromPoint(cur, pre, next);
if (distance > threshHold) {
ret.push(cur);
}
}
return ret;
}
export {
pathOptimize
}