Robotics: Estimation and Learning.WEEK 2_robotics: estimation and learning 视频-优快云博客

本文链接：https://blog.youkuaiyun.com/weixin_43010548/article/details/88070266

本文深入探讨了Kalman滤波的理论基础，包括离散时间系统的状态和测量建模，贝叶斯滤波的概念及其算法，以及Kalman滤波的具体实现过程。通过解析状态转移、测量更新和递推最小二乘估计，详细阐述了Kalman滤波如何在有噪声的数据中准确估计系统状态。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

$WEEK2{\Large WEEK \qquad 2}$
$Kalman滤波在最后\color{#F00}{Kalman滤波在最后}$

2.1 系统和测量建模

$\qquad$ 离散线性动态运动系统：
$x_{t+1}=Ax_{t}+Bu_{t}\qquad z_{t}=Cx_{t}\qquad(1)$ 其中A是状态转移矩阵， $u_{t}$ 表示不依赖状态 $x_{t}$ 的外部输入，C表示连接测量变量和状态的测量矩阵。
$\qquad$ 基于基于状态动态模型的状态估计值条件概率：
$p(x_{t+1}|x_{t})\qquad（忽略u_{t}）(2)$ $\qquad$ 带噪声的测量值的条件概率：
$p(z_{t}|x_{t})\qquad(3)$ $\qquad$ 应用线性动态模型：
$p(x_{t+1}|x_{t})=Ap(x_{t})\qquad(4)\\ p(z_{t}|x_{t})=Cp(x_{t})\qquad(5)$ $\qquad$ 给运动和观测加入噪声
$p(x_{t+1}|x_{t})=Ap(x_{t})+v_{m}\qquad(6)\\ p(z_{t}|x_{t})=Cp(x_{t})+v_{o}\qquad(7)$ $\qquad$ 引入状态 $x_{t}$ 的高斯模型
$p(x_{t+1}|x_{t})=A\mathcal{N}(x_{t},P_{t})+\mathcal{N}(0,\begin{matrix}\sum_{m}\end{matrix})\qquad(8)\\ p(z_{t}|x_{t})=C\mathcal{N}(x_{t},P_{t})+\mathcal{N}(0,\begin{matrix}\sum_{o}\end{matrix})\qquad(9)$ $\qquad$ 应用高斯分布的线性变换
$p(x_{t+1}|x_{t})=\mathcal{N}(Ax_{t},AP_{t}A^{T})+\mathcal{N}(0,\begin{matrix}\sum_{m}\end{matrix})\qquad(10)\\ p(z_{t}|x_{t})=\mathcal{N}(Cx_{t},CP_{t}C^{T})+\mathcal{N}(0,\begin{matrix}\sum_{o}\end{matrix})\qquad(11)$ $\qquad$ 应用高斯分布求和公式
$p(x_{t+1}|x_{t})=\mathcal{N}(Ax_{t},AP_{t}A^{T}+\begin{matrix}\sum_{m}\end{matrix})\qquad(12)\\ p(z_{t}|x_{t})=\mathcal{N}(Cx_{t},CP_{t}C^{T}+\begin{matrix}\sum_{o}\end{matrix})\qquad(13)$

以下摘自《概率机器人》

2.2 贝叶斯滤波

2.2.1 关于Z=z的贝叶斯准则

$p(x|y,z)=\frac{p(y|x,z)p(x|z)}{p(y|z)}\qquad(14)$ $\qquad$ 以其他变量z为条件的相互独立的随机变量条件联合概率定律:
$p(x,y|z)=p(x|z)p(y|z)\qquad(15)$ 这种关系被称为条件独立，等价于：
$p(x|z)=p(x|z,y)\qquad(16)\\ p(y|z)=p(y|z,x)\qquad(17)$

2.2.2 状态的完整性

$\qquad$ 假设一个状态 $x_{t}$ 可以最好地预测未来，则称其为完整的(complete) 。换句话说， 完整性包括过去状态测量及控制的信息，但不包含其他可以更加精确地预测未来的其他附加信息 。很重要的是，要注意对完整性的定义并不是要求未来是一个关于状态的确定(deterministic) 函数。未来可以是随机的，但是没有先于 $x_{t}$ 的状态变化可以影响未来状态的随机变化，除非这种依赖通过状态 $x_{t}$ 起作用。满足这些条件的暂态过程通常称为马尔可夫链(Markov chain) 。
$\qquad$ 状态完整性的概念主要是理论上的重要性。实际上，对任何一个实际的机器人系统不可能指定一个完整的状态。一个完整的状态不仅包括对未来有影响的环境的所有方面，而且也包括机器人本身、计算机内存的内容以及周围人造成的信息垃圾等。其中有些是很难获得的。现实的实现是挑选所有状态变量的小子集。这样的状态叫作不完整状态(incomplete state) 。

2.2.3 概率生成法则

$\qquad$ 状态和测量的演变由概率法则支配。表征状态演变的概率法则可以由 $p(x_{t}|x_{0:t-1},z_{1:t-1},u_{1:t})$ 概率分布给出（假定机器人先执行一个控制动作 $u_{1}$ ，然后得到一个测量 $z_{1}$ ， $x_{0:t-1}$ 表示从时间0到t-1所获得状态的集合)。
$\qquad$ 如果状态 $x_{t}$ 是完整的，那么它是之前时刻发生的所有状态的充分总结。状态 $x_{t-1}$ 是直到t-1时刻的控制和测量的一个充分统计量，即 $u_{1:t-1}$ 和 $z_{1:t-1}$ 。上述变量中，只有控制 $u_{t}$ 关心是否知道状态 $x_{t-1}$ (From all the variables in the expression above, only the control $u_{t}$ matters if we know the state $x_{t-1}$ .)即只有变量 $u_{t}$ 作用在 $x_{t-1}$ 之后。由此： $p(x_{t}|x_{0:t-1},z_{1:t-1},u_{1:t})=p(x_{t}|x_{t-1},u_{t})\qquad(18)$ $\qquad$ 上式为状态转移概率，由这个等式表达的特性就是条件独立（表明如果知道第三组变量（条件变量）的值，该变量就是独立于其他变量的）。
$\qquad$ 如果状态 $x_{t}$ 是完整的，有如下条件独立：
$p(z_{t}|x_{0:t},z_{1:t-1},u_{1:t})=p(z_{t}|x_{t})\qquad(19)$ $\qquad$ 上式为测量概率，用状态 $x_{t}$ 足以预测（有潜在的噪声的）测量 $z_{t}$ 。如果 $x_{t}$ 是完整的，则任何其他变量的信息，如过去的测量、控制、或过去的状态都是与之无关的。
在这里插入图片描述 $图1\quad表征控制、状态和测量演变的动态贝叶斯网络$

