汉扬编程 C语言入门 C语言实现卡尔曼滤波算法demo

C语言实现卡尔曼滤波算法demo

C语言实现卡尔曼滤波算法demo

C语言实现卡尔曼滤波算法demo

简述

在做单片机开发的时候,特别是数据采集的时候,经常会用到各种滤波算法,其中就有一种叫卡尔曼的滤波算法,下面给出我之前写的一个卡尔曼滤波算法的demo。

卡尔曼滤波算法*.h头文件

/*卡尔曼滤波,是根据估计值(此处的估计值一般默认为上次的最优值)和测量值,计算一个最优值做为结果,这个最优值即为滤波后的值使用说明:使用需要初始化卡尔曼滤波参数,调用一次vKalmanFilterInit()函数即可。之后每次测得的值传递到_KalmanFilter()函数,返回的即是滤波后的最优值。*/====================Kalman.h文件===================#ifndef _KALMAN_H#define _KALMAN_H#include \”math.h\”#define DE_KalmanResult_TYPE doubletypedef struct {DE_KalmanResult_TYPE _KalmanGain;//卡尔曼增益DE_KalmanResult_TYPE _EstimateCovariance;//估计协方差DE_KalmanResult_TYPE _MeasureCovariance;//测量协方差DE_KalmanResult_TYPE _OptimumValue;//最优值}KalmanType;extern KalmanType stKalmanParameter;void vKalmanFilterInit(DE_KalmanResult_TYPE _EstimateCovariance1,DE_KalmanResult_TYPE _MeasureCovariance1,DE_KalmanResult_TYPE _OptimumValue1);//卡尔曼滤波初始化DE_KalmanResult_TYPE _KalmanFilter(DE_KalmanResult_TYPE _MeasureValue);//卡尔曼滤波计算#endif卡尔曼滤波算法*.c文件

====================Kalman.c文件===================

#include \”Kalman.h\”KalmanType stKalmanParameter;void vKalmanFilterInit(DE_KalmanResult_TYPE _EstimateCovariance1,DE_KalmanResult_TYPE _MeasureCovariance1,DE_KalmanResult_TYPE _OptimumValue1){//卡尔曼滤波初始化 stKalmanParameter._EstimateCovariance = _EstimateCovariance1;//0.1; stKalmanParameter._MeasureCovariance = _MeasureCovariance1;//0.02; stKalmanParameter._OptimumValue = _OptimumValue1;//0;}DE_KalmanResult_TYPE _KalmanFilter(DE_KalmanResult_TYPE _MeasureValue){//卡尔曼滤波计算 //计算卡尔曼增益 stKalmanParameter._KalmanGain = stKalmanParameter._EstimateCovariance * \\ sqrt(1/(stKalmanParameter._EstimateCovariance * stKalmanParameter._EstimateCovariance + \\ stKalmanParameter._MeasureCovariance * stKalmanParameter._MeasureCovariance)); //计算本次滤波最优值 stKalmanParameter._OptimumValue = stKalmanParameter._OptimumValue + \\ stKalmanParameter._KalmanGain*(_MeasureValue – stKalmanParameter._OptimumValue); //更新估计协方差 stKalmanParameter._EstimateCovariance = sqrt(1 – stKalmanParameter._KalmanGain) * \\ stKalmanParameter._EstimateCovariance; //更新测量协方差 stKalmanParameter._MeasureCovariance = sqrt(1 – stKalmanParameter._KalmanGain) * \\ stKalmanParameter._MeasureCovariance; //返回最优值 return stKalmanParameter._OptimumValue;}卡尔曼滤波算法demo的*.c文件

