Kalman滤波算法原理(Matlab/C/C++)

仪器的观测存在较大的随机误差，因此会出现极端异常观测值。为此，本研究采用kalman滤波对观测进行最佳估计，进而对时序数据进行降维处理。kalman滤波是r. e. kalman[1, 2]提出的一种时域滤波算法，其采用时间递推的方式，考虑了的过程噪声和测量噪声[3]，是一种对观测值的线性最小方差估计方法[1]。kalman滤波可以基于系统上一时刻的状态预测下一状态，当获得下一状态的观测值时，根据下一状态的预测结果和观测结果获得下一状态的最优化估计。由于在状态预测和最优估计更新时状态的噪声也被更新，因此kalman不仅能够处理平稳变化的随机过程，也能处理多维和非平稳的随机过程[4]。

kalman滤波是一个不断预测和修正的模型，其具体原理如下[1]：
假设在k（k≥1）状态，系统的最优估计为，控制量为，基于 k状态的系统过程对下一状态的预测为：
( 1 )中，表示根据k状态对k+1状态的预测，α、β为系统参数。当获得下一状态的预测，下一状态的噪声也要随之改变：

( 2 )中，为下一状态预测结果的噪声，为上一状态最优估计的噪声，为α的转置矩阵，q为系统过程的噪声。
当获得下一状态的观测结果后，根据预测和观测获得下一状态的最优估计值：

( 3 )中，为k+1状态的最优估计，为k+1状态的观测结果，γ为系统参数，为k+1状态的kalman增益：

( 4 )中，r为系统的观测噪声。为了能实时处理观测数据和模型的自适应修正，k+1状态的噪声也要更新：

( 5 )中，i为全1矩阵。至此就得到了k+1状态的最优估计和噪声估计，从而能递推地计算k+2, k+3…等状态的最优估计。

 

由于观测存在观测噪声，将其加入模拟25度的室内温度的观测值（matlab代码）：

clear all
n=200;
w=0.1*randn(1,n);
x(1)=0;
a=1;
x(1)=25;
v=randn(1,n);
q1=std(v);
rvv=q1.^2;
q2=std(x);
rxx=q2.^2;
q3=std(w);
qww=q3.^2;
c=0.2;
y=x+v;
p(1)=1;
bs(1)=25;
for t=2:n;
x(t)=x(t-1);
p1(t)=p(t-1)+qww;
kg(t)=p1(t)/(p1(t)+rvv);
bs(t)=x(t)+kg(t)*(y(t)-x(t));
p(t)=p1(t)-kg(t)*p1(t);
end
t=1:n;
plot(t,bs,'r',t,y,'g',t,x,'b');

下图中，红色为 基于上一时刻对下一时刻的预测值 与 下一时刻的真实观测值（含噪声） 的最优估计值（也可理解为前两者的加权平均，而两者的权值则由kg来决定），绿色为加入观测值（含噪声），蓝色为根据前一时刻对下一时刻的预测值（假设温度恒定，所以是一条平行x轴的直线）。

 

 

 

 

为了对kalman滤波结果进行评估，可以采用峰值信号噪声比（peak signal to noise ratio，缩写为psnr）作为评价指标，来比较kalman滤波前后的优劣。psnr是一种评价信号失真度的指标，它通过对信号值进行统计和平均计算，来表示信号与真实值之间的误差。

假定一组连续变化的离散数据集为，真实参考值为ξ，则：

 

其中mse为均方误差，psnr为峰值信号噪声比，max为数据集v中的最大值。先计算kalman滤波前后的数据集均值 和 ，并将 和 分别作为数据的真实参考值，用psnr来评估采用kalman滤波结果作为数据集代表的有效性。

 

c/c++代码实现kalman滤波之后 求取均值并采用psnr作为评估指标：

 

//function: calculate the variance of a group of data
//pdata---> list of data
//num-----> length of the data array
//fvariance----> the variance of the data list
void deviation(const float *pdata,const int num, float &fvariance)
{
	int i;
	float fmean,fsum = 0.0,fsum2 = 0.0;
	for (i = 0; i < num; i ++)
	{
		fsum += pdata[i];
	}
	fmean = fsum/num;
	for (i = 0; i < num; i ++)
	{
		fsum2 += (pdata[i] - fmean)*(pdata[i] - fmean);
	}
	fvariance = fsum2/num;
}


//input - vdata. data array in the format of qstringlist(1-d array) 
//output - fkalmanresult. the result of kalman filtering, 
//         i.e., the weighted average.
//         fpsnr- peak signal to noise ratio
void kalmanfilter(qstringlist &vdata, float &fkalmanresult,float &fpsnr)
{
	// load data
    double  d_rand_max = 32767.0;
	int nsize = vdata.size();
	if (nsize == 0)
	{
		return ;
	}
	int i;
	float *pfw = new float[nsize], *pfv = new float[nsize];
	float *pfx = new float[nsize],*pfy = new float[nsize];
	for (i = 0; i < nsize; i ++)
	{
		pfw[i] = 0.1*(double)rand()/d_rand_max -0.05;
		pfv[i] = (double)rand()/d_rand_max - 0.5;
	}
	float rvv,rxx,qww;
	deviation(pfv,nsize,rvv);
	deviation(pfw,nsize,qww);
	for (i = 0; i < nsize; i ++)
	{
		pfy[i] = vdata[i].trimmed().todouble(); 
	}

	//filter
	float *pfp = new float[nsize], *pfbs = new float[nsize];
	float *pfp1 = new float[nsize], *pfkg = new float[nsize];
	float ftemp = 0.0;
	pfx[0] = 0.0,pfp[0] = 1.0,pfbs[0] = 0.0;
	for (i = 1; i < nsize; i ++)
	{
		pfx[i] = pfbs[i-1];
		pfp1[i] = pfp[i-1] + 0.0025;
		pfkg[i] = pfp1[i]/(pfp1[i] + 0.25);
		pfbs[i] = pfx[i] + pfkg[i]*(pfy[i] - pfx[i]);
		pfp[i] = pfp1[i] - pfkg[i]*pfp1[i];
		ftemp += pfbs[i];
	}
	//实际的kalman滤波至此即结束，pfbs为最优估计，pfx为基于上一时刻的估计，pfy为观测值
	//as for the array with only a numerical data
	if (nsize == 1)
	{
		pfbs[0] = pfy[0];
		fkalmanresult = pfbs[nsize-1];
	}
	else
		fkalmanresult = ftemp/(float)(nsize-1);
	
	delete []pfw; delete []pfv;
	delete []pfx; delete []pfy;
	delete []pfp; delete []pfbs;
	delete []pfp1; delete []pfkg;

	// calculate the psnr
	float mse = 0.0;
	float maxdata = 0.0;
	for (i = 0; i < vdata.size(); i ++)
	{
		// search the max data
		float f_data = vdata[i].trimmed().tofloat();
		if (f_data > maxdata)
			maxdata = f_data;
		// calculate the mse: step1
		mse += (f_data-fkalmanresult)*(f_data-fkalmanresult);
	}
	//step2
	mse = mse / (float)(vdata.size());
	if (mse > 0)
	{
		fpsnr = 10.0 * (float)log10(maxdata*maxdata/mse);
	}
	else
		fpsnr = 0.0;
}