2.2.4 置信分布

$\qquad$ 置信度反映了机器人有关环境状态的内部信息。状态（位姿）不能直接测量，机器人必须从数据中推测出其状态。概率机器人中通过条件概率分布表达置信度。对于真实的状态，置信度分布为每一个可能的假设分配一个概率（或者概率密度值）。置信度分布是以可获得数据为条件的关于状态变量的后验概率。使用 $bel(x_{t})$ 表示状态变量 $x_{t}$ 的置信度，后验概率为：
$bel(x_{t})=p(x_{t}|z_{1:t},u_{1:t})\qquad(20)$ $\qquad$ 该后验分布是时刻t下状态 $x_{t}$ 的概率分布，以所有过去测量 $z_{1:t}$ 和所有过去控制 $u_{1:t}$ 为条件。刚执行完控制 $u_{t}$ 之后，综合 $z_{t}$ 之前计算后验为：
$\overline{bel}(x_{t})=p(x_{t}|z_{1:t-1},u_{1:t})\qquad(21)$ $\qquad$ 在概率滤波框架下，式(21)概率常被称为预测，基于以前状态的后验，在综合时刻t的测量之前，预测时刻t的状态。由 $bel‾(xt)\overline{bel}(x_{t})$ 计算 $bel(x_{t})$ 称为修正或测量更新。

2.2.5 贝叶斯算法

$\qquad$ 该算法依据测量和控制数据计算置信度分布bel()，伪代码如下所示：
1: $\qquad$ Algorithm Bayes_filter (bel(x_{t-1}),u_{t},z_{t})
2: $\qquad \quad$ for all $x_{t}$ do
3: $\qquad \quad \quad$ $bel‾\overline{bel}$ (x_{t})= $∫\int$ p(x_{t}|u_{t},x_{t-1}) bel(x_{t-1}) dx_{t-1}
4: $\qquad \quad \quad$ bel(x_{t})= $η\eta$ p(z_{t}|x_{t}) $bel‾\overline{bel}$ (x_{t})
5: $\qquad \quad$ endfor
6: $\qquad \quad$ return bel(x_{t})
$\qquad$ 第3行中，通过 $u_{t}$ 和置信度 ${bel}(x_{t-1})$ 预测状态 $x_{t}$ 得置信度 $bel‾(xt)\overline{bel}(x_{t})$ 。
$\qquad$ 第4行中，通过观测的测量值 $z_{t}$ 的概率乘以置信度 $bel‾(xt)\overline{bel}(x_{t})$ 和归一化常数 $η\eta$ (由全概率公式之和为1算出) 计算 $bel(x_{t})$ 。

