前言

最近项目需求涉及到目标跟踪部分，准备从 DeepSORT 多目标跟踪算法入手。DeepSORT 中涉及的内容有点多，以前也就对其进行了简单的了解，但是真正去做发现总是存在这样或者那样的困惑，头疼，果然欠下的总该还的😂

一个个来吧，这个系列文章主要分享博主在学习 DeepSORT 中的 Kalman Filter 的相关知识，主要从两方面分享，一方面是数学理论推导，另一方面是比较通俗易懂的图例分析。

这篇文章主要分享从数学理论推导的方式去理解卡尔曼滤波器，包括卡尔曼滤波器应用的实例和扩展卡尔曼滤波

博主为初学者，欢迎交流讨论，若有问题欢迎各位看官批评指正！！！😄

数学理论推导

视频链接：【卡尔曼滤波器】_Kalman_Filter_全网最详细数学推导

注：博主也就把 DR_CAN 老师讲解的内容复述了一遍，强烈建议大家观看原视频！！！

1. 直观理解与二维实例

在前面的内容中我们详细推导了卡尔曼滤波器的五个公式和递归算法的概念，在这节内容我们将通过一个二维的例子来给大家分析一下卡尔曼滤波器，再来直观的感受一下它的魅力。

DR_CAN 老师是用 Excel 做了一个完整的二维系统的卡尔曼滤波的示例，里面的公式都是用 Excel 做的，下载链接：Baidu Driver【提取码：txn3】

其中的文档做了加密处理，密码是下面一个简单的数据融合的应用

如果我们用两个尺子去测量一个物体，得到两个数据，分别是 $Z_1=6.5mm$，$Z_2=7.3mm$，又已知两个尺子的标准差分别为 ${\sigma }_1=0.2mm$，${\sigma }_2=0.4mm$，这个时候请问最优的估计值 $\hat{Z}$ 是多少？最优估计值 $\hat{Z}$ 即为文档的密码。

---

那我们一起来看下这个例子，这是一个很简单的应用，一个人在走路，对于这个系统来说可以设定两个状态变量，分别是 $X_1$ 代表了位置，$X_2$ 代表了速度。

如果我们假设它在匀速行驶，那么它的这种离散形式的状态空间方程如下：

速度：$X_{2,k}=X_{2,k-1}$，这里面采样时间定义为 $\Delta T$ 代表 $k$ 时刻与 $k-1$ 时刻之间的时间间隔。

位置：$X_{1,k}=X_{1,k-1}+\Delta T{X}_{2,k-1}$，也就是 $k$ 时刻的位置等于 $k-1$ 时刻时的位置加上速度乘以时间间隔

这里为了简化处理，我们令采样时间 $\Delta T=1$，可以看出来这两个方程是在理想状态下的，而在现实生活中存在着很多的不确定性，有一些误差。比如这个路上有上坡也有下坡，然后走的时候也有时慢有时快，很难保持匀速。所以说这个数学模型就要加上一些不确定性，如下：
$$\begin{array}{rl}{X}_{1,k}& =X_{1,k-1}+X_{2,k-1}+W_{1,k-1}\\ X_{2,k}& =X_{2,k-1}+W_{2,k-1}\end{array}$$
这里面 $W$ 叫做过程噪声，这两个都是不确定的，但是在卡尔曼滤波分析里面，我们假设它们都是符合自然界最广泛的正态分布的，它们的期望是 $0$，协方差矩阵是 $Q$，即 $P(W)\sim N(0,Q)$

所以说在这种情况下我们就无法通过这两个方程来准确地得到 $k$ 时刻的状态变量 $X_1$ 和 $X_2$ 了，但是我们可以估计，根据这个误差的噪声来估计。

同时除了这个数学模型，我们又在天上放了一个卫星来时刻监测这个人的位置和距离，这样的话就得到了一个测量方程：
$$\begin{array}{rl}{Z}_{1,k}& =X_{1,k}\\ Z_{2,k}& =X_{2,k}\end{array}$$
卫星测量位置和速度两个内容，如果说这两个测量准确的话，那就可以精确的知道在 $k$ 时刻这个人的位置和速度，也就用不上什么滤波器了。

但是在现实生活当中都是存在着不确定性的，在测量的过程当中可能会测到不同的物体，而且这个传感器本身这个卫星本身它也有一定的误差，所以这里面也要加上一定的不确定性，如下：
$$\begin{array}{rl}{Z}_{1,k}& =X_{1,k}+V_{1,k}\\ Z_{2,k}& =X_{2,k}+V_{2,k}\end{array}$$
这里面 $V$ 叫做测量误差，这两个都是不确定的，它们也符合正态分布，期望是 $0$，协方差矩阵是 $R$，即 $P(V)\sim N(0,R)$

