HarmonyOS APP开发：定位精度优化与多源融合

核心要点：本文深入讲解基于HarmonyOS的定位精度优化与多源融合技术，涵盖扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）、粒子滤波、多源传感器融合架构、自适应权重分配，以及完整的ArkTS代码实现。将蓝牙/WiFi/UWB/PDR多种定位源融合，实现更精确、更鲁棒的室内定位系统。

一、背景与动机

1.1 单一定位源的局限性

前四篇文章分别介绍了蓝牙信标、WiFi指纹、UWB和室内导航技术。每种技术都有其优势场景，但也存在固有局限：

核心矛盾：没有任何单一技术能在所有场景下都提供稳定可靠的定位。蓝牙在金属密集区域失效，WiFi在人流高峰时漂移，UWB在NLOS下精度骤降，PDR长时间运行后误差累积。

1.2 多源融合的必然性

多源融合定位的核心思想：不同定位源在不同场景下的可靠性不同，融合系统应动态选择最可靠的定位源。

flowchart TB
    subgraph 传感器层["传感器层 - 多源数据采集"]
        direction LR
        S1["蓝牙信标<br/>RSSI测距"]
        S2["WiFi指纹<br/>指纹匹配"]
        S3["UWB<br/>TWR/TDoA"]
        S4["PDR<br/>步频+航向"]
        S5["地磁<br/>磁场匹配"]
    end

    subgraph 预处理层["预处理层 - 信号质量评估"]
        direction TB
        P1["异常值检测"] --> P2["卡尔曼滤波"]
        P2 --> P3["置信度评估"]
    end

    subgraph 融合层["融合层 - 多源融合算法"]
        direction TB
        F1["EKF/UKF<br/>线性化融合"] --> F2["粒子滤波<br/>非参数融合"]
        F2 --> F3["自适应权重<br/>动态分配"]
    end

    subgraph 输出层["输出层"]
        direction TB
        O1["融合坐标<br/>(x, y, z)"]
        O2["精度估计<br/>CEP95"]
        O3["可靠性指标<br/>0-1"]
    end

    S1 --> P1
    S2 --> P1
    S3 --> P1
    S4 --> P1
    S5 --> P1
    P3 --> F1
    F3 --> O1
    F3 --> O2
    F3 --> O3

    classDef sensor fill:#533483,stroke:#e94560,color:#eee,stroke-width:2px
    classDef process fill:#16213e,stroke:#0f3460,color:#eee,stroke-width:2px
    classDef fusion fill:#0f3460,stroke:#4ade80,color:#eee,stroke-width:2px
    classDef output fill:#1a1a2e,stroke:#e94560,color:#eee,stroke-width:2px

    class S1,S2,S3,S4,S5 sensor
    class P1,P2,P3 process
    class F1,F2,F3 fusion
    class O1,O2,O3 output

1.3 融合定位的精度提升

通过多源融合，可以显著提升定位精度和鲁棒性：

二、核心原理

2.1 扩展卡尔曼滤波（EKF）

EKF是传感器融合最常用的算法，通过将非线性系统线性化来应用卡尔曼滤波框架。

状态向量（2D定位）：

$$\mathbf{x} = \begin{pmatrix} x \\ y \\ v_x \\ v_y \end{pmatrix}$$

预测步骤（运动模型）：

$$\hat{\mathbf{x}}_{k|k-1} = \mathbf{F} \cdot \mathbf{x}_{k-1|k-1} + \mathbf{B} \cdot \mathbf{u}_k$$

其中$\mathbf{F}$为状态转移矩阵，$\mathbf{B}$为控制输入矩阵，$\mathbf{u}_k$为PDR输入（步长、航向）。

更新步骤（观测模型）：

$$\mathbf{K}_k = \mathbf{P}_{k|k-1} \cdot \mathbf{H}_k^T \cdot (\mathbf{H}_k \cdot \mathbf{P}_{k|k-1} \cdot \mathbf{H}_k^T + \mathbf{R}_k)^{-1}$$

$$\mathbf{x}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \cdot (\mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1}))$$

其中$\mathbf{z}_k$为观测向量（蓝牙/WiFi/UWB定位结果），$\mathbf{R}_k$为观测噪声协方差。

2.2 无迹卡尔曼滤波（UKF）

EKF通过泰勒展开线性化非线性系统，但高阶截断误差可能导致滤波发散。UKF使用无迹变换（Unscented Transform）更精确地处理非线性：

1. Sigma点采样：从状态分布中选取2n+1个Sigma点
2. 非线性传播：将Sigma点通过非线性函数传播
3. 统计量恢复：从传播后的Sigma点恢复均值和协方差

UKF不需要计算雅可比矩阵，实现更简单，且对强非线性系统精度更高。

2.3 粒子滤波（PF）

粒子滤波是一种非参数化的贝叶斯滤波方法，通过一组加权粒子近似后验分布：

$$p(\mathbf{x}_k | \mathbf{z}_{1:k}) \approx \sum_{i=1}^{N} w_k^{(i)} \cdot \delta(\mathbf{x}_k - \mathbf{x}_k^{(i)})$$

优势：

• 可以处理任意非线性、非高斯系统
• 多模态分布（如多个可能位置）自然表达
• 不需要线性化假设

劣势：

• 计算量大（粒子数通常1000-10000）
• 粒子退化问题（需重采样）

2.4 自适应权重融合

不同定位源在不同场景下的可靠性不同，需要动态调整融合权重：

基于信号质量的权重：

$$w_i = \frac{q_i / \sigma_i^2}{\sum_{j=1}^{N} q_j / \sigma_j^2}$$

其中$q_i$为第$i$个定位源的信号质量指标，$\sigma_i^2$为其方差估计。

基于一致性的权重：

