卡尔曼滤波器

卡尔曼滤波器

预备知识：

协方差：用于表示两个变量的总体误差，如果两个变化趋势一致则协方差为正值，变化趋势不一致则为负值。

从直观上来看，协方差表示的是两个变量总体误差的期望。

公式讲解：

预测状态方程

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

预测协方差方程

根据 k-1时刻的系统协方差 预测 k时刻系统协方差。

卡尔曼增益方程

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

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

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

更新协方差方程

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

程序实现：

参数列表：

序号	类型	名称	描述
1	float	Last_P	上次估计协方差
2	float	Now_P	当前估算协方差
3	float	out	卡尔曼滤波器输出值（最优值）
4	float	Kg	卡尔曼增益
5	float	Q	过程噪声协方差
6	float	R	观测噪声协方差

代码实现：（一维数据滤波）

//1. 结构体类型定义
typedef struct 
{
    float Last_P;//上次估算协方差 初始化值为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->Last_P + 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->Last_P 为下一次运算准备。
     kfp->Last_P = (1-kfp->Kg) * kfp->Now_P;
     
     return kfp->out；
 }

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

注：本文参考于https://blog.csdn.net/CSDN_X_W/article/details/90289021?ops_request_misc=%257B%2522request%255Fid%25