卡尔曼滤波(Kalman filter)是一种高效率的递归滤波器(自回归滤波器),它能够从一系列的不完全及包含噪声的测量中,估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值,考虑各时间下的联合分布,再产生对未知变数的估计,因此会比只以单一测量量为基础的估计方式要准。卡尔曼滤波得名自主要贡献者之一的鲁道夫·卡尔曼。

卡尔曼滤波器的数学基础

递归算法

卡尔曼滤波器(Kalman Filter),它是一种最优化的递归的数字处理算法(Optimal Recursive Data Processing Algorithm)。

为什么使用卡尔曼滤波?

因为显示生活中充满了不确定性,而这些不确定性主要体现在以下几个方面:

  • 不存在完美的数学模型
  • 系统的扰动不可控,也很难建模
  • 测量传感器存在误差

【例】测量硬币的例子

如图所示,不同的人去测量一个硬币的直径,一共测量$k$次,得到的测量值为$z_1,......z_k$,估计硬币的真实直径是多少?

image-20220401093317316

取平均值

设我们的平均值即对真实值的估计值(Estimate)为$\hat{x}$

$$ \begin{align*} \hat{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}{k-1}(z_1+z_2+...+z_{k-1})+\frac{1}{k}z_k\\ &=\frac{k-1}{k} \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{align*} $$

整理得:

$$ \hat{x}_k=\hat{x}_{k-1} +\frac{1}{k}(z_k-\hat{x}_{k-1} ) $$

令$K_k=1/k$

$$ \hat{x}_k=\hat{x}_{k-1} +K_k(z_k-\hat{x}_{k-1} ) $$

即为:当前的估计值=上一时刻的估计值+$K_k$(当前测量值-上一时刻的估计值)

分析:

  • 随着$k$的增加,$K_k$趋近于0,可以得到$\hat{x}_k=\hat{x}_{k-1}$,测量的结果$z_k$不再重要
  • 当$k$比较小的时候,$K_k$很大,则说明测量结果是非常重要的
  • $K_k$就是卡尔曼增益,或叫做卡尔曼因数。

求解卡尔曼增益$K_k$:

设估计误差:$e_{EST}$,测量误差:$e_{MEA}$

$$ K_k=\frac{ {e_{EST} }_{k-1} }{ {e_{EST} }_{k-1}+{e_{MEA} }_{k} } $$

分析:

  • $e_{EST}>>e_{MEA}:$$K_k$趋近于1,则$\hat{x}_k=\hat{x}_{k-1} +z_k-\hat{x}_{k-1} =z_k$,即估计误差大相信测量值
  • $e_{EST}<<e_{MEA}:$$K_k$趋近于0,则$\hat{x}_k=\hat{x}_{k-1} $,测量误差大相信估计值

【例】卡尔曼滤波的应用举例:

  • 一般计算卡尔曼滤波分为三步:

    • 计算卡尔曼增益:$K_k=\frac{ {e_{EST} }_{k-1} }{ {e_{EST} }_{k-1}+{e_{MEA} }_{k} }$
    • 计算$\hat{x}_k=\hat{x}_{k-1} +K_k(z_k-\hat{x}_{k-1} )$
    • 更新${ e_{EST} }_{k}=(1-K_k){e_{EST} }_{k-1}$

已知物体的实际长度为$50mm$,测量误差$e_{MEA}=3mm$,估计误差$e_{MEA}=5mm$,$\hat{x}_0=40mm$,一共测量了13次,根据上面的三个步骤,我们可以计算并画出折线图。

image-20220401102705574

image-20220401102555645

很明显,经过测量次数的增加,最终的估计值是接近真实值的,这就是卡尔曼滤波递归的作用。

数据融合

数据融合(Data Fusion)

【例】现有两个称,已知它们称的结果都符合正态分布,现去称同一个物体,已知称$A$称得物体重量为$z_1=30g$,并且已知它的标准差为$\sigma_1=2g$;称$B$称得物体重量为$z_2=32g$,并且已知它的标准差为$\sigma_2=4g$;估计该物体的真实重量$\hat{z}_k=?$。

根据递归的思想:

