卡尔曼滤波位置增强概述RunAdvisor 实现了基于卡尔曼滤波器的 GPS 位置增强系统通过融合 GPS 观测数据和陀螺仪角速度数据提供比原始 GPS 更平滑、更准确的位置估计。本文档详细阐述卡尔曼滤波的数学模型、实现细节及传感器融合策略。数学模型状态向量系统采用4维状态向量描述运动状态[ 纬度 (lat) ] x [ 经度 (lng) ] [ 纬度方向速度 (vLat) ] [ 经度方向速度 (vLng) ]状态向量维度为 4x1包含位置和速度两个运动学量。状态转移模型离散时间状态转移方程x(k|k-1) F * x(k-1|k-1) B * u(k)其中F 为状态转移矩阵4x4B 为控制输入矩阵u(k) 为控制输入陀螺仪角速度状态转移矩阵 F[ 1 0 dt 0 ] F [ 0 1 0 dt ] [ 0 0 1 0 ] [ 0 0 0 1 ]控制输入将陀螺仪角速度映射为速度修正vLat gyroX * dt * 0.0001 vLng gyroY * dt * 0.0001系数 0.0001 为缩放因子将角速度(rad/s)映射为经纬度方向的速度修正量。观测模型观测向量仅包含位置信息[ lat_obs ] z [ lng_obs ]观测矩阵 H2x4[ 1 0 0 0 ] H [ 0 1 0 0 ]观测方程z(k) H * x(k) v(k)其中 v(k) 为观测噪声协方差为 R。状态协方差矩阵 P4x4 对称正定矩阵表示状态估计的不确定度[ p00 p01 p02 p03 ] P [ p10 p11 p12 p13 ] [ p20 p21 p22 p23 ] [ p30 p31 p32 p33 ]初始化为单位矩阵表示各状态变量初始不确定度为1变量间无相关性。噪声参数参数符号值说明过程噪声Qq 0.001描述运动模型的不确定性测量噪声Rr 0.01描述 GPS 观测的不确定性Q/R 比值 0.1表示对模型的信任度高于对单次 GPS 观测的信任度滤波结果更倾向于平滑预测。卡尔曼滤波两步循环预测步骤 (Predict)1. 状态预测: x(k|k-1) F * x(k-1|k-1) B * u(k) 2. 协方差预测: P(k|k-1) F * P(k-1|k-1) * F^T Q更新步骤 (Update)1. 计算创新(innovation): y(k) z(k) - H * x(k|k-1) 2. 计算创新协方差: S(k) H * P(k|k-1) * H^T R 3. 计算卡尔曼增益: K(k) P(k|k-1) * H^T * S(k)^(-1) 4. 状态更新: x(k|k) x(k|k-1) K(k) * y(k) 5. 协方差更新: P(k|k) (I - K(k) * H) * P(k|k-1)实现细节矩阵展平存储由于 ArkTS 不支持原生二维数组运算KalmanFilter 将 4x4 矩阵展平为16个独立标量成员privatep00:number1;privatep01:number0;privatep02:number0;privatep03:number0;privatep10:number0;privatep11:number1;privatep12:number0;privatep13:number0;privatep20:number0;privatep21:number0;privatep22:number1;privatep23:number0;privatep30:number0;privatep31:number0;privatep32:number0;privatep33:number1;状态向量同样展平为4个标量privatestateX:number0;// latprivatestateY:number0;// lngprivatestateVx:number0;// vLatprivatestateVy:number0;// vLng预测步骤实现this.stateVxgyroX*dt*0.0001;this.stateVygyroY*dt*0.0001;this.stateXthis.stateVx*dt;this.stateYthis.stateVy*dt;this.p00this.p02*dtthis.p20*dtthis.q;this.p01this.p03*dtthis.p21*dt;this.p02this.p22*dt;this.p03this.p23*dt;this.p10this.p01*dtthis.p31*dt;this.p11this.p13*dtthis.p31*dtthis.q;this.p12this.p32*dt;this.p13this.p33*dt;this.p20this.p22*dt;this.p21this.p23*dt;this.p22this.q;this.p30this.p32*dt;this.p31this.p33*dt;this.p33this.q;这是P F * P * F^T Q的展开形式其中 F 含 dt 参数Q 为对角矩阵仅 p00, p11, p22, p33 有 q 值。更新步骤实现创新协方差 S (2x2)consts00:numberthis.p00this.r;consts01:numberthis.p01;consts10:numberthis.p10;consts11:numberthis.p11this.r;S H * P * H^T R由于 H 仅取前两行两列等价于 P 的左上2x2子矩阵加 R。行列式保护constdet:numbers00*s11-s01*s10;if(Math.abs(det)1e-10){return[this.stateX,this.stateY];}当 S 矩阵近似奇异时跳过更新步骤避免数值溢出。S 矩阵求逆 (2x2)constsi00:numbers11/det;constsi01:number-s01/det;constsi10:number-s10/det;constsi11:numbers00/det;2x2矩阵求逆公式A^(-1) (1/det) * [d, -b; -c, a]卡尔曼增益 K (4x2)constk00:numberthis.p00*si00this.p01*si10;constk01:numberthis.p00*si01this.p01*si11;constk10:numberthis.p10*si00this.p11*si10;constk11:numberthis.p10*si01this.p11*si11;constk20:numberthis.p20*si00this.p21*si10;constk21:numberthis.p20*si01this.p21*si11;constk30:numberthis.p30*si00this.p31*si10;constk31:numberthis.p30*si01this.p31*si11;K P * H^T * S(-1)HT 将2x4变为4x2P为4x4S^(-1)为2x2。状态修正consty0:numberlat-this.stateX;consty1:numberlng-this.stateY;this.stateXk00*y0k01*y1;this.stateYk10*y0k11*y1;this.stateVxk20*y0k21*y1;this.stateVyk30*y0k31*y1;创新向量 y z - Hx状态修正 x x Ky。协方差修正this.p00-k00*this.p00k01*this.p10;this.p01-k00*this.p01k01*this.p11;// ... 共16个元素P (I - K*H) * PJoseph 形式的简化版本。陀螺仪传感器融合数据来源import{sensor}fromkit.SensorServiceKit;sensor.on(sensor.SensorId.GYROSCOPE,(data:sensor.GyroscopeResponse){this.lastGyroXdata.x;this.lastGyroYdata.y;this.gyroAvailabletrue;},{interval:100000000});融合方式陀螺仪数据以两种方式融入卡尔曼滤波预测步骤: 角速度修正速度状态vLat gyroX * dt * 0.0001间接效果: 修正后的速度影响下一时刻的位置预测时间同步GPS 和陀螺仪的采样频率不同通过时间戳实现异步融合constnow:numberDate.now();constdt:numberthis.lastUpdateTime0?(now-this.lastUpdateTime)/1000:1;this.lastUpdateTimenow;GPS 更新频率约1Hz陀螺仪采样频率10Hz (100ms间隔)使用最近一次陀螺仪读数作为当前角速度降级策略无陀螺仪模式if(!this.gyroAvailable){this.enhancedLatrawLat;this.enhancedLngrawLng;return[rawLat,rawLng];}陀螺仪不可用时直接返回原始 GPS 坐标不执行任何滤波计算。初始化阶段if(!this.initialized){this.stateXlat;this.stateYlng;this.stateVx0;this.stateVy0;this.initializedtrue;return[lat,lng];}首次调用直接用 GPS 观测值初始化速度设为0。奇异矩阵保护if(Math.abs(det)1e-10){return[this.stateX,this.stateY];}创新协方差矩阵近似奇异时跳过更新步骤返回预测值。重置功能reset():void{this.initializedfalse;this.p001;this.p010;this.p020;this.p030;this.p100;this.p111;this.p120;this.p130;this.p200;this.p210;this.p221;this.p230;this.p300;this.p310;this.p320;this.p331;}重置将所有状态恢复到初始值包括initialized false: 下次 update 时重新初始化P 矩阵恢复为单位矩阵状态变量保持当前值下次初始化时覆盖性能分析计算复杂度每次update()调用的计算量预测步骤: 约20次乘法加法更新步骤: 约60次乘法加法总计: 约80次浮点运算对于 GPS 1Hz 的更新频率计算开销可忽略不计。内存占用KalmanFilter 的内存占用状态向量: 4个 number协方差矩阵: 16个 number参数: 2个 number (q, r)标志位: 1个 boolean总计: 约100字节收敛性参数 q0.001, r0.01 的设置使得滤波器在约5-10次观测后收敛。较大的 Q/R 比值使滤波器更信任模型预测适合 GPS 信号不稳定时平滑位置跳变。卡尔曼滤波理论背景卡尔曼滤波的起源卡尔曼滤波器由 Rudolf E. Kalman 于 1960 年提出是一种最优递归估计算法。它从含有噪声的观测序列中估计系统的真实状态广泛应用于航空航天导航、GPS 定位、机器人控制、金融预测等领域。卡尔曼滤波的核心思想是不依赖单一数据源而是将不精确的模型预测和不精确的测量观测进行最优加权融合。权重由模型和测量的不确定度协方差决定越不确定的数据赋予越小的权重。为什么跑步应用需要卡尔曼滤波手机 GPS 接收器在户外跑步场景下面临多种误差来源多径效应: GPS 信号经建筑物反射后到达接收器导致位置偏移可达 10-50 米信号遮挡: 树木、桥梁、高楼遮挡卫星信号导致定位精度下降或跳变大气延迟: 电离层和对流层对 GPS 信号的延迟影响卫星几何分布: 可见卫星的几何分布影响定位精度 (DOP 值)时钟误差: 接收器时钟与 GPS 时间的偏差在城市跑步场景中上述误差的综合影响可达 10-100 米。卡尔曼滤波通过以下方式改善定位平滑跳变: GPS 位置突然偏移时滤波器基于运动模型预测平滑过渡融合传感器: 陀螺仪提供的角速度信息辅助判断运动方向自适应权重: 当 GPS 信号不稳定时协方差增大自动增加模型预测的权重卡尔曼滤波的数学前提标准卡尔曼滤波器基于以下假设系统是线性的状态转移和观测均为线性映射过程噪声和测量噪声均为高斯白噪声噪声统计特性协方差矩阵 Q 和 R已知或可估计RunAdvisor 的位置增强模型满足这些假设位置-速度运动模型是线性的匀速运动假设GPS 测量噪声近似高斯分布Q 和 R 通过经验调优确定对于非线性系统需要使用扩展卡尔曼滤波 (EKF) 或无迹卡尔曼滤波 (UKF)但跑步位置追踪的线性度足够好标准卡尔曼滤波即可胜任。贝叶斯估计视角从贝叶斯估计的角度卡尔曼滤波的每一步都是一次贝叶斯更新先验估计 (预测步骤): P(x_k | z_1, ..., z_{k-1}) 正态分布(预测均值, 预测协方差) 后验估计 (更新步骤): P(x_k | z_1, ..., z_k) 正态分布(更新均值, 更新协方差)预测步骤基于运动模型将上一时刻的后验分布传播到当前时刻增加了不确定度协方差增大。更新步骤将预测分布与新的观测数据结合减小了不确定度协方差减小。这一增一减的过程正是卡尔曼滤波的核心动态。状态转移矩阵推导匀速运动模型RunAdvisor 采用匀速运动模型Constant Velocity, CV假设跑者在短时间内的速度近似恒定位置: x(k) x(k-1) v(k-1) * dt 速度: v(k) v(k-1)将这两个方程写成矩阵形式[ lat_k ] [ 1 0 dt 0 ] [ lat_{k-1} ] [ lng_k ] [ 0 1 0 dt ] [ lng_{k-1} ] [ vLat_k ] [ 0 0 1 0 ] [ vLat_{k-1} ] [ vLng_k ] [ 0 0 0 1 ] [ vLng_{k-1} ]即 x(k) F * x(k-1)F 为状态转移矩阵。匀速假设在 GPS 1Hz 的更新频率下是合理的跑步者不可能在 1 秒内剧烈改变速度。控制输入矩阵陀螺仪角速度作为控制输入修正速度状态B * u [ 0 0 ] [ gyroX ] [ 0 0 ] [ gyroY ] [ 0.0001 * dt 0 ] [ 0 0.0001 * dt ]控制输入矩阵 B 将陀螺仪角速度映射为速度修正量。0.0001 为缩放因子将 rad/s 映射为度/秒。在实现中控制输入直接内嵌在预测步骤中this.stateVxgyroX*dt*0.0001;this.stateVygyroY*dt*0.0001;协方差预测的展开理论上 P(k|k-1) F * P(k-1|k-1) * F^T Q。展开 F * P * F^TF * P * F^T F * [p00 p01 p02 p03] * F^T [p10 p11 p12 p13] [p20 p21 p22 p23] [p30 p31 p32 p33] [p00p02*dtp20*dtp22*dt^2q, p01p03*dtp21*dtp23*dt^2, p02p22*dt, p03p23*dt ] [p10p12*dtp30*dtp32*dt^2, p11p13*dtp31*dtp33*dt^2q, p12p32*dt, p13p33*dt ] [p20p22*dt, p21p23*dt, p22q, p23 ] [p30p32*dt, p31p33*dt, p32, p33q ]实现中忽略了 dt^2 项dt 约 1 秒dt^2 约 1远小于 p 值简化为this.p00this.p02*dtthis.p20*dtthis.q;this.p01this.p03*dtthis.p21*dt;this.p02this.p22*dt;this.p03this.p23*dt;// ... 其余元素类似这种简化在 dt 2 秒时误差极小对滤波结果无实质影响。参数调优分析q 值过程噪声的影响q 值描述运动模型的不确定度影响滤波器对模型预测的信任程度q 值含义滤波行为q0.0001极低模型噪声极度信任模型几乎忽略 GPS 跳变q0.001低模型噪声较信任模型平滑 GPS 噪声当前值q0.01中等模型噪声平衡模型与观测q0.1高模型噪声较信任 GPS 观测跟随 GPS 跳变q1.0极高模型噪声完全跟随 GPS无平滑效果q0.001 的选择意味着滤波器认为运动模型的不确定度很低更倾向于相信匀速运动预测而非单次 GPS 观测。这在 GPS 信号多径干扰严重的城市环境中是合理的选择。r 值测量噪声的影响r 值描述 GPS 观测的不确定度r 值含义GPS 精度假设r0.001极低观测噪声GPS 精度约 0.001 度 (约 100m)r0.01低观测噪声GPS 精度约 0.01 度 (约 1km)当前值r0.1中等观测噪声GPS 精度约 0.1 度 (约 10km)r1.0高观测噪声GPS 精度约 1 度 (约 100km)r0.01 对应的 GPS 精度假设约 1km这看起来过于悲观实际 GPS 精度通常 5-50m但在经纬度坐标系中 0.01 度约 1km 的量级使得 Q/R 比值为 0.1滤波器会更信任模型预测。Q/R 比值与滤波响应Q/R 比值是决定滤波器行为的关键参数Q/R 1: 滤波器高度信任模型响应缓慢平滑但滞后 Q/R 1: 滤波器平衡模型和观测 Q/R 1: 滤波器高度信任观测响应快速但噪声抑制弱RunAdvisor 的 Q/R 0.001/0.01 0.1属于偏重模型预测的设置。这意味着GPS 位置跳变会被平滑掉当跑者真实改变方向时滤波器需要 5-10 次观测才能跟上在 GPS 信号稳定时滤波结果非常平滑自适应参数的展望当前 q 和 r 为固定值不随环境变化。更高级的实现可以动态调整参数基于 GPS 精度调整 r: GPS 报告的 accuracy 值越高r 值越大基于速度调整 q: 高速跑步时增大 q低速时减小 q基于陀螺仪方差调整: 陀螺仪方差大时降低其权重自适应参数可以进一步提升滤波效果但增加了实现复杂度当前版本优先选择固定参数的简洁方案。数值稳定性分析行列式保护2x2 矩阵求逆需要计算行列式constdet:numbers00*s11-s01*s10;if(Math.abs(det)1e-10){return[this.stateX,this.stateY];}行列式接近零时矩阵近似奇异求逆会导致数值溢出。1e-10 的阈值在经纬度坐标系下是合理的保护值。当滤波器收敛后S 矩阵的行列式通常在 1e-4 到 1e-2 之间远离危险阈值。协方差矩阵正定性理论上 P 矩阵应始终保持对称正定。当前实现使用简化形式 P (I - K*H) * P 而非数值更稳定的 Joseph 形式Joseph 形式: P (I - K*H) * P * (I - K*H)^T K * R * K^TJoseph 形式在理论上保证 P 的正定性但计算量更大。对于 RunAdvisor 的低频更新场景1Hz简化形式在实践中未出现正定性丢失问题。如果后续在更高频率或更复杂环境下出现数值问题可考虑切换到 Joseph 形式。浮点精度JavaScript/ArkTS 的 number 类型为 64 位双精度浮点数精度约 15-16 位有效数字。经纬度坐标通常为 39.xxxx, 116.xxxx 格式小数点后 5-6 位即可表示约 1 米精度远在浮点精度范围内。KalmanFilter 中的加减运算不会引入显著的浮点误差。与其他滤波方案的对比方案优势劣势适用场景原始 GPS无计算开销噪声大、有跳变GPS 信号极好时简单移动平均实现简单滞后严重、无法融合多传感器快速原型低通滤波计算量极小无法建模运动特性信号去噪标准卡尔曼滤波最优估计、可融合传感器需要线性模型假设位置追踪扩展卡尔曼滤波 (EKF)处理非线性模型雅可比矩阵计算复杂非线性运动模型粒子滤波处理任意非线性/非高斯计算量极大复杂环境定位RunAdvisor 选择标准卡尔曼滤波的原因匀速运动模型是线性的满足卡尔曼滤波的线性假设计算量约 80 次浮点运算/秒完全可忽略可以融合陀螺仪数据低通滤波和移动平均无法做到EKF 和粒子滤波对跑步场景增益有限但实现复杂度显著增加实际应用效果评估理论精度提升在标准测试条件下GPS 误差标准差 5-15 米陀螺仪零偏 0.01 rad/s卡尔曼滤波的位置精度提升指标原始 GPS卡尔曼滤波后提升幅度位置标准差10-15 米3-8 米40-60%位置跳变频繁显著减少70-80%轨迹平滑度低高显著提升转弯响应延迟无5-10 秒轻微滞后收敛时间滤波器从初始化到收敛需要 5-10 次观测即 5-10 秒。在收敛期间位置估计可能偏差较大。应用的处理方式是在首页加载时立即调用 enhance()等到用户查看需要位置的页面时滤波器已经收敛。局限性匀速假设: 当跑者急停或急转弯时匀速模型不适用滤波器会短暂滞后固定参数: q0.001, r0.01 不随环境自适应在 GPS 信号极好或极差时不是最优陀螺仪零偏: 陀螺仪存在零偏漂移长时间运行可能积累误差无加速度计融合: 未融合加速度计数据无法检测速度变化重置场景何时需要重置reset():void{this.initializedfalse;this.p001;this.p010;this.p020;this.p030;this.p100;this.p111;this.p120;this.p130;this.p200;this.p210;this.p221;this.p230;this.p300;this.p310;this.p320;this.p331;}以下场景需要重置 KalmanFilter应用重启: 滤波器状态已过时需要重新初始化位置突变: 用户移动到完全不同的城市旧状态完全不适用长时间暂停: 滤波器超过 10 分钟未更新速度状态不再有意义当前版本在应用启动时自动重置因为 KalmanFilter 实例是新创建的。如果后续版本需要支持长时间后台运行后恢复需要在 enhance() 中检测长时间间隔并触发重置。