Android室内定位实战：Wi-Fi指纹+PDR融合的卡尔曼滤波方案

简介本资源是一套基于WiFi信号强度与行人航迹推算PDR融合的Android室内定位系统Matlab仿真方案面向本科及硕士阶段的智能算法、信号处理与移动计算方向学习者解决多源异构传感器数据融合建模与定位精度提升问题。压缩包共706个文件含192个.dex可执行字节码、134个.flat编译资源、125个.json配置与轨迹数据、70个.xml界面与权限定义、36个.java核心逻辑源码以及apk安装包、mp4演示视频、bat批处理脚本等完整覆盖Android端采集、Matlab端建模、融合定位算法实现与结果可视化全流程包体大小为42.41MB。目前已有323人学习下载。用户可直接运行Matlab 2014a/2019a环境下的仿真脚本复现WiFi指纹匹配与PDR步态积分联合优化过程获取含定位误差曲线、轨迹热力图、原始传感器时序数据在内的全套运行结果并参考配套结构化代码组织快速理解模块耦合关系与接口设计逻辑。 接手过室内定位项目的人应该都有过这种体验单独用Wi-Fi喊定位RSSI信号在走廊里来回跳实测轨迹画出来像喝醉了一样单独用PDR行人航位推算硬推前20米还行走两分钟就开始明显跑偏最后不知道拐到哪个房间去了。我这次的Android室内定位项目核心思路就是把Wi-Fi指纹定位和PDR拧成一股绳——用Wi-Fi给出绝对位置约束用PDR做步与步之间的连续递推最终在办公楼实测里把融合后的均方根误差RMSE压到了3米出头。这类项目适合谁如果你正在做室内导航、门店级人流动线分析、机器人室内定位的预处理或者只是想在Android手机上亲自验证一套定位算法这篇笔记都能直接参考。它覆盖的内容包括Wi-Fi指纹库怎么建、PDR的步态检测和航向解算是怎么实现的、两者怎么用卡尔曼滤波融合以及我在Android实机上调试时碰到的一堆坑。下面按项目的实际推进顺序来写。1. 为什么是Wi-Fi加PDR单点方案的硬伤与互补逻辑1.1 Wi-Fi指纹定位的真实痛点Wi-Fi指纹定位的基本逻辑很简单先在场地里按固定间隔布设采样点记录每个点能扫到的各AP信号强度RSSI形成一张信号地图在线定位时拿当前扫到的RSSI向量去地图里比对找最相似的参考点把它的坐标当作当前位置。这套方案落地过的人都知道精度上不去不是算法不行而是RSSI本身太不稳定。常见的干扰源包括人体遮挡拿着手机转向身体就能吃掉5~8dBm、金属门和电梯井反射、AP自动信道调整、甚至相邻房间的门开没开。我在办公楼走廊实测过同一个位置静止不动连续30秒扫描同一AP的信号波动范围可以达到6~8dBm。这个波动映射到位置估计上就是两米到五米的抖动。所以单独用Wi-Fi指纹定位静态场景还能给出一个大概位置动态行走时轨迹会明显漂移跳变——一会儿左一会儿右明明在走直线轨迹却像在画波浪线。1.2 PDR单独跑的累积漂移问题PDR的思路是只依赖手机里的惯性传感器加速度计判断走了几步、步长大概多少陀螺仪和磁力计判断朝向哪儿然后从初始点一步一步往后推。它最大的优势是短时间内的相对精度非常好。你从A点出发正常步速走20米PDR推出来的终点往往比Wi-Fi指纹定位给出的跳动结果更接近真实位置。但问题也出在这个推字上——每一步都带着误差步长估计偏一点、航向偏一度走几十步之后就全攒起来了。典型情况在80米的L形走廊里走一圈单纯PDR的终点误差能到10~15米航向一旦没稳住轨迹会直接穿墙。还有一个现实问题PDR必须知道起始点。如果没有外部定位给它喂初始位置它只能从原点盲推这在真实使用场景里不可接受。1.3 组合的切入点绝对修正 相对递推把两者放一起看互补关系非常清楚Wi-Fi指纹定位每隔几秒给一个绝对位置虽然单次误差大但没有累积性长期来看在真实位置附近波动PDR每走一步给一个相对位移短时间精度高但误差会随时间累积。组合后的工作方式可以理解为PDR负责走在两步之间给出平滑连续的轨迹Wi-Fi负责认路每隔几秒把PDR累计跑的偏差拉回来一次。我用这个思路跑出来的效果不是简单地取平均而是让PDR的每一步都受到Wi-Fi绝对位置的约束最终轨迹既平滑又不会越走越偏。如果把这次项目比作搭房子Wi-Fi指纹库是地基PDR是房子骨架融合算法则是水泥砂浆——三部分缺一不可。下面的章节就按这三个模块依次展开。2. Wi-Fi指纹定位落地信号采集到位置估计2.1 为什么选指纹库而不是三角定位做Wi-Fi定位有两条路三角定位根据AP坐标和信号测距解算位置和指纹库匹配。我在项目早期试过三角定位结果很快放弃了。原因是办公环境里绝大多数AP是部署在走廊天花板上的信号穿墙之后衰减模型非常难拟合基于路径损耗公式计算距离的误差经常超过5米而且你还需要拿到各AP的精确安装坐标这在很多场地根本没条件做到。指纹库虽然前期采集工作量更大但它不关心AP在哪只关心每个位置大概能收到哪些信号、强度是多少这个思路天然把多径、穿墙衰减这些复杂因素都吸收进了指纹数据里。对于测试验证性质的项目指纹库是性价比最高的选择。2.2 指纹采集的关键参数与数据清洗我这次选择的测试场地是办公楼3层面积大约600平米包含一条L形走廊和几个开放办公区。指纹库的参考点间隔设了1米走廊里布了大约200个参考点。每个参考点的采集要点在每个格点停留15~20秒连续扫描不要刚停下就记录因为系统Wi-Fi扫描本身有1~3秒延迟同一格点采集多个方向的RSSI面向走廊、背向走廊避免方向导致指纹差异过大只保留信号强度大于-90dBm的AP太弱的信号纯属噪声对匹配没有帮助按BSSIDAP的MAC地址去重同一AP如果有多个SSID保留下最强的那个。数据清洗方面我会对每个格点同一BSSID的多条采样做均值滤波同时剔除明显异常值比如突然出现又突然消失的临时信号。清洗后的数据格式大致长这样floor, ref_x, ref_y, bssid:rssi_1, bssid:rssi_2, ... 3, 12.0, 34.0, a4:0b:1c:22:33:44:-45, b8:0d:2e:55:66:77:-62, ...这个数据后面会直接加载成内存里的指纹表供在线查询。2.3 WKNN在线匹配与参数调优在线定位算法我选了加权K近邻WKNN理由是它在精度和计算量之间平衡最好。算法步骤在线扫描得到当前RSSI向量记为待匹配向量计算它和指纹库中每个参考点RSSI向量的欧氏距离选出距离最小的K个参考点这K个点的坐标按距离倒数的权重进行加权求和得到最终位置。欧氏距离计算公式d sqrt( sum( (rssi_i - fp_i)^2 ) )权重用w_j 1 / (d_j eps)其中eps取0.01防止距离为零时除零。K值我用离线数据做过一组对比测试结果如下K值平均定位误差14.8m33.9m53.6m73.8mK1就是最近邻对单点异常太敏感K7之后会引入太远的噪声点精度反而下降。最终选了K5。另外要注意WKNN算出来的是参考点坐标的加权平均如果待测位置在场地边缘可能出现位置被拉到场地外的情况。我当时加了一个兜底逻辑如果最终估计位置超出场地边界就用距离最小的那个参考点坐标作为结果。3. PDR行人航位推算步态检测与航向解算3.1 PDR的核心递推公式PDR的核心递推关系并不复杂x_new x_old step_length * sin(yaw) y_new y_old step_length * cos(yaw)这里的yaw是当前航向角以正北为0度顺时针为正step_length是这一步的长度。整个PDR模块要做的事情就是回答三个问题什么时候走了步这一步有多长朝向是哪边3.2 步频检测加速度峰值怎么判我用的步频检测方法是基于加速度幅值的峰值检测。具体做法是每收到一组加速度计数据ax, ay, az先计算合成幅值acc sqrt(ax^2 ay^2 az^2)然后减去当地重力约9.8得到去除重力后的加速度值。正常行走时这个值会出现明显的先上升后下降的波峰且波峰间隔大约在0.4~0.7秒之间。我在检测时用了以下条件波峰阈值设置为1.2 m/s²低于这个值的一律不算避免抖动误检相邻两步最小间隔设置为0.3秒防止同一个波峰被重复计数每检测到一次波峰记录一次时间戳和波峰幅值。这个方案在正常走路、慢走情况下都能稳定检出步数。我测试过30米走廊直线行走实际26步算法检出25步漏检率在可接受范围。跑动状态下阈值需要下调但室内定位场景主要覆盖步行所以没有专门优化跑步模式。3.3 步长模型Weinberg公式与标定步长估计是一条容易翻车的路。如果步长固定设为0.65米速度和不同身高带来的误差会一路累积。我采用的是比较经典的Weinberg动态步长模型step_length K * (Amax - Amin)^(1/4)Amax是这一步周期内加速度幅值的最大值Amin是最小值K是一个需要标定的常数。K值的标定方法在平地上用卷尺量出一段20米的直线距离正常步速走一遍记下算法检测到的步数N那么K就可以由下式反算出来K 20 / ( sum( (Amax_i - Amin_i)^(1/4) ) )我在办公室实测标定的K值大约是0.48左右。不同手机、不同人步态下K会有差异所以换测试人或者换手机后最好重新标定一次。3.4 航向解算磁力计、陀螺仪与互补滤波航向是PDR里最容易出问题的环节没有之一。磁力计给出的绝对航向受金属物体干扰大办公楼里的工位隔断、钢管、电脑都会让磁力计数据飘忽不定陀螺仪积分得到的相对航向短期稳定但会随时间慢慢漂移。我的处理办法是互补滤波yaw 0.95 * (yaw_prev gyro_z * dt) 0.05 * mag_yaw也就是95%相信陀螺仪积分结果5%相信磁力计。整段更新使用陀螺仪积分每收到一次磁力计航向把积分结果缓慢拉向磁力计的绝对航向。这样既避免了磁力计瞬时抖动又抑制了陀螺仪的长期漂移。这个公式看起来简单但它要求陀螺仪的z轴角速度积分值必须是连贯的不能掉帧。所以传感器事件的时间戳一定要用好不能直接用固定的dt比如16ms去算否则高采样和低采样交替时会引入额外误差。3.5 手机坐标系到地理坐标系的转换Android的加速度计和陀螺仪返回的都是手机坐标系数据直接拿去算航向肯定不对——手机稍微倾斜一点z轴就不是地理方向了。这里要用Android的传感器融合APISensorManager.getRotationMatrix(rotationMatrix, null, accelerometerValues, magnetometerValues); SensorManager.getOrientation(rotationMatrix, orientationValues); float yaw (float) Math.toDegrees(orientationValues[0]);getOrientation返回的angles[0]就是设备当前的方位角0表示正北90表示正东顺时针增大。要注意的是这个输出是相对于地理坐标系的并且会受到磁偏角的影响。正式使用前我会用GeomagneticField把磁偏角补偿进去yaw yaw geoField.getDeclination();如果不想处理磁偏角也可以不补偿因为室内小范围定位时几度的磁偏角对最终轨迹的影响并不致命。4. 融合模块简化的卡尔曼滤波与同步策略4.1 为什么用卡尔曼而不用平滑加权很多人一开始会想直接把Wi-Fi定位结果和PDR推算结果取平均不就行了我试过效果很差。原因是两种定位的误差特性完全不同PDR短时的相对轨迹是准的直接加权平均会把稳定的PDR轨迹破坏掉让路线重新变得跳跃。卡尔曼滤波的核心价值在于它会给每个量分配一个信任度。PDR预测结果是状态量的递推Wi-Fi定位作为观测值来修正修正的强度取决于当前协方差和观测噪声的比值。我刚走两步时PDR的协方差很小Wi-Fi即使跳了一下也不会把轨迹拉歪走了很久没有Wi-Fi更新、PDR协方差变大时Wi-Fi的修正作用才会增强。4.2 状态量与噪声参数怎么设我实现的是一个最简化的二维位置卡尔曼滤波器。状态向量是 (x, y)没有速度和加速度。过程模型用PDR给的位置递推观测模型用Wi-Fi指纹定位的位置。关键参数参数含义取值Q过程噪声协方差0.5m²RWi-Fi观测噪声协方差16m²P_initial初始协方差20m²过程噪声Q对应的是PDR每一步的误差水平我按步长估计误差约0.5米来设观测噪声R对应Wi-Fi单次定位误差按标准差4米、方差16来设。这两个值在不同场地需要重新调我的调试经验是先固定R然后从0.1开始增大Q观察轨迹的跟随性和平滑性的平衡。卡尔曼更新核心代码// 预测PDR步进 double xPred fusedX stepLen * Math.sin(yawRad); double yPred fusedY stepLen * Math.cos(yawRad); double pPred pPrev Q; // 观测更新Wi-Fi给了一个绝对位置 double K pPred / (pPred R); fusedX xPred K * (wifiX - xPred); fusedY yPred K * (wifiY - yPred); pPrev (1 - K) * pPred;就这么十几行融合效果已经比单独跑任何一套好很多。4.3 Wi-Fi低频与PDR每步高频的同步问题Wi-Fi扫描不是瞬时的从发出startScan到收到ScanResults回调通常要等1~3秒。PDR呢每一步大约0.5秒就更新一次。两者频率差异很大而且Wi-Fi每次返回的结果实际反映的是几百毫秒前的无线环境时间戳天然滞后。我的处理策略是PDR保持每步更新融合状态当Wi-Fi结果到达时先把当前融合状态里包含的上次Wi-Fi时间到当前时间之间的PDR位移记录下来用Wi-Fi结果修正融合位置后再把这段位移补回去。简单说就是收到Wi-Fi定位结果 (wifiX, wifiY)时间戳记为t_wifi当前融合位置是t_now时刻的状态求出从t_wifi到t_now之间PDR累积的位移(dx, dy)用卡尔曼更新得到t_wifi时刻的修正后位置把修正后位置加上(dx, dy)作为t_now时刻的最终融合位置。这样处理可以避免位置突然跳回几秒前的违和感。如果你的业务对实时性要求不高也可以直接丢弃旧数据但走动的场景下轨迹会明显滞后。4.4 异常Wi-Fi跳变怎么剔除指纹匹配偶尔会出现一个很离谱的结果——比如当前位置明明在走廊中间Wi-Fi定位却给到房间角落。原因通常是某个AP信号瞬时突变导致指纹匹配到了远距离参考点。如果不加保护卡尔曼更新会把轨迹猛拉一下再慢慢收回来非常影响观感。我的做法是加一个距离门限计算Wi-Fi定位结果与当前融合位置之间的距离如果超过8米判定为异常本次观测直接丢弃。这个8米是基于场地尺寸和Wi-Fi单点误差分布取的。如果场地较小可以收紧到5米如果环境信号很差可以放宽到10米。距离门限不是拍脑袋定的需要结合测试数据里Wi-Fi跳变的统计分布来设。5. Android应用实现从传感器调度到轨迹输出5.1 权限与运行时处理这个应用需要用到定位和Wi-Fi扫描因此要在AndroidManifest里声明uses-permission android:nameandroid.permission.ACCESS_FINE_LOCATION / uses-permission android:nameandroid.permission.ACCESS_WIFI_STATE / uses-permission android:nameandroid.permission.CHANGE_WIFI_STATE / uses-permission android:nameandroid.permission.ACCESS_COARSE_LOCATION /需要特别提醒Android 6.0以上Wi-Fi扫描结果受定位开关和定位权限双重控制只申请权限不打开系统定位开关getScanResults()返回的列表是空的。所以应用里最好加一个检查逻辑提示用户打开定位服务。另外Android 9开始限制后台应用进行Wi-Fi扫描。我们的测试应用是前台应用但有段时间我把采集流程放到了后台Service里结果扫描频率被系统限得非常低。如果要做长时间采集建议保持前台Service并配套通知栏常驻通知。5.2 Wi-Fi扫描线程与结果过滤扫描使用WifiManager的startScan和广播接收WifiManager wifiManager (WifiManager) context.getSystemService(Context.WIFI_SERVICE); wifiManager.startScan(); BroadcastReceiver wifiReceiver new BroadcastReceiver() { Override public void onReceive(Context context, Intent intent) { ListScanResult results wifiManager.getScanResults(); handleScanResults(results); } }; context.registerReceiver(wifiReceiver, new IntentFilter(WifiManager.SCAN_RESULTS_AVAILABLE_ACTION));扫描频率上我控制了两次扫描间隔至少3秒。太频繁会触发系统级限频反而得不偿失。在Android 8及以上系统限制后台应用在30分钟内最多执行4次扫描前台应用相对宽松但保险起见我在测试时把扫描间隔设置为了5秒。ScanResult里有个timestamp字段表示该扫描结果生成时系统启动的纳秒时间。这个字段非常有用因为广播可能携带的是上一次的缓存结果。我把每个BSSID和它的timestamp缓存起来只接受距离当前时间3秒以内的结果旧数据直接丢弃。5.3 传感器监听与步态处理线程传感器注册用SensorManagerSensorManager sm (SensorManager) getSystemService(SENSOR_SERVICE); sm.registerListener(sensorEventListener, sm.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_GAME); sm.registerListener(sensorEventListener, sm.getDefaultSensor(Sensor.TYPE_GYROSCOPE), SensorManager.SENSOR_DELAY_GAME); sm.registerListener(sensorEventListener, sm.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_GAME);SENSOR_DELAY_GAME大约是20ms一次对步态检测已经足够。没必要用SENSOR_FASTEST一个是耗电增加明显一个是数据抖动更多反而要额外滤波。在onSensorChanged里我按传感器类型分别处理然后把加速度幅值写进一个环形缓冲区。步态检测在同一个传感器回调线程里做轻量计算然后把检测到的步长、航向封装成定位事件投递给一个后台的定位引擎线程。这里不建议在UI线程里跑PDR和卡尔曼更新因为传感器的回调频率很高UI线程卡顿会直接导致传感器事件抖动。5.4 地图绘制与CSV数据导出为了直观看到轨迹我写了一个自定义View来画场地简图。先用Canvas画一个矩形表示场地边界用灰色小圆点标记指纹参考点然后用不同颜色的折线画出参考轨迹、Wi-Fi定位轨迹、PDR推算轨迹、融合轨迹。这样四条轨迹叠在同一张图上任何一条有问题都能立刻看出来。数据导出方面定位引擎每收到一个结果就往CSV文件里追加一行timestamp, fused_x, fused_y, wifi_x, wifi_y, step_count, yawCSV文件存到应用的私有目录测试结束后用adb pull导出在Python里做后续的误差分析。这一步强烈建议做因为你看屏幕上的轨迹只能定性判断像不像RMSE这种定量结论必须依赖离线分析。6. 实测评估与踩坑记录办公楼场景的精度表现6.1 测试场地与参考轨迹怎么搭我在办公楼3层规划了三条测试路线路线A走廊直线长约30米路线BL形走廊绕行总长约80米路线C开放办公区内部走曲线长约50米。参考轨迹我用激光测距仪和地面标记辅助生成先在走廊地面上每隔1米贴一个标记点然后用预先量好的坐标表让测试人员按标记点行走途中在手机App上同步标记经过某参考点的时间戳。这样就能把真实坐标和算法估计坐标对齐到同一时间轴上。测试手机用的是两台不同型号的Android机一台高通骁龙平台一台联发科天玑平台目的是观察不同Sensor硬件下的表现差异。步态标定在两台手机上分别做过一次。6.2 融合前后的精度对比离线处理测试数据用真实坐标和估计坐标逐点匹配后计算RMSE测试路线单独Wi-Fi RMSE单独PDR RMSE融合RMSE走廊直线30m4.2m2.1m1.8mL形走廊80m5.6m13.4m3.2m开放办公区50m4.8m8.5m3.5m单独PDR在L形走廊的表现崩得最明显因为途中有一个90度转弯航向估计在转弯瞬间偏了大约8度后面没被拉回来轨迹直接偏向墙体方向。融合结果受Wi-Fi修正转弯后很快被拉回正确路径最终RMSE为3.2米。开放办公区的情况类似Wi-Fi指纹里桌椅隔断比较多RSSI波动大单独Wi-Fi的RMSE到了4.8米PDR短时精度好但多走几步后漂移增加。融合后3.5米的精度在这个场景下已经能满足室内导航的需求。6.3 实测中不得不提的几个坑手机姿态切换最影响PDR航向的是手机的姿态。测试人把手机拿在手里和放进口袋同一路线跑出来的轨迹差异极大。拿在手里时手持姿态相对稳定航向准放口袋里时手机随腿部摆动姿态乱跳。我的解决方案是固定测试动作手持屏幕朝上在这个前提下把航向准确率调好。Wi-Fi扫描结果的时间戳过滤前面提到过不按timestamp过滤会把老旧扫描结果当成新数据用。我遇到过一次诡异现象应用打开了5分钟Wi-Fi定位结果却一直停在起点附近排查后发现是系统在返回缓存扫描结果因为Android对扫描频率做了限制旧结果被重复回调。加了时间戳过滤后问题消失。双频AP的干扰办公楼里大部分AP同时广播2.4GHz和5GHz两个频段指纹库和在线扫描如果不区分频段同一个BSSID可能对应两个不同的RSSI序列WKNN匹配时会引入大量噪声。我在指纹采集和在线匹配时都按freq字段把5GHz信号过滤掉了因为5GHz穿墙衰减快室内定位的稳定性不如2.4GHz。磁力计被金属干扰办公楼走廊里的金属门和工位立柱对磁力计的干扰是瞬时的互补滤波里即使只给了5%的权重磁力计一个大跳变也会把航向拉偏。我的处理方式是对磁力计航向再做一次一阶平滑同时限制单次修正量不超过3度这样即使磁力计抽风也只是微调。手机间步长参数差异两台测试手机标定的K值差了约15%不重新标定直接用同一套参数步长估计会明显偏短或偏长。建议不同机型务必单独标定至少走30米做一次反算。6.4 还能继续优化的几个方向这套方案跑通后有几个明显的优化方向。第一是楼层判断目前只在单层工作加上气压计后可以通过气压差判断所在楼层配合Wi-Fi指纹实现跨层定位。第二是地图约束办公楼的结构信息墙体、走廊、可用区域可以做成栅格地图在每次步进后把轨迹约束回可通行区域这能显著改善转弯处的轨迹。第三是步态细分现在只区分步行/暂停可以进一步识别跑步、上下楼、转弯等动作针对不同动作切换步长模型和航向平滑参数。另外Wi-Fi指纹库本身也有一个可优化的点——我这次用的是静态指纹实际场景中AP信号会随人流、门开关等变化。如果想提高长时间稳定性可以在后台做在线指纹更新把PDR轨迹修正后的位置反过来作为自动采样点逐步更新指纹库中的RSSI值这样就能让指纹库跟着环境走。最后分享一个调试技巧在办公室里用美纹纸在地面贴出1米间隔的网格实测时让测试人员沿网格线走同时把参考轨迹和算法输出的轨迹叠加显示在自定义View上。这样调试比看RMSE数据直观太多——哪一段偏左、哪一段航向跳了一眼就能看出来。我调试L形走廊转弯问题的时候就是靠这张叠加图定位到航向偏差发生在那次转弯前两秒的。本文还有配套的精品资源点击获取