使用里程表运动模型的扩展卡尔曼滤波器
在EKF定位的预测步骤中，必须进行线性化处理（如Probabilistic Robotics [THRUN，BURGARD，FOX]第206页中所述），使用速度运动模型时的雅可比矩阵定义为 ⎡⎣⎢xyθ⎤⎦⎥′=⎡⎣⎢xyθ⎤⎦⎥+⎡⎣⎢⎢⎢v^tω^t(−sinθ+sin(θ+ω^tΔt))v^tω^t(cosθ−cos(θ+ω^tΔt))ω^tΔt⎤⎦⎥⎥⎥[xyθ]′=[xyθ]+[v^tω^t(−sinθ+sin(θ+ω^tΔt))v^tω^t(cosθ−cos(θ+ω^tΔt))ω^tΔt]\begin{bmatrix} x \\ y \\ \theta \end{bmatrix}' = \begin{bmatrix} x \\ y \\ \theta \end{bmatrix} + \begin{bmatrix} \frac{\hat{v}_t}{\hat{\omega}_t}(-\text{sin}\theta + \text{sin}(\theta + \hat{\omega}_t{\Delta}t)) \\ \frac{\hat{v}_t}{\hat{\omega}_t}(\text{cos}\theta - \text{cos}(\theta + \hat{\omega}_t{\Delta}t)) \\ \hat{\omega}_t{\Delta}t \end{bmatrix} 计算为 GT=⎡⎣⎢⎢100010υtωt(−cosμt−1,θ+cos(μt−1,θ+ωtΔt))υtωt(−sinμt−1,θ+sin(μt−1,θ+ωtΔt))1⎤⎦⎥⎥GT=[10υtωt(−cosμt−1,θ+cos(μt−1,θ+ωtΔt))01υtωt(−sinμt−1,θ+sin(μt−1,θ+ωtΔt))001]G_{T}= \begin{bmatrix} 1 & 0 & \frac{υ_{t}}{ω_{t}}(-cos {μ_{t-1,θ}} + cos(μ_{t-1,θ}+ω_{t}Δ{t})) \\ 0 & 1 & …