如果某个定位源的结果与其他源差异过大，降低其权重：

$$w_i' = w_i \cdot \exp\left(-\frac{d_i^2}{2\sigma_c^2}\right)$$

其中$d_i$为第$i$个定位源结果与融合结果的偏差。

三、代码实战

3.1 定位源抽象与统一接口

// PositionSource.ets - 定位源抽象接口

// 定位结果统一格式
interface PositionEstimate {
  x: number;              // X坐标(米)
  y: number;              // Y坐标(米)
  z?: number;             // Z坐标(米)，可选
  floor: number;          // 楼层
  building: string;       // 建筑
  accuracy: number;       // 精度估计(米)
  confidence: number;     // 置信度 0-1
  timestamp: number;      // 时间戳
  sourceType: PositionSourceType;  // 定位源类型
  rawRssi?: number;       // 原始RSSI（蓝牙/WiFi）
  velocityX?: number;     // X方向速度(m/s)
  velocityY?: number;     // Y方向速度(m/s)
}

// 定位源类型
enum PositionSourceType {
  BLE_BEACON = 'ble_beacon',
  WIFI_FINGERPRINT = 'wifi_fingerprint',
  UWB = 'uwb',
  PDR = 'pdr',
  MAGNETIC = 'magnetic',
  FUSED = 'fused',
}

// 定位源质量指标
interface SourceQuality {
  signalStrength: number;   // 信号强度 0-1
  stability: number;        // 稳定性 0-1
  coverage: number;         // 覆盖率 0-1
  overallQuality: number;   // 综合质量 0-1
}

// 定位源抽象接口
interface IPositionSource {
  readonly type: PositionSourceType;
  isAvailable(): boolean;
  getPosition(): PositionEstimate | null;
  getQuality(): SourceQuality;
  start(): void;
  stop(): void;
  setOnPositionUpdate(callback: (position: PositionEstimate) => void): void;
}

3.2 PDR航位推算实现

// PDRProcessor.ets - 行人航位推算
import { sensor } from '@kit.SensorServiceKit';

// PDR状态
interface PDRState {
  x: number;               // 当前X坐标
  y: number;               // 当前Y坐标
  heading: number;          // 当前航向(弧度)
  stepLength: number;       // 当前步长估计
  stepCount: number;        // 步数计数
  lastStepTime: number;     // 上次步频时间
  isInitialized: boolean;   // 是否已初始化
}

// 步频检测结果
interface StepDetectionResult {
  isStep: boolean;
  stepLength: number;
  confidence: number;
}

export class PDRProcessor {
  private state: PDRState;
  private accelerometerData: number[] = [];  // 加速度模值缓冲
  private gyroscopeData: Array<{ wx: number; wy: number; wz: number; t: number }> = [];
  private lastAccelMagnitude: number = 0;
  private peakThreshold: number = 1.2;       // 步频检测峰值阈值(g)
  private valleyThreshold: number = 0.8;     // 步频检测谷值阈值(g)
  private minStepInterval: number = 250;     // 最小步间隔(ms)
  private lastPeakTime: number = 0;
  private isAboveThreshold: boolean = false;

  // 步长估计参数
  private stepFrequencyBuffer: number[] = [];
  private readonly STEPLENGTH_K = 0.4;       // 步长系数

  // 陀螺仪积分
  private headingFromGyro: number = 0;
  private lastGyroTime: number = 0;

  // 回调
  private onPositionUpdate?: (position: PositionEstimate) => void;

  constructor() {
    this.state = {
      x: 0,
      y: 0,
      heading: 0,
      stepLength: 0.65,  // 默认步长65cm
      stepCount: 0,
      lastStepTime: 0,
      isInitialized: false
    };
  }

  // 初始化PDR起始位置
  initialize(x: number, y: number, heading: number): void {
    this.state.x = x;
    this.state.y = y;
    this.state.heading = heading;
    this.state.isInitialized = true;
    console.info(`[PDR] 初始化: (${x}, ${y}), 航向 ${heading * 180 / Math.PI}°`);
  }

  // 设置位置更新回调
  setOnPositionUpdate(callback: (position: PositionEstimate) => void): void {
    this.onPositionUpdate = callback;
  }

  // 启动传感器监听
  start(): void {
    // 加速度计监听
    sensor.on(sensor.SensorType.ACCELEROMETER, (data: sensor.AccelerometerResponse) => {
      this.processAccelerometer(data.x, data.y, data.z);
    }, { interval: 20000000 }); // 50Hz

    // 陀螺仪监听
    sensor.on(sensor.SensorType.GYROSCOPE, (data: sensor.GyroscopeResponse) => {
      this.processGyroscope(data.x, data.y, data.z);
    }, { interval: 10000000 }); // 100Hz

    console.info('[PDR] 传感器已启动');
  }

  // 停止传感器监听
  stop(): void {
    sensor.off(sensor.SensorType.ACCELEROMETER);
    sensor.off(sensor.SensorType.GYROSCOPE);
    console.info('[PDR] 传感器已停止');
  }

  // 处理加速度计数据
  private processAccelerometer(ax: number, ay: number, az: number): void {
    // 计算加速度模值
    const magnitude = Math.sqrt(ax * ax + ay * ay + az * az);

    // 步频检测（峰值检测法）
    const stepResult = this.detectStep(magnitude);

    if (stepResult.isStep && this.state.isInitialized) {
      // 更新步长估计
      this.state.stepLength = stepResult.stepLength;
      this.state.stepCount++;

      // 更新位置
      this.state.x += this.state.stepLength * Math.cos(this.state.heading);
      this.state.y += this.state.stepLength * Math.sin(this.state.heading);
      this.state.lastStepTime = Date.now();

      // 发送位置更新
      this.emitPosition();
    }
  }

