HarmonyOS开发：多传感器融合与卡尔曼滤波

核心要点：本文系统讲解HarmonyOS多传感器融合的核心理论、卡尔曼滤波算法原理与实现、扩展卡尔曼滤波（EKF）在非线性系统中的应用，以及基于加速度计+陀螺仪+磁力计的9轴姿态估计完整方案，涵盖从数学推导到工程实践的完整技术链路。

一、背景与动机

1.1 为什么需要传感器融合

单一传感器存在固有局限性，无法满足高精度应用需求：

传感器融合的核心思想：利用不同传感器的互补特性，通过数学算法将多源数据融合为比任何单一传感器都更准确、更稳定的估计值。

1.2 传感器融合的典型应用

• 9轴姿态估计：加速度计+陀螺仪+磁力计融合，实现无漂移的3D姿态跟踪
• 步行导航（PDR）：加速度计检测步数，陀螺仪估计方向，气压计估计楼层
• 运动识别：多传感器特征融合，识别走路/跑步/骑行/静止等行为
• 手势识别：加速度+角速度融合，识别复杂手势
• VR/AR头部追踪：低延迟高精度的头部姿态估计

1.3 传感器融合架构

二、核心原理

2.1 卡尔曼滤波数学基础

卡尔曼滤波（Kalman Filter, KF）是一种最优递推估计算法，它基于系统的状态空间模型和噪声统计特性，通过预测-更新两步循环实现对系统状态的最小方差估计。

系统模型

状态方程（描述状态如何随时间演化）：

x_k = A·x_{k-1} + B·u_k + w_k

观测方程（描述如何从状态得到观测值）：

z_k = H·x_k + v_k

其中：

• x_k：k时刻的状态向量（我们要估计的量）
• A：状态转移矩阵（描述状态如何演化）
• B：控制输入矩阵
• u_k：控制输入向量
• w_k：过程噪声，~N(0, Q)
• z_k：k时刻的观测向量
• H：观测矩阵（描述状态到观测的映射）
• v_k：观测噪声，~N(0, R)
• Q：过程噪声协方差矩阵
• R：观测噪声协方差矩阵

卡尔曼滤波五步迭代

flowchart LR
    subgraph Predict["预测步 Prediction"]
        P1["状态预测<br/>x̂ₖ⁻ = A·x̂ₖ₋₁"]
        P2["协方差预测<br/>Pₖ⁻ = A·Pₖ₋₁·Aᵀ + Q"]
    end

    subgraph Update["更新步 Update"]
        U1["卡尔曼增益<br/>Kₖ = Pₖ⁻·Hᵀ·(H·Pₖ⁻·Hᵀ + R)⁻¹"]
        U2["状态更新<br/>x̂ₖ = x̂ₖ⁻ + Kₖ·(zₖ - H·x̂ₖ⁻)"]
        U3["协方差更新<br/>Pₖ = (I - Kₖ·H)·Pₖ⁻"]
    end

    P1 --> P2 --> U1 --> U2 --> U3
    U3 -->|下一时刻| P1

    classDef predictStyle fill:#1a1a2e,stroke:#e94560,color:#eee,stroke-width:2px
    classDef updateStyle fill:#0f3460,stroke:#533483,color:#eee,stroke-width:2px

    class P1,P2 predictStyle
    class U1,U2,U3 updateStyle

卡尔曼增益的物理意义：

• 当观测噪声R大时，K趋近0，更信任预测值
• 当过程噪声Q大时，K趋近1，更信任观测值
• K本质上是在预测和观测之间做最优加权

2.2 扩展卡尔曼滤波（EKF）

标准卡尔曼滤波要求系统是线性的。对于姿态估计等非线性系统，需要使用扩展卡尔曼滤波（Extended Kalman Filter, EKF）：

核心思想：在当前状态估计点对非线性函数进行一阶泰勒展开，得到线性化模型，然后应用标准卡尔曼滤波。

非线性状态方程: x_k = f(x_{k-1}, u_k) + w_k
非线性观测方程: z_k = h(x_k) + v_k

线性化:
A_k = ∂f/∂x |_{x̂_{k-1}}   (雅可比矩阵)
H_k = ∂h/∂x |_{x̂_k⁻}       (雅可比矩阵)

EKF的代价是引入了线性化误差，但对于温和的非线性系统，EKF仍然是实用且高效的选择。

2.3 互补滤波

互补滤波是一种更简单的传感器融合方法，特别适合加速度计+陀螺仪的姿态融合：

θ_fused = α · (θ_prev + ω·dt) + (1-α) · θ_accel

其中：

• α：互补滤波系数（通常0.95-0.98）
• θ_prev + ω·dt：陀螺仪积分（高频可信）
• θ_accel：加速度计计算的角度（低频可信）

互补滤波本质上是高通滤波陀螺仪 + 低通滤波加速度计，与卡尔曼滤波在简单场景下效果类似，但计算量更小。

三、代码实战

3.1 标量卡尔曼滤波器实现

// KalmanFilter.ets - 卡尔曼滤波器通用实现

/**
 * 矩阵工具类
 * 提供基本的矩阵运算，用于卡尔曼滤波
 */
export class Matrix {
  private data: number[][];
  readonly rows: number;
  readonly cols: number;

