请注意,本文编写于  103  天前,最后编辑于  103  天前,内容可能已经不具有时效性,请谨慎参考。

轨迹平滑优化算法:

去噪 -> 滤波 -> 抽稀

  1. 去噪
    轨迹去噪,删除垂距大于mNoiseThreshholdm的点

  2. 滤波
    轨迹多点滤波,卡尔曼滤波

  3. 抽稀
    抽稀算法,删除垂距小于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
}


Q.E.D.

知识共享署名-非商业性使用-相同方式共享 4.0 国际许可协议

Do what i love and just do it !