Welcome to CDJ's Page

自律更自由

卡尔曼滤波

发布于 2020-08-31 | 标签: DSP | 25分钟 | 5109字数

Author🚹:CofCai
personal page🏠
CSDN page🏠
Email📧:cai.dongjun@nexuslink.cn

References

How a Kalman filter works, in pictures | Bzarg
详解卡尔曼滤波原理_清风莞尔的博客-CSDN博客
参考链接3：当然还有DR_CAN的视频啦
注：链接1是一位国外大神写的，所以是英文，链接2是其翻译版。

卡尔曼滤波的作用

你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。

世界中充满着不确定性

系统的运行过程中有过程噪音，仪器测量有测量误差。[就连喜欢一个人也都有不确定性，时而喜欢、时而无感觉，~嗯、渣男]。既然现实生活中充满着不确定性，那么有没有一种方法可以获得相对准确的结果呢？这就是卡尔曼滤波的作用。

在工程中

我们常常根据系统的上一状态来计算出系统的当前状态，但是由于外界的影响，计算出来的结果和系统真实的结果是有差异的；我们又同时通过系统中的传感器获得系统的当前状态，但是由于传感器本身的误差，我们从传感器获得的结果和系统真实值也是有差异的。

即我们得到了系统两个具有误差的结果:

  • 一个是根据系统的状态空间方程计算而得的计算值，真实值是在计算值的基础上会被加入过程噪声（比如小车在行驶过程中被风吹歪了，那么真实值就不是计算值了。）；
  • 一个是根据传感器测量而得的、具有测量误差的测量值。
    我们的目标是把两个具有误差的结果融合为一个误差不太大的、可信的结果。

整体感受

状态空间方程

系统的状态方程：

Xk−=AXk−1+Buk−1X_k^- = AX_{k-1} + Bu_{k-1}\\

但系统会受到**环境噪声wk−1w_{k-1}**的影响，系统真实的状态并不是数学表达式所表示的结果，即系统真实的状态XkX_k应该加上环境噪声，为：

Xk=Xk−+wk−1=AXk−1+Buk−1+wk−1 X_k = X_k^- + w_{k-1}\\ = AX_{k-1} + Bu_{k-1} + w_{k-1}\\

系统的计算值（先验估计）：Xk−=AXk−1+Buk−1X_k^- = AX_{k-1} + Bu_{k-1}。

传感器测量方程：

Zk=HXk{Z_k} = HX_k

系统的测量值：XkMeasure=H−ZkX_k^{Measure}=H^-Z_k，记住：这个测量值包含了测量误差。因为传感器本身有误差，即传感器读数值ZkZ_k包括了误差，即真实的传感器测量方程是：

Zk=HXk+vk Z_k = HX_k + {\color{green}v_k}

综上可得系统真实的状态方程为：

Xk=Xk−+wk−1=AXk−1+Buk−1+wk−1(6−1)Zk=HXk+vk(6−2) X_k = {\color{blue}X_k^-} + w_{k-1} = {\color{blue}AX_{k-1} + Bu_{k-1}} + w_{k-1}{\quad}(6-1)\\ Z_k = HX_k + v_k{\quad}(6-2)

式中：

  • XkX_k是当前系统真实的状态；
  • Xk−X_k^-是根据状态方程算出来当前系统本应该处于的状态（计算值）；
  • uk−1u_{k-1}是外部控制输入；
  • ZkZ_k是当前传感器的读数值（包含了测量误差vkv_k）；
  • wk−1w_{k-1}和vkv_k一个是过程噪声，一个是测量噪声，它们在卡尔曼滤波中都被认为服从高斯分布；
  • HH是一个系数，比如有的传感器测出的值是0~1024之间的值，还需要转换才能得到对应的物理量。

结合例子理解公式

一辆小车上装有位置传感器，并且向前匀速的运动。

公式6-1说明

