手撕自动驾驶算法——贝叶斯角度推导卡尔曼滤波

用户1127074244967
169 阅读4分钟

本文已参与「新人创作礼」活动，一起开启掘金创作之路。

运动、观测概念

什么是运动：考虑从k-1时刻到k时刻，位置x如何变化 什么是观测：在k时刻xkx_k处探测一个路标yjy_j

运动方程：

xk=f(xk−1,uk,wk)x_k=f(x_{k-1},u_k,w_k)

uku_k表示传感器的读数，wkw_k表示噪声

观测方程：在xkx_k处看到某个路标点yjy_j，产生一个观测数据zk,jz_{k,j}

zk,j=h(yj,xk,vk,j)z_{k,j}=h(y_j,x_k,v_{k,j})

vk,jv_{k,j}是观测的噪声

多元高斯分布

高斯分布有两种表达方式：

  • 协方差矩阵+均值
  • 信息矩阵+信息矢量

协方差矩阵+均值的方式比较常见，如下

∫−∞∞(x−μ)(x−μ)T1(2π)Ndet∑⋅exp(−12(x−μ)T∑−1(x−μ))\int_{-\infty }^{\infty }(x-\mu)(x-\mu)^T\frac{1}{\sqrt{}(2\pi)^Ndet\sum}\cdot exp\left ( -\frac{1}{2}(x-\mu)^T\sum{}{}^{-1}(x-\mu) \right )

左边常数项记为ηη，p(x)p(x)可以记为：

其中对称正定矩阵ΣΣ为随机变量x的协方差矩阵，μ为x的均值，简记为

信息矩阵+信息矢量的形式可以由上式推导而来

=exp{12xT∑−1x+xT∑−1μ}= exp \left\{ \frac{1}{2}x^T\sum{}{}^{-1}x +x^T\sum{}{}^{-1}\mu \right\}

现在定义信息矩阵Ω=Σ−1Ω=Σ^{−1} ，信息矢量ξ=Σ−1μ=Ωμξ=Σ^{−1}μ=Ωμ，则

卡尔曼滤波推导过程

卡尔曼滤波中的条件独立：

  • 当给定某个时刻的状态变量xnx_n，该时刻的观测变量znz_n与之前的任意观测变量条件独立
p(zk∣xk,zk−1,...,z1)=p(zk∣xk)(1){\color{Red} p(z_k|x_k,z_{k-1},...,z_1)=p(z_k|x_k) } \tag{1}

根据卡尔曼滤波模型的假设，当我们知道了状态变量的取值时，那么对应的观测变量的分布就确定了，它与更早之前的观测无关（事实上与更早之前的任意状态变量也无关），状态变量的取值就是所需的全部信息。

  • 当给定某一时刻的状态变量xk−1x_{k-1}，下一时刻的状态变量xkx_k与之前的任意观测变量条件独立
p(xk∣xk−1,zk−1,...,z1)=p(xk∣xk−1)(2){\color{Red} p(x_k|x_{k-1},z_{k-1},...,z_1) = p(x_k|x_{k-1})} \tag{2}

根据卡尔曼滤波模型的假设，当我们知道了上一时刻状态变量t的取值时，那么下一时刻状态变量的分布就确定了，它与更早之前的观测无关（事实上与更早之前的任意状态变量也无关），前一时刻状态变量的取值就是所需的全部信息。

在推到卡尔曼滤波过程中，都是为了“凑出”运动变量、观测变量的条件概率p(xk∣xk−1)p(x_k|x_{k-1})和p(zk∣xk)p(z_k|x_k)，基于以上两点，应用贝叶斯定理：

p(xk∣zk,zk−1,...,z1)=p(zk∣xk,zk−1,...,z1)p(xk∣zk−1,...,z1)p(zk∣zk−1,...,z1)=p(zk∣xk)p(xk∣zk−1,...,z1)p(zk∣zk−1,...,z1)(3){\color{Red} \begin{aligned} p(x_k|z_k,z_{k-1},...,z_1)&= \frac{p(z_k|x_k,z_{k-1},...,z_1)p(x_k|z_{k-1},...,z_1)}{p(z_k|z_{k-1},...,z_1)} \\ &= \frac{p(z_k|x_k)p(x_k|z_{k-1},...,z_1)}{p(z_k|z_{k-1},...,z_1)} \tag{3} \end{aligned}}

