Kalman Filter
00 前言
在RoboMaster比赛中，由于工业相机帧率、算法效率、信息传输速率等影响，我们通常需要对目标进行预测，才能更好的击中装甲板。
首先感谢robomaster比赛中各个学校开源的代码和各类教程和代码，共同进步，以及感谢卡尔曼滤波作者Rudolf Emil Kalman。
原论文地址：http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf
如本文出现错误欢迎指出，一起学习交流共同进步
01 卡尔曼滤波简介
百度百科：卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
数据滤波是去除噪声还原真实数据的一种数据处理技术，Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。
卡尔曼滤波的应用领域非常广泛，在航空航天、信息技术等领域尤为广泛。
02 卡尔曼滤波概要
由于我们在实际工程中，测量往往存在很多不确定性，这些不确定性包括有1）不存在完美的数学模型；2）系统的扰动是不可控的，也很难建模的；3）测量的传感器本身存在着误差。而影响我们这种误差的称为噪声，例如我们在使用PNP测距的时候，远距离是非常不精确的，它的波动可能会很大，这个时候我们就需要进行一下滤波，如下图所示（蓝色为滤波后数据、红色为实际测量值、绿色为理想测量值）
在我的理解里，卡尔曼滤波就相当于一个带有权重的低通滤波，即调整观测值的权重(测量值)和估计值的权重，是更相信观测值还是更相信估计值的一个过程，观测值*权重+估计值*权重 = 修正值，即最优估计。
Kalman滤波是一种递归过程，主要两个更新过程：时间更新和观测更新，其中时间更新主要包括状态预测和协方差预测，主要是对系统的预测，而观测更新主要包括计算卡尔曼增益、状态更新和协方差更新，因此整个递归过程主要包括五个方面的计算：
1、状态预测；
2、协方差预测；
3、卡尔曼增益
4、状态更新（修正）；
5、协方差更新（修正）；
卡尔曼滤波的适用条件主要有：
1、应用系统必须是线性的；
2、对测量造成影响的噪声必须是符合高斯分布的白噪声。
白噪声：是一种功率谱密度为常数的随机信号或随机过程。即此信号在各个频段上的功率一致。其余的统称为有色噪声
03 卡尔曼公式推导及理解
3.1 基本模型
在介绍卡尔曼公式理解前先介绍一下状态空间方程，便于后面的公式理解。
状态方程：
观测方程：
$xk$：当前$k$时刻的系统状态
$x{k-1}$：$k-1$时刻的系统状态
$A$：传输参数
$B$：控制参数
$u{k-1}$：$k-1$对系统的控制量
$y_k$：$k$时刻的测量值或观测值
$H$：状态转移矩阵
$\omega{k-1}$和$\upsilon_k$：分别为$k-1$时刻的过程噪声和$k$时刻的观测噪声。
过程噪声和观测噪声假设是符合高斯分布，即：
$\omega_k$：过程噪声
$\upsilon_k$：观测噪声
$Q$：过程噪声协方差矩阵
$R$：观测噪声协方差矩阵
3.2卡尔曼公式
时间更新：
状态更新：
$\hat{x}^-k$：当前状态先验估计值
$\hat{x}{k-1}$：上一状态的最优估计
$u{k-1}$：上一状态的系统控制量
$P^-_k$：当前状态先验估计值的协方差
$P{k-1}$：上一状态的协方差修正值
$A$：状态转移矩阵
$B$：控制矩阵
$Q$：过程噪声的方差
$K_k$：当前状态的卡尔曼增益（Kalman Gain）
$H$：观测系统的参数
$R$：测量噪声的方差
$I$：单位矩阵
协方差（$cov(X, Y)$）可以理解为$X、Y$两个变量的数值上的相关性，即正相关还是负相关
均方误差：误差的平方的期望值，也就是多个样本的时候，均分误差等于每个样本的误差的平方乘以该样本出现的概率和
方差：方差是描述随机变量的离散程度，是变量离期望值的距离
两个变量间的协方差：它表示两个变量之间的总体误差，当$Y=X$的时候，就是方差$D(X) = D(Y)$。
我们将均值去掉就成了两个公式相乘，即当样本数据$X$大于自身期望，$Y$也大于自身期望时，那么协方差就为正值，为正相关，反之为负相关。
在现实生活中，我们常常会遇到多维的数据，方差是不足以满足我们的需求的，所以我们要引入协方差来处理多维的数据。
通俗理解就是使用上一个状态的最优估计（后验估计）来得出当前的先验估计，在由当前的观测值来修正得到最优估计，即后验估计的一个递归过程。
3.3卡尔曼公式推导
时间更新公式：
其中$\hat{x}^{-}k = F\hat{x}{k-1}+Bu_{k}$为状态空间方程去除过程噪声得到的先验估计推导公式
关于
我们知道
那么
这个$Q$是过程噪声的协方差，即$cov(\omega_k,\omega_k)$
故
状态更新公式：
关于$\hat{x}_k = \hat{x}^-_k + K_k(z_k - H\hat{x}^-_k)$，我们可以通过一个一维的例子来对该公式进行一个引入，例如：
我们需要测量一个物体的长度，通常我们都会通过多次测量进而取平均值，即
所以
我们将
则
即当前的估计值 = 上一次的估计值+系数x(当前的测量值-上一次的估计值)
而在卡尔曼滤波中，我们将这个系数$K_k$称为卡尔曼增益（Kalman Gain）
而在多维的情况下已知状态空间方程：
而这个$\omega_{k-1}$和$\upsilon_k$是我们没办法建模的，所以，我们只取前面部分作为估计值
即
而
所以有
即当$G = 0$的时候，$\hat{x}_k = \hat{x}^-_k$，即相信估计值，当$G = 1$的时候，$\hat{x}_k = H^-z_k$，即更相信测量值。
而在卡尔曼滤波中，$G = K_kH$，代入就可以得到：
所以当$K_k = 0$时，$\hat{x}_k=\hat{x}^-_k$；当$K_k = H^-$时，$\hat{x}_k = H^-z_k$。
这里我们引入真实值与估计值的误差$e_k$和真实值与先验估计的误差：
我们假设这个误差也符合高斯分布，即$p(e_k)\sim N(0,P)$，这里的协方差矩阵$P = E[ee^T]$
重点：如果我们新估计的结果$\hat{x}_k$与实际值$x_k$最小呢，也就是说它整个的误差的方差最小，方差越小，那么它的期望值越接近于0，所以我们需要选取一个合适的值，使得它的协方差矩阵$P$的迹$tr(P)$最小。
已知$e_k = x_k - \hat{x}_k$，那么
因为我们要求$K_k$，那么我们将$\hat{x}_k = \hat{x}^-_k + K_k(z_k - H\hat{x}^-_k)$代入上式中
先化简一下$x_k-\hat{x}_k$：
那么
第二项
因为$e^-_k$与$v^T_k$是相互独立的，即先验误差和测量误差是没有什么关系的，所以
又因为$p(v^T_k)\sim N(0, R_k)$以及$p(e^-_k)\sim N(0,P)$，所以
所以
同理第三项
故
由于$P_k=E(e_ke^T_k)$，则
所以代入得
又因为$p(v_k)\sim N(0, R_k)$，即$E(v_kv^T_k) = R_k$，所以
我们要求的是$tr(P_k)$，因为第三项的转置
又因为协方差矩阵是对称矩阵，所以$P^{-T}_k=P^-_k$，所以第二项和第三项互为转置，又因为互为转置，那么对角线上的元素是一样的，故$tr(K_kHP^-_k)=tr(P^-_kH^TK^T_k)$，所以
由于目标是寻找$K_k$使得$tr(P_k)$有最小值，所以我们令$tr(P_k)$对$K_k$求导，令其等于0，即
所以
这里需要一个用到两个矩阵微分公式
公式一：公式二：
所以
令其为0
所以
由上面可知
所以当$R$特别大的时候，即测量噪声特别大时，$K_k\rightarrow 0$，这时$\hat{x}_k = \hat{x}^-_k$；
当$R$特别小的时候，即$R\rightarrow0$，这时$K_k\rightarrow H^-$,这时$\hat{x}_k = H^-z_k$。
由上面的时间更新的推导，已知
即
下面再推导一下$P_k$的修正公式
由刚刚推导$K_k$时的结论可知
我们将
代入上式
故
到现在我们卡尔曼滤波的五个公式就全部推到完成了
|: 时间更新 :|: 状态更新 :|
|—|—|
| $\hat{x}^-k=A\hat{x}{k-1}+Bu{k-1}$ | $K_k = P^-_kH^{T}(HP^-_kH^{T} + R)^{-1}$ |
| $P^-_k=AP{k-1}A^T+Q$ | $\hat{x}_k = \hat{x}^-_k + K_k(z_k - H\hat{x}^-_k)$ |
|| $P_k = (I - K_kH)P^-_k$ |
04卡尔曼滤波超参数调节
已知
将$P^-_k$代入$K_k$中
所以我们知道，$Kalman$ $Gain$的调节和过程噪声$Q$以及观测噪声$R$都有关系
再集合最优估计（后验估计）公式：
1）当我们要更信任观测值$z_k$时，那么我们需要将$K_k$增大，所以可以将观测噪声$R$调小
2）当我们要更信任估计值$\hat{x}_k$时，那么我们需要将$K_k$调小，所以可以将$R$调大，也可以将$Q$调大
关于$\hat{x}_0$的取值和$P_0$的取值，我们习惯将$\hat{x}_0=0,P_0=1$
05 实际应用举例-RoboMaster预测跟踪
5.1 建立卡尔曼滤波器模型
我们将我们能获取到的状态向量定义为$x=\begin{bmatrix} \theta{pitch} \ \theta{yaw} \ \omega{pitch} \ \omega{yaw} \end{bmatrix}$，控制向量$u=\begin{bmatrix} a{pitch} \ a{yaw} \end{bmatrix}$，观测向量定义为$z=\begin{bmatrix} \theta{pitch} \ \theta{yaw} \ \omega{pitch} \ \omega{yaw} \end{bmatrix}$。
1）状态预测：
根据变速运动模型运动方程：$\thetak=\theta{k-1}+\omega \Delta t+a\Delta t^2$，得时间更新方程，角度的先验估计值：
$A$为状态转移矩阵（state transition matrix），$B$为控制矩阵（control matrix）
2）状态协方差矩阵（state covariance matrix）预测：
$Q$为过程噪声（process covariance matrix）的协方差矩阵，即$cov(\omega_k, \omega_k)=E(\omega_k\omega^T_k)$
3）更新卡尔曼增益
这是卡尔曼滤波器中最重要的一个值——卡尔曼增益（Kalman Gain）。$R$为测量噪声的协方差矩阵（measurement covariance matrix），表示测量值与真实值之间的差距。
4）状态更新：
完成了当前状态向量$x$的更新，不仅考虑了上一时刻的预测值，也考虑了测量值，和整个系统的噪声。
5）协方差更新：
根据卡尔曼增益，更新系统的协方差$Pk$用于下一个周期中协方差（$P^-{k+1}$）预测的求解。
5.2 核心思想
核心思想：通过陀螺仪当前角度与装甲板相对云台角度的融合，得到装甲板在陀螺仪零轴上基坐标系上的绝对角度，将该角度输入至卡尔曼滤波器作为初始状态向量的角度值，之后主要作为观测向量的角度值。利用时间信息，经过新旧观测值相减，可以得到速度和加速度，输入到目标模型，目标运动模型选用变加速度模型，经过卡尔曼滤波器的更新预测，输出预测角度，实现对装甲板目标的预测跟踪。
5.3 算法流程
算法流程：
- 1、 识别到装甲板，利用 PnPSolver 算法解算得到目标在相机坐标系的坐标，再转换到机器人云台坐标系上，利用该坐标得到目标相对机器人云台中心的 Pitch 和 Yaw 角度；
- 2、 从串口读取接收电控发送到上位机来的 Pitch 和 Yaw 角（陀螺仪）；
- 3、 验证电控角度是否正确（排除 NAN 值）；
- 4、 陀螺仪角度与目标相对云台中心的角度融合，得到目标在陀螺仪零轴上基坐标系上的绝对角度；
- 5、 输入卡尔曼滤波器进行预测：
- 5.1、第一次进入：初始化过程噪声矩阵 Q、测量噪声矩阵 R、测量矩阵 H、误差协方差矩阵 P；更新计时器 t1、状态向量 x；
- 5.2、其余时候：更新计时器 t2、计算 t1 与 t2 的间隔时间 diff_time，更新计时器 t1。更新观测向量 z（里面包括角度和速度）、状态转移矩阵 A、控制矩阵 B 和控制向量 u。
- 5.3、每一次最终会输出后验估计状态向量 x，其中的角度信息即作为预测角度；
- 6、重力补偿；
- 7、写入 Pitch 和 Yaw 角度值至串口，发送至下位机。
5.4 自瞄预测流程图
参考文献/文章
[1]卡尔曼滤波算法详细推导
[2]A New Approach to Linear Filtering and Prediction Problems
[3]卡尔曼滤波详解
[4]卡尔曼增益超详细数学推导
