学习卡尔曼滤波不能一蹴而就，特别是对于基础薄弱者而言，需要一步一步来，在推导kalman滤波算法之前，需要学习一些基础知识作为铺垫。

1、递归算法

卡尔曼滤波，它本质上是一种最优的递归的数字处理算法。卡尔曼滤波器的应用非常广泛，这是因为在实际的系统当中，存在许多的不确定性，主要包含以下三个方面：
① 不存在完美的数学模型
② 系统的扰动不可控，也很难建模
③ 测量传感器存在误差

例子：找不同的人测量一个硬币的直径（测量的人不同，尺子存在误差）

测量结果：zk{z_k}zk表示第k次的测量结果。

此时，如果我们要去估计真实数据 → 自然想到取平均值

数学表达如下：
x^k=1k(z1+z2+⋯+zk)，x^k表示第k次的估计值。{\hat x_k} = {1 \over k}\left( {{z_1} + {z_2} + \cdots + {z_k}} \right)，{\hat x_k}表示第k次的估计值。x^k=k1(z1+z2+⋯+zk)，x^k表示第k次的估计值。

将取平均推导成递归表达式:
x^k=1k(z1+z2+⋯+zk−1)+1kzk=1k⋅k−1k−1(z1+z2+⋯+zk−1)+1kzk=k−1k⋅1k−1(z1+z2+⋯+zk−1)+1kzk=k−1k⋅x^k−1+1kzk=x^k−1−1kx^k−1+1kzk\begin{aligned} {{\hat x}_k} &= \frac{1}{k}\left( {{z_1} + {z_2} + \cdots + {z_{k - 1}}} \right) + \frac{1}{k}{z_k}\\ &= \frac{1}{k} \cdot \frac{{k - 1}}{{k - 1}}\left( {{z_1} + {z_2} + \cdots + {z_{k - 1}}} \right) + \frac{1}{k}{z_k}\\ & = \frac{{k - 1}}{k} \cdot \frac{1}{{k - 1}}\left( {{z_1} + {z_2} + \cdots + {z_{k - 1}}} \right) + \frac{1}{k}{z_k}\\ & = \frac{{k - 1}}{k} \cdot {{\hat x}_{k - 1}} + \frac{1}{k}{z_k}\\ &= {{\hat x}_{k - 1}} - \frac{1}{k}{{\hat x}_{k - 1}} + \frac{1}{k}{z_k} \end{aligned}x^k=k1(z1+z2+⋯+zk−1)+k1zk=k1⋅k−1k−1(z1+z2+⋯+zk−1)+k1zk=kk−1⋅k−11(z1+z2+⋯+zk−1)+k1zk=kk−1⋅x^k−1+k1zk=x^k−1−k1x^k−1+k1zk

整理可得：
x^k=x^k−1+1k(zk−x^k−1){\hat x_k} = {\hat x_{k - 1}} + \frac{1}{k}\left( {{z_k} - {{\hat x}_{k - 1}}} \right)x^k=x^k−1+k1(zk−x^k−1)

当k↑，1k→0\frac{1}{k} \to 0k1→0，x^k→x^k−1{\hat x_k} \to {\hat x_{k - 1}}x^k→x^k−1 ，随着k增加，测量的结果zk{z_k}zk就不重要了。

理解: 当你有了大量的数据之后，测量了很多次
对估计的结果很有信心了，所以以后的测量值就变得不那么重要了

相反，当k小，1k\frac{1}{k}k1大，zk{z_k}zk的作用较大。

当k↑，，，随着k增加，测量的结果就不重要了
理解: 当你有了大量的数据之后，测量了很多次
对估计的结果很有信心了，所以以后的测量值就变得不那么重要了
相反，当k小，大，的作用较大。

我们用Kk{K_k}Kk来代替1k\frac{1}{k}k1，就可以得到
x^k=x^k−1+Kk(zk−x^k−1){\hat x_k} = {\hat x_{k - 1}} + {K_k}\left( {{z_k} - {{\hat x}_{k - 1}}} \right)x^k=x^k−1+Kk(zk−x^k−1)

当前的估计值 = 上一次的估计值 + 系数 × （当前的测量值 – 上一次的估计值）

在Kalman滤波器里面这个Kk{K_k}Kk叫作： Kalman Gain 卡尔曼增益/因数。