  // 步频检测（峰值检测算法）
  private detectStep(accelMagnitude: number): StepDetectionResult {
    const currentTime = Date.now();
    const result: StepDetectionResult = {
      isStep: false,
      stepLength: this.state.stepLength,
      confidence: 0
    };

    // 归一化加速度（除以重力加速度9.8）
    const normalizedMag = accelMagnitude / 9.8;

    // 峰值检测状态机
    if (!this.isAboveThreshold && normalizedMag > this.peakThreshold) {
      this.isAboveThreshold = true;
    } else if (this.isAboveThreshold && normalizedMag < this.valleyThreshold) {
      this.isAboveThreshold = false;

      // 检查步间隔
      if (currentTime - this.lastPeakTime > this.minStepInterval) {
        result.isStep = true;
        this.lastPeakTime = currentTime;

        // 步长估计：基于步频
        if (this.state.lastStepTime > 0) {
          const stepInterval = currentTime - this.state.lastStepTime;
          const stepFrequency = 1000 / stepInterval; // Hz

          // 步长与步频的关系：L = K * f + C
          // 简化模型：步长随步频增加而增加
          result.stepLength = Math.max(0.4, Math.min(0.9,
            this.STEPLENGTH_K * stepFrequency + 0.2
          ));

          result.confidence = Math.min(1, stepFrequency / 3.0);
        }
      }
    }

    this.lastAccelMagnitude = normalizedMag;
    return result;
  }

  // 处理陀螺仪数据
  private processGyroscope(wx: number, wy: number, wz: number): void {
    const currentTime = Date.now();

    if (this.lastGyroTime === 0) {
      this.lastGyroTime = currentTime;
      return;
    }

    const dt = (currentTime - this.lastGyroTime) / 1000.0; // 秒
    this.lastGyroTime = currentTime;

    // 绕Z轴旋转（航向变化）
    // wz为Z轴角速度(rad/s)
    this.headingFromGyro += wz * dt;

    // 归一化到 [0, 2π)
    while (this.headingFromGyro < 0) this.headingFromGyro += 2 * Math.PI;
    while (this.headingFromGyro >= 2 * Math.PI) this.headingFromGyro -= 2 * Math.PI;

    // 更新PDR航向
    this.state.heading = this.headingFromGyro;
  }

  // 发送位置更新
  private emitPosition(): void {
    if (!this.onPositionUpdate) return;

    const position: PositionEstimate = {
      x: Math.round(this.state.x * 100) / 100,
      y: Math.round(this.state.y * 100) / 100,
      floor: 1, // PDR不感知楼层
      building: '',
      accuracy: this.estimateAccuracy(),
      confidence: Math.max(0, 1 - this.state.stepCount * 0.005), // 逐步衰减
      timestamp: Date.now(),
      sourceType: PositionSourceType.PDR,
      velocityX: this.state.stepLength * Math.cos(this.state.heading),
      velocityY: this.state.stepLength * Math.sin(this.state.heading)
    };

    this.onPositionUpdate(position);
  }

  // 估计PDR精度（随步数增加而降低）
  private estimateAccuracy(): number {
    // PDR精度随步数线性退化
    return 0.2 + this.state.stepCount * 0.02; // 起始0.2m，每步增加0.02m
  }

  // 获取当前状态
  getState(): PDRState {
    return { ...this.state };
  }

  // 校正位置（由融合引擎调用）
  correctPosition(x: number, y: number, heading?: number): void {
    this.state.x = x;
    this.state.y = y;
    if (heading !== undefined) {
      this.state.heading = heading;
      this.headingFromGyro = heading;
    }
    // 校正后重置精度估计
    this.state.stepCount = 0;
  }
}

3.3 扩展卡尔曼滤波融合引擎

// FusionEngine.ets - 多源融合定位引擎
import { PositionEstimate, PositionSourceType, SourceQuality } from './PositionSource';
import { PDRProcessor } from './PDRProcessor';

// EKF状态向量：[x, y, vx, vy]
// 观测向量：[x_obs, y_obs]（来自各定位源）

// 融合定位结果
interface FusionResult {
  x: number;
  y: number;
  z?: number;
  floor: number;
  building: string;
  accuracy: number;           // CEP95精度(米)
  confidence: number;         // 置信度 0-1
  sourceWeights: Map<PositionSourceType, number>;  // 各源权重
  timestamp: number;
}

// EKF参数
interface EKFParams {
  processNoisePos: number;    // 位置过程噪声
  processNoiseVel: number;    // 速度过程噪声
  observationNoise: number;   // 观测噪声基准
  pdrWeight: number;          // PDR预测权重
}

export class FusionEngine {
  // EKF状态
  private stateX: number = 0;
  private stateY: number = 0;
  private stateVx: number = 0;
  private stateVy: number = 0;
  private floor: number = 1;
  private building: string = '';

  // EKF协方差矩阵 P (4x4)
  private P: number[][] = [
    [100, 0, 0, 0],
    [0, 100, 0, 0],
    [0, 0, 10, 0],
    [0, 0, 0, 10]
  ];

  // 各定位源最新结果
  private sourcePositions: Map<PositionSourceType, PositionEstimate> = new Map();
  // 各定位源权重
  private sourceWeights: Map<PositionSourceType, number> = new Map();
  // 各定位源质量
  private sourceQualities: Map<PositionSourceType, SourceQuality> = new Map();

  // PDR处理器
  private pdr: PDRProcessor;

  // EKF参数
  private params: EKFParams = {
    processNoisePos: 0.5,
    processNoiseVel: 1.0,
    observationNoise: 2.0,
    pdrWeight: 0.7
  };

  // 融合结果回调
  private onFusionUpdate?: (result: FusionResult) => void;

