卡尔曼滤波算法（C语言）

此处感谢华南虎和互联网的众多大佬的无偿分享。

入门常识

先简单了解以下概念：叠加性，齐次性。

用大白话讲，叠加性：多个输入对输出有影响。齐次性：输入放大多少倍，输出也跟着放大多少倍

卡尔曼滤波符合这两个特性。

卡尔曼滤波 ：修正值 = 估计值 + 观测值。符合叠加性和齐次性。

协方差的基础知识

把Xt代入，然后将式子进行化简。

公式推导

基础公式理解

对于卡尔曼滤波，我们可以先从状态方程和观测方程开始理解，重点搞懂重点是状态方程和观测方程的实际含义。
 

状态方程公式的参数解析。Xk：当前状态值，Uk：输入值， Wk：过程噪声。A，B为系数。

观测方程公式的参数解析。Yk：观测值。Xk：当前状态值。Vk：观测噪声

简单理解：Vk是不可避免的仪器系统误差，Wk是由外界因素引起的误差。

Q：过程噪声的方差。R：观测噪声的方差。

最优估计值，先验估计值，（观测值）后验估计值。

卡尔曼滤波其实就是取估计值和测量值之间重合的部分。两个概率之间重合的部分。

卡尔曼的实现过程

使用上一次的最优结果预测当前值，同时使用观测值修正之前预测的当前值，得到最终结果（修正后的结果叫做最优估计）。

不需要严格推导，了解是什么意思，怎么用即可。

状态更新方程

卡尔曼增益化简后

过程噪声少，Q可以取小一点。反之，过程噪声大，Q取大一点。

传感器误差小，R可以取小一点。反之，传感器误差大，R取大一点。

执行流程

卡尔曼滤波算法介绍

基本思想：通过不精确的测量来估测真实值。测出来的是不精确的测量值，再利用数学模型和不精确的测量来估测真实值。

一句话概括：根据系统模型和现有状态预测下一刻的状态，然后根据实际的测量结果进行校正。

使用场景：卡尔曼主要用来多传感器的数据融合，单一传感器建议用其他滤波算法。

如何使用？

以下面的代码为例：先定义好卡尔曼滤波结构体，并为其赋值。之后把结构体参数传入，把要过滤的数据传入。卡尔曼滤波算法就会把过滤好的数据，放在其结构体的输出成员中存储好。我们再用变量把卡尔曼滤波结构体中的输出成员的值接收到就行了。

代码

kalman.c

/*** 一维卡尔曼滤波器实现* @param ekf 卡尔曼滤波器结构体指针，包含滤波所需参数* @param input 当前时刻的测量值* * 算法流程说明：* 1. 预测阶段：根据上一时刻的状态估计当前状态* 2. 计算卡尔曼增益：确定测量值和预测值的权重* 3. 更新估计：结合预测值和测量值得到最优估计* 4. 更新协方差：为下一时刻的预测做准备*/
void kalman_1(struct _1_ekf_filter *ekf, float input)
{// 1. 预测协方差矩阵（时间更新）// Now_P = LastP + Q// 预测误差 = 上一时刻误差 + 过程噪声// Q值越大，表明系统模型越不可靠，滤波器对新测量值更敏感ekf->Now_P = ekf->LastP + ekf->Q;// 2. 计算卡尔曼增益// Kg = Now_P / (Now_P + R)// 卡尔曼增益权衡预测值和测量值的权重// R值越大（测量噪声大），增益越小，更信任预测值ekf->Kg = ekf->Now_P / (ekf->Now_P + ekf->R);// 3. 状态更新方程（测量更新）// out = out + Kg * (input - out)// 本质是预测值与测量值的加权融合// 当Kg=1时完全信任测量值，当Kg=0时完全信任预测值ekf->out = ekf->out + ekf->Kg * (input - ekf->out);// 4. 更新误差协方差矩阵// LastP = (1-Kg) * Now_P// 更新当前估计误差，用于下一时刻的预测// 随着迭代进行，P值会收敛到稳态ekf->LastP = (1 - ekf->Kg) * ekf->Now_P;
}

kalman.h

#ifndef _KALMAN_H    // 防止头文件被重复包含
#define _KALMAN_H// 定义一维扩展卡尔曼滤波器结构体
struct _1_ekf_filter
{float LastP;    // 上一时刻的协方差（预测误差）float Now_P;    // 当前时刻的协方差（估计误差）float out;      // 滤波器输出值（最优估计值）float Kg;       // 卡尔曼增益，权衡预测值和测量值的权重float Q;        // 过程噪声协方差，反映系统模型的不确定性float R;        // 测量噪声协方差，反映测量设备的不确定性
};// 一维卡尔曼滤波器函数
// 参数：ekf-卡尔曼滤波器结构体指针，input-当前时刻的测量值
extern void kalman_1(struct _1_ekf_filter *ekf, float input);#endif // _KALMAN_H

调用示例

最后我们要使用的数据在结构体的.out中

for (i = 0; i < 6; i++) // 处理读取的数据{pMpu[i] = (((int16_t)buffer[i << 1] << 8) | buffer[(i << 1) + 1]) - MpuOffset[i]; // 整合为16bit，并减去水平静止校准值if (i < 3)																		  // 以下对加速度做卡尔曼滤波{{static struct _1_ekf_filter ekf[3] = {{0.02, 0, 0, 0, 0.001, 0.543}, {0.02, 0, 0, 0, 0.001, 0.543}, {0.02, 0, 0, 0, 0.001, 0.543}};kalman_1(&ekf[i], (float)pMpu[i]); // 一维卡尔曼pMpu[i] = (int16_t)ekf[i].out;}}// 如果是陀螺仪数据，进行低通滤波if (i > 2) // 以下对角速度做一阶低通滤波{uint8_t k = i - 3;const float factor = 0.15f; // 滤波系数static float tBuff[3];		// 滤波缓冲区pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;}}