Bluetooth integration of inertial navigation positioning algorithm

Foreword

Bluetooth is positioned to do for some time, Bluetooth positioning algorithm is very difficult to want to do precision. Since Bluetooth accuracy is not enough in itself, but also a variety of receiving devices difficult to unity, resulting in pure Bluetooth positioning effect is difficult to achieve the ideal state. So we often need to improve the positioning accuracy through other means, such as the title says, here is the choice of inertial navigation. The focus of this article is to describe how the integration of Bluetooth positioning and inertial navigation , use the following Bluetooth positioning and inertial navigation is a very simple algorithm for reference purposes only.

Thinking

Bluetooth integration of inertial navigation difficulty lies in who's more weight, I know who to believe a little more. In both targeting accuracy is not too high, we want to choose according to their characteristics, in order to improve positioning accuracy.

Bluetooth advantages : learned three-point positioning method friends should know that anchor point always within three points of. So within a wide range, precision Bluetooth is relatively reliable, and with Bluetooth density increases, the accuracy will be increased.
Inertia advantage : Games are relatively accurate in a small range.

That result is obvious, a wide range of mobile Bluetooth positioning prevail, moving mainly dependent on small-scale inertial navigation. Design ideas algorithm is such that , first of all get Bluetooth positioning coordinates of the point P (requires a Bluetooth positioning algorithm, the mainstream is the three-point method, fingerprinting), considered more accurate inertial navigation within a radius of ten meters P may inertia walk around, but once inertial navigation over Bluetooth ten meters, they think loss of precision inertial navigation, Bluetooth positioning prevail.

achieve

//位置结构
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;
}