蓝牙定位算法融合惯性导航

前言

做了蓝牙定位有一段时间了，蓝牙定位算法想要做到高精度是非常困难的。由于蓝牙本身精度不够，接收设备也多种多样难以统一，造成纯蓝牙定位的效果很难做到理想状态。所以我们往往需要通过别的的手段来提高定位精度，如标题所言，这里选择的是惯性导航。这篇文章的重点是描述怎样融合蓝牙定位和惯性导航，下面运用的蓝牙定位和惯性导航都是用的非常简单的算法，仅供参考。

思路

蓝牙融合惯性导航的难点在于谁的权重更大，我该相信谁多一点。在两种定位方式精度都不太高的时候，我们要根据他们的特点进行取舍，以此来提高定位精度。

蓝牙的优势：学过三点定位法的朋友应该知道，定位点始终在三点之内。所以在一个大范围内，蓝牙的精度是相对靠谱的，而且随着蓝牙的密度提高，精度会随之提高。
惯性的优势：在是小范围下运动会相对准确。

那结果显而易见，大范围移动以蓝牙定位为准，小范围移动主要依赖惯性导航。算法的设计思路是这样的，首先获取蓝牙定位坐标P点(需要一套蓝牙定位算法，主流的是三点法，指纹法)，在P的半径十米的范围内认为惯性导航更加准确，可以利用惯性随意走动，然而一旦惯性导航超过了蓝牙十米范围，就认为惯性导航失精，以蓝牙定位为准。

实现

//位置结构
struct Point{
    float x;
    float y;
    Point(float x0=0,float y0=0){
        x=x0;
        y=y0;
    };
};

//显示位置
static Point p;

//蓝牙位置
static Point bt;

//蓝牙定位算法所需数据
static vector<Point> ps;//坐标
static vector<float> ds;//距离
static vector<long> ts;//时间
static long pretime=0;

//指南针方向
static float angle=0;

//蓝牙定位算法，简单，每次传入蓝牙的距离坐标即可
//算法保留过去4秒内的蓝牙数据
//time,系统时间(单位:ms)
//距离dis，坐标x,y（单位：m）
void BlueTooth(long time,float dis,float x,float y){
    if(time==0||dis==0)
        return ;
    
    ps.push_back(Point(x,y));
    ds.push_back(dis);
    ts.push_back(time);
    
    //删除存在超过四秒的数据
    while(time-ts[0]>4000){
        ps.erase(ps.begin());
        ds.erase(ds.begin());
        ts.erase(ts.begin());
    }
    
    //每隔5秒更新一次蓝牙信息
    if(time-pretime>5000){
        pretime=time;
        
        //计算蓝牙定位点，用权值分配法
        float sum=0;
        vector<float> w(ds);
        for(int i=0;i<w.size();++i){
            w[i]=pow(2, -w[i]/2);
            sum+=w[i];
        }
        
        Point p0;
        for (int i=0; i<w.size(); ++i) {
            p0.x+=ps[i].x*w[i]/sum;
            p0.y+=ps[i].y*w[i]/sum;
        }
        
        bt.x=p0.x;
        bt.y=p0.y;
        
        //初始位置以蓝牙为准
        if(p.x==0&&p.y==0){
            p.x=bt.x;
            p.y=bt.y;
        }
    }
}

//获取指南针方向
void Direction(float angle0){
    angle=angle0;
}

//刷新计步数据
void Steps(int step){
    //判断位置是否初始化，指南针数据是否刷新
    if(!(p.x==0&&p.y==0)&&angle!=0){
        //0.7为步长
        p.x+=0.7*cos(angle)*(float)step;
        p.y+=0.7*sin(angle)*(float)step;
    }
}

//获取位置信息，建议0.5秒查询一次
Point getPosition(){
    Point re;
    
    //判断位置是否初始化
    if(!(p.x==0&&p.y==0)){
        //判断是否超过蓝牙10米半径
        if(sqrt((p.x-bt.x)*(p.x-bt.x)+(p.y-bt.y)*(p.y-bt.y))>10){
            p.x=bt.x;
            p.y=bt.y;
        }else{
            //缓慢向蓝牙定位点移动（可忽略）
            p.x+=(bt.x-p.x)/100;
            p.y+=(bt.y-p.y)/100;
        }
        re.x=p.x;
        re.y=p.y;
    }
    
    return re;
}