上式用到式（1）的条件独立性，已经凑出观测变量的条件概率：p(zk∣xk)p(z_k|x_k)，为了推到出目标表达式，分子的第二项着手，引入xk−1x_{k-1}，对其积分得：

p(xk∣zk−1,...,z1)=∫xk−1p(xk∣xk−1,zk−1,...,x1)p(xk−1∣zk−1,...,z1)dxk−1=∫xk−1p(xk∣xk−1)α^(xk−1)dxk−1(4){\color{Red} \begin{aligned} p(x_k|z_{k-1},...,z_1) &= \int_{x_{k-1}}^{}p(x_k|x_{k-1},z_{k-1},...,x_1)p(x_{k-1}|z_{k-1},...,z_1) dx_{k-1}\\ & = \int_{x_{k-1}}^{}p(x_k|x_{k-1})\hat {\alpha }(x_{k-1})dx_{k-1} \tag{4} \end{aligned}}

上式用到式（2）的条件独立性，又凑出p(xk∣xk−1)p(x_k|x_{k-1})，将式（4）带入式（3）得：

α^(xk)=p(zk∣xk)∫xk−1p(xk∣xk−1)α^(xk−1)dxk−1p(zk∣zk−1,...,z1)(5){\color{Red} \begin{aligned} \hat {\alpha }(x_{k})=\frac{p(z_k|x_k)\int_{x_{k-1}}^{}p(x_k|x_{k-1})\hat {\alpha }(x_{k-1})dx_{k-1}}{p(z_k|z_{k-1},...,z_1)} \tag{5} \end{aligned}}

因为在卡尔曼滤波模型中，观测模型和状态转移概率模型都是线型高斯分布，因此基于此推导出的所有概率分布也是高斯分布。高斯分布由均值向量和协方差矩阵表示，接下来就是要把（5）式关于概率分布的递推式转换为关于均值向量和协方差矩阵的递推式，这样才能用计算机实现。 为此，需要引入另一个预备知识：

高斯分布的贝叶斯定理

已知一个多维高斯分布： p(x)∼N(x∣μ,Λ−1)p(x) \sim N(x| \mu, \Lambda ^{-1}) 另一个多维随机变量在已知的条件下也服从高斯分布，且其均值为线性变换，即

p(x)∼N(y∣Ax+b,L−1)p(x) \sim N(y| Ax+b,L ^{-1})

其中，AA和bb是线性变换参数，LL和Λ\Lambda是信息矩阵（协方差的逆矩阵） p(y∣x)p(y|x)定义了一个线性高斯模型，则p(y)p(y)和p(x∣y)p(x|y)也服从高斯分布，均值和方差分别为：

cov(y)=L−1+AΛ−1AT(6)cov(y)=L^{-1}+A \Lambda ^{-1}A^T \tag{6}
cov(x∣y)=(Λ+ATLA)−1(7)cov(x|y)= (\Lambda +A^TLA)^{-1} \tag{7}

结论：已知一个随机变量的高斯分布及其条件高斯分布，求另一个随机变量的高斯分布及其条件高斯分布。

记α^\hat {\alpha }的均值和方差为μk\mu_k和VkV_k，有：

转化为问题：已知一个随机变量的高斯分布及其条件高斯分布，求另一个随机变量的高斯分布

其中：

贝叶斯公式：

p(xk∣zk,zk−1,...,z1)=α^(xk)=p(zk∣xk).N(xk∣Aμk−1,Pk−1)p(zk∣zk−1,...,z1)(12){\color{Red} \begin{aligned} p(x_k|z_k,z_{k-1},...,z_1) &=\hat {\alpha }(x_{k})=\frac{p(z_k|x_k).N(x_k|A\mu_{k-1},P_{k-1})}{p(z_k|z_{k-1},...,z_1)} \tag{12} \end{aligned}}

对比贝叶斯公式（11）和式（12），（9）~（10）式的结果相当于我们已经求出了其中的p(xk)p(x_k)。通过观察，这又是“已知一个随机变量的高斯分布及其条件高斯分布，求另一个随机变量的高斯分布”的问题，只不过此时变量的对应关系变为：

y……zkμ……Aμk−1Λ−1……Pk−1A……Cb……0L−1……∑y……z_k\\ \mu……A\mu_{k-1}\\ \Lambda^{-1}……P_{k-1}\\ A……C\\ b……0\\ L^{-1}……\sum