观察公式 x^k=x^k−1+Kk(zk−x^k−1){\hat x_k} = {\hat x_{k - 1}} + {K_k}\left( {{z_k} - {{\hat x}_{k - 1}}} \right)x^k=x^k−1+Kk(zk−x^k−1)，估计值x^k{\hat x_k}x^k和上一次的估计值x^k−1{\hat x_{k - 1}}x^k−1有关，这就是递归的思想。

下面对Kk{K_k}Kk做一个简单的讨论，做一个初步的认识。
引入两个概念。
估计误差，用eEST{e_{EST}}eEST表示，（eee是error误差的缩写， ESTESTEST是estimate估计的缩写)
测量误差，用eMEA{e_{MEA}}eMEA表示，（MEAMEAMEA是measurement测量的缩写）
Kk=eESTk−1eESTk−1+eMEAk{K_k} = \frac{{{e_{ES{T_{k - 1}}}}}}{{{e_{ES{T_{k - 1}}}} + {e_{ME{A_k}}}}}Kk=eESTk−1+eMEAkeESTk−1

讨论：在k时刻
① eESTk−1{e_{ES{T_{k - 1}}}}eESTk−1 >> eMEAk{e_{ME{A_k}}}eMEAk , Kk→1{K_k} \to 1Kk→1 ,x^k=x^k−1+1⋅(zk−x^k−1)=zk{\hat x_k} = {\hat x_{k - 1}} + 1 \cdot \left( {{z_k} - {{\hat x}_{k - 1}}} \right) = {z_k}x^k=x^k−1+1⋅(zk−x^k−1)=zk 。当k-1的估计误差远大于 k次的测量误差的时候，第k的估计就很趋近于测量值。
② eESTk−1{e_{ES{T_{k - 1}}}}eESTk−1 << eMEAk{e_{ME{A_k}}}eMEAk, Kk→0{K_k} \to 0Kk→0 , x^k=x^k−1+0⋅(zk−x^k−1)=x^k−1{\hat x_k} = {\hat x_{k - 1}} + 0 \cdot \left( {{z_k} - {{\hat x}_{k - 1}}} \right) = {\hat x_{k - 1}}x^k=x^k−1+0⋅(zk−x^k−1)=x^k−1。当测量误差很大的时候，我们去选择更多的相信估计值。

下面用一个例子来简单体会一下上面学到的知识。
Step1 计算Kalman Gain Kk=eESTk−1eESTk−1+eMEAk{K_k} = \frac{{{e_{ES{T_{k - 1}}}}}}{{{e_{ES{T_{k - 1}}}} + {e_{ME{A_k}}}}}Kk=eESTk−1+eMEAkeESTk−1
Step2 计算估计值x^k=x^k−1+Kk(zk−x^k−1){\hat x_k} = {\hat x_{k - 1}} + {K_k}\left( {{z_k} - {{\hat x}_{k - 1}}} \right)x^k=x^k−1+Kk(zk−x^k−1)
Step3 更新估计误差，eESTk=(1−Kk)eESTk−1{e_{ES{T_k}}} = \left( {1 - {K_k}} \right){e_{ES{T_{k - 1}}}}eESTk=(1−Kk)eESTk−1，此部分会在后面详细推导。

有了这三步以后，开始可以应用了。
比如一个物体他实际的长度是x=50mmx = 50mmx=50mm，我们第一次的估计值随便估计一个x^0=40mm{\hat x_0} = 40mmx^0=40mm,第一次的估计误差随便给一个数字eEST0=5mm{e_{ES{T_0}}} = 5mmeEST0=5mm，假设我们的测量误差都是eMEA=3mm{e_{MEA}} = 3mmeMEA=3mm。
下面给出Excel的计算结果

根据上面的内容，有没有体会到递归的味道呢？

2、数据融合

假设我们用两个称去称一个东西，得到了两个结果分别是

z1=30g{z_1} = 30gz1=30g, δ1=2g{\delta _1} = 2gδ1=2g标准差 z2=32g{z_2} = 32gz2=32g, δ1=4g{\delta _1} = 4gδ1=4g标准差假设它们都不准，符合正态分布如果我们现在需要去估算真实值 z^=?\hat z = ?z^=?

