【理解】Kalman卡尔曼滤波器附python&matlab代码

墨蓝 2022-02-01 10:45 599阅读 0赞

> 一般我是把参考链接放在最后的，但这一次，我放最前排，以表示我对这两位博主的极大感谢，写得真的是太好了！  
> 可能我即将写的这篇是最详细明白的一篇关于卡尔曼滤波器的理解，如果还有疑问，我会补充修正

### 文章目录 ###

*  一、参考链接
 *  二、概述
 *  三、举例
 *  四、公式
 *   *  1.模型
     *  2.核心方程
     *  3.参数说明
 *  五、推导证明
 *  六、代码
 *  七、引申

# 一、参考链接 #

[ 细说Kalman滤波][_Kalman]  
[卡尔曼滤波 – 从推导到应用(一)][Link 1]  
[卡尔曼滤波 – 从推导到应用(二)][Link 2]

# 二、概述 #

**卡尔曼滤波的核心，预测+测量反馈**

# 三、举例 #

一个匀加速运动的小车，系统的状态表示为【位移】+【速度】，控制输入表示为【受力（或者叫力）】

这样貌似已经完全建模出来了，还要卡尔曼干什么？

因为很多实际问题无法完全建模，误差会被累积和放大

所以引入测量反馈

我们测量【位移】

测量值有误差噪声

现在有了理论预测（先验）+测量值（后验）–> 最优的估计值

建立位移和速度状态模型  \[ x t x ˙ t \] = \[ 1 Δ t 0 1 \] \[ x t − 1 x ˙ t − 1 \] + \[ ( Δ t ) 2 2 Δ t \] f t m \\left\[ \\begin\{array\}\{c\}\{x\_\{t\}\} \\\\ \{\\dot\{x\}\_\{t\}\}\\end\{array\}\\right\]=\\left\[ \\begin\{array\}\{cc\}\{1\} & \{\\Delta t\} \\\\ \{0\} & \{1\}\\end\{array\}\\right\] \\left\[ \\begin\{array\}\{c\}\{x\_\{t-1\}\} \\\\ \{\\dot\{x\}\_\{t-1\}\}\\end\{array\}\\right\]+\\left\[\\begin\{array\}\{c\}\{\\frac\{(\\Delta t)^\{2\}\}\{2\}\}\\\\\{\\Delta t\}\\end\{array\}\\right\] \\frac\{f\_\{t\}\}\{m\} \[xtx˙t\]=\[10Δt1\]\[xt−1x˙t−1\]\+\[2(Δt)2Δt\]mft

观测只看位移  z k = \[ 1 0 \] \[ x k x ˙ k \] z\_\{k\}=\\left\[ \\begin\{array\}\{ll\}\{1\} & \{0\}\\end\{array\}\\right\] \\left\[ \\begin\{array\}\{l\}\{x\_\{k\}\} \\\\ \{\\dot\{x\}\_\{k\}\}\\end\{array\}\\right\] zk=\[10\]\[xkx˙k\]

从概率论贝叶斯模型的观点来看前面预测的结果就是先验，测量出的结果就是后验。

# 四、公式 #

## 1.模型 ##

状态方程： x t = A x t − 1 + B u t − 1 + w t − 1 x\_\{t\}=A x\_\{t-1\}+B u\_\{t-1\}+w\_\{t-1\} xt=Axt−1\+But−1\+wt−1

观测方程： z t = H x t + v t z\_\{t\}=H x\_\{t\}+v\_\{t\} zt=Hxt\+vt

## 2.核心方程 ##

通过经验得到预测值

x ‾ t = A x ^ t − 1 + B u t − 1 \\overline\{x\}\_\{t\} = A \\hat\{x\}\_\{t-1\} + B u\_\{t-1\} xt=Ax^t−1\+But−1

P ‾ t = A P ^ t − 1 A T + Q \\overline\{P\}\_t = A \\hat\{P\}\_\{t-1\} A^T + Q Pt=AP^t−1AT\+Q

结合预测值和观测值得到一个更好的估计值

K t = P ‾ t H T ( H P ‾ t H T + R ) − 1 K\_t = \\overline\{P\}\_t H^T(H \\overline\{P\}\_t H^T + R)^\{-1\} Kt=PtHT(HPtHT\+R)−1

x ^ t = x ‾ t + K t ( z t − H x ‾ t ) \\hat\{x\}\_\{t\}=\\overline\{x\}\_\{t\}+K\_\{t\}(z\_\{t\}-H \\overline\{x\}\_\{t\}) x^t=xt\+Kt(zt−Hxt)