$$ \hat{z}_k=z_1+K_k(z_2-z_1),K_k\in[0,1] $$

为了求解最接近真实值的$\hat{z}_k$,即求解最优的$K_k$值,使得方差$Var(\hat{z}_k)$最小。

$$ \begin{align*} \sigma^2_{\hat{z}_k }&=Var(z_1+K_k(z_2-z_1))\\ &=Var(z_1+K_kz_2-K_kz_1)\\ &=Var((1-K_k)z_1+K_kz_2)\\ &=Var((1-K_k)z_1)+Var(K_kz_2)\\ &=(1-K_k)^2Var(z_1)+K_k^2Var(z_2)\\ &=(1-K_k)^2\sigma_1^2+K_k^2\sigma_2^2\\ \end{align*} $$

对$K_k$求导:

$$ \begin{align*} &\frac{d\sigma^2_{\hat{z}_k } }{dK_k}=0\\ -2(1-K_k)&\sigma_1^2+2K_k\sigma_2^2=0\\ &K_k=\frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2} \end{align*} $$

已知$\sigma_1=2,\sigma_2=4$:

$$ K_k=\frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2}=\frac{4}{4+16}=0.2 $$

带入:

$$ \begin{align*} \hat{z}_k&=z_1+K_k(z_2-z_1)\\ &=30+0.2(32-20)\\ &=30.4g \end{align*} $$

对应方差:

$$ \begin{align*} \sigma^2_{\hat{z}_k } &=(1-K_k)^2\sigma_1^2+K_k^2\sigma_2^2\\ &=(1-0.2)^2*4+0.2^2*16\\ &=3.2\\ \sigma_{\hat{z}_k }&=\sqrt{3.2} \\ &= 1.79 \end{align*} $$

因此,我们根据两个称的特性,根据测量值对物体的真实重量进行了预测,预测值为$30.4g$,而且它是最优解。

这个过程就是数据融合。

公式:

对于两个高斯分布的融合,如$p(A)\sim N(\mu_a,\sigma^2_a)$,$p(B)\sim N(\mu_b,\sigma^2_b)$

融合之后的高斯分布$p(C)\sim N(\mu_c,\sigma^2_c)$:

$$ \begin{align*} \mu_c&=\frac{\sigma^2_b\mu_a+\sigma^2_a\mu_b}{\sigma^2_a+\sigma^2_b}\\ \sigma_c&=\frac{\sigma^2_a\sigma^2_b}{\sigma^2_b+\sigma^2_b} \end{align*} $$

WeChat Image_20220401141112


协方差矩阵

协方差矩阵(Covariance Matrix),它可以把方差和协方差在一个矩阵中表现出来,体现了变量之间的;联动关系。

【例】

球员身高x体重y年龄z
瓦尔迪1797433
奥巴梅杨1878031
萨拉赫1757128
平均值180.37530.7

分别求三者的方差以及相互之间的协方差,

$$ Var(x)=\sigma_x^2=\frac{1}{n}\sum^n_{i=1}(x_i-\mu)^2=\frac{1}{n}(\sum^n_{i=1}x_i^2-n\mu^2) $$

方差:$\sigma_x^2=24.89, \sigma_y^2=14,\sigma_z^2=4.22$,

$$ Cov(X,Y)=E((X-\mu)(Y-v))=E(X·Y)-\mu v $$

协方差:$\sigma_x\sigma_y=\sigma_y\sigma_x=18.7,\sigma_x\sigma_z=\sigma_z\sigma_x=4.4,\sigma_y\sigma_z=\sigma_z\sigma_y=3.3$。所以协方差矩阵为:

$$ 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} $$

可以看出运动员的身高与体重相关性比较大,而身高体重和年龄的相关性是比较小的,这种规律,数据越多越明显。

如何使用矩阵批量计算协方差矩阵?

一般需要设置一个过渡矩阵:

$$ 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} $$

协方差:

$$ P=\frac{1}{3}a^Ta $$

状态空间表达式

【例】:有一个弹簧阻尼系统,对质量为$m$的物块施加一个力$F$,弹簧向右的位移量为$x$,已知弹簧的胡克系数为$k$,阻尼系数为$B$

