大家好，欢迎来到IT知识分享网。
目录
卡尔曼滤波
卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准。卡尔曼滤波得名自主要贡献者之一的鲁道夫·卡尔曼。
卡尔曼滤波的算法是二步骤的程序。在估计步骤中，卡尔曼滤波会产生有关目前状态的估计，其中也包括不确定性。只要观察到下一个量测（其中一定含有某种程度的误差，包括随机噪声）。会通过加权平均来更新估计值，而确定性越高的量测加权比重也越高。算法是迭代的，可以在实时控制系统中执行，只需要目前的输入量测、以往的计算值以及其不确定性矩阵，不需要其他以往的资讯。
应用实例
卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，通过对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。在很多工程应用（如雷达、机器视觉）中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题。
例如，对于雷达来说，人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计（滤波），也可以是对于将来位置的估计（预测），也可以是对过去位置的估计（插值或平滑）。
基本动态系统模型
卡尔曼滤波建立在线性代数和隐马尔可夫模型（hidden Markov model）上。其基本动态系统可以用一个马尔可夫链表示，该马尔可夫链建立在一个被高斯噪声（即正态分布的噪声）干扰的线性算子上的。系统的状态可以用一个元素为实数的向量表示。随着离散时间的每一个增加，这个线性算子就会作用在当前状态上，产生一个新的状态，并也会带入一些噪声，同时系统的一些已知的控制器的控制信息也会被加入。同时，另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。
为了从一系列有噪声的观察数据中用卡尔曼滤波器估计出被观察过程的内部状态，必须把这个过程在卡尔曼滤波的框架下建立模型。也就是说对于每一步k，定义矩阵 F k F_k Fk, H k H_k Hk, Q k Q_k Qk, R k R_k Rk，有时也需要定义 B k B_k Bk，如下。
卡尔曼滤波模型假设 k + 1 k+1 k+1时刻的真实状态是从 k k k时刻的状态演化而来，符合状态方程：
x k = F k x k − 1 + B k u k + w k x_{k}=F_kx_{k-1}+B_{k}u_{k}+w_{k} xk=Fkxk−1+Bkuk+wk
其中
- F k F_k Fk是作用在 x k − 1 x_{k-1} xk−1上的状态变换模型（矩阵或向量）。
- B k B_k Bk是作用在控制器向量 u k u_k uk上的输入-控制模型。
- w k w_k wk是过程噪声，并假定其符合均值为零，协方差矩阵为 Q k Q_k Qk的多元正态分布。
w k ∼ N ( 0 , Q k ) w_k{\sim} N(0,Q_k) wk∼N(0,Qk)
其中 c o v { w ( k ) } = Q ( k ) cov\{w(k)\}=Q(k) cov{
w(k)}=Q(k)表示协方差矩阵。
时刻k，对真实状态 x k x_k xk的一个测量 z k z_k zk满足下式，即观测方程：
z k = H k x k + v k z_k=H_kx_k+v_k zk=Hkxk+vk
其中 H k H_k Hk是观测模型，它把真实状态空间映射成观测空间， v k v_k vk是观测噪声，其均值为零，协方差矩阵为 R k R_k Rk且服从正态分布。
v k ∼ N ( 0 , R k ) v_k{\sim}N(0,R_k) vk∼N(0,Rk)
其中 c o v { v ( k ) } = R ( k ) cov\{v(k)\}=R(k) cov{
v(k)}=R(k)表示协方差矩阵。
初始状态以及每一时刻的噪声 { x 0 , w 1 , … , w k , v 1 , … , v k } \{x_0,w_1,…,w_k,v_1,…,v_k\} {
x0,w1,…,wk,v1,…,vk}都认为是互相独立的。
算法逻辑
卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之处，在于它是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计再转换到时域实现。
卡尔曼滤波主要分为预测和更新两部分，需要构建了系统的状态方程和观测方程，且已知系统的初始状态。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。
卡尔曼滤波器的状态由以下两个变量表示：
- X ^ k ∣ k = E ( X k ∣ Y 1 , Y 2 , … , Y k ) \hat{X}_{k|k}=E(X_k|Y_1,Y_2,…,Y_k) X^k∣k=E(Xk∣Y1,Y2,…,Yk)表示在时刻k的状态的估计。
- X ^ k ∣ k − 1 = E ( X k − 1 ∣ Y 1 , Y 2 , … , Y k − 1 ) \hat{X}_{k|k-1}=E(X_{k-1}|Y_1,Y_2,…,Y_{k-1}) X^k∣k−1=E(Xk−1∣Y1,Y2,…,Yk−1)表示已知过去k-1个时刻的状态，对k时刻状态的预测。
- P ^ k ∣ k \hat{P}_{k|k} P^k∣k，后验估计误差协方差矩阵，度量估计值的精确程度。
1. 预测 X ^ k − 1 ∣ k − 1 ⇒ X ^ k ∣ k − 1 \hat{X}_{k-1|k-1}\Rightarrow\hat{X}_{k|k-1} X^k−1∣k−1⇒X^k∣k−1
2. 更新 X ^ n ∣ n − 1 ⇒ X ^ n ∣ n \hat{X}_{n|n-1}\Rightarrow\hat{X}_{n|n} X^n∣n−1⇒X^n∣n
3. 卡尔曼滤波过程
公式推导
1. 符号定义
状态方程
x k = F k x k − 1 + B k u k + w k x_{k}=F_kx_{k-1}+B_{k}u_{k}+w_{k} xk=Fkxk−1+Bkuk+wk
其中 u k u_k uk为输入信号， x k − 1 x_{k-1} xk−1为状态变量， w k w_k wk表示过程噪声。其中 c o v { w ( k ) } = Q ( k ) cov\{w(k)\}=Q(k) cov{
w(k)}=Q(k)。
观测方程
z k = H k x k + v k z_k=H_kx_k+v_k zk=Hkxk+vk
其中 z k z_k zk为观测变量， v k v_k vk表示观测噪声。其中 c o v { v ( k ) } = R ( k ) cov\{v(k)\}=R(k) cov{
v(k)}=R(k)。
状态估计误差
x ~ k ∣ k − 1 = x k − x ^ k ∣ k − 1 \tilde{x}_{k|k-1}=x_k-\hat{x}_{k|k-1} x~k∣k−1=xk−x^k∣k−1
其中 x ~ k ∣ k − 1 \tilde{x}_{k|k-1} x~k∣k−1表示估计在第 k − 1 k-1 k−1时刻对 x k x_k xk的估计。在第k-1时刻的时候，对k时刻的任何值，都只能是估计（预测未来值）。
状态估计方程
x ^ k ∣ k − 1 = F k x ^ k − 1 + B k u k \hat{x}_{k|k-1}=F_k\hat{x}_{k-1}+B_{k}u_{k} x^k∣k−1=Fkx^k−1+Bkuk
观测估计误差
e ~ k = z k − z ^ k ∣ k − 1 \tilde{e}_{k}=z_{k}-\hat{z}_{k|k-1} e~k=zk−z^k∣k−1
观测估计方程
z ^ k ∣ k − 1 = H k x ^ k ∣ k − 1 \hat{z}_{k|k-1}=H_k\hat{x}_{k|k-1} z^k∣k−1=Hkx^k∣k−1
基于上述公式，可定义两个重要的误差协方差矩阵：
状态误差协方差矩阵
P k ∣ k − 1 = c o v { x ~ k ∣ k − 1 } P_{k|k-1}=cov\{\tilde{x}_{k|k-1}\} Pk∣k−1=cov{
x~k∣k−1}
观测误差协方差矩阵
S K = c o v { e ~ k } S_{K}=cov\{\tilde{e}_{k}\} SK=cov{
e~k}
卡尔曼滤波的目的是得到一个基于误差能够不断修正的迭代估计表达式，其具体形式应该如下：
x ^ k ∣ k = x ^ k ∣ k − 1 + K k e ~ k \hat{x}_{k|k}=\hat{x}_{k|k-1}+K_k\tilde{e}_k x^k∣k=x^k∣k−1+Kke~k
即基于误差对预测值进行修正。这里的 K k K_k Kk就是卡尔曼增益。因此，我们需要推导出所用变量的迭代式，并求得卡尔曼增益。
2. 推导
推导误差协方差矩阵
状态误差协方差矩阵：
P k ∣ k − 1 = c o v { x k − x ^ k ∣ k − 1 } = c o v { F k x k − 1 + B k u k + w k − F k x ^ k − 1 − B k u k } = c o v { F k ( x k − 1 − x ^ k − 1 ) + w k } = c o v { F k ( x k − 1 − x ^ k − 1 ) } + c o v { w k } = F k c o v { x k − 1 − x ^ k − 1 } F k T + Q k = F k P k ∣ k F k T + Q k \begin{align}P_{k|k-1}&=cov\{x_k-\hat{x}_{k|k-1}\}\\ &=cov\{F_kx_{k-1}+B_ku_k+w_k-F_k\hat{x}_{k-1}-B_{k}u_{k}\}\\ &=cov\{F_k(x_{k-1}-\hat{x}_{k-1})+w_k\}\\ &=cov\{F_k(x_{k-1}-\hat{x}_{k-1})\}+cov\{w_k\}\\ &=F_kcov\{x_{k-1}-\hat{x}_{k-1}\}F^T_k+Q_k\\ &=F_kP_{k|k}F^T_k+Q_k \end{align} Pk∣k−1=cov{
xk−x^k∣k−1}=cov{
Fkxk−1+Bkuk+wk−Fkx^k−1−Bkuk}=cov{
Fk(xk−1−x^k−1)+wk}=cov{
Fk(xk−1−x^k−1)}+cov{
wk}=Fkcov{
xk−1−x^k−1}FkT+Qk=FkPk∣kFkT+Qk
这里我们得到了 P k ∣ k P_{k|k} Pk∣k 到 P k ∣ k − 1 P_{k|k-1} Pk∣k−1的一个递推形式， P k ∣ k P_{k|k} Pk∣k的标记形式是为了更方便后面的推导，即为了得到
P k − 1 ∣ k − 1 → P k ∣ k − 1 → P k ∣ k \begin{align} P_{k-1|k-1}\rightarrow P_{k|k-1}\rightarrow P_{k|k} \end{align} Pk−1∣k−1→Pk∣k−1→Pk∣k
同理可得观测误差协方差矩阵的递推形式：
S k = H k P k ∣ k − 1 H k T + R k \begin{align} S_{k}=H_kP_{k|k-1}H^T_k+R_k \end{align} Sk=HkPk∣k−1HkT+Rk
能够发现， S k S_k Sk和 S k − 1 S_{k-1} Sk−1没有直接的联系，这就是为什么不把 S k S_k Sk写成 S k ∣ k − 1 S_{k|k-1} Sk∣k−1的形式。
因此，目前还缺少 P k ∣ k − 1 → P k ∣ k P_{k|k-1}\rightarrow P_{k|k} Pk∣k−1→Pk∣k的递推式。
推导后验协方差矩阵
由定义可知，误差协方差 P k ∣ k P_{k|k} Pk∣k的定义如下，代入上述定义逐行推导化简即可。
P k ∣ k = c o v { x ~ k ∣ k } = c o v { x k − x ^ k ∣ k } = c o v { x k − ( x ^ k ∣ k − 1 + K k e ~ k ) } = c o v { x k − ( x ^ k ∣ k − 1 + K k ( z k − z ^ k ∣ k − 1 ) ) } = c o v { x k − ( x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) ) } = c o v { x k − ( x ^ k ∣ k − 1 + K k ( H k x k + v k − H k x ^ k ∣ k − 1 ) ) } = c o v { ( x k − x ^ k ∣ k − 1 ) − K k H k ( x k − x ^ k ∣ k − 1 ) − K k v k } = c o v { ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) − K k v k } = c o v { ( I − K k H k ) ( x k − x ^ k ∣ k − 1 ) } + c o v { K k v k } = ( I − K k H k ) c o v { ( x k − x ^ k ∣ k − 1 ) } ( I − K k H k ) T + K k c o v { v k } K k T = ( I − K k H k ) P k ∣ k − 1 ( I − K k H k ) T + K k R k K k T \begin{align} P_{k|k}&=cov\{\tilde{x}_{k|k}\} \\&=cov\{x_k-\hat{x}_{k|k}\} \\&=cov\{x_k-(\hat{x}_{k|k-1}+K_k\tilde{e}_k)\} \\&=cov\{x_k-(\hat{x}_{k|k-1}+K_k(z_{k}-\hat{z}_{k|k-1}))\} \\&=cov\{x_k-(\hat{x}_{k|k-1}+K_k(z_{k}-H_k\hat{x}_{k|k-1}))\} \\&=cov\{x_k-(\hat{x}_{k|k-1}+K_k(H_kx_k+v_k-H_k\hat{x}_{k|k-1}))\} \\&=cov\{(x_k-\hat{x}_{k|k-1})-K_kH_k(x_k-\hat{x}_{k|k-1})-K_kv_k\} \\&=cov\{(I-K_kH_k)(x_k-\hat{x}_{k|k-1})-K_kv_k\} \\&=cov\{(I-K_kH_k)(x_k-\hat{x}_{k|k-1})\}+cov\{K_kv_k\} \\&=(I-K_kH_k)cov\{(x_k-\hat{x}_{k|k-1})\}(I-K_kH_k)^T+K_kcov\{v_k\}K_k^T \\&=(I-K_kH_k)P_{k|k-1}(I-K_kH_k)^T+K_kR_kK_k^T \end{align} Pk∣k=cov{
x~k∣k}=cov{
xk−x^k∣k}=cov{
xk−(x^k∣k−1+Kke~k)}=cov{
xk−(x^k∣k−1+Kk(zk−z^k∣k−1))}=cov{
xk−(x^k∣k−1+Kk(zk−Hkx^k∣k−1))}=cov{
xk−(x^k∣k−1+Kk(Hkxk+vk−Hkx^k∣k−1))}=cov{(xk−x^k∣k−1)−KkHk(xk−x^k∣k−1)−Kkvk}=cov{(I−KkHk)(xk−x^k∣k−1)−Kkvk}=cov{(I−KkHk)(xk−x^k∣k−1)}+cov{
Kkvk}=(I−KkHk)cov{(xk−x^k∣k−1)}(I−KkHk)T+Kkcov{
vk}KkT=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkT
其中，测量误差 v k v_k vk与其他项非相关，因此可以单独提出来。这一公式对任何卡尔曼增益 K k K_k Kk都成立，如果 K k K_k Kk是最优卡尔曼增益，则可以进一步简化。
最优卡尔曼增益的推导
卡尔曼滤波器是一个最小均方误差估计器，后验状态误差估计是 x k − x ^ k ∣ k x_k-\hat{x}_{k|k} xk−x^k∣k
最小化这个矢量幅度平方的期望值 E [ ∣ X k − x ^ k ∣ k ∣ 2 ] E[|X_k-\hat{x}_{k|k}|^2] E[∣Xk−x^k∣k∣2]，这等同于最小化后验估计协方差矩阵 P k ∣ k P_{k|k} Pk∣k的迹。将上面方程中的项展开、抵消得到：
P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 H k T K k T + K k H k P k ∣ k − 1 H k T K k T + K k R k K k T = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 H k T K k T + K k ( H k P k ∣ k − 1 H k T + R k ) K k T = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 H k T K k T + K k S k K k T \begin{align} P_{k|k}&=P_{k|k-1}-K_kH_kP_{k|k-1}-P_{k|k-1}H^T_kK^T_k+K_kH_kP_{k|k-1}H^T_kK^T_k+K_kR_kK^T_k \\&=P_{k|k-1}-K_kH_kP_{k|k-1}-P_{k|k-1}H^T_kK^T_k+K_k(H_kP_{k|k-1}H^T_k+R_k)K^T_k \\&=P_{k|k-1}-K_kH_kP_{k|k-1}-P_{k|k-1}H^T_kK^T_k+K_kS_kK^T_k \end{align} Pk∣k=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1HkTKkT+KkHkPk∣k−1HkTKkT+KkRkKkT=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1HkTKkT+Kk(HkPk∣k−1HkT+Rk)KkT=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1HkTKkT+KkSkKkT
当矩阵导数为0的时候得到 P k ∣ k P_{k|k} Pk∣k的迹（trace）的最小值：
d t r ( P k ∣ k ) d K k = − 2 ( H k P k ∣ k − 1 ) T + 2 K k S k = 0 \begin{align} \frac{\mathrm{d}tr(P_{k|k})}{\mathrm{d}K_k}=-2(H_kP_{k|k-1})^T+2K_kS_k=0 \end{align} dKkdtr(Pk∣k)=−2(HkPk∣k−1)T+2KkSk=0
此处须用到一个常用的式子，如下：[参考1][参考2][参考3]
d t r ( B A C ) d A = B T C T \begin{align} \frac{\mathrm{d}tr(BAC)}{\mathrm{d}A}=B^TC^T \end{align} dAdtr(BAC)=BTCT
则可解得卡尔曼增益：
K k = P k ∣ k − 1 H k T S k − 1 \begin{align} K_k=P_{k|k-1}H^T_kS^{-1}_k \end{align} Kk=Pk∣k−1HkTSk−1
这个增益称为最优卡尔曼增益，在使用时得到最小均方误差。
后验误差协方差公式的化简
在最优卡尔曼增益公式两侧都乘以 S k K k T S_kK^T_k SkKkT得到
K k S k K k T = P k ∣ k − 1 H k T K k T \begin{align} K_kS_kK^T_k=P_{k|k-1}H^T_kK^T_k \end{align} KkSkKkT=Pk∣k−1HkTKkT
根据后验误差协方差展开公式，有
P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 H k T K k T + K k S k K k T = P k ∣ k − 1 − K k H k P k ∣ k − 1 \begin{align} P_{k|k}&=P_{k|k-1}-K_kH_kP_{k|k-1}-P_{k|k-1}H^T_kK^T_k+K_kS_kK^T_k \\&=P_{k|k-1}-K_kH_kP_{k|k-1} \end{align} Pk∣k=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1HkTKkT+KkSkKkT=Pk∣k−1−KkHkPk∣k−1
实际中由于计算简单，往往使用这个公式，但需要注意该公式仅在使用最优卡尔曼增益的时候它才成立。如果算术精度总是很低而导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，那么就不能使用这个简化；必须使用上面导出的后验误差协方差公式。
参考文献
An Introduction to the Kalman Filter
可能是讲解最清楚的Kalman filter
【EKF】卡尔曼滤波原理
卡尔曼滤波wiki百科
卡尔曼滤波最完整公式推导
矩阵的迹(Trace)及相关性质证明
迹-wiki百科
【机器学习】汇总详解：矩阵的迹以及迹对矩阵求导
免责声明：本站所有文章内容,图片，视频等均是来源于用户投稿和互联网及文摘转载整编而成，不代表本站观点，不承担相关法律责任。其著作权各归其原作者或其出版社所有。如发现本站有涉嫌抄袭侵权/违法违规的内容,侵犯到您的权益，请在线联系站长,一经查实,本站将立刻删除。 本文来自网络,若有侵权，请联系删除，如若转载，请注明出处：https://haidsoft.com/129894.html