下面我们可以把这两个式子用比较紧凑的矩阵的方式来表达出来，它就是下面的形式：
$$\begin{array}{rl}\left[\begin{array}{c}{X}_{1,k}\\ X_{2,k}\end{array}\right]& =\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]\left[\begin{array}{c}{X}_{1,k-1}\\ X_{2,k-1}\end{array}\right]+\left[\begin{array}{c}{W}_{1,k-1}\\ W_{2,k-1}\end{array}\right]\\ \left[\begin{array}{c}{Z}_{1,k}\\ Z_{2,k}\end{array}\right]& =\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{X}_{1,k}\\ X_{2,k}\end{array}\right]+\left[\begin{array}{c}{V}_{1,k}\\ V_{2,k}\end{array}\right]\end{array}$$
然后这个时候如果我们把它归纳一下，它就变成了如下的形式：
$$\begin{array}{rl}{X}_k& =A{X}_{k-1}+W_{k-1}\\ Z_k& =H{X}_k+V_k\end{array}$$
这样一来，如果去估计 $k$ 时刻时 $X_{1,k}$ 和 $X_{2,k}$ 的值，想得到一个相对准确的结果的话，这就是一个数据融合的概念。通过一个不太准确的数学模型和一个不太准确的测量结果去估计一个最优值，而这个最优值估计的算法就是卡尔曼滤波器。

说它是最优算法是因为它在数学上得到了证明，大家可以去回看前几篇文章的理论推导。而卡尔曼滤波器分为两个步骤，第一个步骤是预测，第二个步骤是校正

预测：
$$\begin{array}{rl}{\hat{X}}_k^{-}& =A{\hat{X}}_{k-1}^{-}+B{U}_{k-1}\\ P_k^{-}& =A{P}_{k-1}{A}^T+Q\end{array}$$
校正：
$$\begin{array}{rl}{K}_k& =\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}\\ {\hat{X}}_k& ={\hat{X}}_k^{-}+K_k(Z_k-H{\hat{X}}_k^{-})\\ P_k& =(I-K_{k}H)P_k^{-}\end{array}$$
通过上面这五个公式来完成，这五个公式在我们之前的文章中已经详细讲过了，这边就不再赘述了。

我们直接来看这个例子，由于 DR_CAN 老师的 Excel 程序不方便展示，这边博主参考了另一位同学的 Python 实现，参考链接为：https://github.com/liuchangji/2D-Kalman-Filter-Example_Dr_CAN_in_python

完整的代码如下：