利用上小节学到的内容，我们进行如下估计：
z^=z1+k(z2−z1)\hat z = {z_1} + k\left( {{z_2} - {z_1}} \right)z^=z1+k(z2−z1)
kkk为kalman gain，k∈[0,1]k \in \left[ {0,1} \right]k∈[0,1],当k=0k = 0k=0时，z^=z1\hat z = {z_1}z^=z1,我们更相信称1的结果，当k=1k = 1k=1时，z^=z2\hat z = {z_2}z^=z2,我们更相信称2的结果。

下面来求解最优的kkk，使得估计值z^\hat zz^的标准差δz^\delta \hat zδz^最小，标准差最小，即方差var(z^){\mathop{\rm var}} \left( {\hat z} \right)var(z^)最小（方差是标准差的平方）。
δz^2=var(z^)=var[z1+k(z2−z1)]=var[(1−k)z1+kz2],两个称是相互独立的，互不影响=(1−k)2var(z1)+k2var(z2)=(1−k)2δ12+k2δ22\begin{aligned} \delta _{\hat z}^2 &= {\mathop{\rm var}} \left( {\hat z} \right) = {\mathop{\rm var}} \left[ {{z_1} + k\left( {{z_2} - {z_1}} \right)} \right]\\\\ & = {\mathop{\rm var}} \left[ {\left( {1 - k} \right){z_1} + k{z_2}} \right],两个称是相互独立的，互不影响\\\\ & = {\left( {1 - k} \right)^2}{\mathop{\rm var}} \left( {{z_1}} \right) + {k^2}{\mathop{\rm var}} \left( {{z_2}} \right)\\\\ &= {\left( {1 - k} \right)^2}\delta _1^2 + {k^2}\delta _2^2 \end{aligned}δz^2=var(z^)=var[z1+k(z2−z1)]=var[(1−k)z1+kz2],两个称是相互独立的，互不影响=(1−k)2var(z1)+k2var(z2)=(1−k)2δ12+k2δ22
要寻找一个合适的kkk，使得δz^2\delta _{\hat z}^2δz^2最小，那么就对δz^2\delta _{\hat z}^2δz^2求kkk的导数，并令其=0=0=0。

dδz^2dk=2⋅(−1)⋅(1−k)δ12+2kδ22=0⇒k(δ12+δ22)=δ12⇒k=δ12δ12+δ22\begin{aligned} \frac{{d\delta _{\hat z}^2}}{{dk}} &= 2 \cdot \left( { - 1} \right) \cdot \left( {1 - k} \right)\delta _1^2 + 2k\delta _2^2 = 0\\\\ & \Rightarrow k\left( {\delta _1^2 + \delta _2^2} \right) = \delta _1^2 \Rightarrow k = \frac{{\delta _1^2}}{{\delta _1^2 + \delta _2^2}} \end{aligned}dkdδz^2=2⋅(−1)⋅(1−k)δ12+2kδ22=0⇒k(δ12+δ22)=δ12⇒k=δ12+δ22δ12

把实际的参数代入可得：
k=δ12δ12+δ22=2222+42=0.2,z^=z1+k(z2−z1)=30+0.2(32−30)=30.4gk = \frac{{\delta _1^2}}{{\delta _1^2 + \delta _2^2}} = \frac{{{2^2}}}{{{2^2} + {4^2}}} = 0.2, \hat z = {z_1} + k\left( {{z_2} - {z_1}} \right) = 30 + 0.2\left( {32 - 30} \right) = 30.4gk=δ12+δ22δ12=22+4222=0.2,z^=z1+k(z2−z1)=30+0.2(32−30)=30.4g

另外，估计值的标准差也可以代入计算。
δz^2=(1−k)2δ12+k2δ22=(1−0.2)222+0.2242=3.2⇒δz^=δz^2=1.79g\delta _{\hat z}^2 = {\left( {1 - k} \right)^2}\delta _1^2 + {k^2}\delta _2^2 = {\left( {1 - 0.2} \right)^2}{2^2} + {0.2^2}{4^2} = 3.2 \Rightarrow \delta \hat z = \sqrt[{}]{{\delta _{\hat z}^2}} = 1.79gδz^2=(1−k)2δ12+k2δ22=(1−0.2)222+0.2242=3.2⇒δz^=δz^2=1.79g

