怎么开跨境电商网店,seo课程培训机构,做网站计划表,做村易通网站站长要收费吗?目录 前言数学理论推导1. 递归算法2. 数学基础结语参考 前言 最近项目需求涉及到目标跟踪部分#xff0c;准备从 DeepSORT 多目标跟踪算法入手。DeepSORT 中涉及的内容有点多#xff0c;以前也就对其进行了简单的了解#xff0c;但是真正去做发现总是存在这样或者那样的困惑… 目录 前言数学理论推导1. 递归算法2. 数学基础结语参考 前言 最近项目需求涉及到目标跟踪部分准备从 DeepSORT 多目标跟踪算法入手。DeepSORT 中涉及的内容有点多以前也就对其进行了简单的了解但是真正去做发现总是存在这样或者那样的困惑头疼果然欠下的总该还的 一个个来吧这个系列文章主要分享博主在学习 DeepSORT 中的 Kalman Filter 的相关知识主要从两方面分享一方面是数学理论推导另一方面是比较通俗易懂的图例分析。 这篇文章主要分享从数学理论推导的方式去理解卡尔曼滤波器包括递归算法和数学基础 博主为初学者欢迎交流讨论若有问题欢迎各位看官批评指正 数学理论推导 视频链接【卡尔曼滤波器】_Kalman_Filter_全网最详细数学推导 注博主也就把 DR_CAN 老师讲解的内容复述了一遍强烈建议大家观看原视频 1. 递归算法
我们将系统的学习卡尔曼滤波器由浅入深
首先我们来初尝下递归算法的味道谈到卡尔曼滤波器Kalman Filter这个 Filter 这个词在这里面并不是很能准确地体现出来它的特性。
Kalman Filter 如果用一句话来总结它是一种 optimal recursive data processing algorithm
它是一种算法是最优化的、递归的数字处理的算法。它更像是一种观测器而不是一般意义上的滤波器。卡尔曼滤波器的应用非常广泛尤其是在导航当中它的广泛应用是因为我们生活的世界中存在着大量的不确定性。当我们去描述一个系统的时候这个不确定性主要体现在三个方面
1. 不存在完美的数学模型
2. 系统的扰动往往是不可控的也是很难建模的
3. 测量的传感器本身存在误差 下面我们来看一个例子找不同的人用一个尺子去测量一个硬币的直径 这里面我们用 Z k Z_k Zk 来表示测量的结果下标 k k k 就代表了第 k k k 次的测量结果因为每个人测量的值都不一样而且这个尺子本身也有误差所以说每次测量的结果也会不尽相同。比如说前三次测量的结果分别为 Z 1 50.1 m m Z_1 50.1mm Z150.1mm Z 2 50.4 m m Z_2 50.4mm Z250.4mm Z 3 50.2 m m Z_3 50.2mm Z350.2mm这个时候如果我让你去估计一下这个真实结果, 很自然的, 你就会想到去取一个平均值就可以了。
如果用数学来表达这边我们用 X ^ k \hat{X}_k X^k 来表示第 k k k 次的估计值其计算如下所示 X ^ k 1 k ( Z 1 Z 2 ⋅ ⋅ ⋅ Z k ) 1 k ( Z 1 Z 2 ⋅ ⋅ ⋅ Z k − 1 ) 1 k Z k 1 k k − 1 k − 1 ( Z 1 Z 2 ⋅ ⋅ ⋅ Z k − 1 ) 1 k Z k k − 1 k X ^ k − 1 1 k Z k X ^ k − 1 − 1 k X ^ k − 1 1 k Z k \begin{aligned} \hat{X}_k \frac{1}{k} (Z_1 Z_2 \cdot\cdot\cdot Z_k) \\ \frac{1}{k} (Z_1 Z_2 \cdot\cdot\cdot Z_{k-1}) \frac{1}{k}Z_k \\ \frac{1}{k} \frac{k-1}{\color{blue}k-1} \color{blue} (Z_1 Z_2 \cdot\cdot\cdot Z_{k-1})\color{black} \frac{1}{k}Z_k \\ \frac{k-1}{k} \color{blue}\hat{X}_{k-1} \color{black} \frac{1}{k}Z_k \\ \hat{X}_{k-1}-\frac{1}{k}\hat{X}_{k-1}\frac{1}{k}Z_k \end{aligned} X^kk1(Z1Z2⋅⋅⋅Zk)k1(Z1Z2⋅⋅⋅Zk−1)k1Zkk1k−1k−1(Z1Z2⋅⋅⋅Zk−1)k1Zkkk−1X^k−1k1ZkX^k−1−k1X^k−1k1Zk 重新整理下可得到第 k k k 次数的估计值 X ^ k \hat{X}_k X^k 等于下式 X ^ k X ^ k − 1 1 k ( Z k − X ^ k − 1 ) \hat{X}_k \hat{X}_{k-1} \frac{1}{k}(Z_k - \hat{X}_{k-1}) X^kX^k−1k1(Zk−X^k−1) 去分析下这个公式你可以看到随着 k k k 的增加即随着次数的增加 1 k → 0 \frac{1}{k} \to 0 k1→0 X ^ k → X ^ k − 1 \hat{X}_k \to \hat{X}_{k-1} X^k→X^k−1也就是说随着 k k k 的增加测量的结果就不再重要了。这个也非常好理解当你有了大量的数据之后测量了很多次以后你就对估计的结果很有信心了所有以后的测量值就变得不那么重要了。而相反当 k k k 值比较小的时候这个 1 k \frac{1}{k} k1 就会比较大那这个时候测量的结果就可以起到很大的作用尤其是它和估计值差距比较大的时候。
下面我们把这个公式提炼出来我们令 1 k \frac{1}{k} k1 等于 K k \color{blue}K_k Kk则 X ^ k X ^ k − 1 K k ( Z k − X ^ k − 1 ) \hat{X}_k \hat{X}_{k-1} \color{blue}{K}_k \color{black}(Z_k - \hat{X}_{k-1}) X^kX^k−1Kk(Zk−X^k−1) 它所代表的意思就是 当前的估计值 上一次的估计值 系数 × ( 当前测量值 − 上一次的估计值 ) 当前的估计值 \color{green} \color{black}上一次的估计值 \color{green} \color{blue}系数 \color{green} \times \color{black}(当前测量值 \color{green}- \color{black}上一次的估计值) 当前的估计值上一次的估计值系数×(当前测量值−上一次的估计值) 在卡尔曼滤波器里面这个 K k \color{blue}K_k Kk 就代表了卡尔曼增益Kalman Gain或者叫做卡尔曼因数。通过这个公式可以看出来新的估计值 X ^ k \hat{X}_k X^k 与上一次的估计值 X ^ k − 1 \hat{X}_{k-1} X^k−1 有关然后上一次的又会与上上次的有关这就是一种递归的思想。而且这也是卡尔曼滤波器的一个优势它不需要去追溯很久以前的数据只需要上一次的就可以了。
而关于这个 Kalman Gain K k \color{blue}{K_k} Kk在这里先做一个最简单的讨论让大家有一个初步的认识首先先引入两个参数一个是估计误差也就是估计值和真实值的差距我们用 e E S T e_{EST} eEST 表示上面的 e e e 代表 Error 误差下标的 E S T EST EST 就代表 Estimate 估计还有一个是测量误差也就是测量值和真实值的差距用 e M E A e_{MEA} eMEA 来表示下标的 M E A MEA MEA 就代表 Measurement 测量
Kalman Gain 就等于下式 K k e E S T k − 1 e E S T k − 1 e M E A k \color{blue}{K_k} \color{black} \frac{e_{EST_{k-1}}}{e_{EST_{k-1}}e_{MEA_{k}}} KkeESTk−1eMEAkeESTk−1 也就是第 k − 1 k-1 k−1 次的估计误差除以第 k − 1 k-1 k−1 的估计误差加上第 k k k 次的测量误差这个公式实际上就是卡尔曼滤波器的核心公式。我们会在后面的分析中详细的讲解
它是怎么被推导出来的呢
这个时候我们先来分析一下它在 k k k 时刻当 e E S T k − 1 ≫ e M E A k {e_{EST_{k-1}} \gg e_{MEA_{k}}} eESTk−1≫eMEAk 时 K k → 1 \color{blue}{K_k} \to 1 Kk→1 X ^ k X ^ k − 1 Z k − X ^ k − 1 Z k \hat{X}_k \hat{X}_{k-1} Z_k - \hat{X}_{k-1} Z_k X^kX^k−1Zk−X^k−1Zk这就说明了当 k − 1 k-1 k−1 时刻的估计误差远大于第 k k k 次的测量误差的时候这时第 k k k 次的估计值就很趋近于测量值 Z k Z_k Zk 了。这个非常好理解因为你估计的误差大测量得更加准确所以就需要去更加信任这个测量值。
而同理在 k k k 时刻当 e E S T k − 1 ≪ e M E A k {e_{EST_{k-1}} \ll e_{MEA_{k}}} eESTk−1≪eMEAk 时 K k → 0 \color{blue}{K_k} \to 0 Kk→0 X ^ k X ^ k − 1 \hat{X}_k \hat{X}_{k-1} X^kX^k−1也就是说当你的测量误差很大的时候我们去选择相信这个估计值相信上一次的估计值。 有了上面的知识我们先来看一个例子用一个非常简单的卡尔曼滤波器的思想去解决一个问题它可以分为三步
Step1计算 Kalman Gain K k e E S T k − 1 e E S T k − 1 e M E A k \color{blue}K_k \color{black} \frac{e_{EST_{k-1}}}{e_{EST_{k-1}}e_{MEA_{k}}} KkeESTk−1eMEAkeESTk−1
Step2计算 X ^ k X ^ k − 1 K k ( Z k − X ^ k − 1 ) \hat{X}_k \hat{X}_{k-1} \color{blue}{K}_k \color{black}(Z_k - \hat{X}_{k-1}) X^kX^k−1Kk(Zk−X^k−1)
Step3更新 e E S T k ( 1 − K k ) e E S T k − 1 e_{EST_k} (1 - \color{blue} K_k\color{black})e_{EST_{k-1}} eESTk(1−Kk)eESTk−1
有了这三步以后我们就可以去解决一些实际的问题了。比如说还是测量一个物体它的实际长度比如说是 50mm假设我们第一次的估计值 X ^ 0 40 m m \hat{X}_0 40mm X^040mm第一次的估计误差 e E S T 0 5 m m e_{EST_0} 5mm eEST05mm第一次的测量值 Z 1 51 m m Z_1 51mm Z151mm测量误差是 e M E A k 3 m m e_{MEA_k} 3mm eMEAk3mm由于使用同样的一个测量工具所以它第 k k k 次的测量误差也是 3mm
现在可以做一个表格然后把有的内容填在上面我们就可以进行递归的计算了 k k k Z k Z_k Zk e M E A k e_{MEA_k} eMEAk X ^ k \hat{X}_k X^k K k K_k Kk e E S T k e_{EST_k} eESTk0 40 \color{sienna}40 40 5 \color{red}5 51 51 \color{orange}51 51 3 \color{red}3 3
当 k 1 k 1 k1 时 K k 5 5 3 0.625 X ^ k 40 0.625 ( 51 − 40 ) 46.875 e E S T ( 1 − 0.625 ) 5 1.875 \begin{aligned} K_k \color{red}\frac{5}{53} \color{green}0.625 \\ \hat{X}_k \color{sienna}40 \color{green}0.625(\color{orange}51 -\color{sienna}40) \color{black} 46.875 \\ e_{EST} (1 - \color{green}0.625)\color{red}5 1.875 \end{aligned} KkX^keEST5350.625400.625(51−40)46.875(1−0.625)51.875 更新下表格 k k k Z k Z_k Zk e M E A k e_{MEA_k} eMEAk X ^ k \hat{X}_k X^k K k K_k Kk e E S T k e_{EST_k} eESTk0 40 \color{sienna}40 40 5 \color{red}5 51 51 \color{orange}51 51 3 \color{red}3 3 46.875 46.875 46.875 0.625 \color{green}0.625 0.625 1.875 1.875 1.875
当 k 2 k 2 k2 时 K k 1.875 1.875 3 0.3846 X ^ k 46.875 0.3846 ( 48 − 46.875 ) 47.308 e E S T ( 1 − 0.3846 ) 1.875 1.154 \begin{aligned} K_k \frac{1.875}{1.8753} 0.3846 \\ \hat{X}_k 46.875 0.3846(48-46.875) 47.308 \\ e_{EST} (1 - 0.3846)1.875 1.154 \end{aligned} KkX^keEST1.87531.8750.384646.8750.3846(48−46.875)47.308(1−0.3846)1.8751.154 填入到表格中 k k k Z k Z_k Zk e M E A k e_{MEA_k} eMEAk X ^ k \hat{X}_k X^k K k K_k Kk e E S T k e_{EST_k} eESTk0 40 \color{sienna}40 40 5 \color{red}5 51 51 \color{orange}51 51 3 \color{red}3 3 46.875 46.875 46.875 0.625 \color{green}0.625 0.625 1.875 1.875 1.8752 48 48 48 3 3 3 47.308 47.308 47.308 0.3846 0.3846 0.3846 1.154 1.154 1.154
后面的递归工作我们交给 Python 来做代码如下
import matplotlib.pyplot as plte_MEAk 3
e_ESTk 5
Xk [40] [0]*16
Zk [51, 48, 47, 52, 51, 48, 49, 53, 48, 49, 52, 53, 51, 52, 49, 50]Kk 0
for idx in range(len(Zk)):Kk e_ESTk / (e_ESTk e_MEAk)Xk[idx 1] Xk[idx] Kk * (Zk[idx] - Xk[idx])e_ESTk (1 - Kk) * e_ESTkprint(f第 {idx:2} 次: 卡尔曼增益 {Kk:.5f}, 估计值 {Xk[idx 1]:.5f}, 估计误差 {e_ESTk:.5f})k [i for i in range(17)]
plt.plot(k, Xk, markero, colorred, labelEstimates)
plt.plot(k[:-1], Zk, marker o, colorblue, labelMeasurements)
plt.xlabel(Times)
plt.ylabel(Value)
plt.title(Kalman Filter Example)
plt.legend(locright)plt.savefig(kf.png, bbox_inchestight, dpi300)
plt.show()输出如下
第 0 次: 卡尔曼增益 0.62500, 估计值 46.87500, 估计误差 1.87500
第 1 次: 卡尔曼增益 0.38462, 估计值 47.30769, 估计误差 1.15385
第 2 次: 卡尔曼增益 0.27778, 估计值 47.22222, 估计误差 0.83333
第 3 次: 卡尔曼增益 0.21739, 估计值 48.26087, 估计误差 0.65217
第 4 次: 卡尔曼增益 0.17857, 估计值 48.75000, 估计误差 0.53571
第 5 次: 卡尔曼增益 0.15152, 估计值 48.63636, 估计误差 0.45455
第 6 次: 卡尔曼增益 0.13158, 估计值 48.68421, 估计误差 0.39474
第 7 次: 卡尔曼增益 0.11628, 估计值 49.18605, 估计误差 0.34884
第 8 次: 卡尔曼增益 0.10417, 估计值 49.06250, 估计误差 0.31250
第 9 次: 卡尔曼增益 0.09434, 估计值 49.05660, 估计误差 0.28302
第 10 次: 卡尔曼增益 0.08621, 估计值 49.31034, 估计误差 0.25862
第 11 次: 卡尔曼增益 0.07937, 估计值 49.60317, 估计误差 0.23810
第 12 次: 卡尔曼增益 0.07353, 估计值 49.70588, 估计误差 0.22059
第 13 次: 卡尔曼增益 0.06849, 估计值 49.86301, 估计误差 0.20548
第 14 次: 卡尔曼增益 0.06410, 估计值 49.80769, 估计误差 0.19231
第 15 次: 卡尔曼增益 0.06024, 估计值 49.81928, 估计误差 0.18072可视化图如下 蓝色的是我们的测量结果红色的是我们一个的估计结果。可以看到我们最开始是从 40 开始的就是我们初始的估计值它大概用了五步就到了 49 这个位置我们之前有说实际值是 50然后再经过几步的迭代之后它就越来越靠近我们的实际值。这就是卡尔曼滤波器的一种递归的思想随着我们不断的去更新不断的有新的数据进来它会越来越靠近真实的值。
可以看得出来这是一个非常简单的利用卡尔曼滤波器的递归思想来做估计的一个例子我们会在后面的分析中详细展开卡尔曼滤波器的推导过程并给出一些更深层次的应用。
2. 数学基础
这节我们主要讨论四个方面四个数学基础分别是数据融合Data Fusion、协方差矩阵Covariance Matrix、状态空间方程State Space以及观测器Observation
我们先来讨论 Data Fusion 数据融合还是从一个例子开始比如说我们用两个称去称一个东西得到了两个结果分别是 Z 1 30 g \color{blue}Z_130g Z130g Z 2 32 g \color{red}Z_232g Z232g。而且我们知道这两个称都不准测量都有误差比如说对于第一个称来说它的标准差 σ 1 2 g \color{blue}\sigma_12g σ12g 第二个称的标准差 σ 2 4 g \color{red}\sigma_24g σ24g。
它们都符合正态分布也叫高斯分布所以如果说给出这样的一个条件的话它们在图中表现出来的就是中型的曲线比如第一个结果的概率分布如下图 中间的位置是 30g然后标准差是 2向左向右各一个标准差就覆盖了 68.4% 的可能性也就是说测出来的结果在 28g~32g 之间的概率是 68.4%
而对于另外一个因为它的标准差是 4所以它的图像更矮一些更胖一些如下图所示 关于正态分布和标准差更多细节看观看【工程数学基础】9_阈值如何选取在机器视觉中应用正态分布和6-Sigma
我们来把这两个分布绘制在用一张图中如下图所示 这个时候我们有了这两个结果如果让你去根据这两个结果去估算一下真实值将会是多少呢
各位看官可以自己思考一下如果凭感觉的话它应该是在这两个结果的中间而且由于第一个称的误差小即标准差小所以它会靠近第一个称称出来的结果。
而如果从数学上去找到一个最优的一个估计值我们又该如何做呢
这就要用到我们上一节介绍过的 Kalman Gain 的这个思想了可以令这个估计值等于下式 Z ^ Z 1 K ( Z 2 − Z 1 ) \hat{Z} \color{blue}Z_1 \color{black} \color{green}K\color{black}(\color{red}Z_2 \color{black}- \color{blue}Z_1\color{black}) Z^Z1K(Z2−Z1) 这里面的 K \color{green}K K 就是 Kalman Gain其值是一个从 0~1 的一个数可以看出来
当 K 0 K 0 K0 时 Z ^ Z 1 \hat{Z} \color{blue}Z_1 Z^Z1
当 K 1 K 1 K1 时 Z ^ Z 2 \hat{Z} \color{red}Z_2 Z^Z2
下面就是要去求解 K K K 了我们的目标就是去求解合适的 K K K 使得 Z ^ \hat{Z} Z^ 这个估计值的标准差最小也就是方差最小。它的方差计算如下 σ Z ^ 2 V a r ( Z ^ ) V a r ( Z 1 K ( Z 2 − Z 1 ) ) V a r ( Z 1 − K Z 1 K Z 2 ) V a r ( ( 1 − K ) Z 1 K Z 2 ) V a r ( ( 1 − K ) Z 1 ) V a r ( K Z 2 ) ( 1 − K ) 2 V a r ( Z 1 ) K 2 V a r ( Z 2 ) ( 1 − K ) 2 σ 1 2 K 2 σ 2 2 \begin{aligned} \sigma^2_{\hat{Z}} Var(\hat{Z}) \\ Var(Z_1 K(Z_2-Z_1)) \\ Var(Z_1 - KZ_1 KZ_2) \\ Var((1-K)Z_1 KZ_2) \\ Var((1-K)Z_1) Var(KZ_2) \\ (1-K)^2Var(Z_1) K^2Var(Z_2) \\ (1-K)^2\sigma_1^2 K^2\sigma_2^2 \\ \end{aligned} σZ^2Var(Z^)Var(Z1K(Z2−Z1))Var(Z1−KZ1KZ2)Var((1−K)Z1KZ2)Var((1−K)Z1)Var(KZ2)(1−K)2Var(Z1)K2Var(Z2)(1−K)2σ12K2σ22 其中 Z 1 Z_1 Z1 和 Z 2 Z_2 Z2 相互独立我们的目标是要求 σ Z ^ 2 \sigma^2_{\hat{Z}} σZ^2 最小因此我们可以让它对 K K K 求导求取其对应的极值如下 d σ Z ^ 2 d K 0 − 2 ( 1 − K ) σ 1 2 2 K σ 2 2 0 − σ 1 2 K σ 1 2 K σ 2 2 0 K ( σ 1 2 σ 2 2 ) σ 1 2 \begin{aligned} \frac{d\sigma^2_{\hat{Z}}}{dK} 0 \\ -2(1-K)\sigma_1^2 2K\sigma_2^2 0 \\ -\sigma_1^2 K\sigma_1^2 K\sigma_2^2 0 \\ K(\sigma_1^2\sigma_2^2) \sigma_1^2 \\ \end{aligned} dKdσZ^2−2(1−K)σ122Kσ22−σ12Kσ12Kσ22K(σ12σ22)000σ12 因此最终的 K K K 计算如下 K σ 1 2 σ 1 2 σ 2 2 K \frac{\sigma_1^2}{\sigma_1^2\sigma_2^2} Kσ12σ22σ12 我们把 σ 1 2 \sigma_1 2 σ12 σ 2 4 \sigma_2 4 σ24 代入有 K σ 1 2 σ 1 2 σ 2 2 2 2 2 2 4 2 4 4 16 0.2 K \frac{\sigma_1^2}{\sigma_1^2\sigma_2^2} \frac{2^2}{2^24^2}\frac{4}{416}0.2 Kσ12σ22σ1222422241640.2 我们把 K 0.2 K 0.2 K0.2 代入有 Z ^ Z 1 K ( Z 2 − Z 1 ) 30 0.2 ( 32 − 30 ) 30.4 \begin{aligned} \hat{Z} \color{blue}Z_1 \color{black} \color{green}K\color{black}(\color{red}Z_2 \color{black}- \color{blue}Z_1\color{black}) \\ \color{blue}30 \color{black} 0.2 (\color{red}32 \color{black}- \color{blue}30\color{black}) \\ 30.4 \end{aligned} Z^Z1K(Z2−Z1)300.2(32−30)30.4 也就是说根据这两个称的特性和测量出来的结果我们做出了预测这个预测是 30.4g并且通过数学证明了它是最优解。这时候也可以去计算它的方差 σ Z ^ 2 ( 1 − K ) 2 σ 1 2 K 2 σ 2 2 ( 1 − 0.2 ) 2 × 2 2 0. 2 2 × 4 2 3.2 \begin{aligned} \sigma^2_{\hat{Z}} (1-K)^2\sigma_1^2 K^2\sigma_2^2 \\ (1-0.2)^2\times2^20.2^2\times4^2 \\ 3.2 \end{aligned} σZ^2(1−K)2σ12K2σ22(1−0.2)2×220.22×423.2 其标准差 σ Z ^ 3.2 1.79 \sigma_{\hat{Z}} \sqrt{3.2} 1.79 σZ^3.2 1.79
所以说我们的这个最优的估计值 σ Z ^ 2 30.4 g \sigma^2_{\hat{Z}} 30.4g σZ^230.4g 标准差 σ Z ^ 1.79 \sigma_{\hat{Z}} 1.79 σZ^1.79
如果将它绘制在之前的图中它应该是一个更高更瘦的图像。如下图所示 这个过程就叫做数据融合大家要掌握这个思想记住这个理念过一会我们会再回过头来看它。 第二个要聊的就是协方差矩阵 Covariance Matrix它把方差和协方差在一个矩阵当中表现出来也就是体现了变量间的一个联动关系。还是从一个例子入手
球员身高体重年龄瓦尔迪1797433奥巴梅扬1878031萨拉赫1757128Mean180.37530.7
上表是 2020 年英超中射手榜前三球员的一些基本的数据包括身高、体重和年龄。我们将它们当作三个变量分别为 x x x y y y 和 z z z然后我们求下它们的平均值 x ˉ 1 3 ( 179 187 175 ) 180.3 y ˉ 1 3 ( 74 80 71 ) 75 z ˉ 1 3 ( 33 31 28 ) 30.7 \begin{aligned} \bar x \frac{1}{3}(179187175) 180.3 \\ \bar y \frac{1}{3}(748071) 75 \\ \bar z \frac{1}{3}(333128) 30.7 \end{aligned} xˉyˉzˉ31(179187175)180.331(748071)7531(333128)30.7 分别是身高 180.3体重 75年龄 30.7然后我们还可以计算下它们对应的方差 σ x 2 1 3 ( ( 179 − 180.3 ) 2 ( 187 − 180.3 ) 2 ( 175 − 180.3 ) 2 ) 24.89 σ y 2 1 3 ( ( 74 − 75 ) 2 ( 80 − 75 ) 2 ( 71 − 75 ) 2 ) 14 σ z 2 1 3 ( ( 33 − 30.7 ) 2 ( 31 − 30.7 ) 2 ( 28 − 30.7 ) 2 ) 4.22 \begin{aligned} \sigma_x^2 \frac{1}{3}((179-180.3)^2(187-180.3)^2(175-180.3)^2) 24.89 \\ \sigma_y^2 \frac{1}{3}((74-75)^2(80-75)^2(71-75)^2) 14 \\ \sigma_z^2 \frac{1}{3}((33-30.7)^2(31-30.7)^2(28-30.7)^2) 4.22 \end{aligned} σx2σy2σz231((179−180.3)2(187−180.3)2(175−180.3)2)24.8931((74−75)2(80−75)2(71−75)2)1431((33−30.7)2(31−30.7)2(28−30.7)2)4.22 协方差计算如下 σ x σ y 1 3 ( ( 179 − 180.3 ) ( 74 − 75 ) ( 187 − 180.3 ) ( 80 − 75 ) ( 175 − 180.3 ) ( 71 − 75 ) ) 18.7 σ y σ x σ x σ z 1 3 ( ( 179 − 180.3 ) ( 33 − 30.7 ) ( 187 − 180.3 ) ( 31 − 30.7 ) ( 175 − 180.3 ) ( 28 − 30.7 ) ) 4.4 σ z σ x σ y σ z 1 3 ( ( 74 − 75 ) ( 33 − 30.7 ) ( 80 − 75 ) ( 31 − 30.7 ) ( 71 − 75 ) ( 28 − 30.7 ) ) 3.3 σ z σ y \begin{aligned} \sigma_x \sigma_y \frac{1}{3}((179-180.3)(74-75)(187-180.3)(80-75)(175-180.3)(71-75)) 18.7 \sigma_y \sigma_x\\ \sigma_x \sigma_z \frac{1}{3}((179-180.3)(33-30.7)(187-180.3)(31-30.7)(175-180.3)(28-30.7)) 4.4 \sigma_z \sigma_x \\ \sigma_y \sigma_z \frac{1}{3}((74-75)(33-30.7)(80-75)(31-30.7)(71-75)(28-30.7)) 3.3 \sigma_z \sigma_y \end{aligned} σxσyσxσzσyσz31((179−180.3)(74−75)(187−180.3)(80−75)(175−180.3)(71−75))18.7σyσx31((179−180.3)(33−30.7)(187−180.3)(31−30.7)(175−180.3)(28−30.7))4.4σzσx31((74−75)(33−30.7)(80−75)(31−30.7)(71−75)(28−30.7))3.3σzσy 可以看到如果最后计算出来的值大于 0则说明两个变量的变化方向是一样的这时候我们称这两个变量正相关。而如果计算出来的值小于 0则说明两个变量的变化方向是相反的这时候我们称这两个变量负相关
然后说到协方差矩阵 P P P它的形式如下所示 P [ σ x 2 σ x σ y σ x σ z σ y σ x σ y 2 σ y σ z σ z σ x σ z σ y σ z 2 ] [ 24.89 18.7 4.4 18.7 14 3.3 4.4 3.3 4.22 ] P\begin{bmatrix} \sigma_x^2 \sigma_x\sigma_y \sigma_x\sigma_z \\ \sigma_y\sigma_x \sigma_y^2 \sigma_y\sigma_z \\ \sigma_z\sigma_x \sigma_z\sigma_y \sigma_z^2 \\ \end{bmatrix} \begin{bmatrix} 24.89 18.7 4.4 \\ 18.7 14 3.3 \\ 4.4 3.3 4.22 \\ \end{bmatrix} P σx2σyσxσzσxσxσyσy2σzσyσxσzσyσzσz2 24.8918.74.418.7143.34.43.34.22 然后我们就可以分析各个数据之间的关系因为这里只取了三组数据所以不是非常准确我们就希望把它扩展一下在扩展之前我们先来看看如何通过矩阵的运算来计算这个协方差。
这个对于以后要编程的话非常有帮助我们还是看这个例子首先我们要先求一个过渡矩阵 a a a a [ x 1 y 1 z 1 x 2 y 2 z 2 x 3 y 3 z 3 ] − 1 3 [ 1 1 1 1 1 1 1 1 1 ] [ x 1 y 1 z 1 x 2 y 2 z 2 x 3 y 3 z 3 ] a\begin{bmatrix} x_1 y_1 z_1 \\ x_2 y_2 z_2 \\ x_3 y_3 z_3 \\ \end{bmatrix}- \frac{1}{3} \begin{bmatrix} 1 1 1 \\ 1 1 1 \\ 1 1 1 \\ \end{bmatrix} \begin{bmatrix} x_1 y_1 z_1 \\ x_2 y_2 z_2 \\ x_3 y_3 z_3 \\ \end{bmatrix} a x1x2x3y1y2y3z1z2z3 −31 111111111 x1x2x3y1y2y3z1z2z3 协方差矩阵 P P P 的计算则为 P 1 3 a T a P \frac{1}{3}a^Ta P31aTa通过这样的方法我们就可以算出协方差矩阵了。
我们现在来分析一下这个协方差矩阵说明了什么下表中给出了 15 位球员的一个基本信息它涵盖了五大联赛在 2020 年射手榜的前三位
球员身高体重年龄瓦尔迪1797433奥巴梅扬1878031萨拉赫1757128梅西1707233本泽马1858132杰拉德1777528因莫比莱1787130C罗1878335卢卡库1909427姆巴佩1787321本耶德尔1706829邓贝莱1837323莱万多夫斯基1847831维尔纳1807524桑乔1807630Mean180.276.2728.3
下表则是它们的协方差矩阵
身高体重年龄身高32.6929.751.4体重29.7538.063.98年龄1.43.97819.4
大家可以仔细观察下先看协方差矩阵对角线的这几个数可以看到身高体重的变化还是蛮大的它们的方差还是很大的。这也就说明了如果你想成为射手并不会很局限于身高和体重年龄的跨度也比较大说明一个射手和年龄的关系也不是特别的大。
接着我们再来看下它们的协方差的关系首先是身高和体重可以看到它们的协方差等于 29.75也就是说明体重和身高是非常的正相关的随着身高的增加体重也增加这是非常非常合理的。然后我们再看年龄和体重和身高它们的相关性就非常小一个是 1.4 一个是 3.98这就说明了对于这些运动员来说他们的身高、体重和年龄就没有太大的一个关系了就说明他们确实是保持的还是很好的并没有因为年龄大了就变胖 最后一个话题就是状态空间的表达式和观测器其实关于状态空间的表达来说它是一个完整的体系。现代控制理论就是以状态空间方程为基础的感兴趣的可参考视频现代控制理论
在这里我们只是给出一些最基本的一些概念依然从一个例子入手一个典型的弹簧振动阻尼系统如下图所示 它的质量是 m m m向右施加的力是 F F F向右的方向是 x x x弹簧的胡可系数是 k k k阻尼系数是 B B B它的动态方程的表达式如下 m x ¨ B x ˙ k x F m\ddot{x}B\dot{x}kxF mx¨Bx˙kxF 我们可以把 F F F 定义成 u u u 也就是系统的输入这个时候如果要把它化成状态空间的一种表达形式首先就要确定两个状态变量我们可以令 x 1 x x_1 x x1x x 2 x ˙ x_2 \dot{x} x2x˙这样的话有 x ˙ 1 x 2 x ˙ 2 x ¨ 1 m u − B m x ˙ − k m x 1 m u − B m x 2 − k m x 1 \begin{aligned} \dot{x}_1 x_2 \\ \dot{x}_2 \ddot{x} \\ \frac{1}{m}u-\frac{B}{m}\dot{x}-\frac{k}{m}x \\ \frac{1}{m}u-\frac{B}{m}x_2-\frac{k}{m}x_1 \end{aligned} x˙1x˙2x2x¨m1u−mBx˙−mkxm1u−mBx2−mkx1 这样就用两个一阶微分方程把这个形式表达出来了如果我们这里面还有一个测量量我们可以把它定义成 z 1 z_1 z1 z 1 z_1 z1 测的是它的位置 z 1 x x 1 z_1 x x_1 z1xx1 z 2 z_2 z2 测的是它的速度 z 2 x ˙ x 2 z_2 \dot{x} x_2 z2x˙x2这样的话就是一个测量的一个方程。
然后我们可以把上面的四个式子用矩阵的方式把它很紧凑的表达出来如下所示 [ x ˙ 1 x ˙ 2 ] [ 0 1 − k m − B m ] [ x 1 x 2 ] [ 0 1 m ] u [ z 1 z 2 ] [ 1 0 0 1 ] [ x 1 x 2 ] \begin{aligned} \begin{bmatrix} \dot{x}_1 \\ \dot{x}_2 \\ \end{bmatrix} \begin{bmatrix} 0 1 \\ -\frac{k}{m} -\frac{B}{m} \\ \end{bmatrix} \begin{bmatrix} {x}_1 \\ {x}_2 \\ \end{bmatrix} \begin{bmatrix} 0 \\ \frac{1}{m} \\ \end{bmatrix} u \\ \\ \begin{bmatrix} z_1 \\ z_2 \\ \end{bmatrix} \begin{bmatrix} 1 0 \\ 0 1 \\ \end{bmatrix} \begin{bmatrix} {x}_1 \\ {x}_2 \\ \end{bmatrix} \end{aligned} [x˙1x˙2][z1z2][0−mk1−mB][x1x2][0m1]u[1001][x1x2] 这就是状态空间的表达形式归纳出来如下所示 X ˙ ( t ) A X ( t ) B U ( t ) Z ( t ) H X ( t ) \begin{aligned} \dot{X}(t) AX(t)BU(t) \\ Z(t) HX(t) \end{aligned} X˙(t)Z(t)AX(t)BU(t)HX(t) 上式是一种连续的表达形式我们看到 X ˙ ( t ) \dot{X}(t) X˙(t) 就是 X X X 对时间 t t t 的导数它体现了随时间的变化。如果我们把它写成离散的一种形式每相隔一个时刻来看这个变化的话它就可以写成下面的形式 X k A X k − 1 B U k Z k H X k \begin{aligned} X_k AX_{k-1}BU_{k} \\ Z_k HX_k \end{aligned} XkZkAXk−1BUkHXk 这个下标 k k k k − 1 k-1 k−1 k 1 k1 k1每一个单位 1 都代表了一个时间单位叫做 sample time也就是采样时间。这样的话它也代表了一种变化趋势。
当然从连续型到离散型的表达不是直接照抄上面的矩阵结果是需要根据采样时间来计算的但这部分内容不是这里所讨论的重点这里面你只需要理解它的基本概念就可以了。它是体现了一种变化是从上一步到这一步的一种变化。
好最后如果大家还记得之前所说的世界中充满了不确定性。这个时候我们就可以加一些不确定性在里边了。比如说我们的模型也许不准确加一个 W k − 1 \color{red}W_{k-1} Wk−1 它叫做过程噪音Process Noise关于测量我们也可以加上 V k \color{red}V_k Vk 它叫做测量噪音Measurement Noise是在测量当中产生的不确定性那我们整个表达式就变成了下面的形式 X k A X k − 1 B U k W k − 1 Z k H X k V k \begin{aligned} X_k AX_{k-1}BU_{k}\color{red}W_{k-1} \\ Z_k HX_k\color{red}V_{k} \end{aligned} XkZkAXk−1BUkWk−1HXkVk 现在我们就有了两个不确定性了也就是说模型不准确然后测出来的值也不准确在这两个都不确定的情况下如何去估计一个精确的 x ^ k \hat{x}_k x^k 呢
这就是卡尔曼滤波器需要解决的问题
大家回想一下我们在开头讲解的数据融合的例子我们现在遇到的情况其实和上面的那个情况也是差不多的只不过我们现在有一个不太准的计算结果和一个不太准的测量结果我们要根据这两个结果来估计一个相对准确的值来找到一个误差比它们两个本身都要小的一个结果。
这就是我们后面要重点去分析的内容到此为止关于 Kalman Filter 的基本理念就讲完了概念性的直观理解也到此结束了从之后开始就是比较枯燥的数学推导了请大家做好准备。
结语 本篇博客首先从递归算法出发来理解卡尔曼滤波器卡尔曼滤波器是一种最优化的、递归的数字处理算法它更像是一种观测器而不是一般意义上的滤波器。接着我们讲解了一个实际的例子利用卡尔曼滤波器的递归思想来估计硬币的直径随着我们不断地去更新我们估计的值会越来越接近实际值这是递归思想的体现。 随后我们讲解了推导卡尔曼滤波器需要的数学基础包括数据融合、协方差矩阵、状态空间方程和观测器四个部分。我们从两个不准确的称称同一个物体如何获得一个比较准确的结果这个例子出发讲解了数据融合的概念我们从运动员的身高体重年龄之间的相关性这个例子出发讲解了协方差矩阵的概念最后我们通过弹簧振动阻尼系统讲解了状态空间表达式的概念。 在下一篇文章中我们将会详细推导卡尔曼增益和误差协方差矩阵敬请期待 参考
现代控制理论常用的MarkDown颜色 笔记【卡尔曼滤波器】_Kalman_Filter_全网最详细数学推导【博主强烈推荐】【工程数学基础】9_阈值如何选取在机器视觉中应用正态分布和6-Sigma
