卡尔曼滤波学习笔记整理（其一）

最新推荐文章于 2024-09-15 10:16:11 发布

jellyfish_fishman

最新推荐文章于 2024-09-15 10:16:11 发布

阅读量758

点赞数 9

文章标签：机器学习算法人工智能

本文链接：https://blog.csdn.net/jellyfish_song/article/details/137558059

版权

最近在学习卡尔曼滤波相关内容，在此做些整理。

卡尔曼滤波是什么？

卡尔曼滤波（Kalman filter）是一种高效的自回归滤波器，它能在存在诸多不确定性情况的组合信息中估计动态系统的状态，是一种强大的、通用性极强的工具，主要针对线性系统。它通过综合考虑估计值和测量值，迭代给出不确定性最小的数值。

按照我的理解，卡尔曼滤波实质就是

将基于前一时间点的系统状态的给出当前时间点的系统状态的高斯分布预测P1和基于当前时间点的观测量的概率分布Q给出另一组对当前时间点系统状态的高斯分布预测P2，进行数据融合，生成一个新的高斯分布预测，新的预测为更正后的当前时刻下的系统状态的高斯分布，其中新的高斯分布的均值和方差是两个独立的高斯分布的相关参量的加权，这个加权就是卡尔曼增益。不断迭代预测和更正的步骤。

卡尔曼滤波公式一共5条。分为两条预测和三条更正，分别为：

${\hat x_{{k^ - }}} = {{\bf{A}}_k}{\hat x_{k - 1}} + {{\bf{B}}_k}{u_{k - 1}}$ (1)

${P_{{k^ - }}} = {{\bf{A}}_k}{P_{k - 1}}{{\bf{A}}_k}^{\rm{T}} + Q$ (2)

${{\bf{K}}_k} = \frac{{{P_{{k^ - }}}{{\bf{H}}_k}^{\rm{T}}}}{{{{\bf{H}}_k}{P_{{k^ - }}}{{\bf{H}}_k}^{\rm{T}} + R}}$ (3)

${\hat x_k} = {\hat x_{{k^ - }}} + {{\bf{K}}_k}({z_k} - {\bf{H}}{\hat x_{{k^ - }}})$ (4)

${P_k} = (I - {{\bf{K}}_k}{{\bf{H}}_k}){P_{{k^ - }}}$ (5)

卡尔曼滤波的计算步骤如下：

在给定初值 ${x_0},{P_0}$ 的基础上，

基于式（1）式（2）得到先验估计 ${\hat x_{{k^ - }}}$ 和先验误差的协方差矩阵 ${P_{{k^ - }}}$

将 ${P_{{k^ - }}}$ 代入式（3）得到卡尔曼增益 ${{\bf{K}}_k}$

将 ${{\bf{K}}_k}$ 代入式（4）更新得到系统的后验估计 ${\hat x_k}$

将 ${{\bf{K}}_k}$ 代入式（5）更新得到后验误差的协方差矩阵 ${P_k}$

不断迭代向前。

下面是具体的推导过程：

1.首先是系统的先验估计公式

预测方程可以表达为如下状态空间形式

${x_k} = {{\bf{A}}_k}{x_{k - 1}} + {{\bf{B}}_k}{u_{k - 1}} + {w_k}$ （6）

其中x为状态向量，A为状态转移矩阵，B为控制变量矩阵，u为状态控制向量，对于轮轨振动方程而言，u可以视为外激励向量，w为控制系统噪声，且服从高斯分布 ${w_k}\~N(0,Q)$ 。

测量方程可以表示为如下形式：

${z_k} = {{\bf{H}}_k}{x_k} + {v_k}$ （7）

其中z为测量向量，H为转化矩阵，v为控制系统噪声，且服从高斯分布 ${v_k}\~N(0,R)$

由于w,v为未知量，将其去掉，可以得到系统的先验估计与测量估计为

${\hat x_{{k^ - }}} = {{\bf{A}}_k}{\hat x_{k - 1}} + {{\bf{B}}_k}{u_{k - 1}}$ (1-1)

${\hat x_{kmea}} = {{\bf{H}}^ - }{z_k}$ (1-2)

由于去掉了过程噪声，式(1)并非完整预测矩阵和测量方程的，头顶加上^表达结果为估计值,未经处理在右上加上-表示先验，合起来就叫先验估计。

2.其次是系统的后验估计公式

系统的后验估计可以表示为

${\hat x_k} = {\hat x_{{k^ - }}} + G({{\bf{H}}^ - }{z_k} - {\hat x_{{k^ - }}})$ (8)

