扩展卡尔曼滤波，也就是EKF，常用于在动态系统中对状态的估计。比如，在机器人领域，EKF则常常用于对状态（位置，方向）的估计，也就是我们常说的数据融合，结合运动模型和观测数据，得到一个比较靠谱的状态估计。

系统模型

在EKF中，运动模型和观测模型是非常重要的。我们对运动模型都不陌生，其实就是根据运动学定律对机器人的运动规律进行建模，给定输入，输出会是什么。但是什么是观测模型呢？别着急，下面我们一个个的来解释。

状态方程（描述系统的演变过程）

$$x_k=A{x}_{k-1}+B{u}_k+w_k$$

• $x_k$：系统在时刻 $k$ 的状态（我们希望估计的变量）。
• $A$：状态转移矩阵，用于描述系统从上一时刻 $k-1$ 到当前时刻 $k$ 的状态演变。
• $B$：控制输入矩阵，用于描述控制输入对系统状态的影响。
• $u_k$：控制输入。
• $w_k$：过程噪声，通常假设为均值为 0 的高斯噪声，表示系统中的不确定性或噪声。

所以，状态方程是利用上一时刻的状态和当前时刻的输入$u$推导下一时刻的状态。

观测方程（描述如何从系统中获取观测数据）

$$z_k=H{x}_k+v_k$$

其中：

• $z_k$：时刻 $k$ 的观测值（由传感器测量得到的数据）。
• $x_k$：系统在时刻 $k$ 的状态（我们希望估计的变量）。
• $H$：观测矩阵，描述状态 $x_k$ 和观测值 $z_k$ 之间的关系。
• $v_k$：观测噪声，通常假设为均值为 0 的高斯噪声，表示观测过程中的不确定性，$v_k\sim N(0,R)$。

所以，观测方程到底是什么？
观测方程通过连接状态变量$x_k$ 和观测值 $z_k$，使卡尔曼滤波器可以利用传感器的测量数据修正对状态的估计。通过观测方程，系统能够利用测量值来更新状态估计，从而减小预测误差。说的更直白一点，我有一些传感器可以告诉我机器人现在的状态$x_k$，我要搞一个方程，能够让我从观测值 $z_k$得到状态$x_k$。

举个例子，假设我们正在追踪一辆车的位置和速度，系统的状态向量为：

$$x_k=\left[\begin{array}{c}{x}_{\text{position}}\\ x_{\text{velocity}}\end{array}\right]$$

假设我们的传感器只能测量车辆的位置，那么观测方程为：

$$z_k=H{x}_k+v_k$$

其中，观测矩阵 ( H ) 为：

$$H=\left[\begin{array}{cc}1& 0\end{array}\right]$$

因此，观测方程变为：

$$z_k=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}{x}_{\text{position}}\\ x_{\text{velocity}}\end{array}\right]+v_k=x_{\text{position}}+v_k$$

这意味着观测值$z_k$ 是车辆的位置加上观测噪声 $v_k$。

EKF工作步骤

为什么称之为EKF(extended KF)? 就是因为我们在使用的时候，状态方程和观测方程是非线性的，而不是像上面写的那样的线性方程。

• 状态方程（非线性）：
  $${\mathbf{x}}_k=f({\mathbf{x}}_{k-1},{\mathbf{u}}_k)+{\mathbf{w}}_k$$

• 观测方程（非线性）：
  $${\mathbf{z}}_k=h({\mathbf{x}}_k)+{\mathbf{v}}_k$$

基于上面的两个非线性方程，EKF的工作步骤如下：

预测步骤

状态预测

根据系统的非线性状态方程，利用当前状态 ${\hat{\mathbf{x}}}_{k-1}$ 和控制输入${\mathbf{u}}_k$ 预测下一时刻的状态：

$${\hat{\mathbf{x}}}_k^{-}=$$