P t ^ = ( I − K t H ) P ‾ t \\hat\{P\_\{t\}\}=(I-K\_\{t\} H) \\overline\{P\}\_\{t\} Pt^=(I−KtH)Pt

z t = H x t + v t z\_\{t\}=H x\_\{t\}+v\_\{t\} zt=Hxt\+vt

## 3.参数说明 ##

x t x\_\{t\} xt，  n n n 维向量，表示 t t t时刻观测状态的均值

u t u\_t ut，  l l l 维向量，表示 t t t时刻的输入

P t P\_\{t\} Pt，  n ∗ n n \* n n∗n 方差矩阵，表示 t t t时刻被观测的 n n n个状态的方差

A t A\_t At，  n ∗ n n \* n n∗n 矩阵，表示状态从 t − 1 t−1 t−1到 t t t在没有输入影响时转移方式

B t B\_t Bt，  n ∗ n n \* n n∗n 矩阵，表示 u t u\_t ut如何影响 x t x\_t xt

z t z\_t zt，  m m m 维向量，表示 t t t时刻的观测

H t H\_t Ht，  m ∗ n m∗n m∗n 矩阵，表示状态 x t x\_t xt如何被转换为观测 z t z\_t zt

Q t Q\_t Qt，  n ∗ n n\*n n∗n 矩阵，表示过程噪声 w t w\_\{t\} wt的方差矩阵

R t R\_t Rt，  m ∗ m m\*m m∗m 矩阵，表示观测噪声 v t v\_\{t\} vt的方差矩阵

# 五、推导证明 #

K的存在是为了让估计值和真实值之间的误差尽可能小，所以用协方差来表示

P ^ k = E \[ ( x k − x ^ k ) ( x k − x ^ k ) T \] \\hat\{P\}\_\{k\}=E\\left\[\\left(x\_\{k\}-\\hat\{x\}\_\{k\}\\right)\\left(x\_\{k\}-\\hat\{x\}\_\{k\}\\right)^\{T\}\\right\] P^k=E\[(xk−x^k)(xk−x^k)T\]

代入(4)(6)式，得到

P ^ t = ( I − K t H ) E \[ ( x t − x ‾ t ) ( x t − x ‾ t ) T \] ( I − K t H ) + K t E \[ v t v t T \] K t T \\hat\{P\}\_t = (I-K\_\{t\} H) E\[(x\_\{t\}-\\overline\{x\}\_\{t\})(x\_\{t\}-\\overline\{x\}\_\{t\})^\{T\}\](I-K\_\{t\} H)+K\_\{t\} E\[v\_\{t\} v\_\{t\}^\{T\}\] K\_\{t\}^\{T\} P^t=(I−KtH)E\[(xt−xt)(xt−xt)T\](I−KtH)\+KtE\[vtvtT\]KtT

再引入预测值和真实值之间的协方差，得到

P ^ t = ( I − K t H ) P ‾ k ( I − K t H ) + K t R K t T \\hat\{P\}\_t = (I-K\_\{t\} H) \\overline\{P\}\_k(I-K\_\{t\} H)+K\_\{t\} R K\_\{t\}^\{T\} P^t=(I−KtH)Pk(I−KtH)\+KtRKtT

展开，得到

P ^ t = P ‾ t − K t H P ‾ t − P ‾ t H T K t T + K t ( H P ‾ t H T + R ) K t T \\hat\{P\}\_t = \\overline\{P\}\_t-K\_t H \\overline\{P\}\_t-\\overline\{P\}\_t H^T K\_t^T+K\_\{t\}(H \\overline\{P\}\_t H^T+R) K\_\{t\}^\{T\} P^t=Pt−KtHPt−PtHTKtT\+Kt(HPtHT\+R)KtT

协方差的对角线是方差，所以用最小均方误差的话，应该求 P ^ t \\hat\{P\}\_t P^t的迹

T r \[ P ^ t \] = T r \[ P t ^ \] − 2 T r \[ K t H P ‾ t \] + T r \[ K t ( H P ‾ t H T + R ) K t T \] Tr\[\\hat\{P\}\_t\] = Tr\[\\hat\{P\_t\}\]-2Tr\[K\_t H \\overline\{P\}\_t\]+Tr\[K\_\{t\}(H \\overline\{P\}\_t H^T+R) K\_\{t\}^\{T\}\] Tr\[P^t\]=Tr\[Pt^\]−2Tr\[KtHPt\]\+Tr\[Kt(HPtHT\+R)KtT\]

