解决GPS漂移主要从两方面入手:
一、 主系统的设计主要减少在近距离内对GPS信号的干扰。
二、 软件处理。软件处理主要集中在导航软件处完成,导航软件会将坐标定位在道路之内,如果GPS接收到的信号超出道路的半径范围将自动过滤这个数据,并根据上次的速度及方向推算出当前点的位置。
对于GPS静态漂移,也有建议做软件判断:
1. 检测到的状态为静止时,强制速度为0;
2. 速度为0时,强制方向为0;
3. 数据中的速度值为0时,就不去更新地图上的经纬度;
4. 通过比较上次定位数据的经纬度差的绝对值(同时包括时间)再来判定是否有慢速移动;
5. 对于车载终端,只能通过辅助手段来解决GPS静态漂移的问题,如通过检测ACC钥匙电的方法来检测是否为静态漂移,因为钥匙电是关闭时,车一定是不动的了,另外有些GPS模块(UBLOX)可设置静止模式、行走模式、汽车模式、海上模式、飞行模式,通过设置这些参数来解决漂移问题。
二、坐标系统的转换
模块拿到的数据转换成地图坐标系的数据:
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <termios.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <stdlib.h>
#include <math.h>
#include <stdbool.h>
static double x_pi = 3.14159265358979324 * 3000.0 / 180.0;
//高德转百度
static int bd_encrypt(double gg_lat, double gg_lon, double *bd_lat,
double *bd_lon)
{
double x = gg_lon, y = gg_lat;
double z = sqrt(x * x + y * y) + 0.00002 * sin(y * x_pi);
double theta = atan2(y, x) + 0.000003 * cos(x * x_pi);
*bd_lon = z * cos(theta) + 0.0065;
*bd_lat = z * sin(theta) + 0.006;
return 0;
}
// 百度转高德
static void bd_decrypt(double bd_lat, double bd_lon, double *gg_lat,
double *gg_lon)
{
double x = bd_lon - 0.0065, y = bd_lat - 0.006;
double z = sqrt(x * x + y * y) - 0.00002 * sin(y * x_pi);
double theta = atan2(y, x) - 0.000003 * cos(x * x_pi);
*gg_lon = z * cos(theta);
*gg_lat = z * sin(theta);
}
static double transformlat(double x, double y)
{
double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y +
0.2 * sqrt(abs(x));
ret += (20.0 * sin(6.0 * x * PI) + 20.0 * sin(2.0 * x * PI)) * 2.0 / 3.0;
ret += (20.0 * sin(y * PI) + 40.0 * sin(y / 3.0 * PI)) * 2.0 / 3.0;
ret += (160.0 * sin(y / 12.0 * PI) + 320 * sin(y * PI / 30.0)) * 2.0 / 3.0;
return ret;
}
static double transformlon(double x, double y)
{
double ret =
300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * sqrt(abs(x));
ret += (20.0 * sin(6.0 * x * PI) + 20.0 * sin(2.0 * x * PI)) * 2.0 / 3.0;
ret += (20.0 * sin(x * PI) + 40.0 * sin(x / 3.0 * PI)) * 2.0 / 3.0;
ret +=
(150.0 * sin(x / 12.0 * PI) + 300.0 * sin(x / 30.0 * PI)) * 2.0 / 3.0;
return ret;
}
// GPS转高德(火星坐标系)
int transform(double wglat, double wglon, double *mglat, double *mglon)
{
const double a = 6378245.0;
const double ee = 0.00669342162296594323;
double dlat = transformlat(wglon - 105.0, wglat - 35.0);
double dlon = transformlon(wglon - 105.0, wglat - 35.0);
double radlat = wglat / 180.0 * PI;
double magic = sin(radlat);
magic = 1 - ee * magic * magic;
double sqrtmagic = sqrt(magic);
dlat = (dlat * 180.0) / ((a * (1 - ee)) / (magic * sqrtmagic) * PI);
dlon = (dlon * 180.0) / (a / sqrtmagic * cos(radlat) * PI);
*mglat = wglat + dlat;
*mglon = wglon + dlon;
return 0;
}
转完之后就可以在地图上输入经纬度定位了,另外编译的时候需要加上 -lm参数。
简单的算法:
class test{
private LocationPojo preLocation;
private List<LocationPojo> nowLocation;
private Long preTime;
public boolean test(List<LocationPojo> now,LocationPojo pre) {
this.nowLocation = now;
this.preLocation = pre;
double distance = 0;//两点坐标点距离
int tmp = 40;//精准度上行初始阀值(固定)
int AccuracyThresholdUp = tmp;//精准度上行阀值
int AccuracyThresholdDown = 30;//精准度下行阀值
int stopCount = 0; //静止状态坐标计数
int rectCountDown = 0; //坐标在30M围栏内计数
int rectCountUp = 0; //坐标在100M围栏外计数
int notCheckUpCount = 0; //超出大围栏计数,不检查精准度
/*
*
* 如果没有上一次的GPS数据,那么直接返回这次的GPS数据。
**/
if (this.preLocation() == null){
this.preLocation(this.nowLocation.get(0));
this.preTime = this.preLocation().getAddTime();//上一次记录的时间
return true;
}
LocationPojo b = null;
//循环计数(我这边是每次定位间隔是1秒,每次定位数据达到10条后进入计算,所以有这个循环)
//就是用10条现在的GPS数据与上一次的GPS数据,进行数据计算。
for (LocationPojo pojo:this.nowLocation){
if (b == null){
b = pojo;
}
//判断不是GPS数据,如果不是,改变阀值的上下值
if (pojo.getProvider().equals(GPS.GPS)) {
AccuracyThresholdUp = (int)(tmp * 1.5);//网络定位普遍在40以上,所以需要改变精准度的阀值。
}else{
AccuracyThresholdUp = tmp;//由于是循环的,所以每次都需要重新赋值。
}
//没有速度,或者有速度但是精准度很高,我会把这类的数据归于静止状态(GPS漂移数据)
if (pojo.getSpeed() <= 0 || (pojo.getSpeed() > 0 && pojo.getAccuracy() > AccuracyThresholdDown)){
stopCount++;
}
//测算距离(测算距离的方法有很多,我现在把它封装成工具类了)
distance = CommUtils.getLocationDistance(pojo.getLatitude(),pojo.getLongitude(),preLocation.getLatitude(),preLocation.getLongitude());
//优化速度精准度
if(pojo.getSpeed() > 0 && distance > 0){
//距离 / 时间 * 3.6 = 速度(KM)
// float speed = CommUtils.fromatNumber(distance / ((pojo.getAddTime() - this.preTime) / 1000) * 3.6,null);
// pojo.setSpeed(speed); pojo.setSpeed(CommUtils.formatNumber(pojo.getSpeed().doubleValue(),"#0.00").floatValue());
}
//latlnt电子围栏 30 - 100m
//超出围栏(条件是:lat或者lnt与上一次坐标匹配大于[100m]并且精确度在30m以内,条件成立)
if (distance > 100){
notCheckUpCount++;
//高精准度(GPS数据应该是可靠的)
if(pojo.getAccuracy() < AccuracyThresholdUp){
rectCountUp++;
//如果上一次GPS精准度大于这一次,那么次数GPS数据是有效的。
if(pojo.getAccuracy() <= preLocation.getAccuracy()){
b = pojo;
}
}
}else if (distance > 30 && pojo.getAccuracy() < AccuracyThresholdUp){
//如果在电子围栏内,并且精确度在30m以内,条件成立
rectCountDown++;
if(pojo.getAccuracy() <= preLocation.getAccuracy()){
b = pojo;
}
}
}
//a:在30米的围栏中必须有速度值,而且超出小围栏的计数>=5个,条件成立则正在移动(30M直径的正方形)
//a1:在100米的围栏中有8个条数据均超出,不管有没有速度,条件均成立(也许他是坐飞机,也许他瞬移)
double a = getNowLocation().size() * 0.5;
double a1 = getNowLocation().size() * 0.8;
if ((stopCount <= 5 && rectCountDown >= a) || rectCountUp >= a1 || (notCheckUpCount == getNowLocation().size() && rectCountUp >= a) || (stopCount >= a && rectCountUp >= a)){
this.setPreLocation(b);
this.setPreTime(b.getAddTime());
return true;
}
return false;
}
}
GPS 偏移校正(WGS-84) 到(GCJ-02) java版本实现:
public class EvilTransform {
final static double pi = 3.14159265358979324;
//
//
// a = 6378245.0, 1/f = 298.3
// b = a * (1 - f)
// ee = (a^2 - b^2) / a^2;
final static double a = 6378245.0;
final static double ee = 0.00669342162296594323;
//
// World Geodetic System ==> Mars Geodetic System
public static double[] transform(double wgLat, double wgLon)
{
double mgLat=0;
double mgLon=0;
if (outOfChina(wgLat, wgLon))
{
mgLat = wgLat;
mgLon = wgLon;
}else{
double dLat = transformLat(wgLon - 105.0, wgLat - 35.0);
double dLon = transformLon(wgLon - 105.0, wgLat - 35.0);
double radLat = wgLat / 180.0 * pi;
double magic = Math.sin(radLat);
magic = 1 - ee * magic * magic;
double sqrtMagic = Math.sqrt(magic);
dLat = (dLat * 180.0) / ((a * (1 - ee)) / (magic * sqrtMagic) * pi);
dLon = (dLon * 180.0) / (a / sqrtMagic * Math.cos(radLat) * pi);
mgLat = wgLat + dLat;
mgLon = wgLon + dLon;
}
double[] point={mgLat,mgLon};
return point;
}
private static boolean outOfChina(double lat, double lon)
{
if (lon < 72.004 || lon > 137.8347)
return true;
if (lat < 0.8293 || lat > 55.8271)
return true;
return false;
}
private static double transformLat(double x, double y)
{
double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * Math.sqrt(Math.abs(x));
ret += (20.0 * Math.sin(6.0 * x * pi) + 20.0 * Math.sin(2.0 * x * pi)) * 2.0 / 3.0;
ret += (20.0 * Math.sin(y * pi) + 40.0 * Math.sin(y / 3.0 * pi)) * 2.0 / 3.0;
ret += (160.0 * Math.sin(y / 12.0 * pi) + 320 * Math.sin(y * pi / 30.0)) * 2.0 / 3.0;
return ret;
}
private static double transformLon(double x, double y)
{
double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * Math.sqrt(Math.abs(x));
ret += (20.0 * Math.sin(6.0 * x * pi) + 20.0 * Math.sin(2.0 * x * pi)) * 2.0 / 3.0;
ret += (20.0 * Math.sin(x * pi) + 40.0 * Math.sin(x / 3.0 * pi)) * 2.0 / 3.0;
ret += (150.0 * Math.sin(x / 12.0 * pi) + 300.0 * Math.sin(x / 30.0 * pi)) * 2.0 / 3.0;
return ret;
}
}
以下是网上搜集的资料:
一种gps速度漂移过滤的方法
http://www.xjishu.com/zhuanli/52/201210382905.html
ublox模块的参考用列代码:
http://www.pudn.com/Download/item/id/2691890.html http://www.pudn.com/Download/item/id/2364776.html
关于解决GPS定位设备:GPS静态漂移的方法
一篇关于GPS定位写得最详实清晰的文章之一
轨迹记录App是怎样对定位轨迹进行过滤、优化和平滑处理的
https://www.zhihu.com/question/39983016
卡尔曼滤波原理
卡尔曼滤波学习笔记
卡尔曼滤波的原理说明
http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf
免费的纠偏代码:
http://www.zdoz.net/interfaces.aspx
GPS纠偏算法,适用于google,高德体系的地图:
/**
* gps纠偏算法,适用于google,高德体系的地图
* @author Administrator
*/
public class GpsCorrect {
final static double pi = 3.14159265358979324;
final static double a = 6378245.0;
final static double ee = 0.00669342162296594323;
public static void transform(double wgLat, double wgLon, double[] latlng) {
if (outOfChina(wgLat, wgLon)) {
latlng[0] = wgLat;
latlng[1] = wgLon;
return;
}
double dLat = transformLat(wgLon - 105.0, wgLat - 35.0);
double dLon = transformLon(wgLon - 105.0, wgLat - 35.0);
double radLat = wgLat / 180.0 * pi;
double magic = Math.sin(radLat);
magic = 1 - ee * magic * magic;
double sqrtMagic = Math.sqrt(magic);
dLat = (dLat * 180.0) / ((a * (1 - ee)) / (magic * sqrtMagic) * pi);
dLon = (dLon * 180.0) / (a / sqrtMagic * Math.cos(radLat) * pi);
latlng[0] = wgLat + dLat;
latlng[1] = wgLon + dLon;
}
private static boolean outOfChina(double lat, double lon) {
if (lon < 72.004 || lon > 137.8347)
return true;
if (lat < 0.8293 || lat > 55.8271)
return true;
return false;
}
private static double transformLat(double x, double y) {
double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * Math.sqrt(Math.abs(x));
ret += (20.0 * Math.sin(6.0 * x * pi) + 20.0 * Math.sin(2.0 * x * pi)) * 2.0 / 3.0;
ret += (20.0 * Math.sin(y * pi) + 40.0 * Math.sin(y / 3.0 * pi)) * 2.0 / 3.0;
ret += (160.0 * Math.sin(y / 12.0 * pi) + 320 * Math.sin(y * pi / 30.0)) * 2.0 / 3.0;
return ret;
}
private static double transformLon(double x, double y) {
double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * Math.sqrt(Math.abs(x));
ret += (20.0 * Math.sin(6.0 * x * pi) + 20.0 * Math.sin(2.0 * x * pi)) * 2.0 / 3.0;
ret += (20.0 * Math.sin(x * pi) + 40.0 * Math.sin(x / 3.0 * pi)) * 2.0 / 3.0;
ret += (150.0 * Math.sin(x / 12.0 * pi) + 300.0 * Math.sin(x / 30.0 * pi)) * 2.0 / 3.0;
return ret;
}
}
源码下载地址:
GPS漂移过滤算法