Robotics: Estimation and Learning.WEEK 2

$WEEK2$
$Kalman滤波在最后$

2.1 系统和测量建模

\qquad 离散线性动态运动系统：
$$x_{t+1}=A{x}_t+B{u}_t{z}_t=C{x}_t(1)$$其中A是状态转移矩阵，$u_t$表示不依赖状态$x_t$的外部输入，C表示连接测量变量和状态的测量矩阵。
\qquad 基于基于状态动态模型的状态估计值条件概率：
$$p(x_{t+1}|x_t)（忽略u_t）(2)$$ \qquad 带噪声的测量值的条件概率：
$$p(z_t|x_t)(3)$$ \qquad 应用线性动态模型：
$$\begin{gathered} p(x_{t+1}|x_t)=Ap(x_t)(4) \\ p(z_t|x_t)=Cp(x_t)(5) \end{gathered}$$ \qquad 给运动和观测加入噪声
$$\begin{gathered} p(x_{t+1}|x_t)=Ap(x_t)+v_m(6) \\ p(z_t|x_t)=Cp(x_t)+v_o(7) \end{gathered}$$ \qquad 引入状态$x_t$的高斯模型
$$\begin{gathered} p(x_{t+1}|x_t)=A\mathcal{N}(x_t,P_t)+\mathcal{N}(0,\begin{array}{c}{\sum }_m\end{array})(8) \\ p(z_t|x_t)=C\mathcal{N}(x_t,P_t)+\mathcal{N}(0,\begin{array}{c}{\sum }_o\end{array})(9) \end{gathered}$$ \qquad 应用高斯分布的线性变换
$$\begin{gathered} p(x_{t+1}|x_t)=\mathcal{N}(A{x}_t,A{P}_t{A}^T)+\mathcal{N}(0,\begin{array}{c}{\sum }_m\end{array})(10) \\ p(z_t|x_t)=\mathcal{N}(C{x}_t,C{P}_t{C}^T)+\mathcal{N}(0,\begin{array}{c}{\sum }_o\end{array})(11) \end{gathered}$$ \qquad 应用高斯分布求和公式
$$\begin{gathered} p(x_{t+1}|x_t)=\mathcal{N}(A{x}_t,A{P}_t{A}^T+\begin{array}{c}{\sum }_m\end{array})(12) \\ p(z_t|x_t)=\mathcal{N}(C{x}_t,C{P}_t{C}^T+\begin{array}{c}{\sum }_o\end{array})(13) \end{gathered}$$

以下摘自《概率机器人》

2.2 贝叶斯滤波

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

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

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)(18)$$ \qquad 上式为状态转移概率，由这个等式表达的特性就是条件独立（表明如果知道第三组变量（条件变量）的值，该变量就是独立于其他变量的）。
\qquad 如果状态$x_t$是完整的，有如下条件独立：
$$p(z_t|x_{0:t},z_{1:t-1},u_{1:t})=p(z_t|x_t)(19)$$ \qquad 上式为测量概率，用状态$x_t$足以预测（有潜在的噪声的）测量$z_t$。如果$x_t$是完整的，则任何其他变量的信息，如过去的测量、控制、或过去的状态都是与之无关的。
$$图1表征控制、状态和测量演变的动态贝叶斯网络$$

2.2.4 置信分布

\qquad 置信度反映了机器人有关环境状态的内部信息。状态（位姿）不能直接测量，机器人必须从数据中推测出其状态。概率机器人中通过条件概率分布表达置信度。对于真实的状态，置信度分布为每一个可能的假设分配一个概率（或者概率密度值）。置信度分布是以可获得数据为条件的关于状态变量的后验概率。使用$bel(x_t)$表示状态变量$x_t$的置信度，后验概率为：
$$bel(x_t)=p(x_t|z_{1:t},u_{1:t})(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})(21)$$ \qquad 在概率滤波框架下，式(21)概率常被称为预测，基于以前状态的后验，在综合时刻t的测量之前，预测时刻t的状态。由$\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 $\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}) $\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$得置信度$\overline{bel}(x_t)$。
\qquad 第4行中，通过观测的测量值$z_t$的概率乘以置信度$\overline{bel}(x_t)$和归一化常数$\eta$ (由全概率公式之和为1算出) 计算$bel(x_t)$。

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

2.3 Kalman滤波

