扩展卡尔曼滤波 IMU

### 扩展卡尔曼滤波用于IMU数据融合和姿态估计

#### EKF基本概念
扩展卡尔曼滤波（Extended Kalman Filter, EKF）是对标准卡尔曼滤波的一种推广，适用于非线性系统的状态估计问题。EKF通过泰勒级数展开将非线性函数近似为线性形式，在每次迭代过程中更新预测的状态向量及其协方差矩阵。

对于IMU而言，由于其输出的是角速度、加速度等物理量而非直接的姿态信息，因此需要借助于四元数或其他表示方式来描述旋转变化并完成从局部坐标系到全局坐标系之间的转换操作[^1]。

#### 状态空间建模
为了利用EKF来进行IMU的数据处理，首先要建立合适的状态空间模型：

- **状态变量**：通常选取包含位置、速度以及偏置项在内的多个参数构成列向量\( \mathbf{x}=[p_x,p_y,v_x,v_y,b_g,b_a]^T\) ，其中下标g代表陀螺仪偏差而a则对应加速计零位漂移；

- **观测方程**：假设存在外部参照源如GPS可以给出较为精确的位置读数，则可定义如下关系式\[ z=\begin{bmatrix}
h(\mathbf{x})\\
\omega_{imu}\\
f_{imu}\end{bmatrix},\quad H_j=J_h|_\mathbf{\hat{x}}\]

这里\( h() \) 表达了由内部传感器计算所得理论值同实际测量结果间的映射；同时引入雅克比矩阵 \(H_j\) 来反映两者间联系程度的变化趋势[^2].

#### 预测阶段
根据前一时刻已知条件推测当前瞬时可能存在的各种情况:

// C++ Pseudo Code for Prediction Step of EKF with IMU data.
MatrixXd F(n,n); // State transition matrix based on time difference dt and motion model parameters.
VectorXd u(m);   // Control input vector which could be zero if no external force applied.

predict_state = F * last_state + B*u;
P_pred = F*last_covariance*F.transpose()+Q; // Process noise covariance Q should also consider sensor errors from IMU.

此部分涉及到运动学公式的具体应用，即考虑时间间隔dt内物体沿不同方向移动了多少距离，并据此调整预期中的各个分量大小。此外还需加入过程噪声的影响因素以体现不确定性[^3].

#### 更新/校正环节
当接收到新的传感信号后便立即执行修正动作使预估值更贴近真实状况：

// C++ Pseudo Code for Update Step of EKF with IMU/GPS fusion.
VectorXd y = measurement - H*predicted_state;

MatrixXd S = H*P_pred*H.transpose() + R;    // Measurement Noise Covariance Matrix R needs to reflect GPS accuracy level.
MatrixXd K = P_pred*H.transpose()*S.inverse(); 

updated_state = predicted_state + K*y;
updated_P = (I-K*H)*P_pred*(I-K*H).transpose() + K*R*K.transpose();

上述代码片段展示了如何依据最新获取的信息重新评估先前做出的猜测，从而获得更加可靠的结论。特别注意这里的增益系数K决定了信任度分配的比例——既不会过分依赖也不完全忽视任一方所提供的线索[^4].