卡尔曼滤波求平均值
我做的一个有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
{
//电压数据小于异常值
if(f_Voltage[i]
{
avearge[0] = Kalman_Filter(f_Voltage[i]);
//平均值保存
BeforeEstimate=NowEstimate;
BeforeGain=NowGain;
}
}
BeforeEstimate=0; //初始化
BeforeGain=0; //初始化
//大于异常值的电流数据滤除
for(i=1;i
{
//电流数据小于异常值
if(f_Electric[i]
{
avearge[1] = Kalman_Filter(f_Electric[i]);
//平均值保存
BeforeEstimate=NowEstimate;
BeforeGain=NowGain;
}
}
BeforeEstimate=0; //初始化
BeforeGain=0; //初始化
//计算电阻数据的平均值
for(i=1;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
//test souce
#include
#include
//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);
/*子函数*/