系统的状态是可以用数学表达式表达出来的（或者这样说，系统的运行状态满足某些数学公式），也就意味着系统的当前状态可以根据上一状态计算而出，即通过系统的状态方程，可以计算出系统当前的状态Xk−X_k^-。但是由于具有过程噪声wk−1w_{k-1}，所以这个结果不是系统当前的真实状态。

比如：上一时刻小车的位置是5，速度是2，那么通过计算可以知道下一时刻小车的位置是7。但是由于环境风的影响，导致小车真实的位置其实是6.8，所以小车的真实值XkX_k=计算值Xk−X_k^-+过程噪声wk−1w_{k-1}（此处噪声是-0.2，计算值是7，真实值就是6.8了）。在卡尔曼滤波中认为过程噪声ww是服从(0,cov(w))(0,cov(w))的正态分布。

公式6-2说明

通过小车上的传感器，可以得到小车当前的测量位置，但是由于传感器具有测量误差，所以这个传感器传回的值包含了测量噪声，即真实值XkX_k+测量噪声vkv_k=传感器测量的结果（读数值）ZkZ_k。

比如：续上，小车当前的真实位置是6.8（传感器测量的就是小车的真实位置，只不过传感器自己有误差），由于测量误差，传感器测量得到的值是6.7（测量值才是我们可以得到的，是读数值），即6.7=6.8−0.16.7=6.8-0.1，6.7是我们从传感器读数得知，0.1是测量误差（服从正态分布），6.8是真实值。

参数H的意义

这里首先要明白一点:传感器的值不一定就等于系统的状态，这里不是说误差的影响，而是传感器特性的影响。比如传感器的1代表了系统的100，这是生产传感器时就规定好的。所以由传感器上的值得到系统的状态还需要乘以100（就是H这个系数），如果传感器的值就代表了系统的状态，那么H=1，比如位置传感器直接给出小车位置，而速度传感器还需要转换才能得出位置。

总结

在公式7中，我们可以得到的是：

  • 系统的计算值（计算值）Xk−X_k^-；
  • 系统的观测值（读数值）ZkZ_k。
  • 又基于实际生活和大量实验，我们可以大致得到过程噪声和测量噪声的分布情况（卡尔曼滤波认为是正态分布）。

最终我们就可以根据两个和正态误差相关的结果(Xk−X_k^-和ZkZ_k)融合为一个误差较小的结果Xk+X_k^+，这个融合值是最优估计。

怎么融合？

从简单的例子入手-测量一枚硬币的直径

由于尺子误差和认为读数误差，结果是不准确的，我们自然会测量多次以平均值作为其真实值。

xk~=1k(z1+z2+...+zk)=1k(z1+z2+...+zk−1)+1kzk=1kk−1k−1(z1+z2+...+zk−1)+1kzk=k−1kxk−1~+1kzk=xk−1~−1kxk−1~+1kzk=xk−1~+1k(zk−xk−1~) \widetilde{x_k} = \frac{1}{k}(z_1+z_2+...+z_k)\\ =\frac{1}{k}(z_1+z_2+...+z_{k-1})+\frac{1}{k}z_k\\ =\frac{1}{k}\frac{k-1}{\color{blue}{k-1}}{\color{blue}(z_1+z_2+...+z_{k-1})}+\frac{1}{k}z_k\\ =\frac{k-1}{k}\widetilde{x_{k-1}}+\frac{1}{k}z_k\\ =\widetilde{x_{k-1}}-\frac{1}{k}\widetilde{x_{k-1}}+\frac{1}{k}z_k\\ ={\color{red}\widetilde{x_{k-1}}+\frac{1}{k}(z_k-\widetilde{x_{k-1}})}

zkz_k表示第k次的测量值，xk~\widetilde{x_k}表示第k次的估计值（即前k次的平均值）。通过一个简单的变换，我们得到了一个公式，即：当前估计值=上一次估计值+比例(当前测量值-上一次估计值)。这里的比例*其实就是融合比例（卡尔曼增益），谁多一点、谁少一点。