import numpy as np
import matplotlib.pyplot as pltdef gaussian_distribution_generator(var):return np.random.normal(loc=0.0, scale=var, size=None)# 状态转移矩阵，上一时刻的状态转移到当前时刻
A = np.array([[1, 1],[0, 1]])# 过程噪声协方差矩阵Q，p(w)~N(0,Q)，噪声来自真实世界中的不确定性
Q = np.array([[1, 0],[0, 1]])# 观测噪声协方差矩阵R，p(v)~N(0,R)
R = np.array([[0.1, 0],[0, 0.1]])# 状态观测矩阵
H = np.array([[1, 0],[0, 1]])# 控制输入矩阵B
B = None
# 初始位置与速度
X0 = np.array([[0],[1]])# 状态估计协方差矩阵P初始化
P = np.array([[1, 0],[0, 1]])if __name__ == "__main__":# ---------------------------初始化-------------------------X_true = np.array(X0)  # 真实状态初始化X_posterior = np.array(X0)P_posterior = np.array(P)speed_true = []position_true = []speed_measure = []position_measure = []speed_prior_est = []position_prior_est = []speed_posterior_est = []position_posterior_est = []for i in range(30):# -----------------------生成真实值----------------------# 生成过程噪声w = np.array([[gaussian_distribution_generator(Q[0, 0])],[gaussian_distribution_generator(Q[1, 1])]])X_true = np.dot(A, X_true) + w  # 得到当前时刻状态speed_true.append(X_true[1, 0])position_true.append(X_true[0, 0])# -----------------------生成观测值----------------------# 生成观测噪声v = np.array([[gaussian_distribution_generator(R[0, 0])],[gaussian_distribution_generator(R[1, 1])]])Z_measure = np.dot(H, X_true) + v  # 生成观测值,H为单位阵Eposition_measure.append(Z_measure[0, 0])speed_measure.append(Z_measure[1, 0])# ----------------------进行先验估计---------------------X_prior = np.dot(A, X_posterior)position_prior_est.append(X_prior[0, 0])speed_prior_est.append(X_prior[1, 0])# 计算状态估计协方差矩阵PP_prior_1 = np.dot(A, P_posterior)P_prior = np.dot(P_prior_1, A.T) + Q# ----------------------计算卡尔曼增益,用numpy一步一步计算Prior and posteriork1 = np.dot(P_prior, H.T)k2 = np.dot(np.dot(H, P_prior), H.T) + RK = np.dot(k1, np.linalg.inv(k2))# ---------------------后验估计------------X_posterior_1 = Z_measure - np.dot(H, X_prior)X_posterior = X_prior + np.dot(K, X_posterior_1)position_posterior_est.append(X_posterior[0, 0])speed_posterior_est.append(X_posterior[1, 0])# 更新状态估计协方差矩阵PP_posterior_1 = np.eye(2) - np.dot(K, H)P_posterior = np.dot(P_posterior_1, P_prior)# 可视化显示if True:fig, axs = plt.subplots(1,2)axs[0].plot(speed_true, "-", label="speed_true", linewidth=2)  # Plot some data on the axes.axs[0].plot(speed_measure, "-", label="speed_measure", linewidth=2)  # Plot some data on the axes.axs[0].plot(speed_prior_est, "-", label="speed_prior_est", linewidth=2)  # Plot some data on the axes.axs[0].plot(speed_posterior_est, "-", label="speed_posterior_est", linewidth=2)  # Plot some data on the axes.axs[0].set_title("speed")axs[0].set_xlabel('k')  # Add an x-label to the axes.axs[0].legend()  # Add a legend.axs[1].plot(position_true, "-", label="position_true", linewidth=1)  # Plot some data on the axes.axs[1].plot(position_measure, "-", label="position_measure", linewidth=1)  # Plot some data on the axes.axs[1].plot(position_prior_est, "-", label="position_prior_est", linewidth=1)  # Plot some data on the axes.axs[1].plot(position_posterior_est, "-", label="position_posterior_est", linewidth=1)  # Plot some data on the axes.axs[1].set_title("position")axs[1].set_xlabel('k')  # Add an x-label to the axes.axs[1].legend()  # Add a legend.plt.show()

代码的注释已经非常非常详细了，大家自己多调试调试就明白了，可以对照下面的公式来看：

上面第一部分的公式是用来生成实际的位置和实际的速度，即生成真实值，这里面有一个点需要注意，这两个误差 $w$ 和 $v$ 都是随机数，通过 np.random.normal 语句生成。然后这里面我们做了一个简化，也就是说误差项是相互独立的，即 $Q$ 矩阵和 $R$ 矩阵只有对角线上有值，非对角线值为 0，说明它们之间的协方差为 0，只有方差这一项，没有协方差这一项。

上面第二部分的公式是利用卡尔曼滤波器来进行计算的，包括预测和校正两部分。

代码中我们让过程协方差矩阵 $Q$ 的值为 1，测量协方差矩阵 $R$ 的值为 0.1，运行效果如下：

上面的图是速度的比较，下面的图是位置的比较。其中蓝色的直线 $—$ 表示真实值，橙色的点 $\bullet$ 表示测量值，绿色的虚线 $--$ 表示先验估计值，红色的直线 $—$ 表示后验估计值。

从上图你可以非常明显的看到红色的直线 $—$ 和橙色的点 $\bullet$ 非常的靠近，因为我们说过，它的测量协方差要小，也就是测量的结果更加准确，所以我们的后验结果当然就更靠近于测量结果，也可以看到这个测量的结果都是非常靠近于这个实际的结果。

我们在这个地方变换一下，我们让过程误差减小变为 0.1，让测量误差增加变为 1，再次运行效果如下：

这时候你会发现测量值也就是橙色的点 $\bullet$ 会离实际值即蓝色的直线 $—$ 非常的远，因为它的测量误差很大，但是当经过这个卡尔曼滤波器以后，我们发现这个后验估计值并没有因为测量偏差而走得非常远，反而是由于我们有这个先验计算的结果会把后验值拉得距离真实值非常的近，使得估计值不会特别的离谱。

大家可以自己多去调整下各种参数的值来跑几个不同的数据来体会一下，分析一下，这个是最好的去掌握卡尔曼滤波器的方法了，这就需要各位看官自己花一些时间了。

那在卡尔曼滤波器的实际应用中有哪些参数是需要我们去调整的呢？即卡尔曼滤波器的调参是调整那些值？🤔

卡尔曼滤波器的调参是一个重要的过程，涉及到很多参数。在实际应用中，以下是经常需要调整的参数：(from chatGPT)

