在前文中实现了颜色模型的识别和定位，但在给mcu发送位置信息时会有抖动，此时需要对信号进行滤波处理，本文记录了使用卡尔曼滤波的处理方式。

卡尔曼滤波总的来说就是在数学模型预测值和传感器检测值间动态调整权重获得最优结果的最优化算法。（所有带上标“-”的代表先验值，即预测值）

卡尔曼滤波需要用到的公式主要为以下五个

使用滤波算法主要分为4步：1.选择状态量和观测量。2.构建状态方程和观测方程。3.代入公式。4.调节超参数。

这里需要预测的位置为上篇识别得到的目标位置（横坐标x和纵坐标y），有两个量，但是该两量互不相关可用两个一阶kalman处理，就算使用二阶kalman最后的化简结果和一阶是一样的。

下面举个例子。在选取状态量和观测量时，先以x为状态量，图像检测得到的cx为观测量，构建一阶卡尔曼：

一阶kalman

一阶以水平位移x为例。同时为了便于展示使用一个11s左右的视频作为素材，该视频内容为：要识别的物体从左往右移动，为了便于计算距离单位采用像素且视为匀速运动，图像分辨率是1080p的，那么其速度就是1080/11。

1.选择状态量和观测量并构建方程

状态量即x水平位移，观测量为上篇文中所识别的目标在图像中的坐标位置，将机器路过作业目标这段路程视为匀速运动，则系统输入可为速度v，大致计算方法：图像的分辨率1080/目标在图像中出现的时间t。

则可构建状态方程（先验估计值）：

所以F=1,B=△t

观察方程：

，所以H=1.

2.代入公式

其协方差矩阵因为是一维所以只是一个数

kalman增益：

求的后验估计：

接着就是继续代入公式更新后验估计值的协方差矩阵。对于验证选取一阶还是二阶到这便足够了，接下来假定选取二阶卡尔曼，此时需要两个状态量，取横纵坐标x，y作为两个状态量；图像检测到的cx，cy作为观测量。

二阶kalman

这里将水平位移x和竖直位移y，作为状态量。

状态方程：

协方差矩阵：

卡尔曼增益：

到这便可以看出结果了，盯住x的滤波公式，可以发现哪怕是名义上使用了二阶滤波，但是其公式还是和一阶的一样。

一阶	二阶

与一阶对比可以发现，其中主要的两个参数协方差矩阵和卡尔曼增益是一样的，这样可以侧面验证当两个不耦合即相互独立的两个量是不能组成二阶滤波的。哪怕用二阶卡尔曼的公式其效果与一阶一致。

接下来是一阶卡尔曼滤波代码实现，并使用旭日x3派的串口将位置信号发送至pc机，再在pc机上绘制曲线图.

前面知道了要选用一阶卡尔曼，那么接下来就是公式的转换，下面列了个表将公式对应起来。

原公式	应用公式	注
		F=1,B=1
		F=1，Q为需要调试的超参数
		H = 1，由公式Z=HX得到，Z为传感器得到的测量值，X是我们需要状态量，这里是检测目标的横坐标x。H就是他们间的关系系数了，这里为1.
		获得后验值

如此只需要将应用部分公式转换为代码：

#include <iostream>
#include <thread>
#include <opencv/opencv2.hpp>
#include "uart.h"

typedef  struct{
	double filtervalue;  //k-1时刻的滤波值，即是k-1时刻的值
	double kalmangain;   //   Kalamn增益
	double F;   // x(n)=F*x(n-1)+u(n),u(n)~N(0,Q)
	double H;   // z(n)=H*x(n)+w(n),w(n)~N(0,R)
	double Q;   //预测过程噪声偏差的方差
	double R;   //测量噪声偏差，(系统搭建好以后，通过测量统计实验获得)
	double P;   //估计误差协方差
} kalman;
void init_kalman(kalman* info, double Q, double R)
{
	info->F = 1;  //标量卡尔曼
	info->H = 1;  //
	info->P = 1;  //后验状态估计值误差的方差的初始值（不要为0问题不大）
	info->Q = Q;    //预测（过程）噪声方差 影响收敛速率，可以根据实际需求给出
	info->R = R;    //测量（观测）噪声方差 可以通过实验手段获得
	info->filtervalue = 0;// 后验估计值的初始值
}

double kalmanfilter(kalman* info, double predict, double measure)
{
  //协方差矩阵
  info->P = info->F * info->P * info->A + info->Q;

  //kalman gain
  info->kalmangain = info->P * info->H / (info->H * info->P * info->H + info->R);
  info->filtervalue = predict + info->kalmangain * (measure - predict);

  //updata p
  info->P = (1 - info->kalmangain * info->H)*info->P;
  return info->filtervalue;
}
int main()
{
  kalman kalman_x;
  double v = 8.3;
  double x_p, x_m, x;
  char msg[4];
  init_kalman(&kalman_x, 0.01, 0.03);
  int SerialInit(); //串口配置初始化
  while(1)
  {
    x_p = x + v;
    x_m = fobj(); //上篇文代码封装为了函数，检测目标图像位置。
    x = kalmanfilter(&kalman_x, x_p, x_m);
    msg[0] = x_m/256;
    msg[1] = x_m%256;
    msg[2] = x/256;
    msg[3] = x%256;
    send_u8(msg); //通过串口发送滤波前后的数据
    std::this_thread::sleep_for(std::chrono::milliseconds(10)); //休眠10ms，控制频率
  }
  return 0;
}

复制

x3派发送数据给pc机，并绘制得到的曲线：

黑色为滤波处理前的信号数据，红色为滤波后的数据，可以看到变得平滑不少，另外滤波效果可以通过改变超参数Q、R进行调节。

最后，以该种方式获取较为理想的曲线后，使用当前的参数作为最后应用的参数。该滤波算法应用在识别得到目标后的位置（目标在图像中的xy）滤波。简单来说就是：识别模型-图像中定位模型-将坐标通过串口发送至MCU。如此使用旭日x3派进行模型目标识别就完成了，驱动部分交由MCU完成。