54、计算机视觉中的卡尔曼滤波与相机模型：原理、应用与实践

计算机视觉中的卡尔曼滤波与相机模型：原理、应用与实践

1. 扩展卡尔曼滤波器简介

卡尔曼滤波器在系统动态特性为线性时表现出色，但在实际应用中，许多系统的动态特性是非线性的。例如，汽车速度与油门踏板的关系并非线性，或者汽车所受的力在笛卡尔坐标系中表达，而汽车运动在极坐标系中表达。在这些情况下，传统卡尔曼滤波器不再适用，因为时间 $t + 1$ 的状态不再是时间 $t$ 状态的线性函数。

为了解决这些非线性问题，我们可以对相关过程进行局部线性化，这就是扩展卡尔曼滤波器（EKF）的核心思想。在 EKF 中，我们需要根据状态 $x$ 在每个时间步重新计算 $F$ 和 $B$ 的值。虽然这些值只是对真实更新和控制函数在特定 $x$ 值附近的近似，但在实际应用中通常足够了。

OpenCV 没有专门实现 EKF 的例程，但我们可以在每次更新之前重新计算并重置 kalman.transitionMatrix 和 kalman.controlMatrix 的值来实现 EKF。

一般来说，之前介绍卡尔曼滤波器时遇到的矩阵更新方程现在被视为更一般形式的特殊情况：
- $\vec{x} k = \vec{f}(\vec{x} {k - 1}, \vec{u}_k) + \vec{w}_k$
- $\vec{z}_k = \vec{h}(\vec{x}_k) + \vec{v}_k$

其中，$f(x_{k - 1}, u_k)$ 和 $h(x_k)$ 是其参数的任意非线性函数。为了使用卡尔曼滤波器的线性公式，我们根据以下定义的矩阵 $F_k$ 和 $H_k$ 在每个