1. 过程噪声协方差矩阵 $Q$：这个矩阵表示系统的内部噪声，也就是数学模型的不确定性。通常，如果你认为你的模型有更多的不确定性或噪声，你可以增加 $Q$ 的值。反之，如果你认为模型很稳定，可以减少 $Q$ 的值。

2. 观测噪声协方差矩阵 $R$：这个矩阵表示观测值的噪声或不确定性。如果你认为测量传感器非常准确，可以减少 $R$ 的值。反之如果你认为传感器有很大的误差，可以增加 $R$ 的值。

3. 初始状态估计协方差矩阵 $P$：这个矩阵表示对初始状态估计的不确定性。通常，如果对初始状态非常有自信，可以设置 $P$ 为较小的值；反之，如果对初始状态不确定，可以设置较大的值。

4. 初始状态值 $X_0$：根据问题和已知的先验知识选择。

调整这些参数的一般策略如下：

• 模拟和真实数据比较: 如果有可用的真实数据，可以使用它来调整参数，使滤波器的输出尽可能接近真实数据。
• 敏感性分析: 对于每个参数，可以在一定范围内变化它，然后观察输出的变化，以了解哪些参数对结果最敏感。
• 经验和直觉: 有时，根据问题的物理背景和直觉，可以对参数进行合理的估计。
• 优化方法: 有些情况下，可以使用优化算法自动调整参数，以最小化某个误差指标。

$Q$ 和 $R$ 是两个关键参数，它们之间的平衡对估计的质量至关重要。如果数学模型被认为是可靠的，可以减小 $Q$ 的值；如果测量被认为是可靠的，可以减小 $R$ 的值。但过度信任其中一个可能导致估计结果的质量下降。

$P$ 主要影响初始化几个时间步的估计。如果对初始状态非常有信心，可以较小 $P$ 的值。否则，可以增大它，允许估计在开始时有更大的波动，随后逐渐稳定。

在实际应用中，卡尔曼滤波器的参数调整通常需要多次迭代和实验，以找到最佳的参数组合。

2. EKF

前面的文章中我们介绍了卡尔曼滤波器，也通过了数学推导证明了卡尔曼滤波器是最优化的线性滤波器，它用在线性系统里面可以得到最优的的估计值

这节内容我们介绍下卡尔曼滤波器在非线性系统中的应用，对于非线性系统来说去处理它有非常多的方法，有的时候需要具体问题去具体分析，这节我们会介绍一种最基础的方法，将非线性系统线性化，这种滤波器就称为扩展卡尔曼滤波器（Extended Kalman Filter，EKF）

首先我们先来复习一下线性系统，现在有一个线性系统，它的状态空间表达式如下：
$$\begin{array}{rl}{X}_k& =A{X}_{k-1}+B{U}_{k-1}+W_{k-1}\\ Z_k& =H_{k-1}+V_k\end{array}$$
这里面 $W$ 是过程误差，符合正态分布，期望是 $0$，协方差矩阵是 $Q$，即
$$P(W)\sim N(0,Q)$$
$V$ 是测量误差，也符合正态分布，期望是 $0$，协方差矩阵是 $R$，即
$$P(V)\sim N(0,R)$$
在前面的文章中我们推导出了卡尔曼滤波器的五个公式，它的表达形式如下所示，包含预测部分和校正部分

预测：
$$\begin{array}{rl}{\hat{X}}_k^{-}& =A{\hat{X}}_{k-1}+B{U}_{k-1}\\ P_k^{-}& =A{P}_{k-1}{A}^T+Q\end{array}$$
校正：
$$\begin{array}{rl}{K}_k& =\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}\\ {\hat{X}}_k& ={\hat{X}}_k^{-}+K_k(Z_k-H{\hat{X}}_k^{-})\\ P_k& =(I-K_{k}H)P_k^{-}\end{array}$$
预测部分先来计算它的先验 ${\hat{X}}_k^{-}$，然后校正部分得到它的后验估计值 ${\hat{X}}_k$，这是一个数据融合 Data Fusion 的一个思想在这里。

---

下面我们来考虑非线性系统，对于一个非线性系统来说它没办法用线性的状态空间方程来表达，而是用下面的这种形式：
$$\begin{array}{rl}{X}_k& =f(X_{k-1},U_{k-1},W_{k-1})\\ Z_k& =h(X_k,V_k)\end{array}$$
这里面 $f$ 和 $h$ 是两个非线性的表达形式，大家要注意，不管它是线性还是非线性的，这里面所有的误差它都是符合正态分布的，否则的话我们就没有办法操作了