比如：

  • 当k=1k=1时，此时估计值就等于测量值xk~=zk\widetilde{x_k}=z_k，因为目前就测量了一次，没有更多数据做平均嘛；
  • 当k→∞k{\to}\infin时，此时估计值就等于上一次的估计值xk~=xk−1~\widetilde{x_k}=\widetilde{x_{k-1}}，因为我有足够多的历史数据了，这一次的测量数据对我的影响很小。

融合实例

由两把不同的天平（误差都服从正态分布）得出不同的重量，天平1误差的标准差是2g，天平2误差的标准差是4g。现在天平1的结果是30g，天平2的结果是32g，怎么把两个结果融合为一个最优的估计值。从下面的正态分布图可以知道，最终的结果应该在30~32g之间，并且靠近30g更多一点。

z1=30gσ1=2gz2=32gσ2=4gz~=z1+k(z2−z1)z_1=30g {\quad} \sigma_1=2g \\ z_2=32g {\quad} \sigma_2=4g \\ \widetilde{z}=z_1 + {\color{red}k}(z_2 - z_1)

融合目标：找到k，使得var(z~)var(\widetilde{z})最小，融合后误差最小就是使融合后方差最小（方差越小越接近真实值）。

σz~2=var(z1+k(z2−z1))=var(z1−kz1+kz2)=var((1−k)z1+kz2)=var((1−k)z1)+var(kz2)=(1−k)2var(z1)+k2var(z2)=(1−k)2σ12+k2σ22 \sigma_{\widetilde{z}}^2=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 \\

要使σz~2\sigma_{\widetilde{z}^2}最小，求导令其为0，求其最小值。

dσz~2dk=0−2(1−k)σ12+2kσ22=0−σ12+kσ12+kσ22=0k(σ12+σ22)=σ22⟹k=σ12σ12+σ22\frac{d\sigma_{\widetilde{z}}^2}{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_2^2 \\ {\Longrightarrow}{\color{red} k = \frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2}}

所以：

k=44+16=0.2σz~2=(1−0.2)2×22+0.22×42=3.2σz~=3.2=1.79gk=\frac{4}{4+16}=0.2 \\ \sigma_{\widetilde{z}}^2=(1-0.2)^2\times2^2+0.2^2\times4^2=3.2 \\ \sigma_{\widetilde{z}}=\sqrt{3.2}=1.79g

卡尔曼公式详细推导

可以不用看，推导较复杂！！！

协方差矩阵

E[w wT]E[w\ w^T]就代表了ww的协方差矩阵，如下：

过程噪声协方差矩阵为QQ，服从wk−(0,Q)w_k-(0,Q)，没有下标kk的原因是一般认为高斯噪声分布不变化。

E[wkwkT]=QE[w_k w_k^T] = Q

测量噪声协方差矩阵为RR，服从vk−(0,R)v_k-(0,R)

E[vkvkT]=RE[v_k v_k^T] = R

误差协方差矩阵为PkP_k，服从ek−(0,Pk)e_k-(0,P_k)

E[ekekT]=PkE[e_k e_k^T] = P_k

卡尔曼增益的推导

状态空间方程：

Xk=Xk−+wk−1=AXk−1+Buk−1+wk−1(29−1)Zk=HXk+vk(29−2) X_k = {\color{blue}X_k^-} + w_{k-1} = {\color{blue}AX_{k-1} + Bu_{k-1}} + w_{k-1}{\quad}(29-1)\\ Z_k = HX_k + v_k{\quad}(29-2)

其中：

  • Xk−=AXk−1+Buk−1X_k^-=AX_{k-1} + Bu_{k-1}被称为先验估计（计算值），和真实值还是有误差（被风吹歪了）；
  • XkMeasure=H−ZkX_k^{Measure}=H^-Z_k是测量值，包含了测量噪声。

估计值/后验估计Xk+X_k^+：