  constructor(rows: number, cols: number, initData?: number[][]) {
    this.rows = rows;
    this.cols = cols;
    if (initData) {
      this.data = initData;
    } else {
      this.data = Array.from({ length: rows }, () => new Array(cols).fill(0));
    }
  }

  get(i: number, j: number): number {
    return this.data[i][j];
  }

  set(i: number, j: number, value: number): void {
    this.data[i][j] = value;
  }

  /**
   * 矩阵加法
   */
  add(other: Matrix): Matrix {
    const result = new Matrix(this.rows, this.cols);
    for (let i = 0; i < this.rows; i++) {
      for (let j = 0; j < this.cols; j++) {
        result.set(i, j, this.data[i][j] + other.get(i, j));
      }
    }
    return result;
  }

  /**
   * 矩阵减法
   */
  subtract(other: Matrix): Matrix {
    const result = new Matrix(this.rows, this.cols);
    for (let i = 0; i < this.rows; i++) {
      for (let j = 0; j < this.cols; j++) {
        result.set(i, j, this.data[i][j] - other.get(i, j));
      }
    }
    return result;
  }

  /**
   * 矩阵乘法
   */
  multiply(other: Matrix): Matrix {
    const result = new Matrix(this.rows, other.cols);
    for (let i = 0; i < this.rows; i++) {
      for (let j = 0; j < other.cols; j++) {
        let sum = 0;
        for (let k = 0; k < this.cols; k++) {
          sum += this.data[i][k] * other.get(k, j);
        }
        result.set(i, j, sum);
      }
    }
    return result;
  }

  /**
   * 矩阵转置
   */
  transpose(): Matrix {
    const result = new Matrix(this.cols, this.rows);
    for (let i = 0; i < this.rows; i++) {
      for (let j = 0; j < this.cols; j++) {
        result.set(j, i, this.data[i][j]);
      }
    }
    return result;
  }

  /**
   * 矩阵求逆（仅支持小矩阵，使用伴随矩阵法）
   */
  inverse(): Matrix {
    if (this.rows === 1 && this.cols === 1) {
      return new Matrix(1, 1, [[1 / this.data[0][0]]]);
    }
    if (this.rows === 2 && this.cols === 2) {
      const det = this.data[0][0] * this.data[1][1] - this.data[0][1] * this.data[1][0];
      if (Math.abs(det) < 1e-10) {
        return this.identity();
      }
      return new Matrix(2, 2, [
        [this.data[1][1] / det, -this.data[0][1] / det],
        [-this.data[1][0] / det, this.data[0][0] / det]
      ]);
    }
    // 3x3及以上使用高斯消元法
    return this.gaussianInverse();
  }

  /**
   * 高斯消元法求逆
   */
  private gaussianInverse(): Matrix {
    const n = this.rows;
    // 构造增广矩阵 [A|I]
    const augmented: number[][] = [];
    for (let i = 0; i < n; i++) {
      augmented[i] = [...this.data[i], ...new Array(n).fill(0)];
      augmented[i][n + i] = 1;
    }

    // 前向消元
    for (let col = 0; col < n; col++) {
      let maxRow = col;
      for (let row = col + 1; row < n; row++) {
        if (Math.abs(augmented[row][col]) > Math.abs(augmented[maxRow][col])) {
          maxRow = row;
        }
      }
      [augmented[col], augmented[maxRow]] = [augmented[maxRow], augmented[col]];

      const pivot = augmented[col][col];
      if (Math.abs(pivot) < 1e-10) continue;

      for (let j = col; j < 2 * n; j++) {
        augmented[col][j] /= pivot;
      }

      for (let row = 0; row < n; row++) {
        if (row !== col) {
          const factor = augmented[row][col];
          for (let j = col; j < 2 * n; j++) {
            augmented[row][j] -= factor * augmented[col][j];
          }
        }
      }
    }

    // 提取逆矩阵
    const result = new Matrix(n, n);
    for (let i = 0; i < n; i++) {
      for (let j = 0; j < n; j++) {
        result.set(i, j, augmented[i][n + j]);
      }
    }
    return result;
  }

  /**
   * 单位矩阵
   */
  identity(): Matrix {
    const result = new Matrix(this.rows, this.cols);
    for (let i = 0; i < this.rows; i++) {
      result.set(i, i, 1);
    }
    return result;
  }

  /**
   * 缩放
   */
  scale(scalar: number): Matrix {
    const result = new Matrix(this.rows, this.cols);
    for (let i = 0; i < this.rows; i++) {
      for (let j = 0; j < this.cols; j++) {
        result.set(i, j, this.data[i][j] * scalar);
      }
    }
    return result;
  }

  /**
   * 转为一维数组
   */
  toFlatArray(): number[] {
    return this.data.flat();
  }

  /**
   * 从一维数组创建列向量
   */
  static fromColumnVector(vec: number[]): Matrix {
    const m = new Matrix(vec.length, 1);
    for (let i = 0; i < vec.length; i++) {
      m.set(i, 0, vec[i]);
    }
    return m;
  }
}

/**
 * 卡尔曼滤波器
 * 通用实现，支持任意维度的状态和观测
 */
export class KalmanFilter {
  // 状态向量
  private x: Matrix;
  // 状态协方差矩阵
  private P: Matrix;
  // 状态转移矩阵
  private A: Matrix;
  // 观测矩阵
  private H: Matrix;
  // 过程噪声协方差
  private Q: Matrix;
  // 观测噪声协方差
  private R: Matrix;
  // 控制输入矩阵
  private B: Matrix;

