扩展卡尔曼滤波，也就是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^{-}=f({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k)$$

协方差预测

计算状态预测的不确定性。首先，线性化状态方程，计算雅可比矩阵$F_{k-1}$，然后利用它预测状态协方差：

$${\mathbf{P}}_k^{-}=F_{k-1}{\mathbf{P}}_{k-1}{F}_{k-1}^T+Q$$

其中：

• $F_{k-1}=\frac{\partial f}{\partial \mathbf{x}}{|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k}$：状态方程的雅可比矩阵。
• $Q$：过程噪声的协方差矩阵。

这里需要讲一下，为什么会有协方差矩阵？ 在卡尔曼滤波中，协方差矩阵 ( P ) 是用来表示状态估计的不确定性的。它反映了系统状态估计的方差和不同状态变量之间的相关性。协方差矩阵通常定义为状态估计误差的协方差：

$$P_k=\mathbb{E}\left[({\hat{x}}_k-x_k)({\hat{x}}_k-x_k{)}^T\right]$$

其中：

• ${\hat{x}}_k$ 是在时刻 (k) 的状态估计。
• $x_k$ 是真实的状态。
• $P_k$ 的对角元素表示状态估计的方差，而非对角元素表示状态变量之间的相关性。

那什么需要对这个协方差矩阵进行预测呢？
（1）评估系统的估计不确定性

协方差矩阵的预测反映了对状态预测的不确定性。如果协方差矩阵较小，意味着系统对状态的预测更精确；如果协方差矩阵较大，则表示系统对状态的预测不确定性较大。

• 当 $P_k^{-}$ 较小，表示对系统状态的预测非常可靠。
• 当 $P_k^{-}$ 较大，表示状态预测存在较大的不确定性。

（2）融合观测值和预测值

在卡尔曼滤波的更新步骤中，协方差矩阵用于计算卡尔曼增益，卡尔曼增益用于平衡观测值与预测值：

$$K_k=P_k^{-}{H}_k^T(H_k{P}_k^{-}{H}_k^T+R{)}^{-1}$$

• 当协方差矩阵 $P_k^{-}$ 较大时，卡尔曼增益 $K_k$ 会增大，滤波器将更多依赖观测值来修正状态估计。
• 当协方差矩阵 $P_k^{-}$ 较小时，卡尔曼增益 $K_k$ 会较小，滤波器将更多依赖状态预测。

（3）传播过程噪声

协方差矩阵的预测公式中，过程噪声协方差矩阵$Q$反映了系统中的随机扰动或噪声。随着状态预测的进行，噪声可能会对状态估计产生影响，因此我们需要在协方差矩阵中加入这些噪声。

$$P_k^{-}=F_{k-1}{P}_{k-1}{F}_{k-1}^T+Q$$

通过这个过程，协方差矩阵考虑了系统噪声对状态预测的不确定性影响。

（4）动态调整不确定性

协方差矩阵不仅反映当前时刻的估计不确定性，还会随着时间不断更新。每当有新观测值时，协方差矩阵会被更新，以反映状态估计和观测的最新不确定性。

（5）协方差矩阵的更新

在卡尔曼滤波的更新步骤中，协方差矩阵也会随着状态的修正而调整。更新后的协方差矩阵公式如下：

$$P_k=(I-K_k{H}_k)P_k^{-}$$

更新后的协方差矩阵反映了结合观测值后的状态估计的不确定性。

更新步骤（Update Step）

计算观测残差

获取观测值 ${\mathbf{z}}_k$，通过非线性观测方程计算预测的观测值 $h({\hat{\mathbf{x}}}_k^{-})$，然后计算观测残差：

$${\mathbf{y}}_k={\mathbf{z}}_k-h({\hat{\mathbf{x}}}_k^{-})$$

计算卡尔曼增益

首先，线性化观测方程，计算雅可比矩阵$H_k$，然后计算卡尔曼增益 $K_k$：

$$K_k={\mathbf{P}}_k^{-}{H}_k^T(H_k{\mathbf{P}}_k^{-}{H}_k^T+R{)}^{-1}$$

其中：

• $H_k=\frac{\partial h}{\partial \mathbf{x}}{|}_{{\hat{\mathbf{x}}}_k^{-}}$：观测方程的雅可比矩阵。
• $R$：观测噪声的协方差矩阵。

3.3 更新状态估计

利用卡尔曼增益$K_k$修正状态预测${\hat{\mathbf{x}}}_k^{-}$，得到更新后的状态估计：

$${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{-}+K_k{\mathbf{y}}_k$$

更新协方差

更新状态协方差矩阵，以反映状态估计的不确定性：

$${\mathbf{P}}_k=(\mathbf{I}-K_k{H}_k){\mathbf{P}}_k^{-}$$

总结

1. 预测步骤：

   • 通过非线性状态方程预测系统的状态 $ \hat{\mathbf{x}}_k^-$。
   • 通过线性化状态方程（使用雅可比矩阵 $F_{k-1}$）预测协方差 ${\mathbf{P}}_k^{-}$。

2. 更新步骤：

   • 通过非线性观测方程计算预测的观测值。
   • 计算观测残差 ${\mathbf{y}}_k$。
   • 通过线性化观测方程（使用雅可比矩阵$H_k$）计算卡尔曼增益$K_k$。
   • 利用观测值更新状态估计${\hat{\mathbf{x}}}_k$ 和协方差${\mathbf{P}}_k$。

Appendix: 利用雅可比矩阵线性化非线性方程

在扩展卡尔曼滤波（EKF）中，非线性系统的状态方程和观测方程无法直接应用于经典卡尔曼滤波器的线性框架。因此，我们需要通过 雅可比矩阵 将非线性函数 局部线性化，以便在滤波过程中使用。这种线性化过程通过 泰勒展开 的一阶近似来实现。

1. 雅可比矩阵的定义

对于一个非线性函数 $f(\mathbf{x})$，其 雅可比矩阵 是函数对每个状态变量的偏导数矩阵，表示该函数在某个点上的局部线性变化情况。雅可比矩阵的定义如下：

$$J_f(\mathbf{x})=\frac{\partial f}{\partial \mathbf{x}}=\left[\begin{array}{cccc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}& \cdots & \frac{\partial f_1}{\partial x_n}\\ \frac{\partial f_2}{\partial x_1}& \frac{\partial f_2}{\partial x_2}& \cdots & \frac{\partial f_2}{\partial x_n}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{\partial f_m}{\partial x_1}& \frac{\partial f_m}{\partial x_2}& \cdots & \frac{\partial f_m}{\partial x_n}\end{array}\right]$$

其中：

• $f(\mathbf{x})$ 是一个从 $n$ 维状态空间到 $m$ 维观测空间的非线性函数。
• $\mathbf{x}=[x_1,x_2,\dots ,x_n{]}^T$ 是系统的状态向量。
• 每个元素 $\frac{\partial f_i}{\partial x_j}$ 是函数 $f_i(\mathbf{x})$ 对状态变量 $x_j$ 的偏导数。

2. 泰勒展开的一阶近似

为了将非线性函数线性化，我们通常使用 泰勒展开 对函数进行近似。对于非线性函数 $f(\mathbf{x})$，我们可以在某个参考点 ${\mathbf{x}}_0$ 处做泰勒展开，并忽略高阶项，仅保留一阶近似：

$$f(\mathbf{x})\approx f({\mathbf{x}}_0)+J_f({\mathbf{x}}_0)(\mathbf{x}-{\mathbf{x}}_0)$$

其中：

• $f({\mathbf{x}}_0)$ 是在参考点 ${\mathbf{x}}_0$ 处的函数值。
• $J_f({\mathbf{x}}_0)$ 是在参考点 ${\mathbf{x}}_0$ 处的雅可比矩阵。
• $(\mathbf{x}-{\mathbf{x}}_0)$ 是状态偏差。

通过这一展开式，非线性函数在参考点附近被近似为线性函数。

3. 线性化状态方程和观测方程

在扩展卡尔曼滤波器中，我们需要线性化以下两个非线性方程：

• 状态方程：

$${\mathbf{x}}_k=f({\mathbf{x}}_{k-1},{\mathbf{u}}_k)+{\mathbf{w}}_k$$

• 观测方程：

$${\mathbf{z}}_k=h({\mathbf{x}}_k)+{\mathbf{v}}_k$$

其中：

• $f({\mathbf{x}}_{k-1},{\mathbf{u}}_k)$ 是状态转移的非线性函数。
• $h({\mathbf{x}}_k)$ 是从状态到观测值的非线性映射函数。

4. 状态方程的线性化

为了将状态方程线性化，我们对函数 $f({\mathbf{x}}_{k-1},{\mathbf{u}}_k)$ 在当前状态 ${\hat{\mathbf{x}}}_{k-1}$ 处进行泰勒展开，并求取雅可比矩阵 $F_{k-1}$，即对 ${\mathbf{x}}_{k-1}$ 求偏导：

$$F_{k-1}=\frac{\partial f({\mathbf{x}}_{k-1},{\mathbf{u}}_k)}{\partial {\mathbf{x}}_{k-1}}{|}_{{\hat{\mathbf{x}}}_{k-1}}$$

此时，线性化后的状态方程为：

$${\hat{\mathbf{x}}}_k^{-}=f({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k)$$

预测的协方差矩阵为：

$$P_k^{-}=F_{k-1}{P}_{k-1}{F}_{k-1}^T+Q$$

其中，$Q$ 是过程噪声协方差矩阵。

5. 观测方程的线性化

同样地，为了将观测方程线性化，我们对函数 $h({\mathbf{x}}_k)$ 在当前状态 ${\hat{\mathbf{x}}}_k^{-}$ 处进行泰勒展开，并求取雅可比矩阵 $H_k$，即对 ${\mathbf{x}}_k$ 求偏导：

$$H_k=\frac{\partial h({\mathbf{x}}_k)}{\partial {\mathbf{x}}_k}{|}_{{\hat{\mathbf{x}}}_k^{-}}$$

线性化后的观测方程为：

$${\hat{\mathbf{z}}}_k=h({\hat{\mathbf{x}}}_k^{-})$$

协方差矩阵更新步骤中的卡尔曼增益为：

$$K_k=P_k^{-}{H}_k^T(H_k{P}_k^{-}{H}_k^T+R{)}^{-1}$$

其中，$R$ 是观测噪声协方差矩阵。

总结

通过雅可比矩阵，我们可以将非线性函数在局部线性化，使其能够应用于卡尔曼滤波的线性框架。具体步骤如下：

1. 对状态方程和观测方程进行泰勒展开，并只保留一阶项。
2. 计算雅可比矩阵，用以近似线性系统。
3. 在扩展卡尔曼滤波器中，利用线性化的方程进行预测和更新。

Appendix: 卡尔曼增益 $K_k$ 的推导过程

卡尔曼增益 $K_k$ 是卡尔曼滤波器中的关键参数，它决定了如何平衡预测值和观测值，使状态估计达到最优。我们通过最小化观测值和预测值之间的误差来推导出卡尔曼增益。

1. 观测残差（创新项）

首先，我们定义观测残差，它表示实际观测值与预测观测值之间的差异：

$$y_k=z_k-H_k{\hat{x}}_k^{-}$$

其中：

• $y_k$ 是观测残差（创新项），表示观测值与预测观测值的差异。
• $z_k$ 是时刻 $k$ 的实际观测值，由传感器直接测量得到。
• ${\hat{x}}_k^{-}$ 是时刻 $k$ 的状态预测值。注意这里和观测模型中的$x_k$真实值不一样
• $H_k$ 是观测模型的雅可比矩阵，用于将状态空间映射到观测空间。

2. 状态更新公式

状态更新公式表示我们如何根据观测残差 $y_k$ 修正预测状态 ${\hat{x}}_k^{-}$，得到更新后的状态估计 ${\hat{x}}_k$：

$${\hat{x}}_k={\hat{x}}_k^{-}+K_k{y}_k$$

其中：

• ${\hat{x}}_k$ 是更新后的状态估计。
• $K_k$ 是卡尔曼增益，它决定了我们应该在多大程度上依赖观测残差来修正预测状态。

3. 目标：最小化均方误差

我们希望通过选择合适的卡尔曼增益 $K_k$ 来最小化状态估计的均方误差（MSE）。均方误差定义为：

$$\mathbb{E}\left[(x_k-{\hat{x}}_k)(x_k-{\hat{x}}_k{)}^T\right]$$

将状态更新公式 ${\hat{x}}_k={\hat{x}}_k^{-}+K_k{y}_k$ 代入这个均方误差中，展开后得到：

$$\mathbb{E}\left[(x_k-({\hat{x}}_k^{-}+K_k{y}_k))(x_k-({\hat{x}}_k^{-}+K_k{y}_k){)}^T\right]$$

展开括号后可以得到：

$$\mathbb{E}\left[\left((x_k-{\hat{x}}_k^{-})-K_k(z_k-H_k{\hat{x}}_k^{-})\right){\left((x_k-{\hat{x}}_k^{-})-K_k(z_k-H_k{\hat{x}}_k^{-})\right)}^T\right]$$

其中，$(x_k-{\hat{x}}_k^{-})$ 是预测误差，$(z_k-H_k{\hat{x}}_k^{-})$ 是观测残差。

4. 计算协方差矩阵

我们定义预测误差协方差为 $P_k^{-}$，观测噪声协方差为 $R$，这样就可以表示误差项的期望值。对于 $y_k=z_k-H_k{\hat{x}}_k^{-}$，它的协方差为：

$$S_k=H_k{P}_k^{-}{H}_k^T+R$$

其中：

• $S_k$ 是观测残差的协方差矩阵，表示我们对观测残差的不确定性。
• $P_k^{-}$ 是预测状态的协方差矩阵。
• $R$ 是观测噪声的协方差矩阵。

5. 最小化均方误差

为了最小化均方误差，我们对卡尔曼增益 $K_k$ 进行优化。我们可以通过对误差项对 $K_k$ 求导，并设导数为 0 来找到最优解。最终，最优的卡尔曼增益 $K_k$ 通过下列公式表示：

$$K_k=P_k^{-}{H}_k^T{S}_k^{-1}$$

由于 $S_k=H_k{P}_k^{-}{H}_k^T+R$，因此卡尔曼增益的完整形式为：

$$K_k=P_k^{-}{H}_k^T(H_k{P}_k^{-}{H}_k^T+R{)}^{-1}$$

6. 总结

卡尔曼增益 $K_k$ 的推导步骤如下：

1. 定义观测残差： $y_k=z_k-H_k{\hat{x}}_k^{-}$
2. 状态更新公式： ${\hat{x}}_k={\hat{x}}_k^{-}+K_k{y}_k$
3. 目标：最小化均方误差 $\mathbb{E}\left[(x_k-{\hat{x}}_k)(x_k-{\hat{x}}_k{)}^T\right]$
4. 计算残差协方差： $S_k=H_k{P}_k^{-}{H}_k^T+R$
5. 推导卡尔曼增益： $K_k=P_k^{-}{H}_k^T(H_k{P}_k^{-}{H}_k^T+R{)}^{-1}$

最终，卡尔曼增益 $K_k$ 是通过最小化实际观测值和预测观测值之间的误差推导得到的，它用于平衡状态预测和观测数据，使得状态估计最优。