36、电机扩展卡尔曼滤波器：原理、算法与应用

电机扩展卡尔曼滤波器：原理、算法与应用

1. 电机扩展卡尔曼滤波器基础

在运动估计领域，电机扩展卡尔曼滤波器（MEKF）是一种强大的工具。首先，我们来看线运动的动态模型。线的运动可以用以下方程描述：
- (L_i = M_i L_{i - 1} \widetilde{M_i})（式 1）
如果线在 (t_{i - 1}) 和 (t_i) 时刻之间的参数变化由电机速度 (V_{i/i - 1}) 描述，那么有 (L_i = V_{i/i - 1}L_{i - 1}\widetilde{V_{i/i - 1}})（式 2）。综合可得线的递归运动方程为 (L_i = M_i L_{i - 1} \widetilde{M_i} = (V_{i/i - 1}M_{i - 1})L_{i - 1}(\widetilde{M_{i - 1}}\widetilde{V_{i/i - 1}}))（式 3），进而得到理想的动态运动模型 (M_i = V_{i/i - 1}M_{i - 1})（式 4）。

例如，当运动是绕已知线轴（(L_s = \overline{r} + I t_c \wedge\overline{r})）以恒定角速度 (\omega) 旋转，且沿轴有恒定平移速度 (v_s) 的螺旋运动时，若数据采样时间间隔相等，电机方程为 (V_{i/i - 1} = V = (1 + I v_s/2)(\cos(\omega/2) + \sin(\omega/2)L_s))（式 5）。

但在实际工作中，(M_{i - 1}) 和 (M_i) 的关系只是近似已知，含噪声的 3D 运动的真实动态模型为 (M_i = \mathbf{V} {i/i - 1,Ml} M