随笔- 59 文章- 0 评论- 17 阅读- 18万

以NASA之名: 卡尔曼滤波器

'That's one small step for man,one giant leap for mankind.' — Neil Alden Armstron

以NASA之名: 卡尔曼滤波器

引言

二十世纪的阿波罗登月计划在人类历史上是浓墨重彩的一笔, 是人类科学发展极其重要的里程碑. 在此计划中, 阿姆斯特朗在月球上说出了上面的一句话,是对此计划最最恰当的注释. 说起来这个计划很''简单'': 送人到月球转一圈,然后再回来. 这么一个'简单'的计划实施起来得有多困难大家心中早有尺度.而卡尔曼滤波器(Kalman filter)就是其中的功臣.

荣耀骑士

卡尔曼滤波(器)是以其主要贡献者 Rudolf Emil Kalman 命名的. 这个滤波器做什么的呢?

现在,时间闪回到 1969年七月, 当时 Armstrong 正坐在阿波罗11号中,飞向月球. 在此期间掌握飞船的位置及其速度是非常重要的.因为只要偏差超过一定阈值, 飞船的超高速就会将这偏差迅速放大,进而飞船就会偏离预定轨道,最终,极有可能飞向太空就再也回不来了.

那要如何得到飞船在某个时间点的位置与速度呢?

我们知道地面上的科学家在将阿波罗11号发射升空之前肯定会预先计算飞船每个点的位置,或者一定有一个计算精确的计算公式.但, 无论多么精确的公式都无法包含所有因素,不确定性是无法避免的. 而阿波罗上面肯定会有一套复杂的传感系统来测算飞船的位置与速度, 然而, 传感觉器必然地会有偏差. 也就是说我们会从两种不同的途径获得两套参数(位置与速度), 这两套参数一般是不同的,却又都是不准确的.

我们当然会想到:能不能综合地考虑这两套参数,从而获得一套靠谱的参数,来指导飞船航行? 那这两个参数如何综合考虑呢? 在此关键时刻,荣耀骑士 — 卡尔曼滤波器登场了.它的出现就是解决此等棘手问题的.

卡尔曼滤波器是如何工作的呢? 我们知道科学家的公式是不会错的,只不过外界因素干扰,才使其失准的.因此我们以其为基准,用传感器的参数来对基校准. 传感器的参数也有不确定性. 我们的目的是获得更精准的参数, 故需要在传感参数校正公式参数时,首要任务即是在当前所有信息面前最小化不确定性.这就是卡尔曼滤波器的作用. 至于它是如何最小化不确定性的,就要涉及公式了, 你确定要上吗? 如果是,请继续~

卡尔曼滤波器*

卡尔曼滤波(器)基于线性动态系统,是建立在线性模型上的带有白噪声的马氏链. 其与隐马尔可夫模型(HMM)很相像,最主要的不同点在其隐状态是连续的,而HMM是离散的.

模型:

