一、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 米）
