对轨迹偏离进行优化
package com.james.motion.sport_motion;
import com.amap.api.maps.AMapUtils;
import com.amap.api.maps.model.LatLng;
import java.util.ArrayList;
import java.util.List;
/**
* 轨迹优化工具类
* <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 = 1.0f;
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) {
synchronized (this) {
List<LatLng> list = removeNoisePoint(originlist);//去噪
List<LatLng> afterList = kalmanFilterPath(list, mIntensity);//滤波
return reducerVerticalThreshold(afterList, mThreshhold);//抽稀
}
}
/**
* 轨迹线路滤波
*
* @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) {
synchronized (this) {
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) {
synchronized (this) {
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) {
synchronized (this) {
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;
}
}
}