以下总结自《最优状态估计》【美】Dan Simon 著

2.3 Kalman滤波

在这里插入图片描述 $图2\quad Kalman滤波状态随时间变化过程$ $\qquad$ 每一步Kalman滤波过程（由k-1时刻到k时刻）可以分为两个步骤：
$\qquad\quad$ 1. 依据状态时间系统方程，实现由k-1时刻状态估计值的后验( $x^k−1+\hat{x}_{k-1}^{+}$ )到k时刻状态估计值的先验( $x^k−\hat{x}_{k}^{-}$ )的估计。
$\qquad\quad$ 2. 利用在k时刻获得对状态带有噪声的测量值( $y_{k}$ )，依据线性递推方程求解当估计误差的方差和最小的Kalman增益( $K_{k}$ )，再带入到递推方程得到k时刻状态后验( $x^k+\hat{x}_{k}^{+}$ )。

2.3.1 离散时间系统

$\qquad$ 给出离散时间系统方程：
$x_{k}=F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+w_{k-1}\qquad (22)$ $\qquad$ 其中 $u_{k}$ 是已知的输入， $w_{k}$ 是零均值的高斯白噪声，协方差为 $Q_{k}$ ， $x_{k}$ 为在k时刻的状态， $F_{k-1}$ 为由k-1时刻的状态到k时刻的状态转移矩阵， $G_{k-1}$ 为k-1时刻输入到状态的转移矩阵。
$\qquad$ 状态 $x_{t}$ 的均值随时间的变化方程：
$\overline{x}_{k}=E(x_{k})=F_{k-1}\overline{x}_{k-1}+G_{k-1}u_{k-1}\qquad(23)$ $\qquad$ 状态 $x_{t}$ 的方差随时间的变化方程：
$(xk−x‾k)(xk−x‾k)T(x_{k}-\overline{x}_{k})(x_{k}-\overline{x}_{k})^{T}$
$=(Fk−1xk−1+Gk−1uk−1+wk−1−x‾k)(Fk−1xk−1+Gk−1uk−1+wk−1−x‾k)T=(F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+w_{k-1}-\overline{x}_{k})(F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+w_{k-1}-\overline{x}_{k})^{T}$
$=[Fk−1(xk−1−x‾k−1)+wk−1][Fk−1(xk−1−x‾k−1)+wk−1]T=[F_{k-1}(x_{k-1}-\overline{x}_{k-1})+w_{k-1}][F_{k-1}(x_{k-1}-\overline{x}_{k-1})+w_{k-1}]^{T}$
$=Fk−1(xk−1−x‾k−1)(xk−1−x‾k−1)TFk−1T+wk−1wk−1T+Fk−1(xk−1−x‾k−1)wk−1T+wk−1(xk−1−x‾k−1)TFk−1T(24)=F_{k-1}(x_{k-1}-\overline{x}_{k-1})(x_{k-1}-\overline{x}_{k-1})^{T}F_{k-1}^{T}+w_{k-1}w_{k-1}^{T}+F_{k-1}(x_{k-1}-\overline{x}_{k-1})w_{k-1}^{T}+w_{k-1}(x_{k-1}-\overline{x}_{k-1})^{T}F_{k-1}^{T}\qquad(24)$
$\qquad$ 取上式方程期望即为 $x_{k}$ 的协方差，由 $(xk−x‾k)(x_{k}-\overline{x}_{k})$ 与 $w_{k-1}$ 互不相关且 $E(w_{k-1})=0$ 则上式化简为：
$P_{k}=E[(x_{k}-\overline{x}_{k})(x_{k}-\overline{x}_{k})^{T}]=F_{k-1}P_{k-1}F_{k-1}^{T}+Q_{k-1}\qquad(25)$ $\qquad$ 上式称为离散时间Lyapunov方程或Stein方程。由上式可得Kalman滤波先验估计：
$x^k−=Fk−1x^k−1++Gk−1uk−1(状态先验估计)(26)\hat{x}_{k}^{-}=F_{k-1}\hat{x}_{k-1}^{+}+G_{k-1}u_{k-1}\quad(状态先验估计)\qquad(26)$ $Pk−=Fk−1Pk−1+Fk−1T+Qk−1(协方差先验估计)(27)P_{k}^{-}=F_{k-1}P_{k-1}^{+}F_{k-1}^{T}+Q_{k-1}\quad(协方差先验估计)\qquad(27)$

