卡尔曼滤波算法

SocFans 2017-02-10

展开全文

/*
卡尔曼滤波，是根据估计值(此处的估计值一般默认为上次的最优值)和测量值，计算一个最优值做为结果，这个最优值即为滤波后的值
使用说明:
使用需要初始化卡尔曼滤波参数，调用一次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

=======================================

====================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例程
数组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;
}

=======================================