河池市网站建设_网站建设公司_Python_seo优化

一、IMU 卡尔曼滤波简介

IMU 通常包含：

• 三轴加速度计（测量比力f）
• 三轴陀螺仪（测量角速度ω）

由于传感器存在噪声和漂移，需用卡尔曼滤波（Kalman Filter, KF）或其扩展形式（如 EKF）对姿态、速度、位置进行融合估计。

二、状态向量定义

我们估计以下 9 维状态向量：

x= [pᵀ,vᵀ,a_bᵀ ]ᵀ
其中：

• p∈ ℝ³：位置（Position）
• v∈ ℝ³：速度（Velocity）
• a_b∈ ℝ³：加速度计零偏（Bias of accelerometer）

三、系统动态模型（状态方程）

假设采样周期为 Δt，系统动态由牛顿运动学描述：

状态转移方程：

xₖ=F·xₖ₋₁+B·uₖ+wₖ

其中：

• uₖ=fₖ是加速度计测量值（比力，单位：m/s²）
• wₖ～ N(0,Q) 是过程噪声
• F是状态转移矩阵（9×9）
• B是控制输入矩阵（9×3）

具体矩阵形式：

F=
⎡ I₃ Δt·I₃ 0₃ ⎤
⎢ 0₃ I₃ -Δt·I₃ ⎥
⎣ 0₃ 0₃ I₃ ⎦

B=
⎡ 0.5·Δt²·I₃ ⎤
⎢ Δt·I₃ ⎥
⎣ 0₃ ⎦

其中 I₃ 是 3×3 单位阵，0₃ 是 3×3 零矩阵。

物理含义：

• 位置更新：pₖ=pₖ₋₁+ Δt·vₖ₋₁+ 0.5·Δt²·(fₖ−a_b,ₖ₋₁)
• 速度更新：vₖ=vₖ₋₁+ Δt·(fₖ−a_b,ₖ₋₁)
• 偏置假设缓慢变化：a_b,ₖ≈a_b,ₖ₋₁

四、观测模型（量测方程）

假设我们有外部位置观测（如 GPS、视觉定位），提供位置测量：

zₖ=H·xₖ+vₖ

其中：

• zₖ∈ ℝ³：位置观测值（如 GPS 输出）
• vₖ～ N(0,R)：观测噪声
• H= [ I₃, 0₃, 0₃ ]（3×9 矩阵，只观测位置）

若无外部观测，则无法校正加速度计偏置，需引入其他约束（如静止检测、零速修正等）。

五、卡尔曼滤波算法步骤

初始化：x̂₀,P₀

For each time step k:

1. 预测（Predict）
   x̂ₖ⁻=F·x̂ₖ₋₁+B·uₖ
   Pₖ⁻=F·Pₖ₋₁·Fᵀ +Q
2. 更新（Update）
   Kₖ=Pₖ⁻·Hᵀ · (H·Pₖ⁻·Hᵀ +R)⁻¹
   x̂ₖ=x̂ₖ⁻+Kₖ· (zₖ−H·x̂ₖ⁻)
   Pₖ= ( I −Kₖ·H) ·Pₖ⁻

六、噪声协方差矩阵设定

• Q（过程噪声协方差）：反映模型不确定性
  通常设为块对角：
  Q= diag( q_p·I₃, q_v·I₃, q_b·I₃ )
  其中 q_p、q_v、q_b 为标量噪声强度。
• R（观测噪声协方差）：由传感器精度决定
  R= r_gps·I₃（例如 r_gps = 1.0 表示 GPS 1σ 误差为 1 米）