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

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

Kalman滤波是一个不断预测和修正的模型，其具体原理如下[1]：





由于观测存在观测噪声，将其加入模拟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;
}

[1]. Kalman, R.E., A new approach to linear filtering and prediction problems. Journal of basic Engineering, 1960. 82(1): p. 35--45.

[2]. Kalman, R.E. and R.S. Bucy, New results in linear filtering and prediction theory. Journal of basic engineering, 1961. 83(1): p. 95--108.

[3]. 李慧茹, 基于kalman滤波的近实时电离层TEC监测与反演, 2013, 长安大学. 第 126页.

[4]. 邱凤云, Kalman滤波理论及其在通信与信号处理中的应用, 2008, 山东大学. 第 90页.