这里面存在着一个问题，那就是正态分布的随机变量通过非线性系统后就不再是正态分布的了，

就比如说一个正态分布通过一条蓝色的直线，它是线性的，通过它映射到另一个平面上，它依然会是一个正态分布的图，如下图所示：

但是如果说它通过一个非线性的映射，比如说下面红色的曲线，它所表现出来的就不再会是正态分布的一种情况了，如下图所示：

所以如果我们还想对于这个系统使用卡尔曼滤波的话，就需要对它进行线性化 Linearization，线性化会用到泰勒级数，简单来说就是：
$$f(x)=f(x_0)+\frac{\partial f}{\partial x}(x-x_0)$$
如果我们用一个一维的例子来看，这样一个非线性的曲线，我们要去预测 $f(x)$ 可以使用下面的方式：
$$f(x)=f(x_0)+k(x-x_0)$$
这是一个简单的三角的几何关系，而对于高纬度来讲它就要用到雅可比矩阵，关于这部分内容可以参考：【工程数学基础】2_线性化_泰勒级数_泰勒公式_Linearization

可以看得出来，我们如果想去线性化一个系统，我们需要找到一个点比如上面的 $x_0$，它被称为 Operating Point，然后在它附近进行线性化，

对于我们这个非线性的系统来说，最好的一个线性化的点当然就是它的真实点，但是这里面有问题，因为系统有误差，我们永远没办法知道真实点是多少，这个真实值是多少，所以只能退而求其次，对于这个过程方程来说，我们让 $f(x_k)$ 在 ${\hat{x}}_{k-1}$ 处进行线性化，也就是 $k-1$ 时刻的后验估计，我们在这个附近把它进行线性化，因此方程可以写作：
$$X_k=f({\hat{X}}_{k-1},U_{k-1},W_{k-1})+A(X_k-{\hat{X}}_{k-1})+W{w}_{k-1}$$
这里面 $W_{k-1}$ 是误差，我们并不知道它是多少，所以我们简单的把它假设为 0，进一步简化我们令 $f({\hat{X}}_{k-1},U_{k-1},0)={\tilde{X}}_k$

其中 $A$ 是雅可比矩阵，$A=\frac{\partial f}{\partial X}{|}_{{\hat{X}}_{k-1},U_{k-1}}$

举一个二维的例子，比如说有一个二维的系统如下：
$$\begin{array}{rl}{x}_1& =x_1+\sin x_2=f_1\\ x_2& =x_1^2=f_2\\ A& =\frac{\partial f}{\partial x}=\left[\begin{array}{cc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}\\ \frac{\partial f_2}{\partial x_1}& \frac{\partial f_2}{\partial x_2}\end{array}\right]\\ & ={\left[\begin{array}{cc}1& \cos x_2\\ 2x_1& 0\end{array}\right]}_{{|}_{{\hat{x}}_{2,k-1}}^{{\hat{x}}_{1,k-1}}}\\ A_k& =\left[\begin{array}{cc}1& \cos x_{2,k-1}\\ 2x_{1,k-1}& 0\end{array}\right]\end{array}$$
从上式可以看出 $A$ 矩阵随着 $k$ 的变化在不断变化，也就是说我们每一步都要重新去计算 $A$ 矩阵，它是不断变换的（这个线性化的结果是在不断变化的）

同理 $W_k=\frac{\partial f}{\partial W}{|}_{{\hat{X}}_{k-1},U_{k-1}}$

这是关于过程方程的线性化，对于测量方程来说 $Z_k$ 我们把它在 ${\tilde{X}}_k$ 进行线性化，因此方程可以写为：
$$Z_k=h({\tilde{X}}_k,V_k)+H(X_k-{\tilde{X}}_k)+V{v}_k$$
和上面的处理一样，由于我们没办法得到 $V_k$ 的结果，假设它是 0，因此我们也可以令 $h({\tilde{X}}_k,0)={\tilde{Z}}_k$，$H=\frac{\partial h}{\partial x}{|}_{{\tilde{X}}_k}$，$V=\frac{\partial h}{\partial v}{|}_{{\tilde{X}}_k}$

我们可以推导出下式：
$$\begin{array}{rl}{X}_k& ={\tilde{X}}_k+A(X_{k-1}-{\hat{X}}_{k-1})+W{w}_{k-1}\\ Z_k& ={\tilde{Z}}_k+H(X_k-{\tilde{X}}_k)+V{v}_k\end{array}$$
这样整个非线性系统就在 ${\hat{X}}_{k-1}$ 和 ${\tilde{X}}_k$ 附近进行线性化了，然后这里面还要一个需要注意的地方，我们知道 $W$ 是服从正态分布的，期望为 $0$，协方差矩阵为 $Q$，即 $P(W)\sim N(0,Q)$，其实 $Ww$ 也是服从正态分布的，它的期望依然是 $0$，但它的协方差矩阵变成了 $wQ{w}^T$，即 $P(Ww)\sim N(0,wQ{w}^T)$