image-20220401152419675

定义输入为$u_{(t)}=F_{(t)}$,输出为$x$

由胡克定律可知,弹簧拉力$f_k$以及阻力$f_B$:

$$ f_k=kx\\ f_B=B\dot{x} $$

根据牛顿第二定律$F=ma$:

$$ \begin{align*} &m\ddot{x}=F-f_k-f_B\\ &m\ddot{x}+f_B+f_k=F\\ &m\ddot{x}+B\dot{x}+kx=F \end{align*} $$

所以系统的动态方程表达式:

$$ m\ddot{x}+B\dot{x}+kx=F $$

使用经典控制理论可以对上式进行拉普拉斯变换,进而求得系统的传递函数:

$$ ms^2X_{(s)}+BsX_{(s)}+kX_{(s)}=F_{(s)}\\ G_{(s)}=\frac{X_{(s)} }{F_{(s)} }=\frac{1}{ms^2+Bs+K} $$

但在现代控制理论中则更多的使用状态空间方程,状态空间方程可以看成一个集合,它包含系统的输入、系统的输出以及状态变量,最终使用一个一阶微分方程的形式表达出来。

即:

$$ m\ddot{x}+B\dot{x}+kx=u $$

确定两个合适的状态变量,把二阶项消除:

  • 位置:$z_1=x=x_1$
  • 速度:$z_2=\dot{x}=x_2$

由于$x_1=x,x_2=\dot{x}$,因此可以得到:$\dot{x_1}=x_2,\dot{x_2}=\ddot{x}$,利用上述方程化简:

$$ \begin{align*} \dot{x_1}=x_2\\ \dot{x_2}=\ddot{x}&=\frac{1}{m}\mu-\frac{B}{m}\dot{x}-\frac{k}{m}x\\ &=\frac{1}{m}\mu-\frac{B}{m}x_2-\frac{k}{m}x_1 \end{align*} $$

综合上面的四个方程:

$$ \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} $$

进一步归纳:

连续型:

$$ \dot{X}_{(t)}=AX_{(t)}+Bu_{(t)}\\ Z_{(t)}=HX_{(t)} $$

离散型:

$$ {X_k }=AX_{k-1}+Bu_{k}\\ Z_{k}=HX_{k} $$

其中$k$表示采样时间单位。

由于现实中存在着不确定性,比如存在过程噪声(Process Noise):$w_{k}$,测量噪声(Measurement Noise):$v_k$

$$ \begin{align*} {X_k }&=AX_{k-1}+Bu_{k}+w_{k}\\ Z_{k}&=HX_{k}+v_k \end{align*} $$

因此,我们的模型和测量都存在误差,导致计算值以及测量值都是不准确的,如何通过两个不确定的值去估计一个准确的值$\hat{x}_k=?$,根据数据融合的例子,我们可以使用这两个不准确的结果进行数据融合,从而得到一个比较准确的估计值。

线性化

泰勒级数展开式:对于函数$f(x)$在任意一点$x_0$处展开

$$ 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+...+\frac{f^{(n)}(x_0)}{1!}(x-x_0)^{n} $$

一般当$x-x_0\rightarrow0$时,则$(x-x_0)^{n}\rightarrow0,n\geq 2$,只取一阶泰勒:

$$ f(x)=f(x_0)+f^\prime(x_0)(x-x_0) $$

设$f(x_0)=k_1,f^\prime(x_0)=k_2$:

$$ \begin{align*} f(x)&=k_1+k_2(x-x_0)\\ &=k_2x+(k_1-k_2x_0)\\ &=k_2x+b \end{align*} $$

因此这样的一个非线性的函数$f(x)$在点$x_0$附近就被线性化为一个线性的函数。

  • 线性化是对某个点附近进行线性化,而不是全局进行线性化
  • 线性化的前提条件,一定是$(x-x_0)\rightarrow0$

如果对于二维以上的,我们的一阶泰勒展开如下:

$$ f(x)=f(x_0)+\frac{\partial f}{\partial x_0}(x-x_0) $$

🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/16.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 05 月 31 日
赏杯咖啡