【GL09】(算法)卡尔曼滤波

一、简介

卡尔曼滤波(Kalman Filter)是一种有效的递归滤波器(自回归滤波器),它能够从一系列的包含统计噪声的测量中估计动态系统的状态。卡尔曼滤波广泛应用于信号处理、控制理论、自动驾驶、金融等领域。

基本公式:假设系统状态方程和测量方程为:

二、卡尔曼滤波器

cpp 复制代码
//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);
相关推荐
老师我太想进步了20261 小时前
【数据结构-8】栈的实现
c语言·数据结构·算法
kyle~2 小时前
因子图优化(Factor Graph Optimization)
c++·算法·机器人·传感器
Pniubi3 小时前
贪心算法入门
算法·贪心算法
MobotStone3 小时前
AI 能不能越用越会用?从“改答案”到“改进自己”,读懂 RSI
人工智能·算法
复杂网络与机器学习4 小时前
Meta 重写 K-Means:25 轮压成 2 轮,GPU 快 45 倍
人工智能·算法·数据挖掘
lunar_coder4 小时前
【算法】前缀和:把 O(n) 的重复劳动变成 O(1) 的一次查询
c++·算法
Pniubi4 小时前
力扣19 . 删除表的倒数第N个节点(栈)
算法·leetcode·职场和发展
青少儿编程课堂5 小时前
第十八届蓝桥杯青少组图形化 + 全国青少年信息素养大赛 + 电子学会图形化考级:校园三消乐大挑战解析
c++·python·算法·bfs·信息学竞赛
青少儿编程课堂5 小时前
蓝桥杯·全国青少年信息素养大赛·电子学会Python/C++考级:网格连通块与岛屿数量(DFS+BFS双解)
c++·python·算法·bfs·信息学竞赛
forest_0916 小时前
多 Agent 协作第二课:交叉验证≠独立验证
人工智能·python·算法·机器学习·prompt·agi