其中 $G \in [0,1]$ 。不难发现当G=0时， ${\hat x_k} = {\hat x_{{k^ - }}}$ ，当G=1时， ${\hat x_k} = {\hat x_{{k^ - }}}$

令 $G = {{\bf{K}}_k}{\bf{H}}$ ，对式（8）进行变换则有

${\hat x_k} = {\hat x_{{k^ - }}} + {{\bf{K}}_k}({z_k} - {\bf{H}}{\hat x_{{k^ - }}})$ (4)

3.下面推导先验误差的协方差矩阵 ${P_{{k^ - }}}$ ，对先验误差做如下数学变换：

$\begin{array}{l} {e_{{k^ - }}} = {x_k} - {{\hat x}_{{k^ - }}} = {{\bf{A}}_k}{x_{k - 1}} + {{\bf{B}}_k}{u_{k - 1}} + {w_k} - ({{\bf{A}}_k}{{\hat x}_{k - 1}} + {{\bf{B}}_k}{u_{k - 1}})\\ = {{\bf{A}}_k}({x_{k - 1}} - {{\hat x}_{k - 1}}) + {w_k} = {{\bf{A}}_k}{e_{k - 1}} + {w_k} \end{array}$ (9)

将式(9)代入协方差矩阵 ${P_{{k^ - }}}$ ，有：

$\begin{array}{l} {P_{{k^ - }}} = {\mathop{\rm cov}} ({e_{{k^ - }}},{e_{{k^ - }}}) = E[({{\bf{A}}_k}{e_{k - 1}} + {w_k}){({{\bf{A}}_k}{e_{k - 1}} + {w_k})^{\rm{T}}}]\\ = E({{\bf{A}}_k}{e_{k - 1}}{e_{k - 1}}^{\rm{T}}{{\bf{A}}_k}^{\rm{T}}) + E({\bf{A}}{e_{k - 1}}{w_k}^{\rm{T}}) + E({w_k}{e_{k - 1}}^{\rm{T}}{{\bf{A}}_k}^{\rm{T}}) + E({w_k}{w_k}^{\rm{T}})\\ = {{\bf{A}}_k}E({e_{k - 1}}{e_{k - 1}}^{\rm{T}}){{\bf{A}}_k}^{\rm{T}} + 0 + 0 + Q\\ = {{\bf{A}}_k}{P_{k - 1}}{{\bf{A}}_k}^{\rm{T}} + Q \end{array}$ （10）

4.下面推导卡尔曼增益 ${{\bf{K}}_k}$

卡尔曼滤波器的目标是寻找合适的卡尔曼增益 ${{\bf{K}}_k}$ ，使得后验误差 ${e_k} = {x_k} - {\hat x_k}$ 最小，误差满足
${e_k}\~N(0,{P_k})$ 。

易得方差越小，误差越小，而方差最小，也就是协方差矩阵的迹最小。（注1）

推导如下：

对于任意的 ${e_k} = [{e_{k1}},...,{e_{kn}}]$ ，其协方差为

${\mathop{\rm cov}} ({e_k},{e_k}) = E({e_k}e_k^{\rm{T}}) = \left[ {\begin{array}{*{20}{c}} {\sigma e_{k1}^2}&{...}&{\sigma {e_{k1}}\sigma {e_{kn}}}\\ {...}&{...}&{...}\\ {\sigma {e_{kn}}\sigma {e_{k1}}}&{...}&{\sigma e_{kn}^2} \end{array}} \right]$ ,

因此 ${\rm{tr}}({\mathop{\rm cov}} ({e_k},{e_k})) = \sum\limits_1^n {\sigma e_{ki}^2}$ ，其中ki为k时刻的后验误差的第i个分量。

对 ${e_k}$ 做如下数学变换：

$\begin{array}{l} {e_k} = {x_k} - {{\hat x}_k} = {x_k} - ({{\hat x}_{{k^ - }}} + {{\bf{K}}_k}({z_k} - {\bf{H}}{{\hat x}_{{k^ - }}}))\\ = {x_k} - {{\hat x}_{{k^ - }}} - {{\bf{K}}_k}(({{\bf{H}}_k}{x_k} + {v_k}) - {\bf{H}}{{\hat x}_{{k^ - }}})\\ = {x_k} - {{\hat x}_{{k^ - }}} - {{\bf{K}}_k}{{\bf{H}}_k}{x_k} - {{\bf{K}}_k}{v_k} + {{\bf{K}}_k}{\bf{H}}{{\hat x}_{{k^ - }}}\\ = (I - {{\bf{K}}_k}{{\bf{H}}_k})({x_k} - {{\hat x}_{{k^ - }}}) - {{\bf{K}}_k}{v_k}\\ = (I - {{\bf{K}}_k}{{\bf{H}}_k}){e_{{k^ - }}} - {{\bf{K}}_k}{v_k} \end{array}$ （11）