  // 历史结果（用于精度评估）
  private resultHistory: FusionResult[] = [];
  private maxHistorySize: number = 50;

  // 是否已初始化
  private isInitialized: boolean = false;

  constructor() {
    this.pdr = new PDRProcessor();

    // PDR位置更新回调
    this.pdr.setOnPositionUpdate((position: PositionEstimate) => {
      this.handlePDRUpdate(position);
    });
  }

  // 设置融合结果回调
  setOnFusionUpdate(callback: (result: FusionResult) => void): void {
    this.onFusionUpdate = callback;
  }

  // 初始化融合引擎
  initialize(x: number, y: number, floor: number, building: string): void {
    this.stateX = x;
    this.stateY = y;
    this.stateVx = 0;
    this.stateVy = 0;
    this.floor = floor;
    this.building = building;

    // 重置协方差
    this.P = [
      [1, 0, 0, 0],
      [0, 1, 0, 0],
      [0, 0, 1, 0],
      [0, 0, 0, 1]
    ];

    // 初始化PDR
    this.pdr.initialize(x, y, 0);
    this.isInitialized = true;

    console.info(`[Fusion] 初始化: (${x}, ${y}), ${building} ${floor}F`);
  }

  // 处理定位源更新
  handleSourceUpdate(position: PositionEstimate): void {
    this.sourcePositions.set(position.sourceType, position);

    // 更新源质量评估
    this.updateSourceQuality(position.sourceType, position);

    // 如果是绝对定位源（非PDR），执行EKF更新
    if (position.sourceType !== PositionSourceType.PDR) {
      this.ekfUpdate(position);
    }

    // 如果未初始化，使用第一个绝对定位结果初始化
    if (!this.isInitialized && position.sourceType !== PositionSourceType.PDR) {
      this.initialize(position.x, position.y, position.floor, position.building);
    }

    // 计算自适应权重
    this.computeAdaptiveWeights();

    // 发送融合结果
    this.emitFusionResult();
  }

  // 处理PDR更新
  private handlePDRUpdate(pdrPosition: PositionEstimate): void {
    if (!this.isInitialized) return;

    // EKF预测步骤：使用PDR的运动信息
    this.ekfPredict(pdrPosition.velocityX || 0, pdrPosition.velocityY || 0);

    // 发送融合结果
    this.emitFusionResult();
  }

  // EKF预测步骤
  private ekfPredict(vx: number, vy: number): void {
    const dt = 0.5; // 预测时间步长(秒)

    // 状态转移矩阵 F
    const F: number[][] = [
      [1, 0, dt, 0],
      [0, 1, 0, dt],
      [0, 0, 1, 0],
      [0, 0, 0, 1]
    ];

    // 预测状态
    const newState = this.matVecMul(F, [this.stateX, this.stateY, this.stateVx, this.stateVy]);
    this.stateX = newState[0];
    this.stateY = newState[1];
    this.stateVx = vx; // 使用PDR速度
    this.stateVy = vy;

    // 过程噪声协方差 Q
    const Q: number[][] = [
      [this.params.processNoisePos * dt, 0, 0, 0],
      [0, this.params.processNoisePos * dt, 0, 0],
      [0, 0, this.params.processNoiseVel * dt, 0],
      [0, 0, 0, this.params.processNoiseVel * dt]
    ];

    // 预测协方差: P = F * P * F^T + Q
    const FP = this.matMul(F, this.P);
    const FPFt = this.matMul(FP, this.transpose(F));
    this.P = this.matAdd(FPFt, Q);
  }

  // EKF更新步骤
  private ekfUpdate(observation: PositionEstimate): void {
    // 观测矩阵 H (2x4)
    const H: number[][] = [
      [1, 0, 0, 0],
      [0, 1, 0, 0]
    ];

    // 观测向量
    const z: number[] = [observation.x, observation.y];

    // 观测噪声协方差 R (2x2)
    const obsNoise = this.params.observationNoise * (1 - observation.confidence + 0.1);
    const R: number[][] = [
      [obsNoise * obsNoise, 0],
      [0, obsNoise * obsNoise]
    ];

    // 创新（观测残差）
    const predictedObs = this.matVecMul(H, [this.stateX, this.stateY, this.stateVx, this.stateVy]);
    const innovation: number[] = [
      z[0] - predictedObs[0],
      z[1] - predictedObs[1]
    ];

    // 创新协方差: S = H * P * H^T + R
    const HP = this.matMul(H, this.P);
    const HPHt = this.matMul(HP, this.transpose(H));
    const S = this.matAdd(HPHt, R);

    // 卡尔曼增益: K = P * H^T * S^(-1)
    const Sinv = this.mat2x2Inverse(S);
    const PHt = this.matMul(this.P, this.transpose(H));
    const K = this.matMul(PHt, Sinv);

    // 状态更新
    const correction = this.matVecMul(K, innovation);
    this.stateX += correction[0];
    this.stateY += correction[1];
    this.stateVx += correction[2];
    this.stateVy += correction[3];

    // 协方差更新: P = (I - K * H) * P
    const I = this.eye(4);
    const KH = this.matMul(K, H);
    const IKH = this.matSub(I, KH);
    this.P = this.matMul(IKH, this.P);

    // 校正PDR位置（防止PDR漂移）
    this.pdr.correctPosition(this.stateX, this.stateY);
  }

  // 更新定位源质量
  private updateSourceQuality(type: PositionSourceType, position: PositionEstimate): void {
    const existing = this.sourceQualities.get(type);

    const quality: SourceQuality = {
      signalStrength: Math.max(0, Math.min(1, (position.confidence + 0.5) / 1.5)),
      stability: existing ? this.updateStability(existing.stability, position) : 0.5,
      coverage: position.accuracy > 0 ? Math.max(0, 1 - position.accuracy / 20) : 0.5,
      overallQuality: 0
    };

    quality.overallQuality = (quality.signalStrength * 0.4 +
      quality.stability * 0.3 + quality.coverage * 0.3);

    this.sourceQualities.set(type, quality);
  }

