C语言实现卡尔曼滤波算法demo
单片机嵌入式爱好者 2018-11-30 19:55:46
C语言实现卡尔曼滤波算法demo
简述
在做单片机开发的时候，特别是数据采集的时候，经常会用到各种滤波算法，其中就有一种叫卡尔曼的滤波算法，下面给出我之前写的一个卡尔曼滤波算法的demo。
卡尔曼滤波算法*.h头文件
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 |
/* 卡尔曼滤波，是根据估计值(此处的估计值一般默认为上次的最优值)和测量值，计算一个最优值做为结果，这个最优值即为滤波后的值 使用说明: 使用需要初始化卡尔曼滤波参数，调用一次vKalmanFilterInit()函数即可。 之后每次测得的值传递到_KalmanFilter()函数，返回的即是滤波后的最优值。 */ ====================Kalman.h文件=================== #ifndef _KALMAN_H #define _KALMAN_H #include "math.h" #define DE_KalmanResult_TYPE double typedef 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文件===================
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 |
#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文件
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 |
/*以下是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初始化 "); vKalmanFilterInit(); printf("KalmanFilter初始化完成 "); getch(); while(d < 200) { printf("%lf ",_KalmanFilter(a[d++])); } printf("结束！！！ "); getch(); return 0; } |
卡尔曼滤波算法直观结果
然后把输入和输出的数据拷贝到表格中分析如下：
卡尔曼滤波1~10之间的随机数结果
可以看出1~10之间的随机数经过卡尔曼滤波后，数据值比原始数据值波形平滑，而且明显的是在数值5左右偏上些的位置。
C语言实现卡尔曼滤波算法demohttp://t.jinritoutiao.js.cn/8eChQV/
转载请注明：徐自远的乱七八糟小站 » C语言实现卡尔曼滤波算法demo
苏公网安备 32021402001397号