  /**
   * 构造卡尔曼滤波器
   * @param stateDim 状态维度
   * @param observeDim 观测维度
   * @param controlDim 控制输入维度
   */
  constructor(stateDim: number, observeDim: number, controlDim: number = 0) {
    this.x = new Matrix(stateDim, 1);
    this.P = new Matrix(stateDim, stateDim).identity().scale(1);
    this.A = new Matrix(stateDim, stateDim).identity();
    this.H = new Matrix(observeDim, stateDim);
    this.Q = new Matrix(stateDim, stateDim).identity().scale(0.01);
    this.R = new Matrix(observeDim, observeDim).identity().scale(1);
    this.B = new Matrix(stateDim, controlDim);
  }

  // Setter方法
  setState(x: Matrix): void { this.x = x; }
  setCovariance(P: Matrix): void { this.P = P; }
  setTransitionMatrix(A: Matrix): void { this.A = A; }
  setObservationMatrix(H: Matrix): void { this.H = H; }
  setProcessNoise(Q: Matrix): void { this.Q = Q; }
  setObservationNoise(R: Matrix): void { this.R = R; }
  setControlMatrix(B: Matrix): void { this.B = B; }

  /**
   * 预测步
   * @param u 控制输入向量
   */
  predict(u?: Matrix): void {
    // 状态预测: x̂ₖ⁻ = A·x̂ₖ₋₁ + B·uₖ
    this.x = this.A.multiply(this.x);
    if (u) {
      this.x = this.x.add(this.B.multiply(u));
    }

    // 协方差预测: Pₖ⁻ = A·Pₖ₋₁·Aᵀ + Q
    this.P = this.A.multiply(this.P).multiply(this.A.transpose()).add(this.Q);
  }

  /**
   * 更新步
   * @param z 观测向量
   */
  update(z: Matrix): void {
    // 卡尔曼增益: Kₖ = Pₖ⁻·Hᵀ·(H·Pₖ⁻·Hᵀ + R)⁻¹
    const Ht = this.H.transpose();
    const S = this.H.multiply(this.P).multiply(Ht).add(this.R);
    const K = this.P.multiply(Ht).multiply(S.inverse());

    // 状态更新: x̂ₖ = x̂ₖ⁻ + Kₖ·(zₖ - H·x̂ₖ⁻)
    const innovation = z.subtract(this.H.multiply(this.x));
    this.x = this.x.add(K.multiply(innovation));

    // 协方差更新: Pₖ = (I - Kₖ·H)·Pₖ⁻
    const I = new Matrix(this.P.rows, this.P.cols).identity();
    this.P = I.subtract(K.multiply(this.H)).multiply(this.P);
  }

  /**
   * 获取当前状态估计
   */
  getState(): Matrix {
    return this.x;
  }

  /**
   * 获取当前协方差
   */
  getCovariance(): Matrix {
    return this.P;
  }

  /**
   * 获取状态向量的某个分量
   */
  getStateComponent(index: number): number {
    return this.x.get(index, 0);
  }
}

3.2 一维卡尔曼滤波应用：气压海拔平滑

// BarometerKalmanFilter.ets - 气压海拔一维卡尔曼滤波

/**
 * 一维卡尔曼滤波器
 * 简化版本，无需矩阵运算
 * 适用于单变量状态估计
 */
export class ScalarKalmanFilter {
  // 状态估计
  private x: number;
  // 估计误差协方差
  private P: number;
  // 过程噪声方差
  private Q: number;
  // 观测噪声方差
  private R: number;

  constructor(initialValue: number = 0, processNoise: number = 0.01, observationNoise: number = 1.0) {
    this.x = initialValue;
    this.P = 1.0;
    this.Q = processNoise;
    this.R = observationNoise;
  }

  /**
   * 预测步（无控制输入）
   */
  predict(): void {
    // 一维情况：x̂⁻ = x̂（状态不变）
    // P⁻ = P + Q
    this.P += this.Q;
  }

  /**
   * 更新步
   * @param z 观测值
   */
  update(z: number): number {
    // 卡尔曼增益: K = P⁻ / (P⁻ + R)
    const K = this.P / (this.P + this.R);

    // 状态更新: x̂ = x̂⁻ + K·(z - x̂⁻)
    this.x = this.x + K * (z - this.x);

    // 协方差更新: P = (1 - K)·P⁻
    this.P = (1 - K) * this.P;

    return this.x;
  }

  /**
   * 一步滤波（预测+更新）
   */
  filter(z: number): number {
    this.predict();
    return this.update(z);
  }

  /**
   * 获取当前估计值
   */
  getValue(): number {
    return this.x;
  }

  /**
   * 设置过程噪声
   */
  setProcessNoise(Q: number): void {
    this.Q = Math.max(0.0001, Q);
  }

  /**
   * 设置观测噪声
   */
  setObservationNoise(R: number): void {
    this.R = Math.max(0.0001, R);
  }
}

/**
 * 气压海拔卡尔曼滤波应用
 */
@Entry
@Component
struct BarometerKalmanPage {
  // 原始气压
  @State rawPressure: number = 1013.25;
  // 滤波后气压
  @State filteredPressure: number = 1013.25;
  // 原始海拔
  @State rawAltitude: number = 0;
  // 滤波后海拔
  @State filteredAltitude: number = 0;
  // 卡尔曼增益显示
  @State kalmanGain: string = '';
  // 过程噪声参数
  @State processNoise: number = 0.01;
  // 观测噪声参数
  @State observationNoise: number = 1.0;