  // 更新稳定性指标
  private updateStability(prevStability: number, position: PositionEstimate): number {
    // 检查与融合结果的偏差
    const deviation = Math.sqrt(
      (position.x - this.stateX) ** 2 + (position.y - this.stateY) ** 2
    );

    // 偏差越小稳定性越高
    const instantStability = Math.max(0, 1 - deviation / 10);

    // 指数移动平均
    return 0.7 * prevStability + 0.3 * instantStability;
  }

  // 计算自适应权重
  private computeAdaptiveWeights(): void {
    let totalWeight = 0;
    const weights: Map<PositionSourceType, number> = new Map();

    for (const [type, quality] of this.sourceQualities) {
      const position = this.sourcePositions.get(type);
      if (!position) continue;

      // 权重 = 质量指标 / 精度
      const weight = quality.overallQuality / (position.accuracy + 0.1);
      weights.set(type, weight);
      totalWeight += weight;
    }

    // 归一化
    if (totalWeight > 0) {
      for (const [type, weight] of weights) {
        this.sourceWeights.set(type, weight / totalWeight);
      }
    }
  }

  // 发送融合结果
  private emitFusionResult(): void {
    if (!this.isInitialized) return;

    // 计算CEP95精度估计
    const cep95 = 1.73 * Math.sqrt(this.P[0][0] + this.P[1][1]);

    // 计算置信度
    const maxWeight = Math.max(...Array.from(this.sourceWeights.values()), 0);
    const confidence = Math.min(1, maxWeight * 1.5);

    const result: FusionResult = {
      x: Math.round(this.stateX * 100) / 100,
      y: Math.round(this.stateY * 100) / 100,
      floor: this.floor,
      building: this.building,
      accuracy: Math.round(cep95 * 100) / 100,
      confidence: Math.round(confidence * 100) / 100,
      sourceWeights: new Map(this.sourceWeights),
      timestamp: Date.now()
    };

    this.resultHistory.push(result);
    if (this.resultHistory.length > this.maxHistorySize) {
      this.resultHistory.shift();
    }

    this.onFusionUpdate?.(result);
  }

  // 启动PDR
  startPDR(): void {
    this.pdr.start();
  }

  // 停止PDR
  stopPDR(): void {
    this.pdr.stop();
  }

  // 获取融合历史
  getResultHistory(): FusionResult[] {
    return this.resultHistory;
  }

  // 获取当前权重
  getSourceWeights(): Map<PositionSourceType, number> {
    return new Map(this.sourceWeights);
  }

  // === 矩阵运算工具 ===

  // 矩阵乘向量
  private matVecMul(A: number[][], v: number[]): number[] {
    return A.map(row => row.reduce((sum, a, j) => sum + a * v[j], 0));
  }

  // 矩阵乘法
  private matMul(A: number[][], B: number[][]): number[][] {
    const rows = A.length;
    const cols = B[0].length;
    const inner = B.length;
    const result: number[][] = [];

    for (let i = 0; i < rows; i++) {
      result[i] = [];
      for (let j = 0; j < cols; j++) {
        let sum = 0;
        for (let k = 0; k < inner; k++) {
          sum += A[i][k] * B[k][j];
        }
        result[i][j] = sum;
      }
    }
    return result;
  }

  // 矩阵转置
  private transpose(A: number[][]): number[][] {
    const rows = A.length;
    const cols = A[0].length;
    const result: number[][] = [];
    for (let j = 0; j < cols; j++) {
      result[j] = [];
      for (let i = 0; i < rows; i++) {
        result[j][i] = A[i][j];
      }
    }
    return result;
  }

  // 矩阵加法
  private matAdd(A: number[][], B: number[][]): number[][] {
    return A.map((row, i) => row.map((a, j) => a + B[i][j]));
  }

  // 矩阵减法
  private matSub(A: number[][], B: number[][]): number[][] {
    return A.map((row, i) => row.map((a, j) => a - B[i][j]));
  }

  // 单位矩阵
  private eye(n: number): number[][] {
    return Array.from({ length: n }, (_, i) =>
      Array.from({ length: n }, (_, j) => i === j ? 1 : 0)
    );
  }

  // 2x2矩阵求逆
  private mat2x2Inverse(M: number[][]): number[][] {
    const det = M[0][0] * M[1][1] - M[0][1] * M[1][0];
    if (Math.abs(det) < 1e-10) {
      return [[1, 0], [0, 1]]; // 奇异矩阵返回单位阵
    }
    return [
      [M[1][1] / det, -M[0][1] / det],
      [-M[1][0] / det, M[0][0] / det]
    ];
  }
}

3.4 融合定位主页面

// pages/FusionPositioningPage.ets - 融合定位主页面
import { FusionEngine, FusionResult } from '../service/FusionEngine';
import { PositionEstimate, PositionSourceType } from '../service/PositionSource';

@Entry
@Component
struct FusionPositioningPage {
  @State fusionResult: FusionResult | null = null;
  @State blePosition: PositionEstimate | null = null;
  @State wifiPosition: PositionEstimate | null = null;
  @State uwbPosition: PositionEstimate | null = null;
  @State pdrPosition: PositionEstimate | null = null;
  @State isRunning: boolean = false;
  @State statusText: string = '未启动';
  @State bleWeight: number = 0;
  @State wifiWeight: number = 0;
  @State uwbWeight: number = 0;
  @State pdrWeight: number = 0;

