慣性航法測位アルゴリズムのBluetoothの統合

序文

Bluetoothはいくつかの時間のために行うように配置され、ブルートゥース測位アルゴリズムは、精度をやってみたいことは非常に困難です。ブルートゥース精度自体に十分なだけでなく、純粋なブルートゥース位置決め効果が得られ、ユニティに難しい受信装置の種々ないので、理想的な状態を達成することは困難です。私たちは、多くの場合、他の手段によって位置決め精度を向上させるために必要なので、タイトルが言うような、ここでは慣性航法の選択があります。この記事の焦点は、Bluetoothの位置と慣性航法の統合がどのように記述することで、次のBluetoothポジショニングを使用して、慣性航法は、参照目的のみのために非常に単純なアルゴリズムです。

思考

より多くの重量が誰で慣性航法難易嘘のBluetoothの統合は、私はもう少し信じるように誰が知っています。両方のターゲット精度で高すぎないか、我々は、位置決め精度を向上させるためには、その特性に合わせてお選びします。

Bluetoothの利点：学んだ三点測位方法友人が3点以内に必ずそのアンカーポイントを知っている必要があります。それほど広い範囲で、精度のBluetoothは、比較的信頼性があり、およびBluetooth密度が高くなると、精度が向上します。
慣性の利点：ゲームは、小さな範囲では比較的正確です。

その結果は明らかである、モバイルブルートゥース位置決めの広い範囲が小規模慣性航法に主に依存して移動する、優先する。設計アイデアアルゴリズムは、このような点である第一全てのGETブルートゥース位置座標点P（ブルートゥース位置決めアルゴリズムを必要とし、主流のフィンガープリント、3点法で）10メートルP月の半径以内、考えより正確な慣性航法の、慣性歩き回るが、Bluetoothの10メートルを超える慣性航法たら、彼らは精度慣性航法の損失を考えて、Bluetoothの位置決めが優先する。

実現

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