  // 卡尔曼滤波器实例
  private pressureKF: ScalarKalmanFilter = new ScalarKalmanFilter(1013.25, 0.01, 1.0);
  private altitudeKF: ScalarKalmanFilter = new ScalarKalmanFilter(0, 0.1, 5.0);

  /**
   * 处理气压数据
   */
  processBarometerData(pressure: number): void {
    this.rawPressure = Math.round(pressure * 100) / 100;

    // 气压卡尔曼滤波
    this.filteredPressure = Math.round(this.pressureKF.filter(pressure) * 100) / 100;

    // 计算原始海拔
    this.rawAltitude = this.calculateAltitude(pressure);

    // 海拔卡尔曼滤波
    this.filteredAltitude = Math.round(this.altitudeKF.filter(this.rawAltitude) * 10) / 10;
  }

  /**
   * 计算海拔
   */
  private calculateAltitude(pressure: number): number {
    const T0 = 288.15;
    const L = 0.0065;
    const exponent = 0.190263;
    return Math.round((T0 / L) * (1 - Math.pow(pressure / 1013.25, exponent)) * 10) / 10;
  }

  build() {
    Column() {
      Text('气压卡尔曼滤波')
        .fontSize(24)
        .fontWeight(FontWeight.Bold)
        .fontColor('#e0e0e0')
        .margin({ bottom: 24 })

      // 原始 vs 滤波对比
      Row() {
        Column() {
          Text('原始海拔')
            .fontSize(12)
            .fontColor('#888888')
          Text(`${this.rawAltitude}`)
            .fontSize(28)
            .fontWeight(FontWeight.Bold)
            .fontColor('#ff5722')
          Text('m')
            .fontSize(12)
            .fontColor('#888888')
        }
        .layoutWeight(1)

        Column() {
          Text('滤波海拔')
            .fontSize(12)
            .fontColor('#888888')
          Text(`${this.filteredAltitude}`)
            .fontSize(28)
            .fontWeight(FontWeight.Bold)
            .fontColor('#4caf50')
          Text('m')
            .fontSize(12)
            .fontColor('#888888')
        }
        .layoutWeight(1)
      }
      .width('90%')
      .padding(20)
      .borderRadius(16)
      .backgroundColor('#1a1a2e')
      .margin({ bottom: 16 })

      // 参数调节
      Column() {
        Text('滤波参数')
          .fontSize(16)
          .fontWeight(FontWeight.Bold)
          .fontColor('#e0e0e0')
          .margin({ bottom: 12 })

        Row() {
          Text('过程噪声 Q:')
            .fontSize(13)
            .fontColor('#888888')
            .width(100)
          Slider({ value: this.processNoise * 1000, min: 1, max: 100, step: 1 })
            .width('50%')
            .onChange((value: number) => {
              this.processNoise = value / 1000;
              this.altitudeKF.setProcessNoise(this.processNoise);
            })
          Text(`${this.processNoise.toFixed(3)}`)
            .fontSize(13)
            .fontColor('#e0e0e0')
            .width(60)
        }
        .margin({ bottom: 8 })

        Row() {
          Text('观测噪声 R:')
            .fontSize(13)
            .fontColor('#888888')
            .width(100)
          Slider({ value: this.observationNoise, min: 0.1, max: 20, step: 0.1 })
            .width('50%')
            .onChange((value: number) => {
              this.observationNoise = value;
              this.altitudeKF.setObservationNoise(this.observationNoise);
            })
          Text(`${this.observationNoise.toFixed(1)}`)
            .fontSize(13)
            .fontColor('#e0e0e0')
            .width(60)
        }
      }
      .width('90%')
      .padding(20)
      .borderRadius(16)
      .backgroundColor('#1a1a2e')
      .margin({ bottom: 16 })
    }
    .width('100%')
    .height('100%')
    .padding(16)
    .backgroundColor('#0a0a1a')
  }
}

3.3 9轴姿态估计：加速度计+陀螺仪+磁力计融合

// AttitudeEstimator.ets - 9轴姿态估计器
import sensor from '@ohos.sensor';
import { BusinessError } from '@ohos.base';

// 传感器数据接口
interface AccelData { x: number; y: number; z: number; timestamp: number; }
interface GyroData { x: number; y: number; z: number; timestamp: number; }
interface MagData { x: number; y: number; z: number; timestamp: number; }

// 姿态角（欧拉角表示）
interface EulerAngles {
  roll: number;    // 横滚角（-180° ~ 180°）
  pitch: number;   // 俯仰角（-90° ~ 90°）
  yaw: number;     // 航向角（0° ~ 360°）
}

/**
 * 9轴姿态估计器
 * 使用互补滤波融合加速度计、陀螺仪和磁力计
 */
export class AttitudeEstimator {
  // 当前姿态角（度）
  private roll: number = 0;
  private pitch: number = 0;
  private yaw: number = 0;

  // 互补滤波系数
  private alpha: number = 0.98; // 陀螺仪权重

  // 陀螺仪偏置（零偏校准值）
  private gyroBiasX: number = 0;
  private gyroBiasY: number = 0;
  private gyroBiasZ: number = 0;