  private fusionEngine: FusionEngine = new FusionEngine();

  aboutToAppear(): void {
    // 设置融合结果回调
    this.fusionEngine.setOnFusionUpdate((result: FusionResult) => {
      this.fusionResult = result;

      // 更新各源权重
      this.bleWeight = result.sourceWeights.get(PositionSourceType.BLE_BEACON) || 0;
      this.wifiWeight = result.sourceWeights.get(PositionSourceType.WIFI_FINGERPRINT) || 0;
      this.uwbWeight = result.sourceWeights.get(PositionSourceType.UWB) || 0;
      this.pdrWeight = result.sourceWeights.get(PositionSourceType.PDR) || 0;
    });
  }

  aboutToDisappear(): void {
    this.stopFusion();
  }

  build() {
    Scroll() {
      Column() {
        // 标题栏
        this.TitleBar()

        // 融合定位结果卡片
        this.FusionResultCard()

        // 各源权重可视化
        this.SourceWeightsCard()

        // 各定位源详情
        this.SourceDetailsCard()

        // 控制按钮
        this.ControlButtons()
      }
      .width('100%')
      .padding({ left: 16, right: 16, bottom: 24 })
    }
    .width('100%')
    .height('100%')
    .backgroundColor('#0a0a1a')
  }

  @Builder
  TitleBar() {
    Row() {
      Text('多源融合定位')
        .fontSize(22)
        .fontWeight(FontWeight.Bold)
        .fontColor('#ffffff')
      Blank()
      Row() {
        Circle()
          .width(8)
          .height(8)
          .fill(this.isRunning ? '#4ade80' : '#ef4444')
          .margin({ right: 6 })
        Text(this.statusText)
          .fontSize(13)
          .fontColor(this.isRunning ? '#4ade80' : '#94a3b8')
      }
    }
    .width('100%')
    .padding({ top: 16, bottom: 8 })
  }

  @Builder
  FusionResultCard() {
    Column() {
      Text('融合定位结果')
        .fontSize(12)
        .fontColor('#94a3b8')
        .margin({ bottom: 8 })

      if (this.fusionResult) {
        Row() {
          Column() {
            Text('X')
              .fontSize(11)
              .fontColor('#ef4444')
            Text(`${this.fusionResult.x.toFixed(2)}m`)
              .fontSize(24)
              .fontWeight(FontWeight.Bold)
              .fontColor('#ffffff')
              .margin({ top: 2 })
          }
          .alignItems(HorizontalAlign.Center)

          Column() {
            Text('Y')
              .fontSize(11)
              .fontColor('#4ade80')
            Text(`${this.fusionResult.y.toFixed(2)}m`)
              .fontSize(24)
              .fontWeight(FontWeight.Bold)
              .fontColor('#ffffff')
              .margin({ top: 2 })
          }
          .alignItems(HorizontalAlign.Center)
          .margin({ left: 32 })

          Blank()

          Column() {
            Text('精度')
              .fontSize(11)
              .fontColor('#3b82f6')
            Text(`±${this.fusionResult.accuracy.toFixed(2)}m`)
              .fontSize(20)
              .fontWeight(FontWeight.Bold)
              .fontColor('#4ade80')
              .margin({ top: 2 })
          }
          .alignItems(HorizontalAlign.End)
        }
        .width('100%')

        // 置信度条
        Row() {
          Text('置信度')
            .fontSize(11)
            .fontColor('#94a3b8')
          Row() {
            Row()
              .width(`${this.fusionResult.confidence * 100}%`)
              .height(6)
              .borderRadius(3)
              .backgroundColor('#4ade80')
          }
          .width(120)
          .height(6)
          .borderRadius(3)
          .backgroundColor('rgba(148, 163, 184, 0.2)')
          .margin({ left: 8 })
          Text(`${(this.fusionResult.confidence * 100).toFixed(0)}%`)
            .fontSize(11)
            .fontColor('#94a3b8')
            .margin({ left: 6 })
        }
        .margin({ top: 12 })
      } else {
        Text('等待定位...')
          .fontSize(18)
          .fontColor('#475569')
          .margin({ top: 20, bottom: 20 })
      }
    }
    .width('100%')
    .padding(20)
    .margin({ top: 8 })
    .borderRadius(16)
    .backgroundColor('rgba(30, 41, 59, 0.8)')
    .backdropBlur(20)
    .border({ width: 1, color: 'rgba(148, 163, 184, 0.1)' })
  }

  @Builder
  SourceWeightsCard() {
    Column() {
      Text('定位源权重分配')
        .fontSize(14)
        .fontWeight(FontWeight.Medium)
        .fontColor('#e2e8f0')
        .margin({ bottom: 12 })

      // 蓝牙权重
      this.WeightBar('蓝牙信标', this.bleWeight, '#3b82f6')
      // WiFi权重
      this.WeightBar('WiFi指纹', this.wifiWeight, '#f59e0b')
      // UWB权重
      this.WeightBar('UWB', this.uwbWeight, '#8b5cf6')
      // PDR权重
      this.WeightBar('PDR', this.pdrWeight, '#10b981')
    }
    .width('100%')
    .padding(16)
    .margin({ top: 12 })
    .borderRadius(16)
    .backgroundColor('rgba(30, 41, 59, 0.6)')
    .border({ width: 1, color: 'rgba(148, 163, 184, 0.08)' })
  }

  @Builder
  WeightBar(label: string, weight: number, color: string) {
    Row() {
      Text(label)
        .fontSize(12)
        .fontColor('#94a3b8')
        .width(70)
      Row() {
        Row()
          .width(`${weight * 100}%`)
          .height(8)
          .borderRadius(4)
          .backgroundColor(color)
      }
      .layoutWeight(1)
      .height(8)
      .borderRadius(4)
      .backgroundColor('rgba(148, 163, 184, 0.15)')
      Text(`${(weight * 100).toFixed(1)}%`)
        .fontSize(11)
        .fontColor('#94a3b8')
        .width(45)
        .textAlign(TextAlign.End)
    }
    .width('100%')
    .margin({ bottom: 8 })
  }

