卡尔曼滤波(Kalman Filter) 从理论到实战详解 附算法源码

标签：状态 Kalman kfp 估计 卡尔曼滤波 协方差 源码 最优

目录

一、卡尔曼滤波的引入

二、状态观测器

三、最优状态估计

四、最优状态估计算法和方程

五、热成像仪使用卡尔曼滤波器案例

---

一、卡尔曼滤波的引入

卡尔曼滤波用于优化估算我们感兴趣的量，当这些量无法直接测量但可以间接测量的时候，他们还用于估算系统状态，通过组合各种可能受噪音影响的传感器测量值。

卡尔曼滤波（Kalman Filter）是一种高效的递归估计算法，用于动态系统的状态估计。它通过结合系统的数学模型和观测数据，在存在噪声的情况下，提供对系统状态的最优估计（最小均方误差）。卡尔曼滤波最早由 Rudolf E. Kalman 于 1960 年提出，被广泛应用于导航、目标跟踪、信号处理等领域。

卡尔曼滤波的核心思想

卡尔曼滤波基于两个核心步骤：

预测（Predict）：利用系统的状态方程，根据上一时刻的状态估计当前时刻的状态。

更新（Update）：结合当前时刻的观测值，通过加权修正预测值，获得更精确的状态估计。

二、状态观测器

卡尔曼滤波器本质上是一种 状态观测器，用于动态系统的状态估计。它通过结合系统的数学模型和实时的观测数据，对系统的当前状态进行最优估计，特别是在存在噪声的情况下。

什么是状态观测器？

状态观测器（State Observer）是动态系统控制中的一种工具，用来估计系统的内部状态。

• 在许多实际系统中，某些状态变量可能无法直接测量（例如速度、加速度、位置等）。状态观测器通过可测量的输出变量以及系统的数学模型，推断出这些不可测量的状态。

例如在航天航空当中的火箭测温，也是通过数学模型进行对实际值的推测，估算。

卡尔曼滤波器是一种功能强大的状态观测器，能够在复杂动态系统中估计不可直接测量的状态，并优化噪声干扰下的估计精度。其数学基础扎实且应用范围广，适用于各种线性和非线性动态系统，是现代控制理论中的重要工具之一。

三、最优状态估计

能够在动态系统中基于测量数据和数学模型，对系统状态进行最优估计。其核心思想是通过最小化估计误差的均方值，实现系统状态的动态、递归最优估计。

1. 最优状态估计的概念

最优状态估计指在给定测量噪声和过程噪声条件下，寻找一个估计值，使估计误差的期望最小化。卡尔曼滤波器在高斯白噪声的假设下，提供了 最小均方误差估计（Minimum Mean Square Error, MMSE），从而被认为是线性系统的最优估计算法。

卡尔曼滤波器的最优性来源

卡尔曼滤波器的最优性基于以下两个核心特性：

基于贝叶斯理论：在状态估计中，卡尔曼滤波器通过结合先验分布（预测值）和似然分布（测量值），利用 贝叶斯更新公式，得到后验分布的最优估计。

最小均方误差准则：卡尔曼滤波器的估计值通过调整卡尔曼增益 Kk\mathbf{K}_kKk​，使得状态估计误差协方差最小化：

四、最优状态估计算法和方程

卡尔曼滤波是基于动态系统数学模型和观测数据，利用递归过程实现对系统状态的最优估计的算法。其核心在于通过最小化估计误差的均方值，提供对系统状态的 最优状态估计。

卡尔曼滤波器解决的是 状态估计问题，即在动态系统中，给定过程模型和测量数据，推测系统的真实状态（状态变量 x\mathbf{x}x）。
核心步骤：

预测：基于系统模型预测下一时刻的状态。

更新：结合观测值修正预测值，从而得到后验最优估计。

（1）状态方程（系统模型）

描述系统状态随时间的变化：

描述测量值与系统状态之间的关系：

卡尔曼滤波分为两大阶段：预测阶段 和 更新阶段。

五、热成像仪使用卡尔曼滤波器案例

直接附上源码：用于对测量数据进行去噪和平滑处理。卡尔曼滤波器是一种递归算法，能够估算系统的状态，并结合当前的测量值与系统的先验信息来提供最优的状态估计。

//1. 结构体类型定义
#ifndef _KALMAN_H_
#define _KALMAN_H_

#include <Arduino.h>

typedef struct 
{
    float P; //估算协方差
    float G; //卡尔曼增益
    float Output; //卡尔曼滤波器输出 
}KFPTypeS; //Kalman Filter parameter type Struct

const float Q = 1.0;
const float R = 1.5;