2.3.2 递推最小二乘估计

$\qquad$ 利用最小二乘估计计算递推式中最优Kalman增益 $K_{k}$ 。线性的递推估计值为： $y_{k}=H_{k}x_{k}+v_{k}\qquad(28)$ $x^k=x^k−1+Kk(yk−Hkx^k−1)(29)\hat{x}_{k}=\hat{x}_{k-1}+K_{k}(y_{k}-H_{k}\hat{x}_{k-1})\qquad(29)$ 式中 $H_{k}$ 为测量矩阵， $v_{k}$ 为均值为0，方差为 $R_{k}$ 的测量噪声（高斯白噪声）。 $K_{k}$ 为增益矩阵， $(yk−Hkx^k−1)(y_{k}-H_{k}\hat{x}_{k-1})$ 为修正项。 $K_{k}$ 选择的最优标准使k时刻的估计误差的方差和最小，其方差和 $J_{k}$ 如下所示：
$Jk=E[(x1,k−x^1,k)2]+…+E[(xn,k−x^n,k)2]J_{k}=E[(x_{1,k}-\hat{x}_{1,k})^{2}]+\ldots+E[(x_{n,k}-\hat{x}_{n,k})^{2}]$
$=E(εx1,k2+…+εxn,k2)=E(εx,kTεx,k)=E(\varepsilon_{x1,k}^{2}+\ldots+\varepsilon_{xn,k}^{2})=E(\varepsilon_{x,k}^{T}\varepsilon_{x,k})$
$=E[Tr(εx,kεx,kT)]=E[Tr(\varepsilon_{x,k}^{}\varepsilon_{x,k}^{T})]$
$=TrPk(30)=TrP_{k}\qquad\qquad\qquad\qquad\qquad\qquad\qquad\qquad(30)$
由：
$E(εx,k)=E(x−x^k)=E[x−x^k−1−Kk(yk−Hkx^k−1)]E(\varepsilon_{x,k})=E(x_{}-\hat{x}_{k})=E[x_{}-\hat{x}_{k-1}-K_{k}(y_{k}-H_{k}\hat{x}_{k-1})]$
$=E[x−x^k−1−Kk(Hkx+vk−Hkx^k−1)]=E[x_{}-\hat{x}_{k-1}-K_{k}(H_{k}x_{}+v_{k}-H_{k}\hat{x}_{k-1})]$
$=E[εx,k−1−KkHkεx,k−1−Kkvk]=E[\varepsilon_{x,k-1}-K_{k}H_{k}\varepsilon_{x,k-1}-K_{k}v_{k}]\qquad$
$=(I−KkHk)E(εx,k−1)−KkE(vk)(31)=(I-K_{k}H_{k})E(\varepsilon_{x,k-1})-K_{k}E(v_{k})\qquad\qquad(31)$