然后求导，得到

d T r \[ P ^ t \] d K t = − 2 ( H P ‾ t ) T + 2 K t ( H P ‾ t H T + R ) \\frac\{d Tr\[\\hat\{P\}\_\{t\}\]\}\{d K\_\{t\}\}=-2(H \\overline\{P\}\_\{t\})^\{T\}+2 K\_\{t\}(H \\overline\{P\}\_\{t\} H^\{T\}+R) dKtdTr\[P^t\]=−2(HPt)T\+2Kt(HPtHT\+R)

赋值为0，得到

K t = P ‾ t H T ( H P ‾ t H T + R ) − 1 K\_t=\\overline\{P\}\_t H^T(H \\overline\{P\}\_t H^T + R)^\{-1\} Kt=PtHT(HPtHT\+R)−1

将 K t K\_t Kt带入 P ^ t \\hat\{P\}\_t P^t 得到  P t ^ = ( I − K t H ) P ‾ t \\hat\{P\_\{t\}\}=(I-K\_\{t\} H) \\overline\{P\}\_\{t\} Pt^=(I−KtH)Pt

P ‾ t = E \[ ( x t − x ‾ ) ( x t − x ‾ t ) T \] \\overline\{P\}\_t =E\[(x\_\{t\}-\\overline\{x\}\_)(x\_\{t\}-\\overline\{x\}\_\{t\})^T\] Pt=E\[(xt−x)(xt−xt)T\] 递推的话，

P ‾ t = E \[ ( x t − x ‾ t ) ( x t − x ‾ t ) T \] = E \[ ( A x t − 1 − A x ‾ t − 1 + w t − 1 ) ( A x t − 1 − A x ‾ t − 1 + w t − 1 ) T \] \\overline\{P\}\_t = E\[(x\_\{t\}-\\overline\{x\}\_\{t\})(x\_\{t\}-\\overline\{x\}\_\{t\})^T\] = E\[(Ax\_\{t-1\}-A\\overline\{x\}\_\{t-1\}+w\_\{t-1\})(Ax\_\{t-1\}-A\\overline\{x\}\_\{t-1\}+w\_\{t-1\})^T\] Pt=E\[(xt−xt)(xt−xt)T\]=E\[(Axt−1−Axt−1\+wt−1)(Axt−1−Axt−1\+wt−1)T\]

这样得到了(2)式

# 六、代码 #

代码放到[Github][]我觉得更好  
代码是建立在前面的例子上的

# 七、引申 #

阅读了一些相关的论文，说几点  
1.公式变形  
以为不同的模型建立方式，会带来不一样的公式，但不要慌张，我们只需要把握这样几个原则  
对扰动，卡尔曼滤波器的效果会有什么变化吗？

1、如果状态方程建立的不正确？

效果肯定会变差，但建立得越不正确，噪声要设得越大，这样可以使得效果变好

卡尔曼增益K与预测误差协方差矩阵正相关，K、P大意味着更相信测量值

测量系统的协方差矩阵，方差越小，预测结果越接近测量值，

模型系统的噪声越大，预测结果越不稳定，越容易接近模型系统预测值，且单步变化越大，相反，若噪声小，则预测结果与上个计算结果相差不大。

1、Kalman滤波计算快速，计算复杂度为 O ( m 2.376 + n 2 ) O(m^\{2.376\}+n^2) O(m2.376\+n2)，其中 m m m是观测的维数； n n n是状态的个数。

2、对于线性系统，零均值高斯噪声的系统，Kalman是理论上无偏的，最优滤波器。

3、Kalman滤波在实际使用中，要注意参数 R R R和 Q Q Q的调节，这两者实际上是相对的，表示更相信观测还是更相信预测。具体使用时， R R R可以根据过程噪声的幅度决定，然后 Q Q Q可以相对 R R R来给定。当更相信观测时，把 Q Q Q调大，不相信观测时，把 Q Q Q调小。

4、 Q Q Q越小，表示越不相信观测，这是系统状态越容易收敛，对观测的变化响应越慢。 Q Q Q越大，表示越相信观测，这时对观测的变化响应快，但是越不容易收敛。

[_Kalman]: https://www.cnblogs.com/ycwang16/p/5999034.html
[Link 1]: https://blog.csdn.net/heyijia0327/article/details/17487467
[Link 2]: https://blog.csdn.net/heyijia0327/article/details/17667341
[Github]: https://github.com/yzy1996/Python-Code/tree/master/Python%2BAlgorithm/Math/Kalman