  // 上次更新时间
  private lastTimestamp: number = 0;

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

  // 磁力计偏置（硬铁校准）
  private magBiasX: number = 0;
  private magBiasY: number = 0;
  private magBiasZ: number = 0;

  /**
   * 更新姿态（融合三个传感器）
   * @param accel 加速度计数据
   * @param gyro 陀螺仪数据
   * @param mag 磁力计数据
   * @returns 当前姿态角
   */
  update(accel: AccelData, gyro: GyroData, mag: MagData): EulerAngles {
    const now = accel.timestamp;
    let dt = (now - this.lastTimestamp) / 1e9; // 纳秒转秒
    this.lastTimestamp = now;

    // 限制dt范围，防止首帧或异常值
    if (dt <= 0 || dt > 0.5) dt = 0.01;

    // 从加速度计计算倾斜角（低频参考）
    const accelRoll = Math.atan2(accel.y, accel.z) * (180 / Math.PI);
    const accelPitch = Math.atan2(-accel.x,
      Math.sqrt(accel.y * accel.y + accel.z * accel.z)) * (180 / Math.PI);

    // 从磁力计计算航向角（低频参考）
    // 先将磁力计数据旋转到水平面
    const magX = mag.x - this.magBiasX;
    const magY = mag.y - this.magBiasY;
    const magZ = mag.z - this.magBiasZ;

    const rollRad = this.roll * (Math.PI / 180);
    const pitchRad = this.pitch * (Math.PI / 180);

    // 磁力计倾斜补偿
    const magHorizontalX = magX * Math.cos(pitchRad) +
      magY * Math.sin(rollRad) * Math.sin(pitchRad) +
      magZ * Math.cos(rollRad) * Math.sin(pitchRad);
    const magHorizontalY = magY * Math.cos(rollRad) -
      magZ * Math.sin(rollRad);

    const magYaw = Math.atan2(-magHorizontalY, magHorizontalX) * (180 / Math.PI);
    // 转换为0-360度
    const magYaw360 = magYaw < 0 ? magYaw + 360 : magYaw;

    // 陀螺仪积分（高频参考）
    const gyroX = (gyro.x - this.gyroBiasX) * (180 / Math.PI); // rad/s -> deg/s
    const gyroY = (gyro.y - this.gyroBiasY) * (180 / Math.PI);
    const gyroZ = (gyro.z - this.gyroBiasZ) * (180 / Math.PI);

    if (!this.initialized) {
      // 首次使用加速度计和磁力计初始化
      this.roll = accelRoll;
      this.pitch = accelPitch;
      this.yaw = magYaw360;
      this.initialized = true;
      return { roll: this.roll, pitch: this.pitch, yaw: this.yaw };
    }

    // 互补滤波融合
    // Roll: 陀螺仪积分 + 加速度计修正
    this.roll = this.alpha * (this.roll + gyroX * dt) +
      (1 - this.alpha) * accelRoll;

    // Pitch: 陀螺仪积分 + 加速度计修正
    this.pitch = this.alpha * (this.pitch + gyroY * dt) +
      (1 - this.alpha) * accelPitch;

    // Yaw: 陀螺仪积分 + 磁力计修正
    // 航向角需要特殊处理（角度环绕）
    let gyroYaw = this.yaw + gyroZ * dt;
    // 确保在0-360范围内
    gyroYaw = ((gyroYaw % 360) + 360) % 360;

    // 航向角互补滤波（处理角度环绕）
    let yawDiff = magYaw360 - gyroYaw;
    if (yawDiff > 180) yawDiff -= 360;
    if (yawDiff < -180) yawDiff += 360;

    this.yaw = gyroYaw + (1 - this.alpha) * yawDiff;
    this.yaw = ((this.yaw % 360) + 360) % 360;

    return {
      roll: Math.round(this.roll * 10) / 10,
      pitch: Math.round(this.pitch * 10) / 10,
      yaw: Math.round(this.yaw * 10) / 10
    };
  }

  /**
   * 陀螺仪零偏校准
   * 在设备静止时调用，采集N个样本取平均
   * @param samples 校准样本数组
   */
  calibrateGyro(samples: GyroData[]): void {
    if (samples.length === 0) return;

    let sumX = 0, sumY = 0, sumZ = 0;
    for (const s of samples) {
      sumX += s.x;
      sumY += s.y;
      sumZ += s.z;
    }

    this.gyroBiasX = sumX / samples.length;
    this.gyroBiasY = sumY / samples.length;
    this.gyroBiasZ = sumZ / samples.length;
  }

  /**
   * 磁力计硬铁校准
   * @param biasX X轴偏置
   * @param biasY Y轴偏置
   * @param biasZ Z轴偏置
   */
  calibrateMagnetometer(biasX: number, biasY: number, biasZ: number): void {
    this.magBiasX = biasX;
    this.magBiasY = biasY;
    this.magBiasZ = biasZ;
  }

  /**
   * 设置互补滤波系数
   * @param alpha 陀螺仪权重（0-1），越大越信任陀螺仪
   */
  setAlpha(alpha: number): void {
    this.alpha = Math.max(0.5, Math.min(0.999, alpha));
  }

  /**
   * 获取当前姿态
   */
  getAttitude(): EulerAngles {
    return {
      roll: Math.round(this.roll * 10) / 10,
      pitch: Math.round(this.pitch * 10) / 10,
      yaw: Math.round(this.yaw * 10) / 10
    };
  }

