卡尔曼滤波器是如何运用于多传感器融合的？ 第1页

请恕我直言，现有的三个答案都没答到题主问的点子上。题主问的是多传感器使用卡尔曼滤波进行信息融合，而不是单个传感器，也不是经典卡尔曼滤波器得原理。

多传感器指的是系统观测方程不只一个，相当于经典方法，在算新息得时候算法不一样。具体内容我稍后填坑

更新。。。。。。。。。。。。。。。。。。。。。。

首先说题设，先看题设，卡尔曼滤波的多传感器信息融合指的是有以下系统：

w是建模噪声。和传统系统最大的区别是，它的观测方程是有n个的，大于一个，并且每个传感器测到的量不一样，这个传感器上的噪声的统计特性也可以是不一样的。

而回头看单传感器的观测器：

卡尔曼滤波的关键就是这个方程里的L（即卡尔曼增益）是怎么选取。到了多传感器的情况下，问题在于：

(1)不能直接算了，因为维度不一样。

(2)怎么选取最优增益L。

和现在大家讨论的不同的是，它的预测还是使用

计算出来的，而并非是用传感器A做预测，使用传感器B做新息。而是传感器A和B的观测数值都给算到新息里面去了

细节公式还是建议大家直接去看paper吧，还是比较长的，我就不往上敲了。

我在做毕设的时候有用kalman滤波得角度的算法，滤波程序不是我自己写的，但对kalman滤波也算是弄个半懂了。主要是没弄太懂参数、矩阵是如何得到的。

我用的是mpu6050，是个六轴的陀螺仪（3轴加速度+3轴叫速度，也就是三个方向的角速度+加速度），由两个轴的角加速度可以得到角度，但是抖动什么的对它的干扰太大，角速度积分也可以得到角度，但是因为是积分，会有累积误差。简单来说，由角速度得到的角度和加速度得到的角度都不是真实的角度值，需要将两者进行一定比例的融合，kalman滤波就是做这事的。kg（卡尔曼增益）决定两种方法得出的角度哪个可信度比较高，最后按比例将两者数据加在一起。

先简单这么说说，等有空了再贴具体的过程，论文还在我电脑里。

-------------------------------------------------------------------------------------------------------------------------------------------

我回来粘论文了！我看lz的提问，想必卡尔曼滤波的原理应该懂得差不多了，我就不再多赘述了。主要讲讲在实际中的运用了。我是学电子的，毕设做的东西有实物，不是纯软件、做些仿真的。

先介绍我做的东西的硬件基础吧，主要是两个芯片（或者可以说是芯片组成的外围电路）--MPU6050和stm32。MPU6050上面介绍了，不再说了，stm32 就是一单片机，卡尔曼的滤波就在stm32里处理。简单来说就是：

MPU6050得到的角度（两个方向的加速度经过反三角函数得到的）和一个方向的角速度送到stm32里进行卡尔曼滤波，得到最优估计的角度。

-----------------------------------------------------------------------------------------------------------------------------------------------------

-----------------------------------------------------------------------------------------------------------------------------------------------------

在这儿，我介绍一下滤波的必要性，开头也简单说明了。如果不想看这点的人可以略过。而且大部分的东西都是找到的资料里介绍的。

虽然MPU6050可以读出X轴、Y轴和Z轴的加速度，理论上是可以根据X轴和Z轴加速度由式（3-1）得出小车的倾角。

(3-1)

但在实际情况中，静止时MPU6050输出的角度信息噪声非常大；并且加速度计的输出实际包括a方向的加速度和重力加速度分量的叠加。这些干扰信号叠加在测量信号中会使输出信号无法得出准确的角度，如图3-1所示。

图3-1 小车运动过程中引起的加速度信号波动

