基于卡尔曼滤波算法融合图像速度数据和加速度计数据

最近在改进之前做的视觉定点算法，以前只有一个位置环，现在准备再串一级速度环，但是解算出无人机的平移速度还是颇为头疼的，网上的资料很少，需要我们自己动脑去解决这个问题。
首先要测水平速度，传统的方法是GPS，我所设计的无人机的应用场景中的GPS信号虽然有，但是并不那么稳定，其次GPS的精度也达不到我所需的视觉引导的要求，因为GPS的位置环并没有反馈出我需要的靶标物的位置。
其次如果单纯用加速度计进行积分得到速度，那么你会发现静置一段时间后，解算的速度值不知道飘到哪里去了，这种漂移太大，完全不能够满足我们的设计要求。
但是GPS融合加速度计来测量速度却是有很多的应用例子，于是我们可以将视觉解算出的位置信息和加计信息进行融合。

最开始我使用的是互补滤波算法对图像数据和加计数据进行了一次融合，效果差强人意，这里只放出代码，一阶和二阶的差别不大，只放出一阶的互补滤波代码：

    Para_pos.filter_vx = Para_pos.filter_vx_coeff * Para_pos.v_x\
        + (1 - Para_pos.filter_vx_coeff)*(Para_pos.filter_vx + ax*dt);
    Para_pos.filter_vy = Para_pos.filter_vy_coeff * Para_pos.v_y\
        + (1 - Para_pos.filter_vy_coeff)*(Para_pos.filter_vy + ay*dt);

两个方向：融合出x和y方向的速度，调节filter_vx_coeff,filter_vy_coeff这两个系数即可，表示对加积分的速度和图像解算的速度的信任程度，可以看出代码很简单，也确实是这样，不过效果还是有的，像在两轮平衡车上，如果对角度解算的要求不是那么高的话，这个算法就足以满足平衡车的直立要求了。

下面进入正题：卡尔曼滤波对两个传感器的数据进行融合
卡尔曼滤波主要是运用5个方程（推导就不给出了）：两个预测方程和3个更新方程。
预测方程：

更新方程：

卡尔曼滤波器在线性系统中产生最优估计，因此，传感器或者系统必须是(接近)线性系统才能被运用卡尔曼滤波。因为仅仅依靠上一次系统状态和定义系统状态被修正概率的方差矩阵，所以卡尔曼滤波器不需要很长的系统状态历史来经行滤波。这点在可确保系统的高实时性要求。卡尔曼滤波有两类方程:预测方程和更新方程。预测方程根据之前的状态和控制量预测当前状态；更新方程表示相信传感器数据多些还是相信总体估计值多些（由卡尔曼增益Kg决定）。滤波器的大致工作原理为：根据预测方程预测当前状态并用更新方程检测预测结果，该过程一直在重复更新当前状态。预测方程和更新方程的关系如图3-3所示。

下面这段是截取的别人对卡尔曼滤波在陀螺仪与加速度计融合上的应用对卡尔曼滤波做的一个解释，照着文章里的解释和一些矩阵论（矩阵乘法，协方差矩阵等）的知识结合上面的5个方程，就能看懂整个推导过程，最后编程实现即可。

卡尔曼滤波代码主要根据式（3-2）~（3-6）5个公式来写的。在此，以Y轴角速度在时间上的积分为预测值的控制量，即U(k)=Gyro_Y；俯仰角（pitch=arctan(ACCEL_X/ACCEL_Z)）作为观测值，即Z(k)=pitch。因为需要同时得到Y轴的角速度和pitch角，所以进行卡尔曼滤波时需要估计两个值，一个是pitch角，另一个是陀螺仪漂移Q_bias。根据式（3-2）建立角度测量模型方程：（原谅我要直接截图了，公式编辑起来实在麻烦）

觉得看不清楚的可以去百度文库看这篇文档，我觉得还是蛮清楚，我完全明白了是怎么推导出来的。
https://wenku.baidu.com/view/3c42b7733186bceb18e8bb29.html

最后贴出我的卡尔曼融合的代码，只贴了x方向的

void Kalman_Filter(float vx, float acc_x，float dt)
{

        filter_vx.v_acc+=(acc_x-filter_vx.q_bias) * dt;
        filter_vx.v_err = vx - filter_vx.v_acc;
        filter_vx.Pdot[0]=filter_vx.Q_v - filter_vx.P[0][1] - filter_vx.P[1][0];
        filter_vx.Pdot[1]=- filter_vx.P[1][1];
        filter_vx.Pdot[2]=- filter_vx.P[1][1];
        filter_vx.Pdot[3]=filter_vx.Q_acc;
        filter_vx.P[0][0] += filter_vx.Pdot[0] * dt;
        filter_vx.P[0][1] += filter_vx.Pdot[1] * dt;
        filter_vx.P[1][0] += filter_vx.Pdot[2] * dt;
        filter_vx.P[1][1] += filter_vx.Pdot[3] * dt;
        filter_vx.PCt_0 = filter_vx.C_0 * filter_vx.P[0][0];
        filter_vx.PCt_1 = filter_vx.C_0 * filter_vx.P[1][0];
        filter_vx.E = filter_vx.R_angle + filter_vx.C_0 * filter_vx.PCt_0;
        filter_vx.K_0 = filter_vx.PCt_0 / filter_vx.E;
        filter_vx.K_1 = filter_vx.PCt_1 / filter_vx.E;
        filter_vx.t_0 = filter_vx.PCt_0;
        filter_vx.t_1 = filter_vx.C_0 * filter_vx.P[0][1];
        filter_vx.P[0][0] -= filter_vx.K_0 * filter_vx.t_0;
        filter_vx.P[0][1] -= filter_vx.K_0 * filter_vx.t_1;
        filter_vx.P[1][0] -= filter_vx.K_1 * filter_vx.t_0;
        filter_vx.P[1][1] -= filter_vx.K_1 * filter_vx.t_1;
        filter_vx.v_acc += filter_vx.K_0 * filter_vx.v_err; 
        filter_vx.q_bias += filter_vx.K_1 * filter_vx.v_err;
        Para_pos.filter_vx = filter_vx.v_acc;
}

最后看看融合的效果，最上面的是图像解算出的位置波形，中间毛刺和噪声大的是图像差分算出的速度波形。还有绿色的波形是融合后的速度波形。

可以看出，融合后的速度波形基本可以反映出图像的运动方向和运动速度，不像单独的图像速度波形噪声那么大，效果非常不错。

最后再对数据做个低通滤波即可使用了，对于速度环的控制上来讲，这个精度已经足够使用了，如果还想提高解算的精度，要进一步提高图像部分的算法，目前的图像算法上，还是会有丢目标的情况发生，进一步提升之后，效果应该会更好

另外说一句，现在很多光流模块(大多是抄袭的PixFlow)也可以和加速计进行融合，效果应该会更好。我改动过pixflow的代码和参数，对于旋转光流的消除还是有些疑惑，线性的补偿效果都不怎么好，在我的视觉系统中本来打算引入光流模块的，但是后来想到在我应用场景中可能不太适合，也就没有使用了，有光流模块使用经验的朋友欢迎与我交流，怎么消除像旋转光流这样的噪声，有时间我也会写一写我对光流模块的一些研究。

---

转载请注明出处：http://blog.csdn.net/gyh_420/article/details/76762118