其中 ${e_{{k^ - }}}$ 为先验误差。

将式(11)代入后验误差的协方差矩阵 ${P_k}$ ，有：

$\begin{array}{l} {P_k} = E[{e_k}{e_k}^{\rm{T}}]\\ = E[[(I - {{\bf{K}}_k}{{\bf{H}}_k}){e_{{k^ - }}} - {{\bf{K}}_k}{v_k}]{[(I - {{\bf{K}}_k}{{\bf{H}}_k}){e_{{k^ - }}} - {{\bf{K}}_k}{v_k}]^{\rm{T}}}]\\ = E[[(I - {{\bf{K}}_k}{{\bf{H}}_k}){e_{{k^ - }}} - {{\bf{K}}_k}{v_k}][{e_{{k^ - }}}^{\rm{T}}{(I - {{\bf{K}}_k}{{\bf{H}}_k})^{\rm{T}}} - {v_k}^{\rm{T}}{{\bf{K}}_k}^{\rm{T}}]]\\ = E[(I - {{\bf{K}}_k}{{\bf{H}}_k}){e_{{k^ - }}}{e_{{k^ - }}}^{\rm{T}}{(I - {{\bf{K}}_k}{{\bf{H}}_k})^{\rm{T}}} - {{\bf{K}}_k}{v_k}{e_{{k^ - }}}^{\rm{T}}{(I - {{\bf{K}}_k}{{\bf{H}}_k})^{\rm{T}}}\\ - {({{\bf{K}}_k}{v_k}{e_{{k^ - }}}^{\rm{T}}{(I - {{\bf{K}}_k}{{\bf{H}}_k})^{\rm{T}}})^{\rm{T}}} + {{\bf{K}}_k}{v_k}{v_k}^{\rm{T}}{{\bf{K}}_k}^{\rm{T}}]\\ = E[(I - {{\bf{K}}_k}{{\bf{H}}_k}){e_{{k^ - }}}{e_{{k^ - }}}^{\rm{T}}{(I - {{\bf{K}}_k}{{\bf{H}}_k})^{\rm{T}}}] - E[{{\bf{K}}_k}{v_k}{e_{{k^ - }}}^{\rm{T}}{(I - {{\bf{K}}_k}{{\bf{H}}_k})^{\rm{T}}}]\\ - E[{({{\bf{K}}_k}{v_k}{e_{{k^ - }}}^{\rm{T}}{(I - {{\bf{K}}_k}{{\bf{H}}_k})^{\rm{T}}})^{\rm{T}}}] + E[{{\bf{K}}_k}{v_k}{v_k}^{\rm{T}}{{\bf{K}}_k}^{\rm{T}}] \end{array}$ (12)

由于 ${v_k}{e_{{k^ - }}}$ 独立分布，且期望均为0则式(12)中间两项为0，化简后式(12)可表示为

$\begin{array}{l} {P_k} = E[(I - {{\bf{K}}_k}{{\bf{H}}_k}){e_{{k^ - }}}{e_{{k^ - }}}^{\rm{T}}{(I - {{\bf{K}}_k}{{\bf{H}}_k})^{\rm{T}}}] + E[{{\bf{K}}_k}{v_k}{v_k}^{\rm{T}}{{\bf{K}}_k}^{\rm{T}}]\\ = (I - {{\bf{K}}_k}{{\bf{H}}_k})E({e_{{k^ - }}}{e_{{k^ - }}}^{\rm{T}}){(I - {{\bf{K}}_k}{{\bf{H}}_k})^{\rm{T}}} + {{\bf{K}}_k}E({v_k}{v_k}^{\rm{T}}){{\bf{K}}_k}^{\rm{T}}\\ = (I - {{\bf{K}}_k}{{\bf{H}}_k}){P_{{k^ - }}}{(I - {{\bf{K}}_k}{{\bf{H}}_k})^{\rm{T}}} + {{\bf{K}}_k}R{{\bf{K}}_k}^{\rm{T}}\\ = {P_{{k^ - }}} - {{\bf{K}}_k}{{\bf{H}}_k}{P_{{k^ - }}} - {P_{{k^ - }}}{({{\bf{K}}_k}{{\bf{H}}_k})^{\rm{T}}} + {{\bf{K}}_k}{{\bf{H}}_k}{P_{{k^ - }}}{{\bf{H}}_k}^{\rm{T}}{{\bf{K}}_k}^{\rm{T}} + {{\bf{K}}_k}R{{\bf{K}}_k}^{\rm{T}} \end{array}$ (13)