$$图2Kalman滤波状态随时间变化过程$$ \qquad 每一步Kalman滤波过程（由k-1时刻到k时刻）可以分为两个步骤：
\qquad\quad 1. 依据状态时间系统方程，实现由k-1时刻状态估计值的后验(${\hat{x}}_{k-1}^{+}$)到k时刻状态估计值的先验(${\hat{x}}_k^{-}$)的估计。
\qquad\quad 2. 利用在k时刻获得对状态带有噪声的测量值($y_k$)，依据线性递推方程求解当估计误差的方差和最小的Kalman增益($K_k$)，再带入到递推方程得到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}(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}(23)$$ \qquad 状态$x_t$的方差随时间的变化方程：
$(x_k-{\overline{x}}_k)(x_k-{\overline{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$
$=[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$
$=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(24)$
\qquad 取上式方程期望即为$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}(25)$$ \qquad 上式称为离散时间Lyapunov方程或Stein方程。由上式可得Kalman滤波先验估计：
$${\hat{x}}_k^{-}=F_{k-1}{\hat{x}}_{k-1}^{+}+G_{k-1}{u}_{k-1}(状态先验估计)(26)$$$$P_k^{-}=F_{k-1}{P}_{k-1}^{+}{F}_{k-1}^T+Q_{k-1}(协方差先验估计)(27)$$

2.3.2 递推最小二乘估计

\qquad 利用最小二乘估计计算递推式中最优Kalman增益$K_k$。线性的递推估计值为：$$y_k=H_k{x}_k+v_k(28)$$$${\hat{x}}_k={\hat{x}}_{k-1}+K_k(y_k-H_k{\hat{x}}_{k-1})(29)$$式中$H_k$为测量矩阵，$v_k$为均值为0，方差为$R_k$的测量噪声（高斯白噪声）。$K_k$为增益矩阵，$(y_k-H_k{\hat{x}}_{k-1})$为修正项。$K_k$选择的最优标准使k时刻的估计误差的方差和最小，其方差和$J_k$如下所示：
$J_k=E[(x_{1,k}-{\hat{x}}_{1,k}{)}^2]+\dots +E[(x_{n,k}-{\hat{x}}_{n,k}{)}^2]$
$=E({\epsilon }_{x1,k}^2+\dots +{\epsilon }_{xn,k}^2)=E({\epsilon }_{x,k}^T{\epsilon }_{x,k})$
$=E[Tr({\epsilon }_{x,k}^{}{\epsilon }_{x,k}^T)]$
$=Tr{P}_k(30)$
由：
$E({\epsilon }_{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_{}-{\hat{x}}_{k-1}-K_k(H_k{x}_{}+v_k-H_k{\hat{x}}_{k-1})]$
$=E[{\epsilon }_{x,k-1}-K_k{H}_k{\epsilon }_{x,k-1}-K_k{v}_k]$
$=(I-K_k{H}_k)E({\epsilon }_{x,k-1})-K_{k}E(v_k)(31)$

式中$P_k$是估计误差的协方差矩阵
$${\epsilon }_{x,k}=[{\epsilon }_{x1,k},{\epsilon }_{x2,k}\cdots {\epsilon }_{xn,k}{]}^T(32)$$$$P_k=\left[\begin{array}{cccc}E({\epsilon }_{x1,k}^2)& E({\epsilon }_{x1,k}{\epsilon }_{x2,k})& \cdots & E({\epsilon }_{x1,k}{\epsilon }_{xn,k})\\ ⋮& \ddots & ⋮& \\ E({\epsilon }_{xn,k})E({\epsilon }_{x1,k})& E({\epsilon }_{xn,k}{\epsilon }_{x2,k})& \cdots & E({\epsilon }_{xn,k}^2)\end{array}\right](33)$$化简$P_k$可得：
$P_k=E({\epsilon }_{x,k}{\epsilon }_{x,k}^T)$
= E { [ ( I − K k H k ) ε x , k − 1 − K k v k ] [ ( I − K k H k ) ε x , k − 1 − K k v k ] 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}\} =E{[(I−Kk​Hk​)εx,k−1​−Kk​vk​][(I−Kk​Hk​)εx,k−1​−Kk​vk​]T}
$=(I-K_k{H}_k)E({\epsilon }_{x,k-1}{\epsilon }_{x,k-1}^T)(I-K_k{H}_k{)}^T-K_{k}E(v_k{\epsilon }_{x,k-1}^T)(I-K_k{H}_k{)}^T-(I-K_k{H}_k{)}^{}E(v_k^T{\epsilon }_{x,k-1}^{})K_k^T+K_{k}E(v_k{v}_k^T)K_k^T(34)$
\qquad 由${\epsilon }_{x,k-1}$(k-1时刻的估计误差)与$v_k$(k时刻的测量噪声)相互独立又$E(v_k)$=0，因此：
$$E(v_k^T{\epsilon }_{x,k-1}^{})=E(v_k)E({\epsilon }_{x,k-1})=0(35)$$$$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}(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(37)$$ \qquad 求的：$$K_k=P_{k-1}{H}_k^T(H_k{P}_{k-1}{H}_k^T+R_k{)}^{-1}(38)$$由以上可得Kalman滤波测量更新：$$K_k=P_k^{-}{H}_k^T(H_k{P}_k^{-}{H}_k^T+R_k{)}^{-1}(39)$$$${\hat{x}}_k^{+}={\hat{x}}_k^{-}+K_k(y_k-H_k{\hat{x}}_k^{-})(后验分布)(40)$$$$P_k^{+}=(I-K_k{H}_k)P_k^{-}(41)$$ \qquad 递推方程中递推的时间间隔是由k时刻到k-1时刻表示，而应用在更新方程中是在同一个时刻，只是先验和后验的差距（是否获得在k时刻的测量值）。
\qquad 先验是指以k-1时刻和之前的测量值估计$x_k$的状态值，计算方法为以k-1时刻和之前的测量值为条件计算$x_k$的期望值。后验则是在先验的基础上增加k时刻的测量值$y_k$。
\qquad 有以上两步，得Kalman系统方程：
$${\hat{x}}_k^{-}=F_{k-1}{\hat{x}}_{k-1}^{+}+G_{k-1}{u}_{k-1}(状态先验估计)(42)$$$$P_k^{-}=F_{k-1}{P}_{k-1}^{+}{F}_{k-1}^T+Q_{k-1}(协方差先验估计)(43)$$$$K_k=P_k^{-}{H}_k^T(H_k{P}_k^{-}{H}_k^T+R_k{)}^{-1}(44)$$$${\hat{x}}_k^{+}={\hat{x}}_k^{-}+K_k(y_k-H_k{\hat{x}}_k^{-})(后验分布)(45)$$$$P_k^{+}=(I-K_k{H}_k)P_k^{-}(46)$$ \qquad Kalman滤波器初始化如下：
$${\hat{x}}_0^{+}=E(x_0)={\overline{x}}_0(47)$$ $$P_0^{+}=E[(x_0-{\hat{x}}_0^{+})(x_0-{\hat{x}}_0^{+}{)}^T](48)$$ \qquad 由式(47)可以实现推导过程中运算多项式中状态期望和状态估计值的联系。