根据（7）式可以直接写出：

Vk−1=(Pk−1−1+CT∑−1C)−1(13)V_{k-1}=(P_{k-1}^{-1}+C^T\sum{}{}^{-1}C)^{-1} \tag{13}

上式为卡尔曼滤波公式的原始形式，对该式继续变形：

Woodbury等式

(A+BD−1C)−1=A−1−A−1B(D+CA−1B)−1CA−1(14)(A+BD^{-1}C)^{-1} = A^{-1}-A^{-1}B(D+CA^{-1}B)^{-1}CA^{-1} \tag{14}

依式（14）对式（13）变形：

Vk=(Pk−1−1+CT∑−1C)−1=Pk−1−Pk−1CT(∑+CPk−1CT)−1CPk−1(15){\color{Red} \begin{aligned} V_k &=(P^{-1}_{k-1}+C^T\sum{}{}^{-1}C)^{-1} \\ &= P_{k-1}-P_{k-1}C^T(\sum{}{}+CP_{k-1}C^T)^{-1}CP_{k-1} \end{aligned}} \tag{15}

记卡尔曼增益Kk=Pk−1CT(∑+CPk−1CT)−1K_k=P_{k-1}C^T(\sum{}{}+CP_{k-1}C^T)^{-1} 从而后验分布的协方为：

Vk=(Pk−1−1+CT∑−1C)−1=Pk−1−KkCPk−1=(I−KkC)Pk−1(16){\color{Red} \begin{aligned} V_k &=(P^{-1}_{k-1}+C^T\sum{}{}^{-1}C)^{-1} \\ &= P_{k-1}-K_kCP_{k-1} \\ &= (I-K_kC)P_{k-1} \\ \end{aligned}} \tag{16}

将（16）式带入（13）式整理得，后验分布的均值为：

μk=(Pk−1−1+CT∑−1C）−1(CT∑−1zk+Pk−1−1Aμk−1)=(Pk−1−KkCPk−1)(CT∑−1zk+Pk−1−1Aμk−1)=Pk−1CT∑−1zk+Aμk−1−KkCPk−1CT∑−1zk−KkCAμk−1=Pk−1CT(CPk−1CT+∑)−1(CPk−1CT+∑)∑−1zk+Aμk−1−KkCPk−1CT∑−1zk−KkCAμk−1=Kk(CPk−1CT+∑)∑−1xk+Aμk−1−KkCPk−1CT∑−1xk−KkCAμk−1=KkCPk−1CT∑−1zk+Kkzk+Aμk−1−KkCPk−1CT∑−1zk−KkCAμk−1=Aμk−1+Kk(zk−CAk−1)(17){\color{Red} \begin{aligned} \mu_k &=(P^{-1}_{k-1} + C^T\sum{}{}^{-1} C）^{-1}(C^T\sum{}{}^{-1}z_k+P_{k-1}^{-1}A\mu_{k-1} )\\ &= (P_{k-1}-K_kCP_{k-1})(C^T\sum{}{}^{-1}z_k+P^{-1}_{k-1}A\mu_{k-1})\\ &=P_{k-1}C^T\sum{}{}^{-1}z_k+A\mu_{k-1}-K_kCP_{k-1}C^T\sum{}{}^{-1}z_k-K_kCA\mu_{k-1}\\ &=P_{k-1}C^T(CP_{k-1}C^T+\sum)^{-1}(CP_{k-1}C^T+\sum)\sum{}{}^{-1}z_k+A\mu_{k-1}-K_kCP_{k-1}C^T\sum{}{}^{-1}z_k- K_kCA\mu_{k-1} \\ &=K_k(CP_{k-1}C^T+\sum)\sum{}{}^{-1}x_k+A\mu_{k-1}-K_kCP_{k-1}C^T\sum{}{}^{-1}x_k-K_kCA\mu_{k-1} \\ &=K_kCP_{k-1}C^T\sum{}{}^{-1}z_k+K_kz_k+A\mu_{k-1}-K_kCP_{k-1}C^T\sum{}{}^{-1}z_k-K_kCA\mu_{k-1} \\ &= A\mu_{k-1}+K_k(z_k-CA_{k-1}) \end{aligned}} \tag{17}
avatar
文章
阅读
粉丝