// //2.定义卡尔曼结构体参数并初始化
KFPTypeS kfpVar1 = 
{
    0.2,     //估算协方差. 初始化值为 0.02
    0,     //卡尔曼增益. 初始化值为 0
    25    //卡尔曼滤波器输出. 初始化值为 0
};
KFPTypeS kfpVar2 = 
{
    0.2,     //估算协方差. 初始化值为 0.02
    0,     //卡尔曼增益. 初始化值为 0
    25    //卡尔曼滤波器输出. 初始化值为 0
};
KFPTypeS kfpVar3 = 
{
    0.2,     //估算协方差. 初始化值为 0.02
    0,     //卡尔曼增益. 初始化值为 0
    25    //卡尔曼滤波器输出. 初始化值为 0
};



float KalmanFilter(KFPTypeS *kfp, float input)
{
    //估算协方差方程：当前 估算协方差 = 上次更新 协方差 + 过程噪声协方差
    kfp->P = kfp->P + Q;
    //卡尔曼增益方程：当前 卡尔曼增益 = 当前 估算协方差 / （当前 估算协方差 + 测量噪声协方差）
    kfp->G = kfp->P / (kfp->P + R);
    //更新最优值方程：当前 最优值 = 当前 估算值 + 卡尔曼增益 * （当前 测量值 - 当前 估算值）
    kfp->Output = kfp->Output + kfp->G * (input - kfp->Output); //当前 估算值 = 上次 最优值
    //更新 协方差 = （1 - 卡尔曼增益） * 当前 估算协方差。
    kfp->P = (1 - kfp->G) * kfp->P;
 
     return kfp->Output;
}
#endif

P（估算协方差）：

表示估计值的不确定性。

初始值较小（0.2），表明初始状态的置信度较高。

G（卡尔曼增益）：

是一个动态调整的权重因子。

值越接近1，说明测量值对最终估计值的影响越大。

Output（滤波器输出）：

表示经过滤波器计算后的状态值（即最优估计值）。

预测阶段 更新估算协方差

更新阶段 计算卡尔曼增益

卡尔曼滤波器的核心算法，处理单变量数据的平滑与去噪，分为 预测阶段 和 更新阶段 两部分。

其实也就是更新估算协方差、计算卡尔曼增益、更新最优估计值、更新估算协方差

1.更新估算协方差

kfp->P = kfp->P + Q;

2.计算卡尔曼增益

kfp->G = kfp->P / (kfp->P + R);

3.更新最优估计值

kfp->Output = kfp->Output + kfp->G * (input - kfp->Output);

4.更新估算协方差

kfp->P = (1 - kfp->G) * kfp->P;

卡尔曼滤波器通过不断迭代优化估算值与测量值之间的关系，实现对状态的最优估计：

• 信任测量值的程度由测量噪声协方差 R 决定。
• 系统动态变化的假设由过程噪声协方差 Q 控制。
• 滤波器输出可以消除噪声，提高估算值的准确性，适用于传感器信号处理、动态系统状态估计等场景。

• 滤波前：
  • 当前像素点的原始温度值为 mlx90640To[i]，直接从传感器中读取，可能包含噪声。
• 滤波后：
  • 经过卡尔曼滤波，返回最优估计值，并覆盖更新原来的 mlx90640To[i] 值。

&kfpVar3Array[i]:

• kfpVar3Array:
  • 一个 卡尔曼滤波器参数数组，类型为 KFPTypeS。
  • 数组的每个元素与传感器的每个像素点一一对应，用于保存该像素点的滤波参数（协方差、增益、输出等）。
  • 每个像素点的数据独立处理，确保滤波器能够分别适应每个像素点的数据特性。
• &kfpVar3Array[i]:
  • 取第 i 个像素点的滤波器参数结构体地址，传递给 KalmanFilter 函数，进行滤波处理。

原始数据无法直接用于精确测量或图像处理，需要对数据平滑处理。

消除高频噪声，保留有意义的温度变化趋势。

对时间序列数据的动态变化进行优化，适应红外传感器的特性。

• 第一个参数：&kfpVarX（对应的卡尔曼滤波器结构体指针）。
  • 每个温度值使用独立的卡尔曼滤波器（kfpVar1、kfpVar2、kfpVar3）。
• 第二个参数：当前的温度测量值（T_avg、T_max、T_min）。
  • 滤波器将使用这些测量值与其内部状态参数进行计算，得到最优估计值。

滤波后的最优估计值（平滑后的温度值）。

返回值更新至原变量 (T_avg, T_max, T_min)，覆盖原始数据。

标签：状态,Kalman,kfp,估计,卡尔曼滤波,协方差,源码,最优
From： https://blog.csdn.net/weixin_64593595/article/details/144963378