一、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:
- 预测（Predict）
x̂ₖ⁻ = F · x̂ₖ₋₁ + B · uₖ
Pₖ⁻ = F · Pₖ₋₁ · F ᵀ + Q - 更新（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 米）