/*以下是demo例程数组a[]是1~10之间的随机数,通过对这些随机数做卡尔曼滤波,最后得出的结果见下图。*/=============KalmanDemo.c文件=============#include <stdio.h>#include <stdlib.h>#include \”math.h\”#define DE_KalmanResult_TYPE double/*int a[]={11,12,15,18,20,19,16,19,16,20,13,20,20,15,14,15,12,20,15,12,11,18,15,19,15,12,14,19,12,15,15,13,13,12,16,17,20,16,19,20,19,11,18,16,11,11,11,19,20,12,17,18,15,12,16,15,20,16,18,18,15,16,18,20,13,11,11,14,17,20,11,19,14,16,17,19,16,13,11,18,14,15,16,15,11,19,20,16,19,18,11,12,18,18,20,16,16,19,12,13,18,19,19,12,14,18,18,11,11,20,19,20,15,14,11,20,20,19,19,15,15,16,20,11,19,18,11,14,20,15,19,17,12,12,18,17,11,20,13,17,14,13,14,15,14,15,13,18,14,14,18,14,13,15,14,15,16,18,16,18,19,13,13,19,12,19,13,13,11,15,19,18,15,20,16,11,14,11,16,13,15,19,19,18,19,17,16,13,15,12,19,18,14,15,15,16,18,15,14,15,20,11,17,18,15,14,14,19,15,13,20,15,15,20,13,18,18,16,15,12,15,19,13,11,15,18,19,13,15,15,11,13,15,17,18,20,17,19,17,17,20,13,16,15,15,11,18,20,15,15,17,12,12,19,15,12,20,15,11,19,12,12,16,16,13,16,11,16,13,16,18,16,18,18,17,15,11,12,19,12,19,11,16,15,12,16,20,16,18,12,18,11,13,16,15,11,14,19,14,13,18,14,19,17,19,19,15,12,20,20,18,13,20,17,16,19,13,12,16,19,16,18,19,12,19,19,18,14,20,13,17,16,14,14,18,16,14,20,19,17,18,};*/int a[]={1,2,5,8,10,9,6,9,6,10,3,10,10,5,4,5,2,10,5,2,1,8,5,9,5,2,4,9,2,5,5,3,3,2,6,7,10,6,9,10,9,1,8,6,1,1,1,9,10,2,7,8,5,2,6,5,10,6,8,8,5,6,8,10,3,1,1,4,7,10,1,9,4,6,7,9,6,3,1,8,4,5,6,5,1,9,10,6,9,8,1,2,8,8,10,6,6,9,2,3,8,9,9,2,4,8,8,1,1,10,9,10,5,4,1,10,10,9,9,5,5,6,10,1,9,8,1,4,10,5,9,7,2,2,8,7,1,10,3,7,4,3,4,5,4,5,3,8,4,4,8,4,3,5,4,5,6,8,6,8,9,3,3,9,2,9,3,3,1,5,9,8,5,10,6,1,4,1,6,3,5,9,9,8,9,7,6,3,5,2,9,8,4,5,5,6,8,5,4,5,10,1,7,8,5,4,4,9,5,3,10,5,5,10,3,8,8,6,5,2,5,9,3,1,5,8,9,3,5,5,1,3,5,7,8,10,7,9,7,7,10,3,6,5,5,1,8,10,5,5,7,2,2,9,5,2,10,5,1,9,2,2,6,6,3,6,1,6,3,6,8,6,8,8,7,5,1,2,9,2,9,1,6,5,2,6,10,6,8,2,8,1,3,6,5,1,4,9,4,3,8,4,9,7,9,9,5,2,10,10,8,3,10,7,6,9,3,2,6,9,6,8,9,2,9,9,8,4,10,3,7,6,4,4,8,6,4,10,9,7,8,4,4,9,5,4,2,3,7,3,4,9,8,3,7,2,1,2,10,6,8,2,9,3,1,5,3,7,10,6,9,3,4,8,3,1,8,8,7,8,6,6,8,5,8,7,2,1,10,7,10,6,1,8,6,7,10,6,1,6,6,4,7,6,7,1,10,4,8,8,1,1,3,2,1,7,1,9,7,2,6,10,9,7,1,10,6,8,9,3,9,4,5,5,8,1,10,7,1,6,3,4,5,3,8,1,8,8,10,5,6,5,5,1,6,2,10,4,8,6,2,10,9,10,2,3,8,7,4,3,3,9,5,3,2,2,9,5,8,7,9,4,8,4,6,7,3,6,6,10,1,7,4,4,5,3,2,4,4,6,3,3,8,3,5,7,2,10,9,7,6,6,9,7,8,5,8,3,10,8,4,4,3,10,8,9,5,10,9,8,2,9,10,10,4,1,10,7,3,6,10,8,9,5,3,8,1,10,1,1,9,9,3,9,7,1,2,6,3,1,3,3,2,7,7,1,9,7,3,9,5,4,5,4,2,10,2,5,1,7,3,3,9,9,10,8,2,2,6,9,5,10,6,6,9,6,8,4,7,6,6,5,7,2,5,10,6,2,9,4,8,7,3,5,4,8,8,8,4,7,5,2,7,3,2,9,5,4,5,2,9,4,3,5,7,8,6,1,3,4,8,9,6,9,4,1,10,10,1,4,10,7,5,3,3,3,8,10,6,6,2,2,4,2,4,10,8,2,3,7,5,3,4,2,4,4,7,9,6,4,5,10,2,5,3,9,9,5,2,5,3,5,9,9,5,4,6,8,7,5,8,4,6,5,10,4,5,7,4,2,7,3,6,9,2,10,7,10,8,7,9,2,8,5,7,4,3,4,2,7,8,8,7,9,3,1,5,2,4,5,8,6,5,3,2,10,6,4,5,9,2,3,10,2,2,8,10,10,2,1,6,9,8,8,3,7,6,4,2,7,5,2,6,3,8,1,10,3,7,3,1,2,9,4,8,7,5,2,1,7,5,7,3,1,4,8,10,7,7,5,7,5,7,3,3,9,3,9,2,1,7,3,7,2,9,1,9,2,8,4,4,3,1,6,6,5,7,2,7,4,5,7,3,9,6,3,3,1,3,5,6,2,6,3,7,4,6,7,2,10,2,4,1,7,10,2,1,1,2,9,2,6,2,3,10,6,2,4,7,2,5,2,5,4,3,5,9,10,10,2,4,4,10,1,7,1,2,9,3,9,1,5,10,3,8,9,6,2,10,9,8,1,3,1,6,10,4,7,7,9,8,7,9,1,9,7,2,6,2,3,10,4,10,6,6,5,3,1,6,10,9,4,8,1,5,10,2,4,10,10,2,7,2,10,2,3,6,4,3,8,1,7,2,3,10,10,2,8,5,3,10,5,4,7,5,5,5,9,5,8,5,6,2,7,8,1,10,8,1,5,6,8,7,10,1,6,10,7,5,6,3,8,7,1,9,10,9,9,10,4,8,3,10,2,2,1,1,7,8,10,7,2,4,8,2,9,8,10,4,8,3,9,10,2,6,3,7,7,3,3,5,7,3,8,10,2,8,5,2,7,1,10,9,3,2,9,2,2,1,7,10,5,9,2,9,10,5,4,8,10,4,7,8,2,3,3,4,2,10,1,9,1,10,10,4,5,8,6,6,6,8,10,3,4,10,1,3,7,3,1,2,6,3,6,8,10,3,9,1,6,1,7,8,4,10,1,5,9,8,5,10,9,7,2,10,6,3,5,6,3,5,2,2,1,2,8,1,10,10,10,6,6,7,9,9,7,1,10,5,7,1,10,3,8,9,3,6,10,3,6,2,9,9,1,10,2,7,6,1,3,7,7,2,1,6,8,6,6,7,7,8,10,10,5,2,4,3,8,9,1,1,2,4,4,8,1,3,6,5,2,7,5,4,1,7,2,7,6,4,6,5,10,8,1,2,3,4,6,1,3,3,4,3,9,5,5,5,4,10,7,10,5,7,6,6,8,9,6,5,6,7,6,10,4,3,9,8,7,9,10,7,10,8,6,8,8,4,7,9,7,3,6,7,10,9,6,4,1,10,9,4,5,2,3,1,4,3,2,7,6,6,10,1,4,8,2,9,9,6,10,4,8,6,7,4,5,4,8,6,2,7,7,2,5,5,7,7,7,7,7,7,3,2,2,4,8,8,1,3,4,6,4,9,5,1,5,1,3,4,1,2,5,7,1,7,3,10,1,5,8,4,9,10,2,10,5,2,7,2,10,5,8,5,6,6,3,4,7,3,5,4,9,3,1,7,5,6,3,1,4,3,6,5,10,9,5,4,2,2,5,3,5,1,5,9,5,10,5,7,8,3,2,5,6,7,9,3,7,3,4,2,6,7,2,3,10,7,8,2,4,9,2,1,5,6,2,8,9,1,3,9,1,1,8,7,10,7,8,3,5,6,9,4,6,2,8,5,4,1,7,9,7,6,8,7,5,1,1,10,7,9,3,5,2,9,3,2,3,8,2,1,5,8,1,8,3,10,5,7,7,9,3,10,3,6,9,10,7,8,6,1,4,5,5,1,10,4,8,9,8,2,3,10,10,6,2,3,4,8,3,5,3,1,8,6,6,8,9,3,8,2,6,5,7,4,9,4,4,7,3,5,8,4,7,4,5,1,8,10,3,5,10,6,10,4,10,6,10,3,2,7,10,6,7,6,1,2,4,8,5,6,3,7,3,10,5,10,4,10,3,8,4,3,5,8,7,3,5,2,2,9,5,5,3,8,7,5,4,10,1,7,2,1,7,2,3,1,9,7,8,10,1,4,2,4,3,5,6,1,6,3,4,5,2,5,7,6,6,9,10,8,7,3,9,7,8,3,3,10,8,4,8,2,7,2,1,1,5,9,9,9,9,2,7,4,8,8,4,8,1,8,2,9,10,7,2,10,7,1,10,10,2,7,10,2,5,10,9,8,8,4,9,4,5,9,10,2,10,7,3,6,9,1,5,8,3,1,9,5,3,9,6,1,6,6,10,8,5,5,6,6,4,2,1,4,5,9,6,6,7,3,4,10,7,3,5,3,1,3,10,8,6,8,8,7,10,3,9,5,10,7,1,7,9,2,1,1,5,1,1,4,1,1,4,1,4,10,1,5,8,5,6,6,3,7,2,7,8,7,5,9,8,7,8,3,8,3,7,10,10,8,5,7,7,1,8,5,9,9,6,5,7,5,9,1,7,6,2,1,7,6,6,10,3,5,8,1,3,6,5,5,9,3,3,7,4,10,2,4,8,1,5,4,2,6,3,5,9,5,9,1,2,8,6,5,8,7,7,3,9,6,8,3,10,10,3,2,3,6,8,4,6,6,2,3,9,2,7,10,7,5,7,2,10,1,10,9,2,10,5,1,10,2,10,5,10,9,2,5,4,1,4,2,4,7,5,3,3,9,7,10,7,8,10,5,6,10,4,4,9,7,4,6,4,8,5,4,7,10,7,3,7,8,2,5,10,3,1,10,4,7,10,4,7,1,4,5,10,5,1,7,10,1,2,3,9,7,9,6,8,10,1,5,1,5,2,5,9,7,7,7,9,3,7,10,3,5,6,3,9,5,10,10,1,7,2,4,7,9,6,8,4,4,8,1,5,8,8,7,9,3,4,9,10,3,10,1,5,5,3,10,6,3,9,8,4,2,1,10,7,3,3,3,2,2,9,10,10,6,8,10,8,6,6,6,8,8,7,6,10,1,7,1,6,1,10,5,1,5,7,6,6,8,2,2,1,7,6,4,4,3,9,8,2,9,2,6,4,1,2,5,6,8,4,1,9,10,5,7,4,5,4,10,8,6,1,6,2,5,8,1,7,8,8,10,4,10,9,8,3,8,6,9,5,7,1,5,5,2,4,9,3,5,6,9,7,9,6,8,8,9,7,6,1,10,6,2,1,8,1,4,10,3,6,2,7,5,6,2,3,9,5,6,6,4,8,1,1,1,5,9,1,10,3,6,3,7,3,9,10,4,5,4,10,4,7,4,4,6,5,3,6,9,7,8,10,4,7,4,3,4,7,9,5,6,5,7,2,9,6,7,9,6,7,9,1,4,9,3,8,8,7,7,7,7,9,8,10,2,10,1,1,4,1,7,1,5,10,10,8,3,7,5,9,3,9,6,8,1,10,2,5,3,2,10,1,8,10,6,2,10,8,4,3,2,2,5,9,1,7,6,7,3,4,6,8,6,1,9,4,6,4,7,2,3,2,5,5,3,8,5,8,9,1,4,3,4,10,7,2,4,2,9,4,8,10,2,3,3,7,9,5,9,10,10,1,1,8,10,4,2,7,1,7,3,2,4,1,1,3,9,3,4,9,5,5,2,2,10,6,4,2,7,5,10,5,2,1,3,5,7,1,1,7,7,4,2,6,6,4,8,5,3,2,3,9,8,7,4,7,7,2,8,4,8,1,4,7,7,2,5,4,2,9,8,8,6,6,10,4,3,4,7,5,3,8,1,7,7,7,8,10,4,5,6,10,2,6,2,8,4,2,4,3,8,4,10,6,1,8,1,3,3,2,4,2,6,8,5,5,2,6,9,5,7,5,2,3,10,3,3,1,3,1,8,7,7,6,6,3,7,5,8,3,5,9,1,8,7,7,5,10,5,3,5,8,4,5,5,1,8,6,9,3,1,7,4,9,6,9,1,7,7,1,6,8,1,9,5,5,4,2,2,8,3,10,2,1,9,8,10,3,5,9,1,8,4,2,1,3,8,5,5,8,1,6,5,3,10,1,8,5,1,6,4,6,2,7,3,10,5,1,5,4,10,4,10,3,9,7,4,6,2,7,10,2,4,4,1,3,1,6,7,9,4,4,7,10,5,10,2,2,10,8,4,1,3,1,1,2,4,4,4,4,4,8,2,6,7,10,10,9,9,2,5,3,2,8,4,5,4,1,7,5,7,9,9,7,5,9,3,2,4,4,10,8,3,6,5,1,6,9,5,7,8,6,8,7,4,10};typedef struct { DE_KalmanResult_TYPE _KalmanGain;//卡尔曼增益 DE_KalmanResult_TYPE _EstimateCovariance;//估计协方差 DE_KalmanResult_TYPE _MeasureCovariance;//测量协方差 DE_KalmanResult_TYPE _OptimumValue;//最优值}KalmanType;KalmanType stKalmanParameter;void vKalmanFilterInit(void){//卡尔曼滤波初始化 stKalmanParameter._EstimateCovariance = 0.03; stKalmanParameter._MeasureCovariance = 0.2; stKalmanParameter._OptimumValue = 5;}DE_KalmanResult_TYPE _KalmanFilter(DE_KalmanResult_TYPE _MeasureValue){//卡尔曼滤波计算 //计算卡尔曼增益 stKalmanParameter._KalmanGain = stKalmanParameter._EstimateCovariance * \\ sqrt(1/(stKalmanParameter._EstimateCovariance * stKalmanParameter._EstimateCovariance + \\ stKalmanParameter._MeasureCovariance * stKalmanParameter._MeasureCovariance)); //计算本次滤波最优值 stKalmanParameter._OptimumValue = stKalmanParameter._OptimumValue + \\ stKalmanParameter._KalmanGain*(_MeasureValue – stKalmanParameter._OptimumValue); //更新估计协方差 stKalmanParameter._EstimateCovariance = sqrt(1 – stKalmanParameter._KalmanGain) * \\ stKalmanParameter._EstimateCovariance; //更新测量协方差 stKalmanParameter._MeasureCovariance = sqrt(1 – stKalmanParameter._KalmanGain) * \\ stKalmanParameter._MeasureCovariance; //返回最优值 return stKalmanParameter._OptimumValue;}int main (){ int b,d=0; b=sizeof(a)/sizeof(a[1]); printf(\”KalmanFilter初始化\\n\”); vKalmanFilterInit(); printf(\”KalmanFilter初始化完成\\n\”); getch(); while(d < 200) { printf(\”%lf\\n\”,_KalmanFilter(a[d++])); } printf(\”结束!!!\\n\”); getch(); return 0;}卡尔曼滤波算法直观结果

然后把输入和输出的数据拷贝到表格中分析如下:

卡尔曼滤波1~10之间的随机数结果

可以看出1~10之间的随机数经过卡尔曼滤波后,数据值比原始数据值波形平滑,而且明显的是在数值5左右偏上些的位置。

本文来自网络,不代表汉扬编程立场,转载请注明出处:http://www.hyzlch.com/cjia/6973.html

编程经验分享:大神学编程居然从抄代码开始!你敢信?

零基础自学c语言看什么书?该怎么学?

发表评论

您的电子邮箱地址不会被公开。 必填项已用*标注

返回顶部