在机器人控制中，准确的状态估计（如位置、速度、姿态）是反馈控制的基础。传感器（如 IMU、轮速计、关节编码器）存在噪声和漂移，单一传感器往往无法提供完整可靠的信息。因此，我们需要融合多传感器数据，通过滤波算法估计出更精确的状态。本章介绍状态估计的基本方法，包括卡尔曼滤波、扩展卡尔曼滤波（EKF）、IMU-轮速融合以及接触状态估计，并通过 MuJoCo 仿真验证。所有代码在MuJoCo 3.12.0下测试通过。
11.1 基础状态估计
11.1.1 卡尔曼滤波原理
卡尔曼滤波（Kalman Filter, KF）是一种递归的最优线性状态估计器，适用于线性高斯系统。它通过“预测-更新”循环，在存在过程噪声和观测噪声的情况下估计系统状态。
系统模型： \[ x_{k+1} = A x_k + B u_k + w_k \quad (过程噪声 w_k \sim \mathcal{N}(0, Q)) \] \[ z_k = H x_k + v_k \quad (观测噪声 v_k \sim \mathcal{N}(0, R)) \]
卡尔曼滤波步骤：
- 预测：根据模型和上一时刻状态预测当前状态及协方差。
- 更新：结合观测值修正预测，得到最优估计。
11.1.2 小示例：一维位置估计
假设一个物体沿直线匀速运动，我们通过带有噪声的位置传感器观测它。使用卡尔曼滤波估计位置和速度。
python << 'EOF' import