同理 $P(V{v}_k)\sim N(0,vR{v}^T)$

有了这样的结果，后面的事情就理所当然了，就可以用我们以前推过的手段来推导了，我们来看下非线性的和线性的有什么区别。

首先预测部分我们可以直接用非线性化的系统进行预测，

预测：
$$\begin{array}{rl}{\hat{X}}_k^{-}& =f({\hat{X}}_{k-1},U_{k-1},0)\\ P_k^{-}& =A{P}_{k-1}{A}^T+wQ{w}^T\end{array}$$
校正：
$$\begin{array}{rl}{K}_k& =\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+vR{v}^T}\\ {\hat{X}}_k& ={\hat{X}}_k^{-}+K_k(Z_k-h({\hat{X}}_k^{-},0))\\ P_k& =(I-K_{k}H)P_k^{-}\end{array}$$
其中标红的部分代表和线性系统不一样的地方，这就是一个新的扩展卡尔曼滤波器的应用方法

OK！扩展卡尔曼滤波器的内容到这里就结束了，

通过这几篇文章算是把卡尔曼滤波做了一个完整的介绍，整个系统还是比较完整的，希望大家学过以后会有一些帮助，

3. 补充知识-线性化

视频链接：【工程数学基础】2_线性化_泰勒级数_泰勒公式_Linearization

注：博主也就把 DR_CAN 老师讲解的内容复述了一遍，强烈建议大家观看原视频！！！

我们会介绍一个通用的线性化的数学方法，它不只是局限于控制理论方面的应用

我们首先来看什么是线性系统 Linear System，线性系统符合叠加原理 superposition

• $x_1$，$x_2$ 是解
• $x_3=k_1{x}_1+k_2{x}_2$ 其中 $k_1$ $k_2$ 为常数
• $x_3$ 是解

符合以上三条的就是线性系统，

举几个例子

$$\begin{array}{rl}\ddot{x}+2\dot{x}+\sqrt{2}x& =0\\ \ddot{x}+2\dot{x}+\sqrt{2}{x}^2& =0\\ \ddot{x}+\sin \dot{x}+\sqrt{2}x& =0\end{array}$$
其中第一个是线性的，第二个和第三个不是，大家可以用上面三条原则验证下，可以发现不符合叠加原则

---

我们现在来看一下线性化的方法，去线性化一个系统我们会用到一个重要的数学理论：泰勒级数（Taylor Series），泰勒级数的展开式如下：
$$f(x)=f(x_0)+\frac{f^{\prime }(x_0)}{1!}(x-x_0)+\frac{f^{\prime \prime }(x_0)}{2!}(x-x_0{)}^2+\cdots +\frac{f^{(n)}(x_0)}{n!}(x-x_0{)}^n$$
如果 $x-x_0\to 0$，则 $(x-x_0{)}^2\to 0$，$(x-x_0{)}^n\to 0$，这样的话在 $x_0$ 这点 $f(x)$ 的展开就变成了如下形式：
$$f(x)=f(x_0)+f^{\prime }(x_0)(x-x_0)$$
我们来看一下这个式子，其中 $f(x_0)$ 是一个常数，我们令它等于 $k_1$，同理 $f^{\prime }(x_0)$ 也是一个常数，我们令它等于 $k_2$，则有
$$\begin{array}{rl}f(x)& =f(x_0)+f^{\prime }(x_0)(x-x_0)\\ & =k_1+k_2(x-x_0)\\ & =k_1+k_{2}x-k_2{x}_0\\ & =k_{2}x+b\end{array}$$
这其实是一条直线的表达方式，$k_2$ 代表着斜率等于 $\tan \theta$，$b$ 代表着截距

所以说这样的一种方式就把原来的 $f(x)$ 线性化成了 $k_{2}x+b$ 这样一种形式，我们把这个过程就叫做线性化。

比如说我们来举一个例子，$f(x)=\sin x$，我们把上述公式带进去可以得到
$$f(x)=\sin x_0+\cos x_0(x-x_0)$$
当 $x_0=0$ 时，$f(x)=x$，也就是说在零点附近，$\sin x$ 的线性化形式就等于 $x$，我们来简单验证一下，比如 $\sin \frac{\pi }{6}=\frac{1}{2}$，$\frac{\pi }{6}\approx \frac{3.14}{6}=0.52$，二者的误差为 $\frac{0.52-0.5}{0.5}\times 100\%=4\%$