融合值=计算值+G(测量值−计算值)Xk+=Xk−+G(XkMeasure−Xk−)=Xk−+G(H−Zk−Xk−){Xk+=XK−=计算值,G=0XK+=H−Zk=测量值,G=1令G=KkHXk+=Xk−+Kk(Zk−HXk−){Xk+=XK−=计算值,K=0XK+=H−Zk=测量值,K=H−{\color{red}融合值=计算值+G(测量值-计算值)} \\ X_k^+=X_k^-+G({\color{blue}X_k^{Measure}}-X_k^-) \\ =X_k^-+G({\color{blue}H^-Z_k}-X_k^-) \\ \begin{cases} X_k^+ = X_K^-=计算值 {\quad},G=0 \\ X_K^+ = H^-Z_k=测量值 {\quad},G=1 \\ \end{cases} \\ {\color{red}令{\quad} G = K_kH} \\ X_k^+=X_k^-+K_k(Z_k-HX_k^-) \\ \begin{cases} X_k^+ = X_K^-=计算值 {\quad},K=0 \\ X_K^+ = H^-Z_k=测量值 {\quad},K=H^- \\ \end{cases}

  • 定义误差（真实值-估计值）ek=Xk−Xk+e_k=X_k-X_k^+，寻找KkK_k使得Xk+→XkX_k^+{\to}X_k，即ek→0e_k{\to}0；
  • 误差也服从高斯分布，即P(ek)−(0,Pk)P(e_k)-(0,P_k)，PkP_k为误差eke_k的协方差矩阵，即使得PkP_k的迹最小；
  • 定义先验误差（真实值-先验估计）ek−=Xk−Xk−e_k^-=X_k-X_k^-；
  • PkP_k代表第k次的误差协方差矩阵，Pk−P_k^-代表第k次的先验误差协方差矩阵。

详细推导

配合食用：

(1−1)→(1−2)(1-1){\to}(1-2)：

Xk−Xk+=Xk−[Xk−+Kk(Zk−HXk−)]=Xk−Xk−−KkZk+KkHXk−(∵Zk=HXk+vk)=Xk−Xk−−KkHXk−Kkvk+KkHXk−=(Xk−Xk−)−KkH(Xk−Xk−)−Kkvk=(I−KkH)(Xk−Xk−)−Kkvk=(I−KkH)ek−−Kkvk{\color{blue}X_k-X_k^+} = X_k-[X_k^-+K_k(Z_k-HX_k^-)] \\ =X_k-X_k^- - K_k{\color{red}Z_k} + K_kHX_k^- \\ {\qquad}({\because}{\qquad}{\color{red}Z_k} = HX_k+v_k) \\ =X_k-X_k^- - K_kHX_k - K_kv_k + K_kHX_k^- \\ =(X_k - X_k^-) - K_kH(X_k - X_k^-) - K_kv_k \\ =(I - K_kH)(X_k - X_k^-) - K_kv_k \\ =(I - K_kH)e_k^- - K_kv_k

(1−2)→(1−3)(1-2){\to}(1-3)：

(AB)T=BTAT(A+B)T=AT+BT(AB)^T = B^TA^T \\ (A+B)^T = A^T + B^T

(1−4)→(1−5)(1-4){\to}(1-5)：

∵E[(I−KkH)ek−vkTKkT]=(I−KkH)KkTE[ek−vkT]ek−,vkT互不相关=(I−KkH)KkTE[ek−]E[vkT]ek−,vkT均服从(0,σ2)=0{\because}{\quad}E{\color{green}[(I - K_kH)e_k^-v_k^TK_k^T]} \\ {\color{green}=(I - K_kH)K_k^TE[e_k^-v_k^T]} \\ e_k^-,v_k^T互不相关 \\ {\color{green}=(I - K_kH)K_k^TE[e_k^-]E[v_k^T]}\\ e_k^-,v_k^T均服从(0,\sigma^2) \\ =0

(1−6)→(1−7)(1-6){\to}(1-7)：测量噪声协方差矩阵

E[vkvkT]=RE[v_kv_k^T] = R

(1−8)→(1−9)(1-8){\to}(1-9)：

[(Pk−HT)KkT]T=Kk(PK−HT)T=KkHPk−Ttr(KkHPk−)=tr(KkHPk−T)[(P_k^-H^T)K_k^T]^T = K_k(P_K^-H^T)^T = K_kHP_k^{-T} \\ tr(K_kHP_k^-) = tr(K_kHP_k^{-T})

(1−9)→(1−10)(1-9){\to}(1-10)：

Pk−跟Kk无关d(AB)dA=BTd(ABAT)dA=2ABP_k^-跟K_k无关 \\ \frac{d(AB)}{dA} = B^T \\ \frac{d(ABA^T)}{dA} = 2AB

卡尔曼滤波中最核心的公式：卡尔曼增益

Kk=Pk−HTHPK−HT+RK_k = \frac{P_k^-H^T}{HP_K^-H^T+R}

  • R→∞R{\to}\infin，Kk→0K_k{\to}0，RR是测量噪声协方差矩阵，测量噪声很大，更愿意相信计算的结果；
  • R→0R{\to}0，Kk→H−K_k{\to}H^-，测量噪声小，更愿意相信测量的结果。

误差协方差矩阵Pk−P_k^-

先验估计：

Xk−=AXk−1+Buk−1X_k^- = AX_{k-1} + Bu_{k-1}

后验估计：

Xk+=Xk−+Kk(Zk−HXk−)X_k^+=X_k^-+K_k(Z_k-HX_k^-) \\

卡尔曼增益：

Kk=Pk−HTHPK−HT+RK_k = \frac{P_k^-H^T}{HP_K^-H^T+R}

误差协方差矩阵的推导：

Pk−=E[ek−ek−T]=E[(Aek−1+wk−1)(Aek−1+wk−1)T]=...=APk−1AT+QP_k^- = E[{\color{blue}e_k^-} e_k^{-T}] \\ =E[(Ae_{k-1}+w_{k-1})(Ae_{k-1}+w_{k-1})^T] \\ = ... \\ =AP_{k-1}A^T + Q

配合食用（这里的Xk−=AXk−1−−Buk−1X_k^-=AX_{k-1}^--Bu_{k-1}），和前面的先验估计不一样，不好理解：

ek−=Xk−Xk−=AXk−1+Buk−1+wk−1−AXk−1−−Buk−1=Aek−1−+wk−1{\color{blue}e_k^-} = X_k - X_k^- \\ =AX_{k-1} + Bu_{k-1} + w_{k-1} - AX_{k-1}^- - Bu_{k-1} \\ =Ae_{k-1}^- + w_{k-1}

卡尔曼滤波的5个公式

重要变量：先验估计（即计算值），测量值，先验误差，先验误差协方差矩阵，误差，误差协方差矩阵，卡尔曼增益，最优估计。

Xk−=AXk−1+Buk−1Pk−=APk−1AT+QKk=Pk−HTHPK−HT+RXk+=Xk−+Kk(Zk−HXk−)Pk=(I−KkH)Pk− X_k^- = AX_{k-1} + Bu_{k-1} \\ P_k^- = AP_{k-1}A^T + Q \\ K_k = \frac{P_k^-H^T}{HP_K^-H^T+R} \\ X_k^+ = X_k^- + K_k(Z_k - HX_k^-) \\ P_k = (I - K_kH)P_k^-

前两步是预测，后两步是校正，最后一步是更新。

扩展的卡尔曼滤波

参考1

参考2

前面是是标准的卡尔曼滤波，即线性卡尔曼滤波，不能作用于非线性系统，而扩展的卡尔曼滤波可用于非线性系统。

线性化

利用泰勒级数对f(x)f(x)在x0x_0处展开得：

f(x)=f(x0)+f′(x0)1!(x−x0)+f′′(x0)2!(x−x0)2+⋯+fn(x0)n!(x−x0)nf(x) = f(x_0) + \frac{f'(x_0)}{1!}(x-x_0) + \frac{f''(x_0)}{2!}(x-x_0)^2 + \cdots + \frac{f^n(x_0)}{n!}(x-x_0)^n

如果x→x0x{\to}x_0，则(x−x0)2→0,⋯ ,(x−x0)n→0(x-x_0)^2{\to}0,\cdots,(x-x_0)^n{\to}0：

f(x)=f(x0)+f′(x0)(x−x0)=k1+k2(x−x0)=kx+bf(x) = f(x_0) + f'(x_0)(x-x_0) \\ = k_1 + k_2(x-x_0) \\ = kx+b

如上，我们把f(x)f(x)线性化成了kx+bkx+b。

例：

f(x)=sin⁡(x)=sin⁡(x0)+cos⁡(x0)(x−x0)(letx0=0)=0+x−x0=xf(x) = \sin(x) \\ = \sin(x_0) + \cos(x_0)(x-x_0) \\ {\qquad}(let{\quad}x_0=0) \\ = 0 + x-x_0 \\ = x

上式表明，sin(x)sin(x)在x=0x=0附近和xx是相等的，分析误差：

{sin⁡(π6)=1/2,π6=0.52,err=0.52−0.50.5=4%sin⁡(π4)=0.707,π4=0.785,err=0.785−0.7070.707=11%\begin{cases} \sin(\frac{\pi}{6}) = 1/2,{\quad}\frac{\pi}{6}=0.52,{\quad}err=\frac{0.52-0.5}{0.5}=4\% \\ \sin(\frac{\pi}{4}) = 0.707,{\quad}\frac{\pi}{4}=0.785,{\quad}err=\frac{0.785-0.707}{0.707}=11\% \\ \end{cases}

线性化解决EKF

EKF的基本思想是利用泰勒级数展开将非线性系统线性化，然后采用卡尔曼滤波框架对信号进行滤波，因此它是一种次优滤波。

标准(线性)卡尔曼滤波KF的状态转移方程和观测方程为：

{θk=Aθk−1+Buk−1+skzk=Cθk+vk\begin{cases} \theta_k=A\theta_{k-1}+Bu_{k-1}+s_k\\ z_k=C\theta_k+v_k \end{cases}

扩展(非线性)卡尔曼滤波EKF的状态转移方程和观测方程为：

{θk=f(θk−1)+skzk=h(θk)+vk\begin{cases} \theta_k=f(\theta_{k-1})+s_k\\ z_k=h(\theta_k)+v_k \end{cases}

利用泰勒展开式对式xx在上一次的估计值<θk−1><\theta_{k-1}>处展开得：

θk=f(θk−1)+sk=f(<θk−1>)+Fk−1(θk−1−<θk−1>)+sk\theta_k = f(\theta_{k-1})+s_k \\ = f(<\theta_{k-1}>) + F_{k-1}(\theta_{k-1}-<\theta_{k-1}>) + s_k \\

再利用泰勒展开式对式xx在本轮得状态预测值θk′\theta_k'处展开得：

zk=h(θk)+vk=h(θk′)+Hk(θk−θk′)+vkz_k = h(\theta_k) + v_k = h(\theta_k') + H_k(\theta_k - \theta_k') + v_k

其中，Fk−1F_{k-1}和HkH_k分别表示函数f(θ)f(\theta)和h(θ)h(\theta)在<θk−1<\theta_{k-1}和θk′\theta_k'处的雅可比矩阵。

注：这里对泰勒展开式只保留到一阶导，二阶导数以上的都舍去，噪声假设均为加性高斯噪声。

赞赏
# DSP
嵌入式Linux驱动开发相关API 上一篇
嵌入式Linux驱动开发框架-韦东山 下一篇
已运行:
浏览数: 次 | 访客数: 人
0%
召唤看板娘