  @Builder
  SourceDetailsCard() {
    Column() {
      Text('各定位源详情')
        .fontSize(14)
        .fontWeight(FontWeight.Medium)
        .fontColor('#e2e8f0')
        .margin({ bottom: 12 })

      // 各源状态列表
      this.SourceItem('蓝牙信标', this.blePosition, '#3b82f6')
      this.SourceItem('WiFi指纹', this.wifiPosition, '#f59e0b')
      this.SourceItem('UWB', this.uwbPosition, '#8b5cf6')
      this.SourceItem('PDR', this.pdrPosition, '#10b981')
    }
    .width('100%')
    .padding(16)
    .margin({ top: 12 })
    .borderRadius(16)
    .backgroundColor('rgba(30, 41, 59, 0.6)')
    .border({ width: 1, color: 'rgba(148, 163, 184, 0.08)' })
  }

  @Builder
  SourceItem(label: string, position: PositionEstimate | null, color: string) {
    Row() {
      Circle()
        .width(8)
        .height(8)
        .fill(position ? color : '#475569')
        .margin({ right: 8 })

      Text(label)
        .fontSize(13)
        .fontColor('#e2e8f0')
        .width(70)

      if (position) {
        Text(`(${position.x.toFixed(2)}, ${position.y.toFixed(2)})`)
          .fontSize(12)
          .fontColor('#94a3b8')
          .layoutWeight(1)

        Text(`±${position.accuracy.toFixed(1)}m`)
          .fontSize(12)
          .fontColor(color)
      } else {
        Text('无信号')
          .fontSize(12)
          .fontColor('#475569')
          .layoutWeight(1)
      }
    }
    .width('100%')
    .padding({ top: 6, bottom: 6 })
  }

  @Builder
  ControlButtons() {
    Column() {
      Button(this.isRunning ? '停止融合定位' : '启动融合定位')
        .width('100%')
        .height(48)
        .fontSize(16)
        .fontWeight(FontWeight.Medium)
        .fontColor('#ffffff')
        .backgroundColor(this.isRunning ? '#dc2626' : '#2563eb')
        .borderRadius(12)
        .margin({ top: 16 })
        .onClick(() => {
          if (this.isRunning) {
            this.stopFusion();
          } else {
            this.startFusion();
          }
        })
    }
    .width('100%')
  }

  // 启动融合定位
  private startFusion(): void {
    this.isRunning = true;
    this.statusText = '融合定位中';

    // 初始化融合引擎
    this.fusionEngine.initialize(10, 5, 1, 'A栋');
    this.fusionEngine.startPDR();

    // 模拟各定位源数据（实际应连接真实定位模块）
    this.simulatePositionSources();
  }

  // 停止融合定位
  private stopFusion(): void {
    this.isRunning = false;
    this.statusText = '已停止';
    this.fusionEngine.stopPDR();
  }

  // 模拟定位源数据
  private simulatePositionSources(): void {
    // 模拟蓝牙定位更新
    setInterval(() => {
      if (!this.isRunning) return;
      const pos: PositionEstimate = {
        x: 10 + (Math.random() - 0.5) * 3,
        y: 5 + (Math.random() - 0.5) * 3,
        floor: 1,
        building: 'A栋',
        accuracy: 2.0 + Math.random(),
        confidence: 0.7 + Math.random() * 0.2,
        timestamp: Date.now(),
        sourceType: PositionSourceType.BLE_BEACON
      };
      this.blePosition = pos;
      this.fusionEngine.handleSourceUpdate(pos);
    }, 2000);

    // 模拟WiFi定位更新
    setInterval(() => {
      if (!this.isRunning) return;
      const pos: PositionEstimate = {
        x: 10 + (Math.random() - 0.5) * 5,
        y: 5 + (Math.random() - 0.5) * 5,
        floor: 1,
        building: 'A栋',
        accuracy: 4.0 + Math.random() * 2,
        confidence: 0.5 + Math.random() * 0.2,
        timestamp: Date.now(),
        sourceType: PositionSourceType.WIFI_FINGERPRINT
      };
      this.wifiPosition = pos;
      this.fusionEngine.handleSourceUpdate(pos);
    }, 3000);

    // 模拟UWB定位更新
    setInterval(() => {
      if (!this.isRunning) return;
      const pos: PositionEstimate = {
        x: 10 + (Math.random() - 0.5) * 0.6,
        y: 5 + (Math.random() - 0.5) * 0.6,
        floor: 1,
        building: 'A栋',
        accuracy: 0.2 + Math.random() * 0.1,
        confidence: 0.9 + Math.random() * 0.1,
        timestamp: Date.now(),
        sourceType: PositionSourceType.UWB
      };
      this.uwbPosition = pos;
      this.fusionEngine.handleSourceUpdate(pos);
    }, 500);
  }
}

四、踩坑与注意事项

4.1 时间同步问题

问题：不同定位源的更新频率不同（UWB 5Hz、蓝牙 0.5Hz、WiFi 0.3Hz、PDR 2Hz），数据时间戳不一致。

解决方案：

1. 统一时间基准：所有定位源使用Date.now()统一时间戳
2. 时间对齐：在EKF更新前，将旧数据通过运动模型外推到当前时刻
3. 异步更新：每个定位源独立更新EKF，不等待其他源

4.2 坐标系统一

问题：不同定位源可能使用不同的坐标系（像素坐标、米制坐标、地理坐标）。

