使用GCK搭建卡尔曼滤波工程

摘要：本文从入门介绍了卡尔曼滤波的功能和适用系统，从数学公式上描述了卡尔曼滤波的概念。卡尔曼滤波可以处理多变量非平稳随机过程滤波问题，可处理时变系统滤波问题。将真实数据和噪声数据在GCKontrol上搭建的工程运行，调整卡尔曼增益，使得估计值趋于真实值。

已知有一个距离为5米的固体，图示为每毫秒从超声波距离传感器传入的距离，可以看到由于多种原因，传入的距离并不是真实值。

现在引入滤波，可以看到数据更平滑（红色点）且更接近真实值，由此可知滤波可以去掉其中的一些噪声，使得信号更趋于真实值。

卡尔曼滤波适用于线性高斯系统。

（1）线性

 线性一般指线性映射，这个映射同时满足两个条件：

 ①  叠加性

左侧的x1与x2作为输入作用于系统A上，输出为y；

右侧的x1与x2作为输入分别作用于系统B、C上，输出为y1 与y2，y1与y2相加输出y。从方程上可以理解为y=ax1+bx2。

②  齐次性

当输入x增大k倍时，输出也相应增大k倍。

（2）高斯

噪声满足正态分布，均值为μ，方差为σ

（1）状态方程

（2）观测工程

Kalman滤波是在时域上运用状态空间，递推得到的一种滤波算法，便于在计算机上实时实现，计算量和存储量小。该方法可处理多变量非平稳随机过程滤波问题，可处理时变系统滤波问题。例如飞机在飞行过程中，遇到的干扰通常是时变非平稳的噪声，此时运用卡尔曼滤波可有效去除干扰，得到较真实的状态估计数据。

本示例中采用kalman滤波估计火箭的高度，火箭上的机载高度表提供其高度信息，高度表提供的高度信息具有噪声，需要经过kalman滤波去除干扰噪声，得到比较真实的火箭高度信息。

加速度计不会感知重力加速度。如果加速度计放置在桌面上，它会输出1g的重力加速度；如果在自由落体的时候，其输出值为0。所以火箭的真实的加速度为加速度计的测量值减去重力加速度g， 所以加速读计的测量值可以描述为：

转移方程的一般形式如下：

其中：

系统状态的定义如下：

其中

可以得到状态转移方程如下：

上式中：

协方差矩阵一般形式如下：

式中：

P矩阵的形式如下：

矩阵中对角线元素为估计值的方差：

假设的噪声模型的是离散的，每个时间段的噪声都不相同，但是在时间段之间他是恒定的。对于一个加速度为定值的模型，其过程噪声矩阵如下：

在示例工程中Q矩阵如下所示：

其中：

在本例中则有：

观测矩阵的一般形式为：

式中：

测量结果只为我们提供了火箭的高度：

测量值的协方差矩阵为：

上式中，下标m代表测量值的不确定性。为了简化，这里假设偏差为恒值：

卡尔曼增益计算公式如下：

式中：

状态矩阵更新公式如下：

协方差矩阵更新公式如下：

式中：

在上面完成了kalman 滤波的方程推导，本节以火箭高度为示例进行的具体的计算。火箭配备了一个提供高度测量的高度计和一个作为控制输入的加速度计。

转移矩阵F：

控制矩阵G：

过程噪声矩阵Q：

测量误差矩阵R：

单元矩阵I：

观测矩阵H：

模型架构如下图所示：

在Kalman_matrix模块中完成了模型所需矩阵和参数的定义：

观测数据measurements由真实数据加噪声数据叠加而成：

P_matrix_update中完了对协防差矩阵的更新，具体的更新公式参照5.2 和5.8 小结：

 K_matrix_update模块中完成了增益矩阵的迭代更新，具体公式参看 5.6 小结：

X_matrix_update中完成了状态矩阵的迭代更新，具体公式参看5.1和5.7 小结：

曲线区域一的测量值为真实数据加噪声数据叠加的数据；真实值为真实数据；估计值为通过卡尔曼增益，调节超参数后取得的数据。

曲线区域二为卡尔曼增益值，由于协方差矩阵降低，卡尔曼增益值也随之降低。

为了能够很好的对无人机的实时状态进行测量和估计，通常采用的是卡尔曼滤波器。卡尔曼滤波器要求系统的噪声误差满足高斯分布，在这个条件下，卡尔曼滤波器可以在此线性状态空间上将上一时刻的最优估计和当前时刻的加速度计、陀螺仪等传感器所测量到的数据进行融合得到当前时刻的预测值，再将当前时刻的预测值与磁罗盘、GPS等传感器所测量得到的数据进行融合得到当前时刻的最优估计。

雷达目标跟踪环节的性能直接决定雷达系统的安全效能。在雷达跟踪系统中，我们所用到的是离散型卡尔曼滤波。雷达在搜索到目标并记录目标的位置数据，对测量到的目标位置数据（称为点迹）进行处理，自动形成航迹，并对目标在下一时刻的位置进行预测。由于卡尔曼滤波器在状态估计与预测方面具有强大的性能，因此在目标跟踪领域有广泛应用，同时也是是现阶段雷达中最常用的跟踪算法。

卡尔曼滤波利用系统噪声和观测噪声的统计特性，以系统的观测量作为滤波器的输入，以所要估计值（系统的状态或参数）作为滤波器的输出，滤波器的输入与输出之间是由时间更新和观测更新算法联系在一起的，能显著提高所要估计值的估计精度，对两航天器间的相对位置的解算精度都达到了厘米级。

在自主车辆的情况下，可以使用卡尔曼滤波器进行有根据的猜测，对某些动态系统有不确定信息的任何地方，决定系统将要做什么。卡尔曼滤波器可用于根据车辆接收的数据预测自动驾驶车辆前方的车辆将采取的下一组动作。它只根据以前的状态预测下一个状态，不需要历史数据，并且计算速度非常快，非常适合实时问题。即使环境中出现大量噪声/误差/不确定性，卡尔曼滤波器也会使用得到很好的结果。