再比如说 $\sin \frac{\pi }{4}=\frac{\sqrt{2}}{2}\approx 0.707$，$\frac{\pi }{4}\approx \frac{3.14}{4}=0.785$，这个时候二者的误差达到了 $\frac{0.785-0.707}{0.707}\times 100\%=11\%$

这是因为我们在线性化的过程当中，大家一定要记住，我们忽略了后面的平方项 $(x-x_0{)}^2$，但这只是 $x$ 与 $x_0$ 非常接近的情况下我们才可以这样做，所以大家一定要记住这一点，线性化是在某一点附近的线性化，而不是全局的线性化，这一点非常非常的重要

现在我们来看一个系统的例子，我们要将这个系统在平衡点（fixed point）附近线性化
$$\ddot{x}+\dot{x}+\frac{1}{x}=1$$
平衡点的求法就是令所有的导数项都为 $0$，即 $\ddot{x}=\dot{x}=0$，有 $\frac{1}{x}=1$，平衡点 $x_0=1$

然后在 $x_0$ 附近我们对它进行线性化，我们可以在 $x_0$ 附近令 $x_{\delta }=x_0+x_d$，其中 $x_d$ 是一个比较小的值，我们将其代入上式有
$${\ddot{x}}_{\delta }+{\dot{x}}_{\delta }+\frac{1}{x_{\delta }}=1$$
我们现在把非线性的项 $\frac{1}{x_{\delta }}$ 进行线性化，用我们刚才的公式有：
$$\begin{array}{rl}f(x_{\delta })& =f(x_0)+f^{\prime }(x_0)(x_{\delta }-x_0)\\ \frac{1}{x_{\delta }}& =\frac{1}{x_0}-\frac{1}{x_0^2}{x}_d=1-x_d\end{array}$$
也就是 $\frac{1}{x_{\delta }}$ 在 $x_0$ 附近的线性化就可以变成 $1-x_d$，我们再把 ${\ddot{x}}_{\delta }={\ddot{x}}_0+{\ddot{x}}_d={\ddot{x}}_d$，${\dot{x}}_{\delta }={\dot{x}}_0+{\dot{x}}_d={\dot{x}}_d$ 代入有：
$$\begin{array}{rl}{\ddot{x}}_d+{\dot{x}}_d+1-x_d& =1\\ {\ddot{x}}_d+{\dot{x}}_d-x_d& =0\end{array}$$
这就是线性化的结果

---

我们再来看下对于二维的情况，在平衡点附近是如何进行线性化的，对于下面的式子：
$$\begin{array}{rl}{\dot{x}}_1& =f_1(x_1,x_2)\\ {\dot{x}}_2& =f_2(x_1,x_2)\end{array}$$
在平衡点附近有：
$$\left[\begin{array}{c}{\dot{x}}_{1d}\\ {\dot{x}}_{2d}\end{array}\right]=\left[\begin{array}{cc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}\\ \frac{\partial f_2}{\partial x_1}& \frac{\partial f_2}{\partial x_2}\end{array}\right]{\left[\begin{array}{c}{x}_{1d}\\ x_{2d}\end{array}\right]}_{|x=x_0}$$
现在我们依然用上面的例子 $\ddot{x}+\dot{x}+\frac{1}{x}=1$，这个时候我们可以令 $x_1=x$，$x_2=\dot{x}$，我们就可以得到状态空间方程：
$$\begin{array}{rl}{\dot{x}}_1& =x_2\\ {\dot{x}}_2& =\ddot{x}=1-\frac{1}{x}-\dot{x}=1-\frac{1}{x_1}-x_2\end{array}$$
我们现在先来寻找它的平衡点，也就是令 ${\dot{x}}_1=0$，${\dot{x}}_2=0$，我们可以得到 $x_{20}=0$，$x_{10}=1$