{\begin{matrix} x_{k} = A_{k} x_{k - 1} + B_{k} u_{k} + w_{k} \\ y_{k} = C_{k} x_{k} + v_{k} \end{matrix}

$\left\{ \begin{array}\ x_{k} = A_kx_{k-1}+B_k u_k + w_k\\ y_k = C_k x_k + v_k \end{array} \right.$

其中所有变量均为矩阵(或向量), $x_k$ : k 时刻的状态向量; $A_k$ : 状态转移矩阵; $B_k$ : 控制矩阵, $u_k$ : 控制向量; 一般,将此控制部分当作确定部分而不包含在模型中,因此上式可简化为:

{\begin{matrix} x_{k} = A_{k} x_{k - 1} + w_{k} \\ y_{k} = C_{k} x_{k} + v_{k} \end{matrix}

$\left\{ \begin{array}\ x_{k} = A_kx_{k-1} + w_k\\ y_k = C_k x_k + v_k \end{array} \right.$

$y_k$ : k时刻的观测向量; $C_k$ : 观测矩阵; $w_k, v_k$ 均为零均值白噪声,方差分别为 $Q_k, R_k$ , 其互不相关,并与初始状态亦不相关:

w_{k} : E [w_{k}] = 0, σ_{k}^{2} = Q_{k}; v_{k} : E [v_{k}] = 0, σ_{k}^{2} = R_{k}; c o v [x_{0}, w_{k}] = 0; c o v (x_{0}, v_{k}) = 0; E [w_{k} v_{k}^{T}] = 0.

$w_k : E[w_k] = 0,\sigma_k^2 = Q_k;\\ v_k: E[v_k] = 0,\sigma_k^2 = R_k;\\ cov[x_0, w_k] = 0;\\ cov(x_0,v_k) = 0;\\ E[w_kv_k^T] = 0.\\$

卡尔曼滤波器即是利用递推方法及状态方程(式1或式2) 寻找最小均方误差状态变量 $x_k$ 的估计值 $\hat{x}_k$ :

{\tilde{x}}_{k} = x_{k} - {\hat{x}}_{k} min E [{\tilde{x}}_{k} {\tilde{x}}_{k}^{T}] \to {\hat{x}}_{k}

$\tilde{x}_k = x_k - \hat{x}_k\\ \min E[\tilde{x}_k\tilde{x}_k^T] \rightarrow \hat{x}_k$

当不考虑白噪声时:

{\hat{x}}_{k}^{'} = A_{k} {\hat{x}}_{k - 1} {\hat{y}}_{k} = C_{k} {\hat{x}}_{k}^{'} = C_{k} A_{k} {\hat{x}}_{k - 1}

$\hat{x}'_k = A_k \hat{x}_{k-1}\\ \hat{y}_k = C_k \hat{x}'_k = C_k A_k \hat{x}_{k-1}$

其中 $\hat{x}'_k$ 为 k 时刻状态的估计值(由上一时刻状态经转移矩阵得到), $\hat{y}_k$ 为测量值的估计值(由状态估计值推出), 而 $\hat{x}_k$ 现在称为状态校正后的估计值.

观测向量的误差(也即新息):

{\tilde{y}}_{k} = y_{k} - {\hat{y}}_{k}

$\tilde{y}_k = y_k - \hat{y}_k$

用观测值来校正状态变量:

\begin{matrix} {\hat{x}}_{k} & = & A_{k} {\hat{x}}_{k - 1} + H_{k} (y_{k} - {\hat{y}}_{k}) \\ = & A_{k} {\hat{x}}_{k - 1} + H_{k} (y_{k} - C_{k} A_{k} {\hat{x}}_{k - 1}) \end{matrix}

$\begin{array} \\ \hat{x}_k &=&A_k \hat{x}_{k-1} + H_k (y_k - \hat{y}_k)\\ &=&A_k \hat{x}_{k-1} + H_k (y_k - \ C_k A_k \hat{x}_{k-1}) \end{array}$

其中, $H_k$ 称为增益矩阵,即为一新息加权矩阵.

新息(Innovation) $\tilde{y}_k$ 即是在 k 时刻之前没有但在 t 时刻产生的新的新信息. 用代数的语言即是新息与过去的信息是正交的:

E [{\tilde{y}}_{k} y_{j}^{T}] = O, 1 \leq j < k

$E[\tilde{y}_ky_j^T] = \boldsymbol{O}, \qquad 1\le j<k$

而且可以容易的推论出, 新息之间也是正交的:

E [{\tilde{y}}_{i} {\tilde{y}}_{j}^{T}] = O, i \neq j, a n d 1 \leq i, j \leq k

$E[\tilde{y}_i\tilde{y}_j^T] =\boldsymbol{O},\qquad i\ne j, and\quad 1\le i,j\le k$

从而可知, 新息 $\tilde{y}_k$ 是一个与k 之前时刻的数据不相关,且有白噪声性质的随机过程.

校正后的状态变量估计误差及其均方值 ( $P_k$ ) 还有未经校正的状态变量估计误差的均方值 ( $P_k'$ ):

\begin{matrix} {\tilde{x}}_{k} & = & x_{k} - {\hat{x}}_{k} \\ P_{k} & = & E [{\tilde{x}}_{k} {\tilde{x}}_{k}^{T}] = E [(x_{k} - {\hat{x}}_{k}) (x_{k} - {\hat{x}}_{k})^{T}] \\ P_{k}^{'} & = & E [(x_{k} - {\hat{x}}_{k}^{'}) (x_{k} - {\hat{x}}_{k}^{'})^{T}] \end{matrix}

$\begin{array}\\ \tilde{x}_k &=& x_k - \hat{x}_k\\ P_k &=& E[\tilde{x}_k\tilde{x}_k^T] = E[(x_k - \hat{x}_k)(x_k - \hat{x}_k)^T] \\ P_k' &=& E[(x_k - \hat{x}_k')(x_k - \hat{x}_k')^T]\\ \end{array}$

卡尔曼滤波器即是最小化 $P_k$ (不确定性) 的方式来得到 $H_k$ ,最终得到 $\hat{x}_k$ :

min P_{k} \to H_{k} \to {\hat{x}}_{k}

$\min P_k \rightarrow H_k \rightarrow \hat{x}_k$

\begin{matrix} {\tilde{x}}_{k} & = & x_{k} - {\hat{x}}_{k} \\ = & A_{k} x_{k - 1} + w_{k} - (A_{k} {\hat{x}}_{k - 1} + H_{k} (y_{k} - C_{k} A_{k} {\hat{x}}_{k - 1})) \\ = & A_{k} x_{k - 1} + w_{k} \\ - (A_{k} {\hat{x}}_{k - 1} + H_{k} (C_{k} (A_{k} x_{k - 1} + w_{k}) + v_{k} - C_{k} A_{k} {\hat{x}}_{k - 1})) \\ = & (A_{k} - H_{k} C_{k} A_{k}) (x_{k - 1} - {\hat{x}}_{k - 1}) + (I - H_{k} C_{k}) w_{k} - H_{k} v_{k} \end{matrix}

$\begin{array}\\ \tilde{x}_k &=& x_k - \hat{x}_k\\ &=& A_kx_{k-1} + w_k -(A_k \hat{x}_{k-1} + H_k (y_k - \ C_k A_k \hat{x}_{k-1}))\\ &=& A_kx_{k-1} + w_k\\ &&-(A_k \hat{x}_{k-1} + H_k (\ C_k (A_k x_{k-1}+w_k)+ v_k - \ C_k A_k \hat{x}_{k-1}))\\ &=& (A_k -H_kC_kA_k)(x_{k-1} - \hat{x}_{k-1}) + (I - H_kC_k)w_k - H_k v_k\\ \end{array}$

偷个懒吧, 接着就是'暴力'推导, 没有什么技术含量, 只看你有没有仔细,否则结果一定会出来.

经过几张 $A_4$ 纸的推导, 你可以得到以下几个重要公式:

{\begin{matrix} {\hat{x}}_{k} & = & A_{k} {\hat{x}}_{k - 1} + H_{k} (y_{k} - C_{k} A_{k} {\hat{x}}_{k - 1}) \\ H_{k} & = & P_{k}^{'} C_{k}^{T} (C_{k} P_{k}^{'} C_{k}^{T} + R_{k})^{- 1} \\ P_{k}^{'} & = & A_{k} P_{k - 1} A_{k}^{T} + Q_{k - 1} \\ P_{k} & = & (I - H_{k} C_{k}) P_{k}^{'} \end{matrix}

$\left\{\begin{array}\\ \hat{x}_k &=& A_k \hat{x}_{k-1} + H_k(y_k - C_kA_k\hat{x}_{k-1})\\ H_k &=& P_k'C_k^T(C_kP'_kC_k^T+R_k)^{-1}\\ P_k' &=& A_kP_{k-1}A_k^T + Q_{k-1}\\ P_k &=& (I - H_kC_k)P_k' \end{array}\right.$

这就是卡尔曼滤波器的递推公式了.

参考文献:

现代信号处理(第二版), 张贤达, 2002, 清华大学出版社.
How a Kalman filter works, in pictures, 2015, bzarg.com.
Kalman filter with Real-Time Applications, 2009,Springer.
Understanding the Basis of the Kalman filter via a simple and intuitive derivation.

posted on 2017-09-03 18:02 Vpegasus 阅读(1913) 评论(0) 编辑收藏举报

刷新页面返回顶部

登录后才能查看或发表评论，立即登录或者逛逛博客园首页

阅读排行：
· 10年+ .NET Coder 心语 ── 封装的思维：从隐藏、稳定开始理解其本质意义
· 地球OL攻略 —— 某应届生求职总结
· 提示词工程——AI应用必不可少的技术
· Open-Sora 2.0 重磅开源！
· 周边上新：园子的第一款马克杯温暖上架

以NASA之名: 卡尔曼滤波器

引言

荣耀骑士

卡尔曼滤波器*

参考文献:

搜索

常用链接

我的标签

随笔档案

阅读排行榜

评论排行榜

推荐排行榜

最新评论