式中 $P_{k}$ 是估计误差的协方差矩阵
$\varepsilon_{x,k}=[\varepsilon_{x1,k},\varepsilon_{x2,k}\cdots\varepsilon_{xn,k}]^{T}\qquad(32)$ $P_{k}=\begin{bmatrix} E(\varepsilon_{x1,k}^{2}) &E(\varepsilon_{x1,k}\varepsilon_{x2,k}) & \cdots & E(\varepsilon_{x1,k}\varepsilon_{xn,k}) \\ \vdots & \ddots & \vdots \\ E(\varepsilon_{xn,k}) E(\varepsilon_{x1,k}) &E(\varepsilon_{xn,k}\varepsilon_{x2,k})& \cdots & E(\varepsilon_{xn,k}^{2}) \end{bmatrix}\qquad(33)$ 化简 $P_{k}$ 可得：
$Pk=E(εx,kεx,kT)P_{k}=E(\varepsilon_{x,k}\varepsilon_{x,k}^{T})$
$=E{[(I−KkHk)εx,k−1−Kkvk][(I−KkHk)εx,k−1−Kkvk]T}=E\{[(I-K_{k}H_{k})\varepsilon_{x,k-1}-K_{k}v_{k}][(I-K_{k}H_{k})\varepsilon_{x,k-1}-K_{k}v_{k}]^{T}\}$
$=(I−KkHk)E(εx,k−1εx,k−1T)(I−KkHk)T−KkE(vkεx,k−1T)(I−KkHk)T−(I−KkHk)E(vkTεx,k−1)KkT+KkE(vkvkT)KkT(34)=(I-K_{k}H_{k})E(\varepsilon_{x,k-1}\varepsilon_{x,k-1}^{T})(I-K_{k}H_{k})^{T}-K_{k}E(v_{k}\varepsilon_{x,k-1}^{T})(I-K_{k}H_{k})^{T}-(I-K_{k}H_{k})^{}E(v_{k}^{T}\varepsilon_{x,k-1}^{})K_{k}^{T}+K_{k}E(v_{k}v_{k}^{T})K_{k}^{T}\qquad(34)$
$\qquad$ 由 $εx,k−1\varepsilon_{x,k-1}$ (k-1时刻的估计误差)与 $v_{k}$ (k时刻的测量噪声)相互独立又 $E(v_{k})$ =0，因此：
$E(v_{k}^{T}\varepsilon_{x,k-1}^{})=E(v_{k})E(\varepsilon_{x,k-1})=0\qquad(35)$ $Pk=(I−KkHk)Pk−1(I−KkHk)T+KkRkKkT=(I−KkHk)Pk−1(36)\qquad\qquad P_{k}=(I-K_{k}H_{k})P_{k-1}(I-K_{k}H_{k})^{T}+K_{k}R_{k}K_{k}^{T}=(I-K_{k}H_{k})P_{k-1}\qquad(36)$ $\qquad$ 对 $J_{k}$ 求导得： $\frac{\partial J_{k}}{\partial K_{k}}=(I-K_{k}H_{k})P_{k-1}(-H_{k}^{T})+K_{k}R_{k}=0\qquad(37)$ $\qquad$ 求的： $K_{k}=P_{k-1}H_{k}^{T}(H_{k}P_{k-1}H_{k}^{T}+R_{k})^{-1}\qquad(38)$ 由以上可得Kalman滤波测量更新： $Kk=Pk−HkT(HkPk−HkT+Rk)−1(39)K_{k}=P_{k}^{-}H_{k}^{T}(H_{k}P_{k}^{-}H_{k}^{T}+R_{k})^{-1}\qquad(39)$ $x^k+=x^k−+Kk(yk−Hkx^k−)(后验分布)(40)\hat{x}_{k}^{+}=\hat{x}_{k}^{-}+K_{k}(y_{k}-H_{k}\hat{x}_{k}^{-})\quad(后验分布)\qquad(40)$ $Pk+=(I−KkHk)Pk−(41)P_{k}^{+}=(I-K_{k}H_{k})P_{k}^{-}\qquad(41)$ $\qquad$ 递推方程中递推的时间间隔是由k时刻到k-1时刻表示，而应用在更新方程中是在同一个时刻，只是先验和后验的差距（是否获得在k时刻的测量值）。
$\qquad$ 先验是指以k-1时刻和之前的测量值估计 $x_{k}$ 的状态值，计算方法为以k-1时刻和之前的测量值为条件计算 $x_{k}$ 的期望值。后验则是在先验的基础上增加k时刻的测量值 $y_{k}$ 。
$\qquad$ 有以上两步，得Kalman系统方程：
$x^k−=Fk−1x^k−1++Gk−1uk−1(状态先验估计)(42)\hat{x}_{k}^{-}=F_{k-1}\hat{x}_{k-1}^{+}+G_{k-1}u_{k-1}\quad(状态先验估计)\qquad(42)$ $Pk−=Fk−1Pk−1+Fk−1T+Qk−1(协方差先验估计)(43)P_{k}^{-}=F_{k-1}P_{k-1}^{+}F_{k-1}^{T}+Q_{k-1}\quad(协方差先验估计)\qquad(43)$ $Kk=Pk−HkT(HkPk−HkT+Rk)−1(44)K_{k}=P_{k}^{-}H_{k}^{T}(H_{k}P_{k}^{-}H_{k}^{T}+R_{k})^{-1}\qquad(44)$ $x^k+=x^k−+Kk(yk−Hkx^k−)(后验分布)(45)\hat{x}_{k}^{+}=\hat{x}_{k}^{-}+K_{k}(y_{k}-H_{k}\hat{x}_{k}^{-})\quad(后验分布)\qquad(45)$ $Pk+=(I−KkHk)Pk−(46)P_{k}^{+}=(I-K_{k}H_{k})P_{k}^{-}\qquad(46)$ $\qquad$ Kalman滤波器初始化如下：
$x^0+=E(x0)=x‾0(47)\hat{x}_{0}^{+}=E(x_{0})=\overline{x}_{0}\qquad(47)$ $P0+=E[(x0−x^0+)(x0−x^0+)T](48)P^{+}_{0}=E[(x_{0}-\hat{x}^{+}_{0})(x_{0}-\hat{x}^{+}_{0})^{T}]\qquad(48)$ $\qquad$ 由式(47)可以实现推导过程中运算多项式中状态期望和状态估计值的联系。