下面分析一下上述的推导结果
δz^<δz1\delta \hat z < \delta z_1δz^<δz1 且 δz^<δz2\delta \hat z < \delta z_2δz^<δz2，说明什么？说明利用两个不太准确的称的测量值对真实值进行估计计算，能够得到一个比两个称都更准确的结果。这个过程，就叫做数据融合。

夹带点私货：那么如果我有三个称呢？我先两个称融合，用结果再和第三称再融合。
在卡尔曼滤波的时候，是用模型估计和测量估计进行数据融合，如果我的系统存在多个同类型传感器可以得到测量值呢，那是否可以先多个传感器先进行数据融合，然后再进行模型估计和测量估计的数据融合呢？如果我的系统存在多个同类型传感器和多个不同类型的传感器可得到与状态向量相关的测量值呢？那这时候应该如何利用这些去得到一个更准确的状态估计呢？这是后续值得进一步学习的方向。

感谢您的阅读，欢迎留言讨论、收藏、点赞、分享。

滤波算法_标准卡尔曼滤波(SKF, Standard Kalman filter)_①基础铺垫相关推荐

python中值滤波算法_Python实现卡尔曼滤波算法之贝叶斯滤波
Python实现卡尔曼滤波算法之贝叶斯滤波作者:yangjian 卡尔曼滤波器属于贝叶斯滤波器的一种特例,本文主要讲解贝叶斯滤波原理及其算法的python实现. 先来看下贝叶斯公式贝叶斯公式 :后 ...
【笔记】自适应卡尔曼滤波 Adaptive Extended Kalman Filter
0 阅读文章 <Adaptive Adjustment of Noise Covariance in Kalman Filter for Dynamic State Estimation> ...
cie规定的标准光源_标准施照体与标准光源_应用光学与光学设
标准施照体与标准光源_应用光学与光学设 9.2.5 CIE标准施照体与标准光源为了统一颜色的测量与色度计算,必须在人们共同约定一些具有代表性的光源下标定物体的颜色.为此,CIE规定了几种" ...
Ensemble Kalman filter集合卡尔曼滤波
在气象预测领域,很多时候,模型具有O(10e8)O(10e8)O(10e8)以上的量级,如果使用传统的卡尔曼滤波,协方差矩阵的更新将是一个~10e2210e2210e22量级的计算操作,因此传统的卡尔 ...
对Kalman Filter的理解
1. 卡尔曼滤波理论回顾对于一个动态系统,我们首先定义一组状态空间方程状态方程: 测量方程: xk是状态向量,zk是测量向量,Ak是状态转移矩阵,uk是控制向量,Bk是控制矩 ...
Quaternion kinematics for the error-state Kalman filter论文阅读
文章目录 0. 参考文献 1. 四元数的定义和属性 1.1 四元数的定义 1.1.1 四元数的表示方式 1.2 四元数的性质 1.2.1 加和 1.2.2 积 1.2.3 单位1 1.2.4 共轭 1 ...
matlab温度数据怎么滤波_卡尔曼滤波算法思想理解 Kalman filter 第一篇
卡尔曼滤波算法思想理解 Kalman filter 第一篇最近在初步的理解目标跟踪的领域, 其中一个非常经典的算法卡尔曼滤波Kalman filter是需要有很好的理解才行, 由于已经脱离了学校,懂 ...
nasa电池数据集_文章分享—模型与滤波算法结合的锂电池剩余寿命预测方法
论文标题: Remaining useful life prediction of lithium-ion batteries with adaptive unscented kalman filte ...
多目标跟踪（MOT）中的卡尔曼滤波（Kalman filter）和匈牙利(Hungarian)算法详解
多目标跟踪(MOT)中的卡尔曼滤波(Kalman filter)和匈牙利(Hungarian)算法详解 1. 概览在开始具体讨论卡尔曼滤波和匈牙利算法之前,首先我们来看一下基于检测的目标跟踪算法的大 ...

滤波算法_标准卡尔曼滤波(SKF, Standard Kalman filter)_①基础铺垫

1、递归算法

2、数据融合

滤波算法_标准卡尔曼滤波(SKF, Standard Kalman filter)_①基础铺垫相关推荐

最新文章

热门文章