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

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

碎碎念：在忙毕设，慢慢填坑吧

一、参考链接

细说Kalman滤波
卡尔曼滤波 – 从推导到应用(一)
卡尔曼滤波 – 从推导到应用(二)

二、概述

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

三、举例

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

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

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

所以引入测量反馈

我们测量【位移】

测量值有误差噪声

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

建立位移和速度状态模型 $\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}$

观测只看位移 $z_k=\left[\begin{array}{ll}1& 0\end{array}\right]\left[\begin{array}{l}{x}_k\\ {\dot{x}}_k\end{array}\right]$

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

四、公式

1.模型

状态方程：$x_t=A{x}_{t-1}+B{u}_{t-1}+w_{t-1}$

观测方程：$z_t=H{x}_t+v_t$

2.核心方程

通过经验得到预测值

$${\overline{x}}_t=A{\hat{x}}_{t-1}+B{u}_{t-1}$$

$${\overline{P}}_t=A{\hat{P}}_{t-1}{A}^T+Q$$

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

$$K_t={\overline{P}}_t{H}^T(H{\overline{P}}_t{H}^T+R{)}^{-1}$$

$${\hat{x}}_t={\overline{x}}_t+K_t(z_t-H{\overline{x}}_t)$$

$$\widehat{P_t}=(I-K_{t}H){\overline{P}}_t$$

$$z_t=H{x}_t+v_t$$

3.参数说明

$x_t$， $n$ 维向量，表示$t$时刻观测状态的均值

$u_t$， $l$ 维向量，表示$t$时刻的输入

$P_t$， $n\ast n$ 方差矩阵，表示$t$时刻被观测的$n$个状态的方差

$A_t$， $n\ast n$ 矩阵，表示状态从$t-1$到$t$在没有输入影响时转移方式

$B_t$， $n\ast n$ 矩阵，表示$u_t$如何影响$x_t$

$z_t$， $m$ 维向量，表示$t$时刻的观测

$H_t$， $m\ast n$ 矩阵，表示状态$x_t$如何被转换为观测$z_t$

$Q_t$， $n\ast n$ 矩阵，表示过程噪声$w_t$的方差矩阵

$R_t$， $m\ast m$ 矩阵，表示观测噪声$v_t$的方差矩阵

五、推导证明

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

${\hat{P}}_k=E\left[\left(x_k-{\hat{x}}_k\right){\left(x_k-{\hat{x}}_k\right)}^T\right]$

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

${\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$

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

${\hat{P}}_t=(I-K_{t}H){\overline{P}}_k(I-K_{t}H)+K_{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$

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

$Tr[{\hat{P}}_t]=Tr[\widehat{P_t}]-2Tr[K_{t}H{\overline{P}}_t]+Tr[K_t(H{\overline{P}}_t{H}^T+R)K_t^T]$

然后求导，得到

$\frac{dTr[{\hat{P}}_t]}{d{K}_t}=-2(H{\overline{P}}_t{)}^T+2K_t(H{\overline{P}}_t{H}^T+R)$

赋值为0，得到

$K_t={\overline{P}}_t{H}^T(H{\overline{P}}_t{H}^T+R{)}^{-1}$

将$K_t$带入${\hat{P}}_t$ 得到 $\widehat{P_t}=(I-K_{t}H){\overline{P}}_t$

${\overline{P}}_t=E[(x_t-{\overline{x}}_{)}(x_t-{\overline{x}}_t{)}^T]$ 递推的话，

${\overline{P}}_t=E[(x_t-{\overline{x}}_t)(x_t-{\overline{x}}_t{)}^T]=E[(A{x}_{t-1}-A{\overline{x}}_{t-1}+w_{t-1})(A{x}_{t-1}-A{\overline{x}}_{t-1}+w_{t-1}{)}^T]$

这样得到了(2)式

六、代码

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

七、引申

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

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

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

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

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

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

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

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

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

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