avatar

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]卡尔曼增益超详细数学推导

文章作者: 安河桥北以北
文章链接: http://chenai007.github.io/2021/01/26/robomaster/Filter/
版权声明: 本博客所有文章除特别声明外,均采用 CC BY-NC-SA 4.0 许可协议。转载请注明来自 Ash's blog

评论