控制算法学习 一、卡尔曼滤波（2）从贝叶斯的角度

前言

控制算法学习一(1)是以小车为例，推演卡尔曼滤波算法的流程。

举例易于理解算法，但如果要真正掌握算法，必须从算法的本质入手进行推导。

卡尔曼滤波的本质，就是贝叶斯滤波在高斯过程上处理的特例。

系统建模

仍然以小车模型为例：

$$\begin{array}{rl}& x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1},w_k\sim [0,Q]\\ & z_k=H{x}_k+v_k,v_k\sim [0,R]\end{array}$$
$A,B,H$分别表示状态转移矩阵，控制（输入）转换矩阵，观测转换矩阵。

$w_k,v_k$都是高斯白噪声，表示了系统误差（过程噪声）和测量误差。

$x_k,z_k$是向量，表明k时刻的系统状态。

小车系统是具有马尔可夫性的，并假设它的位置和速度服从高斯分布，其有噪状态观测 Z = { z 1 ,   z 2 ,   … , z n } Z=\{ z_1,\ z_2, \ \dots, z_n \} Z={z1​, z2​, …,zn​}，本时刻的观测是$z_n$，卡尔曼滤波的目标是获得本时刻系统真实状态的最优估计$x_n$。

重点：系统具有马尔科夫性，并且系统状态服从高斯分布

贝叶斯滤波

由贝叶斯滤波（3）得到：
$$\begin{array}{rl}p(x_n|x_{n-1},u_n,z_n)& =\frac{p(z_n|x_n,x_{n-1},u_n)p(x_n|x_{n-1},u_n)}{p(z_n|x_{n-1},u_n)}\\ & =\frac{p(z_n|x_n)p(x_n|x_{n-1},u_n)}{p(z_n|x_{n-1},u_n)}\\ & =\frac{p(z_n|x_n)p(x_n|x_{n-1})}{p(z_n)}\end{array}$$
控制输入量$u_n$并不本质，推导省略控制输入

先验

上一时刻的状态最优估计是当前状态的先验，根据高斯分布的性质计算$p(x_n|x_{n-1})$：
$$\begin{gathered} x_n=A{x}_{n-1}+w_{n-1},w_n\sim [0,Q] \\ {\tilde{x}}_n\sim N[A{x}_{n-1},A{P}_{n-1}{A}^T+Q] \end{gathered}$$
这里的$x_{n-1}$即上一时刻系统状态的最优估计。${\tilde{x}}_n$即贝叶斯思想里的先验。

似然

观测得到的状态是真实状态的似然$p(z_n|x_n)$：
$$z_n=H{x}_n+v_n,v_n\sim N[0,R]$$

后验

后验$\propto$先验$\times$似然

$$p(x_n|x_{n-1},z_n)=\frac{p(z_n|x_n)p(x_n|x_{n-1})}{p(z_n)}$$
先验、似然均已知，通过卡尔曼的预测和更新过程的公式就可以把$p(x_n|x_{n-1},z_n)$的具体表达式求出。
$p(x_n|x_{n-1},z_n)$的期望和协方差就作为当前时刻的状态最优估计。