均方根信息滤波（SRIF）测试（三）

均方根信息滤波（SRIF）测试（三）

 数据处理中对时变参数的估计比较常见，而时变参数必然涉及到参数之间的状态转移以及其过程噪声。因此有必要在均方根信息滤波中加入过程噪声的处理以及状态转移矩阵。仍然以抛物线为例，对其进行了模拟及验证。直接上代码。相关函数代码可见 http://blog.csdn.net/hpulizhen/article/details/50420636

/*

* Author: zhen.LI

* date: 2015 12 25, Merry Christmas!!

* 该函数是包含有状态转移过程以及状态方程过程噪声的滤波算法

* reference: P121, equation 2.28 and  2.29

* equation 2.28 is measuremant update, measruement model: z=Ax+v;

* equation 2.29 is the time update, state model: x1 = PHI*X0 + G*w0;

* comments: G: nrow = xnum, ncol =wnum; w0: nrow = wnum, ncol=1;

* inputs:

*       wnum           xnum          1

*      | Rw           Rwx           zw | wnum

* IM = |                               |

*      | 0          R(j+1)     z^(j+1) | xnum

* z^ = R*x0; z^,R are a priori measurement value and covariance(P0 = R^-1*R^-T)

* IM should be initialized before the estimation process; nrow = xnum+wnum; ncol = xnum+wnum+1

* 

* A: the design matrix for measurement equation; nrow= obsnum, ncol = xnum

* z: the current obs value, nrow = obsnum, ncol = 1;

* PHI: the inversion of the state transformation matrxi(IT MUST BE UNSINGULAR); nrow = xnum, ncol = xnum;

* G :  the coefficient of process noise ; nrow = xnum, ncol= wnum;

*/

void SRIF( int xnum, int obsnum, int wnum,double* IM,double* A,double* z,

          double* PHI,double* G)

{

int i = 0, j = 0 , k = 0 ;

int nrow_DM = xnum+obsnum;

int ncol_DM = xnum+1;

double* DM = new double[nrow_DM*ncol_DM];

memset(DM,0,sizeof(double)*nrow_DM*ncol_DM);

    double sum =0.0, tmp =0.0;

/*      xnum  1

*      | R   z^| xnum

* DM = |       |

*      | A   z | obsnum

*

*/

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

{

for( j = 0 ; j< ncol_DM; j++ )

{

if( i< xnum && j< xnum )  // R part, data equation

{

                DM[i*ncol_DM+j] = IM[(i+wnum)*(xnum+wnum+1)+j+wnum];

}

else if( i<xnum && j>=xnum ) // z^ part, data equation

{

                DM[i*ncol_DM+j] = IM[(i+wnum)*(xnum+wnum+1)+j+wnum];

}

else if( i>= xnum && j<xnum ) // A part , measurement equation

{

DM[i*ncol_DM+j] = A[(i-xnum)*xnum+j]; 

}

else if( i>= xnum && j>=xnum ) // z part, measurement equation

{

DM[i*ncol_DM+j] = z[i-xnum];

}

}

}// end of the formation of matrix DM

    

    printf("measurement updata(before) DM matrix:\n");

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

    {

        for( j = 0 ; j< ncol_DM; j++ )

        {