kalman 滤波

卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用
文章目录

卡尔曼滤波
一、滤波效果展示
二、简介
三、组成

1. 预测状态方程
   （1）目的：
   （2）方程：
   （3）备注
2. 预测协方差方程
   （1）目的
   （2）方程
   （3）备注
3. 卡尔曼增益方程
   （1）目的
   （2）方程
   （3）备注
4. 跟新最优值方程（卡尔曼滤波的输出）
   （1）目的
   （2）方程
   （3）备注
5. 更新协方差方程
   （1）目的
   （2）方程
   （3）备注
   四、C 程序代码实现
6. 参数列表
7. 代码实现（一维数据滤波）

作者：Beyonderwei
来源：CSDN
原文：https://blog.csdn.net/CSDN_X_W/article/details/90289021
版权声明：本文为博主原创文章，转载请附上博文链接！

卡尔曼滤波

一、滤波效果展示

蓝色的波形是实际测得的数据，红色的波形是经 Kalman 滤波后的数据波形。
注：这里是实际应用激光测距传感器（TOF）vl53l0x 测得的距离数据。

f
二、简介

采用递归的方法解决线性滤波问题，只需要当前的测量值和前一个采样周期的估计值就能进行状态估计，需要的存储空间小，每一步的计算量小。

三、组成

1. 预测状态方程

（1）目的：

由 系统状态变量k-1时刻的最优值 和 系统输入 计算出k时刻的 系统预测值。

（2）方程：

3）备注

①. 当X为一维数据时，Fk的值是1。

3. 卡尔曼增益方程

（1）目的

根据（k时刻） 协方差矩阵的预测值 计算 卡尔曼增益。

（2）方程

3）备注

①. 当 Pk|k-1 为一个一维矩阵时，Hk 是1。

4. 跟新最优值方程（卡尔曼滤波的输出）

（1）目的

根据 状态变量的预测值 和 系统测量值 计算出 k时刻状态变量的最优值。

（2）方程

（3）备注

①. 当 Pk|k-1 为一个一维矩阵时，Hk 是1。

5. 更新协方差方程

（1）目的

为了求 k时刻的协方差矩阵。（为得到k+1时刻的卡尔曼输出值做准备）

（2）方程

（3）备注

①. 当 Pk|k-1 为一个一维矩阵时，Hk 是1。

四、C 程序代码实现

1. 参数列表
   
2. 代码实现（一维数据滤波）

实际参数是参照别人已经选好的参数，不过也可以自己改变参数，去观察波形的效果，体会每个参数对于滤波效果的影响，这里不详细介绍。
//1. 结构体类型定义
typedef struct
{
float LastP;//上次估算协方差 初始化值为0.02
float Now_P;//当前估算协方差 初始化值为0
float out;//卡尔曼滤波器输出 初始化值为0
float Kg;//卡尔曼增益 初始化值为0
float Q;//过程噪声协方差 初始化值为0.001
float R;//观测噪声协方差 初始化值为0.543
}KFP；//Kalman Filter parameter

//2. 以高度为例 定义卡尔曼结构体并初始化参数
KFP KFP_height={0.02,0,0,0,0.001,0.543};

/**
*卡尔曼滤波器
*@param KFP *kfp 卡尔曼结构体参数

• float input 需要滤波的参数的测量值（即传感器的采集值）
  *@return 滤波后的参数（最优值）
  */
  float kalmanFilter(KFP *kfp,float input)
  {
  //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
  kfp->Now_P = kfp->LastP + kfp->Q;
  //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
  kfp->Kg = kfp->Now_P / (kfp->NOw_P + kfp->R);
  //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
  kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
  //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
  kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
  return kfp->out；
  }

/**
*调用卡尔曼滤波器 实践
*/
int height;
int kalman_height=0;
kalman_height = kalmanFilter(&KFP_height,(float)height);