  /**
   * 重置姿态估计器
   */
  reset(): void {
    this.roll = 0;
    this.pitch = 0;
    this.yaw = 0;
    this.initialized = false;
    this.lastTimestamp = 0;
  }
}

/**
 * 9轴姿态估计页面
 */
@Entry
@Component
struct AttitudeEstimationPage {
  // 姿态角
  @State roll: number = 0;
  @State pitch: number = 0;
  @State yaw: number = 0;
  // 传感器原始数据
  @State accelText: string = '等待数据...';
  @State gyroText: string = '等待数据...';
  @State magText: string = '等待数据...';
  // 状态
  @State isRunning: boolean = false;
  @State statusText: string = '未启动';

  // 姿态估计器
  private estimator: AttitudeEstimator = new AttitudeEstimator();
  // 传感器订阅ID
  private accelSubId: number = -1;
  private gyroSubId: number = -1;
  private magSubId: number = -1;
  // 传感器数据缓存
  private latestAccel: AccelData | null = null;
  private latestGyro: GyroData | null = null;
  private latestMag: MagData | null = null;
  // 陀螺仪校准样本
  private gyroCalibrationSamples: GyroData[] = [];
  private isCalibrating: boolean = false;

  aboutToAppear() {
    this.startSensors();
  }

  aboutToDisappear() {
    this.stopSensors();
  }

  /**
   * 启动传感器
   */
  private startSensors(): void {
    try {
      // 加速度计
      if (sensor.isSensorAvailable(sensor.SensorType.ACCELEROMETER)) {
        this.accelSubId = sensor.on(sensor.SensorType.ACCELEROMETER,
          (data: AccelData) => {
            this.latestAccel = data;
            this.accelText = `X:${data.x.toFixed(2)} Y:${data.y.toFixed(2)} Z:${data.z.toFixed(2)}`;
            this.tryUpdateAttitude();
          }, { interval: 10000000 }); // 10ms
      }

      // 陀螺仪
      if (sensor.isSensorAvailable(sensor.SensorType.GYROSCOPE)) {
        this.gyroSubId = sensor.on(sensor.SensorType.GYROSCOPE,
          (data: GyroData) => {
            this.latestGyro = data;
            this.gyroText = `X:${data.x.toFixed(2)} Y:${data.y.toFixed(2)} Z:${data.z.toFixed(2)}`;
            if (this.isCalibrating) {
              this.gyroCalibrationSamples.push(data);
            }
            this.tryUpdateAttitude();
          }, { interval: 10000000 });
      }

      // 磁力计
      if (sensor.isSensorAvailable(sensor.SensorType.MAGNETOMETER)) {
        this.magSubId = sensor.on(sensor.SensorType.MAGNETOMETER,
          (data: MagData) => {
            this.latestMag = data;
            this.magText = `X:${data.x.toFixed(2)} Y:${data.y.toFixed(2)} Z:${data.z.toFixed(2)}`;
            this.tryUpdateAttitude();
          }, { interval: 20000000 }); // 20ms
      }

      this.isRunning = true;
      this.statusText = '运行中';
    } catch (error) {
      this.statusText = `启动失败: ${(error as BusinessError).message}`;
    }
  }

  private stopSensors(): void {
    if (this.accelSubId !== -1) {
      try { sensor.off(sensor.SensorType.ACCELEROMETER, this.accelSubId); } catch (e) {}
    }
    if (this.gyroSubId !== -1) {
      try { sensor.off(sensor.SensorType.GYROSCOPE, this.gyroSubId); } catch (e) {}
    }
    if (this.magSubId !== -1) {
      try { sensor.off(sensor.SensorType.MAGNETOMETER, this.magSubId); } catch (e) {}
    }
    this.isRunning = false;
    this.statusText = '已停止';
  }

  /**
   * 尝试更新姿态（需要三个传感器都有数据）
   */
  private tryUpdateAttitude(): void {
    if (!this.latestAccel || !this.latestGyro || !this.latestMag) {
      return;
    }

    const attitude = this.estimator.update(
      this.latestAccel, this.latestGyro, this.latestMag
    );

    this.roll = attitude.roll;
    this.pitch = attitude.pitch;
    this.yaw = attitude.yaw;
  }

  /**
   * 开始陀螺仪校准
   */
  private startGyroCalibration(): void {
    this.isCalibrating = true;
    this.gyroCalibrationSamples = [];
    this.statusText = '校准中...请保持设备静止';

    // 3秒后自动完成校准
    setTimeout(() => {
      this.estimator.calibrateGyro(this.gyroCalibrationSamples);
      this.isCalibrating = false;
      this.statusText = `校准完成（${this.gyroCalibrationSamples.length}个样本）`;
      this.gyroCalibrationSamples = [];
    }, 3000);
  }

  /**
   * 获取方向描述
   */
  private getDirectionText(yaw: number): string {
    if (yaw >= 337.5 || yaw < 22.5) return '北';
    if (yaw >= 22.5 && yaw < 67.5) return '东北';
    if (yaw >= 67.5 && yaw < 112.5) return '东';
    if (yaw >= 112.5 && yaw < 157.5) return '东南';
    if (yaw >= 157.5 && yaw < 202.5) return '南';
    if (yaw >= 202.5 && yaw < 247.5) return '西南';
    if (yaw >= 247.5 && yaw < 292.5) return '西';
    return '西北';
  }

