卡尔曼滤波系列教程 - 从一阶低通滤波到卡尔曼滤波
一阶低通滤波
一阶低通滤波的公式如下：
$$
y_k = (1 - a) y_{k - 1} + a x_k
$$
该链接中的博客详细介绍了一阶低通滤波的原理以及公式推导（需要一些拉式变换的前置知识），重点理解该一阶低通滤波的截止频率计算，能够辅助进行微分滤波以及PID输出滤波的系数计算。
我们将该公式转化为：
$$
y_k = (1 - a) y_{k - 1} + a x_k = y_{k-1} + a (x_k - y_{k - 1})
$$
如果你熟悉卡尔曼滤波的话，你会发现这与卡尔曼滤波中黄金五式中的最后一式有异曲同工之妙。
$$
\hat{x_k} = \hat{x_k^-} + K_k(z_k - H\hat{x_k^-})
$$
也即一阶低通滤波是卡尔曼增益为a，且卡尔曼增益不变，测量矩阵H为单位阵的特殊一阶卡尔曼滤波。这就是为什么说，若卡尔曼滤波收敛（卡尔曼滤波收敛意味着卡尔曼增益不变），最终卡尔曼滤波效果将等效于低通滤波。
是不是很神奇呢=￣ω￣=，即便是听起来很“高大上”的卡尔曼滤波，本质上也与低通滤波无异(￣y▽,￣)╭ 。
观测器
我们学校的《自动控制原理（二）》课程将会讲述关于状态空间的现代控制理论，里面提到的龙贝格观测器的形式如下：
$$
\dot{\hat{x}} = A\hat{x} + Bu + L(y - C\hat{x})
$$
利用前向欧拉方法将方程离散化有：
$$
\frac{\hat{x_{k+1}} - \hat{x_{k}}}{\Delta t} = A \hat{x_{k}} + Bu_k + L(y_k - C\hat{x_{k}})
$$
化简有：
$$
\hat{x_{k+1}} = (A\Delta t + I)\hat{x_{k}} + B\Delta t u_k + L\Delta t(y_k - C\hat{x_{k}})
$$
你会发现该龙贝格观测器中的$L\Delta t$矩阵相当于卡尔曼滤波中的卡尔曼增益K，但$L\Delta t$矩阵是一个常数矩阵，故龙贝格观测器也是一个简化版的卡尔曼滤波，卡尔曼滤波本质上也是一个观测器。这里推荐从全状态观测器到卡尔曼滤波器该系列教程。
如果你想学习一下现代控制理论内容，推荐该教程Advanced控制理论
知识瞬间串联起来啦😺
卡尔曼滤波
推荐先看卡尔曼滤波系列
卡尔曼滤波黄金五式如下：
预测过程：
$$
\hat{x_k^-} = A \hat{x_{k-1}^-} + B_k u_k
$$
$$
P_k^- = A_k P_{k-1} A_k^T + Q
$$
更新过程：
$$
K_k = \frac{P_k^- H_k^T}{H_k P_k^- H_k^T + R}
$$
$$
P_k = (I - K_k H_k) P_k^-
$$
$$
\hat{x_k} = \hat{x_k^-} + K_k(z_k - H\hat{x_k^-})
$$
首先来看预测过程：
$$
\hat{x_k^-} = A \hat{x_{k-1}^-} + B_k u_k
$$
$$
P_k^- = A_k P_{k-1} A_k^T + Q
$$
何谓预测呢？也即通过上一个时刻的状态通过预先假设的模型来计算下一个时刻的状态，然后通过更新过程检验模型的正确性。所以，该预先假设的模型决定了卡尔曼滤波的上限，越准确的模型卡尔曼滤波上限越高。
$A_k$矩阵为状态转移矩阵，即表达了如何从$\hat{x_{k-1}^-}$到$\hat{x_k^-}$的转化。
$u_k$为控制量，怎么去理解这个控制量呢？也即我们在从$\hat{x_{k-1}^-}$到$\hat{x_k^-}$的转化时，能够对状态变量产生影响，但本身不在状态变量里面的量。例如若选择位移与速度为状态变量，若我们的机器人上有加速度计，则可以将加速度作为$u_k$，这一步目的是使预测过程更加准确。
$P_{k-1}$为$\hat{x_{k-1}^-}$的协方差矩阵，那么进行$A_k \hat{x_{k-1}^-}$操作之后，方差自然变成了$A_k P_{k-1} A_k^T$。
也就是说$P_k^-$的计算之中，没有考虑$u_k$的噪声，也就是说，在设计过程中，$u_k$需要是足够准确的，可以认为是没有噪声的；或者，$u_k$的噪声需要计入Q矩阵中，作为预测过程的不确定性。
关于Q矩阵，它的定义是预测过程的不确定性。当Q越大，那么意味着你对你自己的猜测越没有把握；当Q为0矩阵时，你完全相信你的预测，更新过程将不起作用，最终状态由预测决定；当Q为系数无穷大矩阵时，你完全不相信你的预测，故预测过程将不起作用，最终状态由更新过程决定。Q矩阵需要程序调试者预先设定，注意Q矩阵不能默认为对角阵，应根据各个状态变量之间的关联来进行设定。你永远不知道你的预测有多不准确，故这个参数是需要在每个应用场景中实践去调参得到的。
再看更新过程：
$$
K_k = \frac{P_k^- H_k^T}{H_k P_k^- H_k^T + R}
$$
$$
P_k = (I - K_k H_k) P_k^-
$$
$$
\hat{x_k} = \hat{x_k^-} + K_k(z_k - H\hat{x_k^-})
$$
看一个不严谨的推导，若$K_k = 0$，则$\hat{x_k} = \hat{x_k^-}$，这意味着结果完全由预测决定，测量量$z_k$将不起作用；若$R = 0$,则$K_k = \frac{1}{H_k}$，$\hat{x_k} = \frac{z_k}{H_k}$，此时结果完全由测量过程（也即更新过程）决定。
根据$K_k$的定义，当R越大，$K_k$越小，$\hat{x_k}$就越不相信更新过程。
$z_k$根据定义应该为传感器直接输出的变量，因为传感器直接测量的值和我们设定的状态变量不一定相同，故需要使用H矩阵对$\hat{x_k^-}$转化为传感器测量的量。
R矩阵的定义为$z_k$的协方差，该值和Q不同，它应该是完全确定的。它反映了传感器的精度，可以通过查看传感器的参数手册，或者通过数理统计的方法来计算得到。
卡尔曼滤波实际应用包括卡尔曼滤波设计以及卡尔曼滤波调参，以下一一分析。
卡尔曼滤波设计
根据该卡尔曼滤波的黄金五式，设计卡尔曼滤波核心在于设计$\hat{x_k^-} = A \hat{x_{k-1}^-} + B_k u_k$以及H矩阵。
$\hat{x_k^-} = A \hat{x_{k-1}^-} + B_k u_k$可以根据典型的假设模型，例如恒速度模型（CV运动模型），恒加速度模型（CA运动模型）等进行假设，也可以根据动力学方程等得到。
H矩阵则是根据状态变量到测量变量的转化得到，注意构建起这两者的线性联系。
卡尔曼滤波调参
初值设定
卡尔曼滤波需要给定$x_{0}$以及$P_0$作为初值，一般来说$x_{0}$会以传感器测量的值通过一定运算得到的值作为初值，而$P_0$一般设置为单位阵让其自动迭代收敛即可（也可以根据你对状态变量的理解提前设定好该矩阵的值）。
矩阵调参
需要调整的参数包括Q矩阵以及R矩阵。
R矩阵的调整上面已经说明。
Q矩阵的调整则首先需要确定预测不确定性的来源，例如CV运动模型中带有匀速假设，故预测不确定性来源在于物体运动的加速度，通过加速度将各个状态变量的状态估计协方差联系起来（具体可以看CV运动模型的介绍）。
调参过程需要不断调整Q矩阵来达到满意的效果。
当我们抛去公式推导来看卡尔曼滤波后，是不是觉得卡尔曼滤波瞬间没有那么难了呢<(￣︶￣)↗[GO!]
