我目前认为的曼滤卡尔曼滤波器是这样的 ：
1.设置状态转移矩阵
2.[预测] 通过 t-1 时的状态预测 t 时的状态
3.[更新] 用传感器的测量值与 2 中的预测值估计出最优值 ，并更新卡尔曼增益等常数
4.重复 2～3 步骤
（如上正是波器opencv库中自带的卡尔曼滤波器例子的步骤，也是何运初学者能最直观理解到的，它只涉及到一个测量值）
那么请问
，用于在某些涉及多传感器融合的多传问题中，例如 陀螺仪（累积误差）和电子罗盘（干扰）的感器数据融合，此时有多个测量值存在 ，融合卡尔曼滤波器又是曼滤怎样工作的呢？

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

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

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

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

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

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

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

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

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

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

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

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

Multi-sensor optimal information fusion Kalman filter

多传感器融合，最经典实用的方面就是组合导航。卡尔曼滤波的原理有大神们阐述，是基础算法
。

多传感器融合的核心其实是如何建立滤波所需的状态方程和观测方程 。在组合导航中，有直接法和间接法 ，其思路有所不同。

直接法是直接用信号过卡尔曼做的滤波融合，间接法是通过误差方程来做的。

当然 ，这些方法也可以推广到各种方面去，如温度测量等等。

其实在这里说一百句都不如直接去看书来的快，来的清楚
，来的权威，来的可靠
。

我在做毕设的时候有用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也可以参考百度文库上的一篇文章。内容差不多
，就排版方式的问题了 。

卡尔曼滤波C代码分析

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

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

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

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

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

然后po一张最后的实物图

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

完！后续有问题再补充！

尝试在一个统一的框架下讨论下各种融合方法。

1）在定位/跟踪中 ，标准卡尔曼滤波一般包括运动方程（1）和观测方程（2）。其实在获取了观测量的前提下，只用观测方程就可以计算出一个对应状态 。但是观测是有噪声的 ，只用观测序列得到的状态序列往往不是平滑的（比如GNSS连续定位的轨迹），所以我们假设运动（状态转移）服从一定的模型  ，对状态估计添加了一个正则项，使估计的状态在时序上更平滑，这就是运动方程的作用。通过迭代进行预测（运动方程）和更新（观测方程）进行滤波。（下图来自 @李崇 的回答）

2）当存在多个传感器时，则存在多个观测方程（2）（3）。

此时，按照 @洪峰 的回答，可分为串行和并行两种（先假设观测数据时间同步）。串行即先预测（运动方程），然后针对每个观测依次更新（每个观测方程）；并行则是先将多个观测方程融合（得到统一的观测和新息），然后再用标准卡尔曼滤波进行预测和更新
。并行思路可参考 @李崇 的回答 。

上述的串行思路也可以处理时间不同步的情况。此时
，对交替到达的不同观测 ，分别进行预测和更新即可。一个LIDAR和RADAR融合的例子 ：

3）有人提到一种方法，用A传感器进行预测，B传感器进行更新 。这种方法是不是卡尔曼滤波，与卡尔曼滤波有何关系 ？因为没有利用运动方程，这并非卡尔曼滤波 ，而是两个观测的融合 。在1）中 ，我们提到运动方程其实是对运动（状态转移）的一个假设
，起到正则化的作用，而A传感器则可以直接观测运动
，所以由A的观测计算状态转移的过程 ，与卡尔曼里的预测步骤是类似的（表面上
，都是状态转移 。本质上 ，一个是基于假设计算，一个是基于观测计算），这就是所谓“用A传感器进行预测”的原因。

4）总结一下 ，还是这张图

• 标准卡尔曼滤波：迭代（1）和（2）
• 多传感器卡尔曼滤波：串行同步 ，则迭代(1)->(2)->(3),串行不同步，则(1)->(2)->(1)->(3); 并行， 迭代(1) -> [ (2)融合(3)]
• A传感器预测，B传感器更新：迭代（2）和（3）

其实很多人都被“融合”一词给吓到了，不明觉厉
，嘴里蹦出这词 ，好厉害的啊。但实际上换一个词  ，校正，好平凡了，再换一个词，互补滤波，好容易理解了。

首先，复习牛顿定律 ，复习运动学方程和动力学方程
，理解姿态由角速度w驱动，位移由速度v驱动。再用IMU测量得到角速度w和线性加速度a。假定这段时间是匀速运动，则位姿变化量为dT(w, a, dt)，方程自己列吧 ，则

当前预测位姿 T = 前一最优位姿 T + dT(w, a, dt)

预测靠不靠谱？我们再用另外的传感器测量看看当前状态

当前测量位姿 T = 测量变换矩阵 x 传感器测量值

当前预测和当前测量都不准 ，我们来互补一下吧

当前最优位姿 T = 当前预测位姿 T x alpha + 当前测量位姿 T x (1 - alpha)

那么这个 alpha 怎么来？谁的误差小，也就是谁的传感器的噪声小 ，我们就更信任谁，那最终谁的值就在最优估计中占有较大比重 。上式再变形一下

残差 = 当前预测位姿 - 当前测量位姿

当前最优位姿 T = 当前预测位姿 T + K x 残差

其中 K 就是增益系数矩阵 ，我们选择用最小二乘法确定，那么 K x 残差 就是校正值。然后物理值是随时间变化的，所以我们要用递归算法动态跟踪
。理解这个解题思路，剩下就是具体的数学公式了
。一堆的矩阵和向量乘来乘去再求逆 ，都是为了上面这些思路
。