卡尔曼滤波求平均值

 

我做的一个有C语言实现卡尔曼滤波求平均值的小程序,

贴出来与大家分享,希望各位达人,多多指点...

 

 

以下是源码

 

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

**kalman.cpp

#include "kalman.h"

 

 

//test souce

void main()

{

       long f_Voltage[120];

       long f_Electric[120];

       long f_Resistance[120];

       long MaxVoltage=35000;

       long MaxElectric=29500;

       int i,dataSize;

       long x;

       dataSize=120;

       srand( (unsigned)time( NULL ) );

       for(i=0;i<120;i++)

       {

              x=rand()%4294967265;

              f_Voltage[i]=x;

       }

       for(i=0;i<120;i++)

       {

              x=rand()%4294967265;

              f_Electric[i]=x;

       }

       for(i=0;i<120;i++)

       {

              x=rand()%4294967265;

              f_Resistance[i]=x;

       }

 

 

       Average(f_Voltage,f_Electric,f_Resistance,MaxVoltage,MaxElectric,dataSize);

       printf("avearge[0]=%d ",avearge[0]);

       printf("avearge[1]=%d ",avearge[1]);

       printf("avearge[2]=%d ",avearge[2]);

       char ch1;

       ch1=getchar( );

}

//test souce

 

//***********************************************************************

//author:wanghongyu

//date:20080214

//Average函数

//功能说明 :计算电压,电流,电阻数据的平均值

//参数1:f_Voltage[] 1分钟内采集电压数据

//参数2:f_Electric[] 1分钟内采集电流数据

//参数3:r_Resistance[] 1分钟内采集电阻数据

//参数4:MaxVoltage 电压的异常临界值

//参数5:MaxElectric 电流的异常临界值

//参数6:DataSize 1分钟内采集数据的个数

//返回值:avearge[0] 电压数据的平均值

//返回值:avearge[1] 电流数据的平均值

//返回值:avearge[2] 电阻数据的平均值

//***********************************************************************

long Average(long f_Voltage[],long f_Electric[],long f_Resistance[],long MaxVoltage,long MaxElectric,int DataSize)

{

 

       int i;

 

       //大于异常值的电压数据滤除

       for(i=1;i<DataSize;i++)

       {

              //电压数据小于异常值

              if(f_Voltage[i]<MaxVoltage)

              {

                     avearge[0] = Kalman_Filter(f_Voltage[i]);

 

                     //平均值保存

                     BeforeEstimate=NowEstimate;

                     BeforeGain=NowGain;

              }

       }

      BeforeEstimate=0;  //初始化

       BeforeGain=0;        //初始化

       //大于异常值的电流数据滤除

       for(i=1;i<DataSize;i++)

       {

              //电流数据小于异常值

              if(f_Electric[i]<MaxElectric)

              {

                     avearge[1] = Kalman_Filter(f_Electric[i]);

 

                     //平均值保存

                     BeforeEstimate=NowEstimate;

                     BeforeGain=NowGain;

              }

       }

      BeforeEstimate=0;  //初始化

       BeforeGain=0;        //初始化

       //计算电阻数据的平均值

       for(i=1;i<DataSize;i++)

       {

              avearge[2] = Kalman_Filter(f_Resistance[i]);

 

              //平均值保存

              BeforeEstimate=NowEstimate;

              BeforeGain=NowGain;

       }

      BeforeEstimate=0;  //初始化

       BeforeGain=0;        //初始化

       return 0;

}

 

//***********************************************************************

//Kalman函数:功能说明

//参数1：测量数据值

//***********************************************************************

 

long Kalman_Filter(long Zk)

{

       int K = 2;

       int A = 1;

       int A1 = 1;

       int B = 1;

       int H = 1;

       int H1 = 1;

       int I = 1;

       int I1 = 1;

       //X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1) 基于系统的上一状态而预测出现在状态

       //U[K]:现在状态的控制量,现在的系统状态是k

       RealEstimate = A * BeforeEstimate + B * U[K];

 

       //P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2) 求现在状态的协方差

       //A1表示A的转置矩阵，Q是系统过程的covariance

       RealGain = sqrt((A * (BeforeGain *BeforeGain) * A1 + Q *Q));

 

       //Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

       //Kg为卡尔曼增益(Kalman Gain)

       Kg[K] = sqrt((RealGain *RealGain * H1) / (H * RealGain *RealGain * H1 + R *R));

 

       //X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)

       //求现在状态(k)的最优化估算值X(k|k)：

       NowEstimate = RealEstimate + Kg[K] * (Zk- H * RealEstimate);

 

       //P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)

       //更新k状态下X(k|k)的covariance：

       //其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。

       NowGain = sqrt(((I - Kg[K] * H) * RealGain *RealGain) );

 

       //返回现时刻的估算值

       return long(NowEstimate);

}

 

  **kalman.h

#include "stdio.h"

#include "conio.h"

#include <math.h>

 

//test souce

#include <stdlib.h>

#include <time.h>

//test souce

 

//

double  R=200;                          //增益参数1

double  Q = 1;                    //增益参数2（R/Q的值越大，kalman返回的结果越稳定）

double  NowEstimate;         //现时刻的估算值 X_k_k

double  RealEstimate;          //通过上一时刻的最优值推算出来的现时刻的估算值 X_k_k_1

double  BeforeEstimate;              //上一时刻的估算值 X_k_1_k_1

double  NowGain;               //现时刻的增益率 P_k_k

double  RealGain;                //通过上一时刻的最优值推算出来的现时刻的增益率 P_k_k_1

double  BeforeGain;                    //上一时刻的增益率 P_k_1_k_1

 

long U[3];

long X[3];

long Z[3];                                   //实际值（Kalman函数的Input值）

double Kg[3];

long KalmanFilter;                //最优值（Kalman函数的返回结果）

 

static long avearge[3];           //返回值保存区域

/*子函数*/ 

long Kalman_Filter(long ); 

long Average(long f_Voltage[],long f_Electric[],long f_Resistance[],long ,long,int);

/*子函数*/