基于扩展卡尔曼滤波(EKF)的机器人状态估计
重磅干货，第一时间送达
EKF的目的是使卡尔曼滤波器能够应用于机器人等非线性运动系统，EKF生成的状态估计比仅使用实际测量值更准确。在本文中，我们将简要介绍扩展卡尔曼滤波器，并了解传感器融合的工作原理。为了讨论EKF，我们将考虑一种机器人车（自驾车车辆在这种情况下）。如下图所示，我们可以在一个全局坐标系中为这辆车建模，坐标为：Xglobal、Yglobal和Zglobal（面朝上）。X_car和Y_car坐标属于以线速度（V）和角速度（ω）移动的车的坐标系。横向角（γ）测量汽车绕全局Z轴旋转的程度。
EKF几乎存在于机器人技术的每个领域，用于估计状态。EKF的目标是平滑汽车的噪声传感器测量值，以便更好地进行状态估计。这里的状态是指汽车的位置，为了估计车辆状态，EKF将噪声传感器测量值与预测传感器测量值相结合，以生成最佳估计值。然而，为了理解EKF的功能，我们需要了解EKF的两个数学模块：
状态空间模型
观测模型
使用状态空间模型，我们可以预测汽车的下一个状态，它也被称为状态转换模型，表示汽车或机器人从当前时间步到下一时间步的运动。例如，它显示了我们汽车的当前状态是如何根据使用转向、油门和制动器对汽车[线速度V和角速度ω]的控制而变化的。转向影响ω，而油门和制动器影响线速V。从现在起，我们将把时间t时汽车的状态表示为[xt，yt，γt]，控制输入表示为[vt，ωt]。现在让我们根据全局参考系推导汽车的速度分量：
如上图所示，通过应用三角学，我们得到角速度为ω的Vx=Vcos（y）和Vy=Vsin（y）。现在，给定我们的初始状态[xt-1，yt-1，γt-1]，我们可以如下估计下一个状态[xt，yt，γt]：
注：我们从t=1开始，初始状态为[x0，y0，γ0]
在上图中，我们用数学方法描述了机器人汽车的运动。在方程式1中，在dt时，汽车会向上移动一段距离。因为我们知道距离可以写成速度（v）和时间（t）的乘积，所以方程1很容易得到。每个项（f1、f2和f3）将用于计算雅可比矩阵F。这里我们使用雅可比矩阵，因为我们需要线性化具有余弦和正弦项的非线性方程。
在等式2中，Xt表示时间t处的运动模型。F和B是雅可比矩阵，在我们的例子中，F是一个单位矩阵，因为汽车的移动仅基于输入控制，即线性和角速度。但是，它并不总是一个单位矩阵。想想飞机的情况，在飞机上，重力起作用，如果我们不提供控制输入，它就会坠落并坠毁。所以，在这种情况下，F不是恒等式。最后，等式3表示添加了噪声项的完整状态空间模型。
我们使用状态空间或运动模型对汽车进行了估计。现在，我们将平滑传感器测量，以更好地估计机器人汽车的当前状态（这称为传感器融合）。首先，我们的状态模型预测下一个状态，然后观察模型获取状态模型的预测输入[xt，yt，yt]，以生成（推断）新的测量值。
想象一下，当我们在一辆自动驾驶汽车上，传感器发生故障时的情形。现在我们希望汽车移动到目标位置。在这种情况下，观测模型可以预测未来某个时间的传感器测量结果。因此，即使汽车传感器发生故障并产生错误数据，观测模型也会试图减少误差。
yt为n维，表示n个传感器测量值。w是每个传感器的噪声。H是将预测状态估计值转换为预测传感器测量值的测量矩阵。
基于上述讨论，我们做出了以下两个假设：
状态模型根据控制输入估计机器人的状态
观测模型使用预测状态推断传感器测量
EKF计算当前时间步长t和预测传感器测量值（如上所述）的这些实际传感器测量值的加权平均值，以生成更好的当前状态估计值。EKF有两个阶段：预测和更新（如下图所示）
上图显示了扩展卡尔曼滤波器的预测和更新步骤。在预测步骤中，我们首先使用状态空间或运动模型来估计状态（Xt）（我们去除了噪声项，只是为了让它看起来干净）。然后，我们使用之前在时间t-1处的协方差矩阵P获得时间t处的状态协方差矩阵P。状态协方差矩阵包含状态的不确定性。然而，对于第一次迭代，我们没有协方差矩阵，所以我们初始化它，如上图所示。此外，汽车的初始状态向量和控制命令将为零。
矩阵F是状态转移矩阵，用于预测下一个值X和协方差矩阵P。矩阵Q是过程噪声协方差矩阵。Q的维数是（状态数*状态数），在我们的例子中，它是3x3。这个Q项很重要，因为状态测量有噪声，我们需要测量方差。
注：Rt（传感器测量噪声协方差矩阵）
K表示卡尔曼增益。如果传感器噪声高（残余协方差高），K值趋于零，传感器测量值将被忽略。如果预测的噪声很高，那么K接近1，我们将依靠传感器测量。最后，更新Xt和Pt并将其用于下一步的预测。换句话说，Kalman增益（K）包含关于当前预测X和当前观测测量z的权重的信息，这将导致最终融合更新状态向量X和过程协方差矩阵P。
至此，我们已经完成了EKF。然而，EKF有一个线性化误差，基本上取决于函数的非线性程度以及用于线性化的工作点的距离。线性化误差可能会对自动驾驶汽车产生灾难性影响，因为它会导致估计器对错误答案过于自信。此外，推导雅可比矩阵是一个繁琐且容易出错的过程。如果自动驾驶汽车移动得很快，那么线性化误差就更大。