小车行驶时产生的加速度使得输出加速度在实际倾角附近波动。这些噪声虽然可以通过数据平滑滤波将其滤除，但这一方面会使系统的实时性变差(无法及时输出当前角度值）；另一方面，平滑滤波也会将角速度变化信息率除掉。因此，为了得到所需要的准确的平衡角度信息，需要融合MPU6050的陀螺仪数据。

陀螺仪可以得到物体当前运动的角速度。对角速度积分就可以得到角度。角速度由于不会受到小车运动的影响，因此该信号的噪声信号很小，得到的角度信号也就跟平稳。然而角速度经过积分运算后，角度信号中存在的微小偏差和飘逸经过积分运算后会逐渐累计。随着时间的延长，误差逐步增加，最终导致电路饱和，无法得到正确的信号，如图3-2。

图3-2 角度积分漂移现象

为了消除这个误差，在此采用卡尔曼滤波方法。加速度计输出和陀螺仪输出是互相弥补的关系:加速度计科院弥补陀螺仪输出零点受温度影响的缺陷，使积分输出信号的误差大大减少；而陀螺仪则可以减少加速度计输出噪声，加大的降低了静态输出噪声。

-----------------------------------------------------------------------------------------------------------------------------------------------------

-----------------------------------------------------------------------------------------------------------------------------------------------------

然后这儿就是卡尔曼滤波的具体运用了！！！

卡尔曼滤波主要是运用5个方程（推导看了半天…………真心看的晕了）：两个预测方程和3个更新方程。

预测方程：（公式就直接截图我论文里打的了，公式编辑器打的公式复制不过来）

更新方程：

-----------------------------------------------------------------------------------------------------------------------------------------------------

在这儿粘些简单的文字性原理描述：

卡尔曼滤波器在线性系统中产生最优估计，因此，传感器或者系统必须是(接近)线性系统才能被运用卡尔曼滤波。因为仅仅依靠上一次系统状态和定义系统状态被修正概率的方差矩阵，所以卡尔曼滤波器不需要很长的系统状态历史来经行滤波。这点在可确保系统的高实时性要求。

卡尔曼滤波有两类方程:预测方程和更新方程。预测方程根据之前的状态和控制量预测当前状态；更新方程表示相信传感器数据多些还是相信总体估计值多些（由卡尔曼增益Kg决定）。滤波器的大致工作原理为：根据预测方程预测当前状态并用更新方程检测预测结果，该过程一直在重复更新当前状态。预测方程和更新方程的关系如图3-3所示。

图3-3 预测方程与更新方程关系

这5个方程的关系就简单用一个流程图介绍下了：

-----------------------------------------------------------------------------------------------------------------------------------------------------

lz最想看的可是这一段了！！！！具体的kalman滤波代码：

       float K1 =0.02;  float angle, angle_dot;   float Q_angle=0.001;// 过程噪声的协方差 float Q_gyro=0.003;//0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵 float R_angle=0.5;// 测量噪声的协方差 既测量偏差 float dt=0.005;//                  char  C_0 = 1; float Q_bias, Angle_err; float PCt_0, PCt_1, E; float K_0, K_1, t_0, t_1; float Pdot[4] ={0,0,0,0}; float PP[2][2] = { { 1, 0 },{ 0, 1 } }; void Kalman_Filter(float Accel，float Gyro)   {  angle+=(Gyro - Q_bias) * dt; //先验估计  Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分  Pdot[1]=-PP[1][1];  Pdot[2]=-PP[1][1];  Pdot[3]=Q_gyro;  PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分  PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差  PP[1][0] += Pdot[2] * dt;  PP[1][1] += Pdot[3] * dt;   Angle_err = Accel - angle; //zk-先验估计  PCt_0 = C_0 * PP[0][0];  PCt_1 = C_0 * PP[1][0];  E = R_angle + C_0 * PCt_0;  K_0 = PCt_0 / E;  K_1 = PCt_1 / E;  t_0 = PCt_0;  t_1 = C_0 * PP[0][1]  PP[0][0] -= K_0 * t_0;   //后验估计误差协方差  PP[0][1] -= K_0 * t_1;  PP[1][0] -= K_1 * t_0;  PP[1][1] -= K_1 * t_1;   angle += K_0 * Angle_err;  //后验估计  Q_bias += K_1 * Angle_err;  //后验估计  angle_dot   = Gyro - Q_bias;  //输出值(后验估计)的微分=角速度 }      

因为每个人有不同的矩阵运算定义，这里就将矩阵的运算用数组来进行运算处理了。

在这里看代码的两个入口参数楼主就知道kalman滤波的运用了吧。不过还要在此进行解释一下！！！！

代码里的卡尔曼滤波有一个周期dt !!!!! 这就要设计到和硬件结合了。

因为MPU6050（陀螺仪）采集数据有个采样频率，采样频率设置为200，也就是周期0.005s。MPU6050每次采样结束，就会产生一个中断，这个中断告诉stm32该进行卡尔曼滤波了。dt我的理解，不仅仅是简单的kalman的滤波周期，同时在底下的（4-1）式中，有一个乘dt的操作，这也是得角度的一个过程（角速度乘时间为角度）。

---------------------------------------------------------------------------------------------------------------------------------------------------

接下来就是代码的解释了。

卡尔曼滤波代码主要根据式（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）建立角度测量模型方程：

（原谅我要直接截图了，公式编辑起来实在麻烦）

我写的对代码的解释可能看起来不是那么清晰，lz也可以参考百度文库上的一篇文章。内容差不多，就排版方式的问题了。

--------------------------------------------------------------------------------------------------------------------------------------------

最后也再来加两张滤波效果图。数据是用串口传送到上位机上，然后用matlab画了个图

图 5-1 静态时角度滤波效果

图5-2 动态时角度滤波效果

个人感觉还是静态时候的滤波效果比较好些，动态效果也还行，但是感觉没那么明显。其实我也只是看着个图自己理解下而已，怎么分析还是不大懂。

然后po一张最后的实物图

----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

完！后续有问题再补充！