值得注意的是 ${P_{{k^ - }}} = {P_{{k^ - }}}^{\rm{T}}$ ,所以式(13)可表示为

${P_k} = {P_{{k^ - }}} - {{\bf{K}}_k}{{\bf{H}}_k}{P_{{k^ - }}} - {({{\bf{K}}_k}{{\bf{H}}_k}{P_{{k^ - }}})^{\rm{T}}} + {{\bf{K}}_k}{{\bf{H}}_k}{P_{{k^ - }}}{{\bf{H}}_k}^{\rm{T}}{{\bf{K}}_k}^{\rm{T}} + {{\bf{K}}_k}R{{\bf{K}}_k}^{\rm{T}}$ (14)

进而

$tr({P_k}) = tr({P_{{k^ - }}}) - 2tr({{\bf{K}}_k}{{\bf{H}}_k}{P_{{k^ - }}}) + tr({{\bf{K}}_k}{{\bf{H}}_k}{P_{{k^ - }}}{{\bf{H}}_k}^{\rm{T}}{{\bf{K}}_k}^{\rm{T}}) + tr({{\bf{K}}_k}R{{\bf{K}}_k}^{\rm{T}})$ (15)

欲使 $tr({P_k})$ 最小，也就是令 $tr({P_k})$ 对 ${{\bf{K}}_k}$ 求导为0，即

$\frac{{{\rm{d}}tr({P_k})}}{{{\rm{d}}{{\bf{K}}_k}}} = 0$ (16)

将式(15)代入可得

$\frac{{{\rm{d}}tr({P_k})}}{{{\rm{d}}{{\bf{K}}_k}}} = 0 - 2{({{\bf{H}}_k}{P_{{k^ - }}})^{\rm{T}}} + 2{{\bf{K}}_k}{{\bf{H}}_k}{P_{{k^ - }}}{{\bf{H}}_k}^{\rm{T}} + 2{{\bf{K}}_k}R = 0$ (15)

此处用到了矩阵迹的运算公式

$\frac{{{\rm{d}}tr(AB)}}{{{\rm{d}}A}} = {B^{\rm{T}}},\frac{{{\rm{d}}tr(AB{A^{\rm{T}}})}}{{{\rm{d}}A}} = 2AB$ (16)

通过对式(15)进行数学变换可得最终的 ${{\bf{K}}_k}$

${{\bf{K}}_k} = \frac{{{P_{{k^ - }}}{{\bf{H}}_k}^{\rm{T}}}}{{{{\bf{H}}_k}{P_{{k^ - }}}{{\bf{H}}_k}^{\rm{T}} + R}}$ (17)

5.最后是后验误差的协方差矩阵 ${P_k}$

在得到最优的卡尔曼增益后，将式(17)代入到式(14)，可以发现：

$\begin{array}{l} {P_k} = {P_{{k^ - }}} - {{\bf{K}}_k}{{\bf{H}}_k}{P_{{k^ - }}} - {({{\bf{K}}_k}{{\bf{H}}_k}{P_{{k^ - }}})^{\rm{T}}} + {{\bf{K}}_k}{{\bf{H}}_k}{P_{{k^ - }}}{{\bf{H}}_k}^{\rm{T}}{{\bf{K}}_k}^{\rm{T}} + {{\bf{K}}_k}R{{\bf{K}}_k}^{\rm{T}}\\ = {P_{{k^ - }}} - {{\bf{K}}_k}{{\bf{H}}_k}{P_{{k^ - }}} - {({{\bf{K}}_k}{{\bf{H}}_k}{P_{{k^ - }}})^{\rm{T}}} + \frac{{{P_{{k^ - }}}{{\bf{H}}_k}^{\rm{T}}}}{{{{\bf{H}}_k}{P_{{k^ - }}}{{\bf{H}}_k}^{\rm{T}} + R}}({{\bf{H}}_k}{P_{{k^ - }}}{{\bf{H}}_k}^{\rm{T}} + R){{\bf{K}}_k}^{\rm{T}}\\ = {P_{{k^ - }}} - {{\bf{K}}_k}{{\bf{H}}_k}{P_{{k^ - }}} = (I - {{\bf{K}}_k}{{\bf{H}}_k}){P_{{k^ - }}} \end{array}$ (5)