此处感谢华南虎和互联网的众多大佬的无偿分享。
入门常识
先简单了解以下概念：叠加性，齐次性。
用大白话讲，叠加性：多个输入对输出有影响。齐次性：输入放大多少倍，输出也跟着放大多少倍
卡尔曼滤波符合这两个特性。
卡尔曼滤波 ：修正值 = 估计值 + 观测值。符合叠加性和齐次性。
协方差的基础知识
把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;
}
}
