当前位置: 首页 > article >正文

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

一、简介

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

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

二、卡尔曼滤波器

//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);


http://www.kler.cn/a/380855.html

相关文章:

  • 【GO环境安装】mac系统+GoLand使用
  • GraalVM完全指南:云原生时代下使用GraalVM将Spring Boot 3应用转换为高效Linux可执行文件
  • 鸿蒙Next之包体积极限优化
  • 解析在OceanBase创建分区的常见问题|OceanBase 用户问题精粹
  • 对于其他管理的理解(中)
  • echarts画风向杆
  • HCIA(DHCP服务)
  • C++优选算法七 分治-快排
  • 江协科技STM32学习- P29 实验- 串口收发HEX数据包/文本数据包
  • DAY67WEB 攻防-Java 安全JNDIRMILDAP五大不安全组件RCE 执行不出网
  • 大型音频模型:AudioLLMs
  • 深度学习基础知识-编解码结构理论超详细讲解
  • java学习2
  • SPI通信详解-学习笔记
  • 练习LabVIEW第三十九题
  • Prometheus套装部署到K8S+Dashboard部署详解
  • Vue:计算属性
  • Spring 实现异步流式接口
  • jmeter脚本-请求体设置变量and请求体太长的处理
  • Webpack入门教程:从基本概念到优化技巧
  • Vision - 开源视觉分割算法框架 Grounded SAM2 视频推理 教程 (2)
  • K8S简单部署,以及UI界面配置
  • Vue指令:v-else、v-else-if
  • 展示+分享|美创科技@2024年数据安全关键技术研究及产业应用成果大会
  • 【云备份】httplib库
  • 信息安全工程师(77)常见网络安全应急事件场景与处理流程