我们现在把上式带进去，求取矩阵中的四项，其实就是一个雅可比矩阵，我们现在的目的是求下面四项在平衡点 $x=x_0$ 的值：
$$\left[\begin{array}{cc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}\\ \frac{\partial f_2}{\partial x_1}& \frac{\partial f_2}{\partial x_2}\end{array}\right]$$
我们已知条件有：
$$\begin{array}{rl}{f}_1(x_1,x_2)& ={\dot{x}}_1=x_2\\ f_1(x_1,x_2)& ={\dot{x}}_2=1-\frac{1}{x_1}-x_2\end{array}$$
还已知在平衡点处有：
$$\begin{array}{rl}{\dot{x}}_1& =0\\ {\dot{x}}_2& =0\\ x_{10}& =1\\ x_{20}& =0\end{array}$$
则根据已知条件和求导规则有：
$$\begin{array}{rl}\frac{\partial f_1}{\partial x_1}& =0\\ \frac{\partial f_1}{\partial x_2}& =1\\ \frac{\partial f_2}{\partial x_1}& =\frac{1}{x_1^2}\\ \frac{\partial f_2}{\partial x_2}& =-1\end{array}$$
然后把平衡点处的值 $x_{20}=0$，$x_{10}=1$ 可以有
$$\left[\begin{array}{c}{\dot{x}}_{1d}\\ {\dot{x}}_{2d}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ \frac{1}{x_1^2}& -1\end{array}\right]{\left[\begin{array}{c}{x}_{1d}\\ x_{2d}\end{array}\right]}_{|x=x_0}=\left[\begin{array}{cc}0& 1\\ 1& -1\end{array}\right]\left[\begin{array}{c}{x}_{1d}\\ x_{2d}\end{array}\right]$$
我们来看下 ${\dot{x}}_{2d}=x_{1d}-x_{2d}$，又已知 $x_1=x$，$x_2=\dot{x}$，${\dot{x}}_1=x_2$，${\dot{x}}_2=\ddot{x}$

代入有 ${\ddot{x}}_d=x_d-{\dot{x}}_d$ => ${\ddot{x}}_d+{\dot{x}}_d-x_d=0$

我们可以发现和上面算出来的结果是一样的，所以这就是一个二维系统线性化的方法

---

我们来做一下总结，首先是线性化的公式：
$$f(x)=f(x_0)+f^{\prime }(x_0)(x-x_0)$$
但是要记住 $x-x_0\to 0$ 我们才可以应用这个公式

第二个就是二维情况下在平衡点处线性化的一个方法：
$$\left[\begin{array}{c}{\dot{x}}_{1d}\\ {\dot{x}}_{2d}\end{array}\right]=\left[\begin{array}{cc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}\\ \frac{\partial f_2}{\partial x_1}& \frac{\partial f_2}{\partial x_2}\end{array}\right]{\left[\begin{array}{c}{x}_{1d}\\ x_{2d}\end{array}\right]}_{|x=x_0}$$

博主观看完了视频后依旧存在着一些困惑，因此不断向 chatGPT 咨询，以下均来自博主与 chatGPT 之间的对话

问题1：什么叫做线性化？

回答：线性化是一个过程，通过这个过程，可以将非线性系统近似为线性系统。这通常是为了简化分析和设计，因为线性系统的数学处理相对简单。

问题2：怎么才叫做把一个非线性化的系统线性化了？

回答：将一个非线性系统线性化意味着用一个线性系统来近似原始的非线性系统。这通常在非线性系统的某个工作点附近进行。泰勒级数是一种常用的方法，通过该方法可以将非线性函数展开为多项式形式，然后保留其中的一些主要项来得到线性近似。

问题3：什么叫做平衡点？

回答：平衡点（也称为稳定点或工作点）是系统中的一个点，在该点上，系统的所有导数（或差分）为零，系统不会自发地改变其状态。对于动态系统，平衡点是系统在没有外部干扰地情况下保持其状态不变的点。

问题4：为什么要在平衡点处进行线性化？

回答：在平衡点处进行线性化是因为平衡点是系统自然存在的稳定状态，没有外部干扰时，系统会停留在这个状态。在此点附近的小扰动可以用线性系统来描述，使得线性点近似更为准确。此外，对于许多应用，如控制系统设计，我们通常对系统在工作点或平衡点附近的行为感兴趣，因此在这些点进行线性化是有意义的。

结语

本篇博客主要通过一个实际的二维例子让我们更加直观的理解了卡尔曼滤波器，在调参过程中主要对过程噪声协方差矩阵 $Q$ 和测量噪声协方差矩阵 $R$ 进行调整，以得到更好的估计值。接着我们简单介绍了在非线性系统中如何去应用卡尔曼滤波器，介绍了一个非常基础的方法，将非线性系统线性化，我们使用泰勒展开在系统平衡点处进行线性化，这样的滤波器也叫做扩展卡尔曼滤波器。

卡尔曼滤波器的相关知识分享到这里就结束了，后续如果还有其它的内容再补充吧😄

参考

• 常用的MarkDown颜色 笔记
• 【卡尔曼滤波器】_Kalman_Filter_全网最详细数学推导【博主强烈推荐👍👍👍】
• 【工程数学基础】2_线性化_泰勒级数_泰勒公式_Linearization