卡尔曼滤波C语言实现

卡尔曼5条基本公式，参考https://wenku.baidu.com/view/8523cb6eaf1ffc4ffe47ac24.html

#include "stdio.h"
#include "stdlib.h"
#include "math.h"#define kal_Q 0.001  /*过程噪声协方差,Q增大，动态响应变快，收敛稳定性变坏*/
#define kal_R 0.542  /*观测噪声协方差,R增大，动态响应变慢，收敛稳定性变好*/double frand()
{return 2 * ((rand() / (double)RAND_MAX) - 0.5);  /*随机噪声 */
}/*Measure 原始测量数据op_flg  0 仅初始化参数  1 执行卡尔曼滤波
*/
double Kalman(double measure,int op_flg)
{double x_mid,kg,p_mid;static double x_last = 0,p_last = 0;double x_now,p_now;if(op_flg)         /*非初始化*/{x_mid = x_last;p_mid = p_last + kal_Q;kg = p_mid / (p_mid + kal_R);x_now = x_mid + kg * (measure - x_mid);p_now = (1 - kg)*p_mid;p_last = p_now;x_last = x_now;}else{x_last = measure;p_last = kal_Q;}return x_now;
}void main()
{float z_real = 0.56;float z_measure,x_now;float summerror_kalman = 0;float summerror_measure = 0;int i;Kalman(z_real,0);for (i = 0; i < 20;i++){z_measure = z_real + frand()*0.05;//测量值x_now = Kalman(z_measure,1);printf("Real position:%6.3f\n", z_real);printf("Measure position:%6.3f [diff:%.3f]\n", z_measure, fabs(z_real - z_measure));printf("Kalman position: %6.3f [diff:%.3f]\n", x_now, fabs(z_real - x_now));printf("\n\n");summerror_kalman += fabs(z_real - x_now);summerror_measure += fabs(z_real - z_measure);}printf("总体测量误差      :%f\n", summerror_measure);printf("总体卡尔曼滤波误差:%f\n", summerror_kalman);printf("卡尔曼滤波效果所占比例:%d%%\n", 100 - (int)((summerror_kalman / summerror_measure) * 100));getchar();
}

设定协方差 Q和R后，能滤除97%的噪声

卡尔曼滤波C语言实现相关推荐

【飞控理论】从零开始学习Kalman Filters之四：卡尔曼滤波C语言代码实现
文章目录前言学习目录 1.卡尔曼线性滤波的五条黄金公式 2.陀螺仪的原始数据 3.C语言源码分析附录 1.矩阵乘法 2.协方差矩阵 3.单位矩阵前言前面的文章系统介绍了卡尔曼滤波算法的数 ...
卡尔曼滤波C语言实现（矩阵版）
卡尔曼滤波算法及其C语言实现(矩阵操作版本) 卡尔曼滤波算法随机数的产生均匀分布随机数的产生正态分布的随机数矩阵的C语言实现卡尔曼滤波测试卡尔曼滤波算法这里就不详细讲解该算法,我觉得比较 ...
卡尔曼滤波--C语言实现
以下是一段用C语言实现的卡尔曼滤波算法,供有需要的小伙伴参考 #include "stdio.h" #include "stdlib.h" #include & ...
卡尔曼滤波 c语言模板
卡尔曼滤波算法 C语言代码编写如下所示 #include <math.h> #include <stdio.h>/*R值固定,Q值越大,代表越信任测量值,Q值无穷大,代表只 ...
卡尔曼滤波 C语言实现
kalmen_filte.h #ifndef _KALMAN_FILTER_H #define _KALMAN_FILTER_H//1d卡尔曼滤波 typedef struct {float x; / ...
【飞控理论】从零开始学习Kalman Filters之三：非线性状态估算器
文章目录前言学习目录 1.开始之前先需要知道的几个概念 2.非线性状态估算器 2.1.扩展卡尔曼滤波器(EKF) 2.2.无味卡尔曼滤波器(UKF) 2.3.粒子滤波器(PF) 2.4.总表 ...
【飞控理论】从零开始学习Kalman Filters之二：最优状态估计、最优估计算法和方程
文章目录前言学习目录 1.开始之前先需要知道的几个概念 2.最优状态估计 3.最优估计算法和方程本文是博主学习笔记,点击这里观看原视频. 前言在上次的学习中我们学习了卡尔曼滤波器的常见 ...
【飞控理论】从零开始学习Kalman Filters之一：Kalman Filters的常见用途、什么是状态观测器？
文章目录前言学习目录 1.Kalman Filters的常见用途 1.1.数据源无法直接被测量时卡尔曼滤波器是如何进行估算系统的状态 1.2.数据源在噪音影响下如何使用卡尔曼滤波器来估计系统的状态 ...
卡尔曼滤波算法c语言stm32,卡尔曼滤波算法及C语言实现_源代码
a往南向北 2019-01-16 20:39:20 11340 收藏 111 分类专栏: C语言嵌入式文章标签: 卡尔曼滤波 C代码卡尔曼滤波理论很容易就可以在MATLAB软件环境下实现,但是 ...

卡尔曼滤波C语言实现

卡尔曼滤波C语言实现相关推荐

最新文章

热门文章