解决方案：

• 定义统一的米制坐标系（以建筑某角点为原点）
• 所有定位源在输出前完成坐标变换
• 地图渲染时再从米制坐标变换为像素坐标

4.3 EKF调参

EKF的性能高度依赖过程噪声Q和观测噪声R的设置：

调参建议：

• Q根据PDR精度设置：步长估计误差约5-10%，对应Q_pos ≈ 0.3-0.8
• R根据各定位源精度设置：蓝牙R ≈ 2-5，WiFi R ≈ 5-15，UWB R ≈ 0.1-0.5
• 使用自适应R：根据信号质量动态调整

4.4 粒子滤波的工程化

粒子滤波虽然理论优势明显，但工程化挑战大：

1. 粒子数选择：太少精度不够，太多计算量大。建议2D定位500-2000个粒子
2. 重采样策略：系统重采样计算简单但方差大；残差重采样更优
3. 粒子退化：有效粒子数低于阈值时触发重采样
4. 实时性：粒子滤波的计算量约为EKF的10-50倍，需在TaskPool中执行

4.5 传感器标定

PDR的精度高度依赖传感器标定：

1. 加速度计零偏校准：静止时采集1000个样本取均值
2. 陀螺仪零偏校准：静止时采集1000个样本取均值
3. 磁力计硬铁/软铁校准：8字形旋转采集数据，椭球拟合
4. 步长模型校准：让用户走已知距离，拟合步长参数

五、HarmonyOS 6适配

5.1 传感器融合框架

HarmonyOS 6提供了更高级的传感器融合API：

// HarmonyOS 6 传感器融合框架
import { sensorFusion } from '@kit.SensorServiceKit';

// 创建融合定位会话
const fusionSession = sensorFusion.createPositionSession({
  sources: [
    sensorFusion.SourceType.BLE,
    sensorFusion.SourceType.WIFI,
    sensorFusion.SourceType.UWB,
    sensorFusion.SourceType.PDR,
  ],
  fusionAlgorithm: sensorFusion.Algorithm.EKF,
  updateInterval: 200, // 200ms
});

// 监听融合定位结果
fusionSession.on('positionUpdate', (result: sensorFusion.FusedPosition) => {
  console.info(`[Fusion] 位置: (${result.x}, ${result.y}), 精度: ${result.accuracy}m`);
});

5.2 自适应融合策略

// HarmonyOS 6 自适应融合
import { sensorFusion } from '@kit.SensorServiceKit';

// 自适应融合策略配置
const adaptiveConfig: sensorFusion.AdaptiveConfig = {
  // 根据场景自动切换融合策略
  strategy: sensorFusion.Strategy.ADAPTIVE,
  // 场景识别
  sceneDetection: true,
  // 精度优先模式（UWB权重最高）
  accuracyMode: {
    uwbWeight: 0.7,
    bleWeight: 0.2,
    pdrWeight: 0.1,
  },
  // 鲁棒性优先模式（多源均匀融合）
  robustMode: {
    uwbWeight: 0.3,
    bleWeight: 0.3,
    wifiWeight: 0.2,
    pdrWeight: 0.2,
  },
};

5.3 TaskPool并行计算

// HarmonyOS 6 TaskPool并行融合计算
import { taskpool } from '@kit.ArkTS';

@Concurrent
function runParticleFilter(particles: Float64Array, observations: Float64Array): Float64Array {
  // 粒子滤波计算（在子线程执行）
  // ...
  return new Float64Array(0);
}

// 在主线程调度
const task = new taskpool.Task(runParticleFilter, particles, observations);
const result = await taskpool.execute(task);

六、总结

本文系统讲解了基于HarmonyOS的定位精度优化与多源融合技术，核心要点回顾：

flowchart TB
    subgraph 多源融合架构
        direction TB
        A["多源数据采集<br/>蓝牙+WiFi+UWB+PDR"] --> B["信号质量评估<br/>置信度+稳定性+覆盖率"]
        B --> C{"融合算法选择"}
        C -->|"线性/弱非线性"| D["EKF/UKF<br/>计算量小"]
        C -->|"强非线性/非高斯"| E["粒子滤波<br/>精度高"]
        D --> F["自适应权重分配<br/>质量/精度/一致性"]
        E --> F
        F --> G["融合定位输出<br/>(x,y)±accuracy"]
    end

    classDef input fill:#533483,stroke:#e94560,color:#eee,stroke-width:2px
    classDef process fill:#16213e,stroke:#0f3460,color:#eee,stroke-width:2px
    classDef decision fill:#0f3460,stroke:#e94560,color:#eee,stroke-width:2px
    classDef output fill:#1a1a2e,stroke:#4ade80,color:#eee,stroke-width:2px

    class A input
    class B,C process
    class D,E process
    class F process
    class G output

多源融合定位设计原则：

1. 分层融合：先同类融合（蓝牙多信标、WiFi多AP），再跨类融合
2. 异步更新：各定位源独立更新EKF，不互相阻塞
3. 降级策略：UWB失效→蓝牙+PDR，蓝牙失效→WiFi+PDR，全部失效→纯PDR
4. 精度评估：实时输出CEP95精度，让应用层根据精度调整行为
5. 能效平衡：高精度场景全源开启，低精度场景关闭UWB省电

多源融合定位是室内定位系统的"大脑"，将多种定位技术的优势整合，弥补各自的不足。通过EKF/UKF/粒子滤波等融合算法，结合自适应权重分配，可以在各种复杂室内环境下实现稳定、精确的定位。至此，室内定位系列五篇文章全部完成，从蓝牙信标到WiFi指纹，从UWB精确定位到室内导航，再到多源融合优化，构建了完整的室内定位技术体系。