  build() {
    Scroll() {
      Column() {
        // 标题
        Text('9轴姿态估计')
          .fontSize(24)
          .fontWeight(FontWeight.Bold)
          .fontColor('#e0e0e0')
          .margin({ bottom: 24 })

        // 姿态角显示
        Column() {
          Text('姿态角')
            .fontSize(14)
            .fontColor('#888888')

          Row() {
            Column() {
              Text('Roll')
                .fontSize(12)
                .fontColor('#888888')
              Text(`${this.roll}°`)
                .fontSize(28)
                .fontWeight(FontWeight.Bold)
                .fontColor('#e94560')
            }
            .layoutWeight(1)

            Column() {
              Text('Pitch')
                .fontSize(12)
                .fontColor('#888888')
              Text(`${this.pitch}°`)
                .fontSize(28)
                .fontWeight(FontWeight.Bold)
                .fontColor('#533483')
            }
            .layoutWeight(1)

            Column() {
              Text('Yaw')
                .fontSize(12)
                .fontColor('#888888')
              Text(`${this.yaw}°`)
                .fontSize(28)
                .fontWeight(FontWeight.Bold)
                .fontColor('#0f3460')
            }
            .layoutWeight(1)
          }

          // 方向指示
          Text(`方向: ${this.getDirectionText(this.yaw)}`)
            .fontSize(18)
            .fontWeight(FontWeight.Bold)
            .fontColor('#4caf50')
            .margin({ top: 8 })
        }
        .width('90%')
        .padding(24)
        .borderRadius(16)
        .backgroundColor('#1a1a2e')
        .margin({ bottom: 16 })

        // 传感器原始数据
        Column() {
          Text('传感器原始数据')
            .fontSize(14)
            .fontColor('#888888')
            .margin({ bottom: 8 })

          Text(`加速度: ${this.accelText}`)
            .fontSize(12)
            .fontColor('#e94560')
            .width('100%')
            .margin({ bottom: 4 })
          Text(`陀螺仪: ${this.gyroText}`)
            .fontSize(12)
            .fontColor('#533483')
            .width('100%')
            .margin({ bottom: 4 })
          Text(`磁力计: ${this.magText}`)
            .fontSize(12)
            .fontColor('#0f3460')
            .width('100%')
        }
        .width('90%')
        .padding(16)
        .borderRadius(12)
        .backgroundColor('#1a1a2e')
        .alignItems(HorizontalAlign.Start)
        .margin({ bottom: 16 })

        // 校准按钮
        Button(this.isCalibrating ? '校准中...' : '陀螺仪校准')
          .fontSize(14)
          .fontColor('#e0e0e0')
          .backgroundColor(this.isCalibrating ? '#333333' : '#0f3460')
          .borderRadius(8)
          .enabled(!this.isCalibrating)
          .onClick(() => this.startGyroCalibration())
          .margin({ bottom: 16 })

        // 状态
        Text(this.statusText)
          .fontSize(12)
          .fontColor('#888888')
      }
      .width('100%')
      .padding(16)
    }
    .width('100%')
    .height('100%')
    .backgroundColor('#0a0a1a')
  }
}

四、踩坑与注意事项

4.1 传感器融合常见问题

4.2 四元数vs欧拉角

// 四元数表示（避免万向节锁）
interface Quaternion {
  w: number;  // 标量部分
  x: number;  // 向量部分
  y: number;
  z: number;
}

/**
 * 四元数乘法
 * 用于组合旋转
 */
function quaternionMultiply(q1: Quaternion, q2: Quaternion): Quaternion {
  return {
    w: q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z,
    x: q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y,
    y: q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x,
    z: q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w
  };
}

/**
 * 四元数归一化
 * 防止数值误差累积导致四元数长度偏离1
 */
function quaternionNormalize(q: Quaternion): Quaternion {
  const norm = Math.sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
  if (norm < 1e-10) return { w: 1, x: 0, y: 0, z: 0 };
  return {
    w: q.w / norm,
    x: q.x / norm,
    y: q.y / norm,
    z: q.z / norm
  };
}

/**
 * 四元数转欧拉角
 */
function quaternionToEuler(q: Quaternion): EulerAngles {
  // Roll
  const sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
  const cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
  const roll = Math.atan2(sinr_cosp, cosr_cosp) * (180 / Math.PI);

  // Pitch
  const sinp = 2 * (q.w * q.y - q.z * q.x);
  const pitch = Math.abs(sinp) >= 1 ?
    Math.sign(sinp) * 90 :
    Math.asin(sinp) * (180 / Math.PI);

  // Yaw
  const siny_cosp = 2 * (q.w * q.z + q.x * q.y);
  const cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
  const yaw = Math.atan2(siny_cosp, cosy_cosp) * (180 / Math.PI);

  return { roll, pitch, yaw: ((yaw % 360) + 360) % 360 };
}

4.3 卡尔曼滤波参数调优

过程噪声Q：

• Q越大，滤波器响应越快，但平滑效果越差
• Q越小，滤波器越平滑，但响应越慢
• 气压海拔：Q = 0.01~0.1（海拔变化缓慢）
• 姿态估计：Q = 0.1~1.0（姿态变化较快）

观测噪声R：

