PythonRobotics 扩展卡尔曼滤波EKF定位实战从状态方程到速度尺度因子修正的完整推导【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRoboticsEKFExtended Kalman Filter定位是移动机器人将航位推算Dead Reckoning与 GNSS 位置观测进行传感器融合的经典方法。本文以 PythonRobotics 仓库中的Localization/extended_kalman_filter模块为蓝本完整推导 EKF 的预测/更新公式、运动模型与观测模型的雅可比矩阵并剖析一个带速度尺度因子修正的进阶变体让读者不仅能读懂代码还能掌握从数学公式到可运行仿真的完整链路。EKF 定位一次传感器融合仿真概览本模块对应仓库中的两个仿真样例文档出处为 extended_kalman_filter_localization_main.rstextended_kalman_filter.py基础的位置估计 EKFekf_with_velocity_correction.py带速度尺度因子修正的 EKF。仿真的可视化含义如下图中各线条的颜色约定在文档中有明确说明元素颜色/样式含义真实轨迹蓝色实线由运动模型无噪声递推得到的理想轨迹True trajectory航位推算轨迹黑色实线仅用带噪声的里程计/陀螺仪输入积分得到的轨迹Dead reckoning位置观测绿色散点类似 GPS/GNSS 的 x-y 位置观测EKF 估计轨迹红色实线EKF 融合后的位置估计Estimated trajectory协方差椭圆红色椭圆由状态协方差矩阵 P 投影到 x-y 平面得到的估计不确定性从仿真结果可以看到航位推算轨迹会因输入噪声随时间漂移GPS 观测点散布在真实轨迹附近而 EKF 估计轨迹始终紧贴真实轨迹且红色协方差椭圆直观反映了每一时刻位置估计的不确定性。这正是传感器融合的价值用低成本、高频的里程计/陀螺仪进行连续预测用低精度、低频的 GNSS 观测周期性校正。EKF 算法核心预测Predict与更新Update文档中给出的 EKF 递推流程为标准的两步结构。设状态向量为 \(\mathbf{x}_t\)协方差矩阵为 \(P_t\)控制输入为 \(\mathbf{u}_t\)观测为 \(\mathbf{z}_t\)。预测步Predict——用运动模型前推状态并用其雅可比矩阵 \(J_f\) 传播协方差\[ x_{Pred} F x_t B u_t \]\[ P_{Pred} J_f P_t J_f^T Q \]更新步Update——用观测模型计算预测观测通过卡尔曼增益 \(K\) 融合新息 \(y\)\[ z_{Pred} H x_{Pred} \]\[ y z - z_{Pred} \]\[ S J_g P_{Pred} J_g^T R \]\[ K P_{Pred} J_g^T S^{-1} \]\[ x_{t1} x_{Pred} K y \]\[ P_{t1} (I - K J_g) P_{Pred} \]其中 \(Q\) 是过程噪声协方差矩阵\(R\) 是观测噪声协方差矩阵\(J_g\) 是观测模型雅可比矩阵。上述流程在代码中与公式一一对应见 extended_kalman_filter.py 的ekf_estimation函数def ekf_estimation(xEst, PEst, z, u): # Predict xPred motion_model(xEst, u) jF jacob_f(xEst, u) PPred jF PEst jF.T Q # Update jH jacob_h() zPred observation_model(xPred) y z - zPred S jH PPred jH.T R K PPred jH.T np.linalg.inv(S) xEst xPred K y PEst (np.eye(len(xEst)) - K jH) PPred return xEst, PEst注意代码中 \(K\) 的求解使用了np.linalg.inv(S)直接求逆在实际工程中当观测维数较高时通常改用 Cholesky 分解等数值更稳定的方式但作为教学演示直接求逆足够直观。滤波器设计状态、输入、观测与噪声参数状态向量4 维仿真机器人在时刻 \(t\) 的状态向量包含 4 个状态\[ \mathbf{x}_t [x_t, y_t, \phi_t, v_t]^T \]其中 \(x, y\) 是二维位置\(\phi\) 是航向角orientation\(v\) 是速度。在代码中状态估计量用xEst表示见 extended_kalman_filter.py初始化为np.zeros((4, 1))协方差矩阵 \(P_t\) 初始化为 4×4 单位阵。输入向量速度 陀螺仪角速度机器人配有速度传感器和陀螺仪gyro sensor因此每个时间步的控制输入为\[ \mathbf{u}_t [v_t, \omega_t]^T \]其中 \(\omega\) 是角速度。仿真中由calc_input()生成恒定输入extended_kalman_filter.py速度 \(v1.0\) m/s、角速度 \(\omega0.1\) rad/s对应一条圆弧轨迹。观测向量GNSS x-y 位置机器人配有 GNSS 传感器可以观测每个时刻的 x-y 位置\[ \mathbf{z}_t [x_t, y_t]^T \]噪声参数与仿真配置代码顶部的噪声协方差矩阵与仿真参数是理解整个仿真的关键extended_kalman_filter.py参数数值含义Qdiag([0.1, 0.1, deg2rad(1.0), 1.0]) ** 2过程噪声协方差x、y 位置 0.1航向角 1°转弧度速度 1.0Rdiag([1.0, 1.0]) ** 2观测噪声协方差GPS x-y 位置各 1.0INPUT_NOISEdiag([1.0, deg2rad(30.0)]) ** 2输入噪声速度 1.0 m/s、角速度 30°模拟较差的里程计/陀螺仪GPS_NOISEdiag([0.5, 0.5]) ** 2观测噪声GPS 位置各 0.5DT0.1时间步长 [s]SIM_TIME50.0仿真总时长 [s]**2表示对角元素先取标准差再平方得到方差即Q中 x 方向位置噪声标准差为 0.1。噪声的注入发生在observation()函数中extended_kalman_filter.py真实状态用无噪声输入推进观测值叠加GPS_NOISE高斯噪声而 EKF 实际收到的输入ud叠加了INPUT_NOISE噪声——这正是航位推算轨迹漂移的根源。运动模型恒速圆弧模型及其雅可比机器人的连续运动学模型为\[ \dot{x} v \cos(\phi), \quad \dot{y} v \sin(\phi), \quad \dot{\phi} \omega \]离散化后得到线性形式 \(\mathbf{x}_{t1} f(\mathbf{x}_t, \mathbf{u}_t) F\mathbf{x}_t B\mathbf{u}_t\)其中\[ F \begin{bmatrix} 1 0 0 0 \ 0 1 0 0 \ 0 0 1 0 \ 0 0 0 0 \end{bmatrix}, \quad B \begin{bmatrix} \cos(\phi)\Delta t 0 \ \sin(\phi)\Delta t 0 \ 0 \Delta t \ 1 0 \end{bmatrix} \]\(\Delta t\) 为时间间隔。展开后的运动函数为\[ \begin{bmatrix} x \ y \ \phi \ v \end{bmatrix} f(\mathbf{x}, \mathbf{u}) \begin{bmatrix} x v\cos(\phi)\Delta t \ y v\sin(\phi)\Delta t \ \phi \omega \Delta t \ v \end{bmatrix} \]对应代码实现见 extended_kalman_filter.py 的motion_model注意F中速度行全为 0、由B的第 4 行[1.0, 0.0]把输入速度直接赋给新状态实现 \(v v\)。由于 EKF 需要对非线性函数线性化其雅可比矩阵为\[ J_f \begin{bmatrix} 1 0 -v\sin(\phi)\Delta t \cos(\phi)\Delta t \ 0 1 v\cos(\phi)\Delta t \sin(\phi)\Delta t \ 0 0 1 0 \ 0 0 0 1 \end{bmatrix} \]实现于jacob_fextended_kalman_filter.py其中 \(\partial x/\partial \phi -v\Delta t\sin\phi\)、\(\partial x/\partial v \Delta t\cos\phi\) 等偏导数的推导在函数 docstring 中有完整注释便于对照学习。观测模型GPS 线性观测机器人从 GPS 获得 x-y 位置观测模型为线性形式 \(\mathbf{z}_t g(\mathbf{x}_t) H\mathbf{x}_t\)\[ H \begin{bmatrix} 1 0 0 0 \ 0 1 0 0 \end{bmatrix} \]即 \(\begin{bmatrix} x \ y \end{bmatrix} \begin{bmatrix} x \ y \end{bmatrix}\)其雅可比 \(J_g H\)。实现见observation_model与jacob_hextended_kalman_filter.py。由于观测是线性的这里 \(J_g\) 为常数矩阵直接用于更新步的 \(S\)、\(K\) 计算。运行与验证环境准备按仓库的 运行说明推荐使用 Python 3.12.x依赖安装方式二选一# 方式一conda conda env create -f requirements/environment.yml # 方式二pip pip install -r requirements/requirements.txt核心依赖包括 numpy、matplotlib、scipy完整清单见 requirements.txt。仿真的绘图功能依赖仓库公共工具 utils/plot.py 中的plot_covariance_ellipse它通过对协方差矩阵做特征分解以 \(\chi^23.0\)约 95% 置信度绘制椭圆。运行仿真python Localization/extended_kalman_filter/extended_kalman_filter.py运行后按 Esc 键可随时退出动画若要在无图形环境下运行可将代码中show_animation置为False。测试用例 test_extended_kalman_filter.py 正是通过m.show_animation False关闭动画后调用main()完成冒烟测试可用pytest tests/test_extended_kalman_filter.py快速验证模块可运行。进阶带速度尺度因子修正的 EKF动机车轮磨损导致的速度比例误差里程计速度测量常带有尺度因子误差scale factor error例如车轮磨损、胎压变化会使实测速度与实际速度成固定比例偏差。若航位推算持续使用错误的速度比例位置估计会系统性漂移。为此第二个样例 ekf_with_velocity_correction.py由 Ryohei Sasaki 修改在状态向量中显式加入比例因子并在线估计。如上图所示仿真在界面中同时显示True Velocity Scale Factor真值仿真中设为 0.9与Estimated Velocity Scale FactorEKF 在线估计值可直观看到估计值随滤波收敛逼近真值。状态向量扩展为 5 维\[ \mathbf{x}_t [x_t, y_t, \phi_t, v_t, s_t]^T \]新增的 \(s_t\) 为速度尺度因子。代码中xEst np.zeros((5, 1))且初始尺度因子设为 1.0而真实值true_scale_factor 0.9ekf_with_velocity_correction.py——即 EKF 需要从错误的初始假设出发通过观测逐步学会真实比例。运动模型与雅可比机器人模型变为\[ \dot{x} v s \cos(\phi), \quad \dot{y} v s \sin(\phi), \quad \dot{\phi} \omega \]\[ F \begin{bmatrix} 1 0 0 0 0 \ 0 1 0 0 0 \ 0 0 1 0 0 \ 0 0 0 0 0 \ 0 0 0 0 1 \end{bmatrix}, \quad B \begin{bmatrix} \cos(\phi)\Delta t \cdot s 0 \ \sin(\phi)\Delta t \cdot s 0 \ 0 \Delta t \ 1 0 \ 0 0 \end{bmatrix} \]运动函数为 \(x x v s \cos(\phi)\Delta t\)、\(y y v s \sin(\phi)\Delta t\)、\(\phi \phi \omega\Delta t\)、\(v v\)、\(s s\)对应的 5×5 雅可比ekf_with_velocity_correction.py\[ J_f \begin{bmatrix} 1 0 -v s \sin(\phi)\Delta t s\cos(\phi)\Delta t v\cos(\phi)\Delta t \ 0 1 v s \cos(\phi)\Delta t s\sin(\phi)\Delta t v\sin(\phi)\Delta t \ 0 0 1 0 0 \ 0 0 0 1 0 \ 0 0 0 0 1 \end{bmatrix} \]新增的两列正是尺度因子对位置更新的偏导数\(\partial x/\partial s v\cos(\phi)\Delta t\)、\(\partial y/\partial s v\sin(\phi)\Delta t\)它使得 GPS 观测能够反向修正 \(s\) 的估计。观测模型不变GPS 仍只观测 x-y 位置\(H\) 扩展为 2×5 矩阵\[ H \begin{bmatrix} 1 0 0 0 0 \ 0 1 0 0 0 \end{bmatrix} \]其余预测/更新逻辑与基础版完全一致The rest is the same as the Position Estimation Kalman Filter核心差异仅在状态维数、运动模型及其雅可比。值得注意的是该样例的噪声配置整体更小INPUT_NOISE速度 0.1、角速度 5°GPS_NOISE各 0.05Q中速度方差 0.4、尺度因子 0.1以便更精细地观测尺度因子的收敛过程。小结通过本文可以看到PythonRobotics 的 EKF 定位模块将公式 → 代码 → 可视化完整贯通标准 EKF用 4 维状态位置、航向、速度融合里程计/陀螺仪与 GPS预测步由运动模型雅可比传播协方差更新步由观测雅可比计算卡尔曼增益速度修正 EKF通过把尺度因子纳入状态向量在线估计可自动校正车轮磨损等造成的速度比例误差且公式推导与代码实现完全对应每个数学量都能在 Localization/extended_kalman_filter 中找到直接实现配合 协方差椭圆绘制工具 与单元测试是学习移动机器人传感器融合的优质入门样例。若希望进一步扩展可以尝试将恒速模型替换为更真实的自行车模型、引入偏航角速率观测、或把尺度因子估计推广到陀螺仪比例误差——现有代码结构motion_model/jacob_f/observation_model/ekf_estimation的分层为这些改动预留了清晰的扩展点。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考