前言
做了蓝牙定位有一段时间了,蓝牙定位算法想要做到高精度是非常困难的。由于蓝牙本身精度不够,接收设备也多种多样难以统一,造成纯蓝牙定位的效果很难做到理想状态。所以我们往往需要通过别的的手段来提高定位精度,如标题所言,这里选择的是惯性导航。这篇文章的重点是描述怎样融合蓝牙定位和惯性导航,下面运用的蓝牙定位和惯性导航都是用的非常简单的算法,仅供参考。
思路
蓝牙融合惯性导航的难点在于谁的权重更大,我该相信谁多一点。在两种定位方式精度都不太高的时候,我们要根据他们的特点进行取舍,以此来提高定位精度。
蓝牙的优势:学过三点定位法的朋友应该知道,定位点始终在三点之内。所以在一个大范围内,蓝牙的精度是相对靠谱的,而且随着蓝牙的密度提高,精度会随之提高。
惯性的优势:在是小范围下运动会相对准确。
那结果显而易见,大范围移动以蓝牙定位为准,小范围移动主要依赖惯性导航。算法的设计思路是这样的,首先获取蓝牙定位坐标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;
}