• R越大，越不信任观测值，更依赖预测
• R越小，越信任观测值，但噪声抑制能力差
• 气压海拔：R = 1.0~5.0（传感器精度约±0.5hPa）
• 加速度计角度：R = 5.0~20.0（受运动加速度干扰大）

调优经验：

1. 先固定R，调整Q使响应速度合适
2. 再固定Q，调整R使平滑效果满意
3. Q/R的比值决定了滤波器的等效时间常数

4.4 性能优化建议

• 矩阵运算优化：对于固定维度的卡尔曼滤波，硬编码矩阵运算比通用矩阵类更高效
• 使用Float32Array：传感器数据使用TypedArray存储，减少GC压力
• 降低融合频率：姿态估计50Hz足够，不需要与传感器采样率一致
• Worker线程：复杂的EKF计算移至Worker线程，避免阻塞UI
• 增量更新：只更新变化的矩阵元素，避免全矩阵运算

五、HarmonyOS 6适配

5.1 新版传感器API变化

5.2 使用系统级旋转向量传感器

HarmonyOS 6提供了系统级融合的旋转向量传感器，可以直接获取融合后的姿态四元数，无需自行实现融合算法：

// HarmonyOS 6 旋转向量传感器（系统级融合）
import sensor from '@ohos.sensor';

// 旋转向量数据
interface RotationVectorData {
  x: number;       // 四元数向量部分x
  y: number;       // 四元数向量部分y
  z: number;       // 四元数向量部分z
  w: number;       // 四元数标量部分w
  headingAccuracy: number; // 航向精度（弧度）
  timestamp: number;
}

class SystemFusedAttitude {
  private subscriberId: number = -1;

  /**
   * 订阅系统级融合姿态
   * 直接获取加速度计+陀螺仪+磁力计融合后的四元数
   */
  subscribeAttitude(callback: (q: Quaternion, euler: EulerAngles) => void): void {
    try {
      // 优先使用旋转向量传感器（含磁力计）
      if (sensor.isSensorAvailable(sensor.SensorType.ROTATION_VECTOR)) {
        this.subscriberId = sensor.on(
          sensor.SensorType.ROTATION_VECTOR,
          (data: RotationVectorData) => {
            const q: Quaternion = {
              w: data.w, x: data.x, y: data.y, z: data.z
            };
            const euler = quaternionToEuler(q);
            callback(q, euler);
          },
          { interval: 20000000 } // 20ms = 50Hz
        );
      }
      // 降级到游戏旋转向量（不含磁力计，无绝对航向）
      else if (sensor.isSensorAvailable(sensor.SensorType.GAME_ROTATION_VECTOR)) {
        this.subscriberId = sensor.on(
          sensor.SensorType.GAME_ROTATION_VECTOR,
          (data: RotationVectorData) => {
            const q: Quaternion = {
              w: data.w, x: data.x, y: data.y, z: data.z
            };
            const euler = quaternionToEuler(q);
            callback(q, euler);
          },
          { interval: 20000000 }
        );
      } else {
        // 最终降级：自行实现融合
        console.warn('旋转向量传感器不可用，请使用自实现融合算法');
      }
    } catch (error) {
      console.error('姿态传感器订阅失败');
    }
  }
}

5.3 HarmonyOS 6传感器批量订阅

// HarmonyOS 6 批量传感器订阅
import sensor from '@ohos.sensor';

class BatchSensorSubscription {
  /**
   * 批量订阅多个传感器
   * 所有传感器数据通过统一回调返回，自动时间对齐
   */
  subscribeMultiple(): void {
    try {
      if (typeof sensor.subscribeMultiple === 'function') {
        sensor.subscribeMultiple(
          [
            sensor.SensorType.ACCELEROMETER,
            sensor.SensorType.GYROSCOPE,
            sensor.SensorType.MAGNETOMETER
          ],
          (data: Record<string, Object>) => {
            // data包含所有传感器的最新数据
            const accel = data['ACCELEROMETER'] as AccelData;
            const gyro = data['GYROSCOPE'] as GyroData;
            const mag = data['MAGNETOMETER'] as MagData;

            // 直接融合，无需手动对齐时间戳
            if (accel && gyro && mag) {
              // 调用融合算法...
            }
          },
          { samplingPeriod: 10000000 } // 10ms
        );
      }
    } catch (error) {
      console.error('批量订阅失败');
    }
  }
}

六、总结

6.1 核心技术要点回顾

6.2 最佳实践清单

1. ✅ 优先使用系统级旋转向量传感器，减少自行融合的开发量
2. ✅ 自行融合时使用互补滤波作为基础，卡尔曼滤波作为进阶
3. ✅ 使用四元数表示姿态，避免欧拉角万向节锁问题
4. ✅ 陀螺仪使用前必须进行零偏校准
5. ✅ 磁力计使用前进行硬铁/软铁校准
6. ✅ 卡尔曼滤波参数Q/R根据应用场景调优
7. ✅ 传感器数据时间对齐是融合的前提
8. ✅ 适配HarmonyOS 6旋转向量传感器和批量订阅

6.3 扩展方向

• 无迹卡尔曼滤波（UKF）：处理强非线性系统，无需计算雅可比矩阵
• 粒子滤波：处理非高斯噪声和非线性系统
• 深度学习融合：使用神经网络学习最优融合权重
• 分布式传感器融合：多设备传感器协同，提高估计精度
• VIO（视觉惯性里程计）：相机+IMU融合，实现6DoF位姿估计