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

来源：互联网发布：讯飞读屏软件下载编辑：程序博客网时间：2024/09/21 08:14

均方根信息滤波（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++ )

{

printf("%6.4f ",DM[i*ncol_DM+j]);

}

printf("\n");

}

* |R z^| |R^ z^^|

* Tj*| |= | |

* |A z | |0 e |

* the process of Householder transformation

* this is also called the measurement updation

HouseholderUpperTri(DM,nrow_DM,ncol_DM);

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

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

{

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

{

printf("%6.4f ",DM[i*ncol_DM+j]);

}

printf("\n");

}

* obtaining the R and z,which are updated by the measurement equation,called measuement updation.

* then, combining with the process noise w and state transformation matrxi PHI

* we can doing the state transformation, which is called time updation as well.

* | Rw 0 zw | | Rw(j+1) Rwx(j+1) zw(j+1) |

* (Tj+1)*| | = | |

* |-R^*PHI^(-1)*G R^*PHI^(-1) z^^ | | 0 R(j+1) z^(j+1) |

* the first row , Rw(j+1) , Rwx(j+1) and zw(j+1) are used in smoothing problem

* the second row, R(j+1) and z^(j+1) are used in filter problme

/*computing R^*PHI^-1*/

double* RPHI = new double[xnum*xnum];

memset(RPHI,0,sizeof(double)*xnum*xnum);

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

{

for( k=0;k<xnum;++k )

{

sum = DM[i*(xnum+1)+k];

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

{

RPHI[i*xnum+j]+= sum*PHI[k*xnum+j];

}

//computing the R^*PHI^-1*G and apply it into the IM matrix

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

{

for( k =0; k<xnum; k++ )

{

tmp = RPHI[i*xnum+k];

sum = 0.0;

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

{

// 这里在赋值之前需要将IM相应部分清0

sum+= ( -tmp*G[k*wnum+j] );

}

IM[(wnum+k)*(wnum+xnum+1)+j] = sum;

}

//apply the RPHI^-1 to the IM matrix

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

{

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

{

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

}

//udpate z^^ from Matrix DM

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

}

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

for( i = 0 ; i< wnum+xnum; i++ )

{

for( j = 0 ; j< wnum+xnum+1; j++ )

{

printf("%6.4f ",IM[i*(wnum+xnum+1)+j]);

}

printf("\n");

}

//Time updating,

HouseholderUpperTri(IM,wnum+xnum,wnum+xnum+1);

printf("measurement updata(after) IM matrix:\n");

for( i = 0 ; i< wnum+xnum; i++ )

{

for( j = 0 ; j< wnum+xnum+1; j++ )

{

printf("%6.4f ",IM[i*(wnum+xnum+1)+j]);

}

printf("\n");

}

if( DM != NULL )

{

delete[] DM; DM = NULL;

}

if( RPHI != NULL )

{

delete[] RPHI, RPHI = NULL;

}

simulate the observables which is time varying

this kind of paraments can be estimated withe SRIF() function

this is a test main function of the SRIF

void testSRIF1()

{

double PI = 3.14159265357;

double error = 0.0;

double a = 3.2 , b = 1.2, c = 3.0;

int N = 1000,N0, i =0 , j =0, k = 0;

int xnum =3;

int wnum =1;

int obsnum = 1;

double w0 = 0.5; // process noise

double pw0 = 10.0; // the initial variance of w0

double Rw0 = sqrt(pw0);

double G[3] ={0.0,0.0,0.0}; // the coefficient in front of the processing noise w0

double IM[4*5] ={0.0}; // the information matrix

double PHI_[9] ={0.0}; // the state transformation matrix

//double x0[3] = {sin(1.0/PI/10.0), cos(1.0/PI/10.0),0.00001}; // initial value , 3 rows and 1 col

double x0[3] = {3.2, 1.2, 3.0}; // initial value , 3 rows and 1 col

double D0[9] = {100,0,0,0,100,0,0,0,100}; // initial cov-variation matrix

double invR[9] = {0.0};

double R[9] = {0.0};

double y_[3] = {0.0};

double yw = 0.0;

double A[3]={0.0};

double *x = new double[N];

double *y = new double[N];

FILE* myfile = fopen("srif.dat","w+");

N0 = 0;

double alfa = 0.005;

double beita = 10.0;

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

{

x[i] = (i-N0)*alfa;

//a = sin(i/PI/beita);

a = 3.2;

//b = cos(i/PI/beita);

b = 1.2;

//c = sin(x[i]);

c = 3.0;

y[i] = a*x[i]*x[i] + b*x[i] + c + randn(0,0.5);

//fprintf(myfile, "%d %8.4f %8.4f %8.4f %8.4f %8.4f\n",i,a,b,c,x[i],y[i]);

}

//start the processing code

CholeskyUUT(D0, xnum, invR);

Rw0 = sqrt(pw0);

InverseUpperTriangular(invR, xnum, R);

Rw0 = 1.0/Rw0;

y_[0] = R[0]*x0[0] + R[1]*x0[1] + R[2]*x0[2];

y_[1] = R[3]*x0[0] + R[4]*x0[1] + R[5]*x0[2];

y_[2] = R[6]*x0[0] + R[7]*x0[1] + R[8]*x0[2];

yw = Rw0*w0;

//set up the information matrix, 6 parts

for( i = 0; i < wnum+xnum ; i++ ) // row

{

for( j=0;j<wnum+xnum+1;j++ ) //col

{

if( i<wnum && j<wnum ) // the Rw part

{

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

}

else if( i<wnum && j>=wnum+xnum) // the zw part

{

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

}

else if( i>=wnum && j>=wnum && j<wnum+xnum ) // the Rx part

{

IM[i*(wnum+xnum+1)+j] = R[(i-wnum)*(xnum)+j-wnum];

}

else if( i>=wnum && j>=wnum+xnum ) // the zx part

{

IM[i*(wnum+xnum+1)+j] = y_[i-wnum];

}

printf("Initial IM:\n");

for( i = 0 ; i<wnum+xnum ; i++)

{

for( j =0; j<wnum+xnum+1;j++ )

{

printf("%6.3f ",IM[i*(wnum+xnum+1)+j]);

}

printf("\n");

}

// start the filtering

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

{

A[0] = x[i]*x[i];

A[1] = x[i];

A[2] = 1.0;

double deltaX = 0.0;

if( i!= 0 )

{

deltaX = x[i] - x[i-1];

}

// reset the PHI matrxi , the state transformation matrix

//PHI_[0] = 1.0/( 1.0+1.0/alfa/beita/PI/tan((1.0/alfa*x[i]+N0)/beita/PI)*deltaX );

//PHI_[4] = 1.0/( 1.0-1.0/alfa/beita/PI*tan((1.0/alfa*x[i]+N0)/beita/PI)*deltaX );

//PHI_[8] = 1.0/( 1.0+1.0/tan(x[i])*deltaX );

PHI_[0] = 1.0; PHI_[4]=1.0; PHI_[8] = 1.0;

printf("state transformation Matrix PHI:\n");

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

{

for( k = 0 ; k< xnum ; k++ )

{

printf("%8.6f ",PHI_[j*xnum+k]);

}

printf("\n");

}

SRIF(xnum, obsnum, wnum, IM, A, y+i, PHI_, G);

// get the solution of abc

double RR[9] ={0.0};

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

{

for( k =0 ; k< xnum ; k++ )

{

RR[j*xnum+k] = IM[(j+wnum)*(wnum+xnum+1)+k+wnum];

}

y_[j] = IM[(wnum+j)*(wnum+xnum+1)+wnum+xnum];

}

double x_est[3]={0.0};

backSubstitution(RR, y_, x_est, xnum);

//double aa = sin(i/PI/beita);

double aa = 3.2;

//double bb = cos(i/PI/beita);

double bb = 1.2;

double cc = 3.0;//sin(x[i]);

double testobs = A[0]*x_est[0] + A[1]*x_est[1] + A[2]*x_est[2];

double trueValue = A[0]*aa + A[1]*bb + A[2]*cc;

printf("X_est:%8.4f %8.4f %8.4f\n", x_est[0],x_est[1],x_est[2]);

fprintf(myfile, "%8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n", x_est[0],x_est[1],x_est[2],aa,bb,cc,y[i],testobs,trueValue); //tan((200*x[i]+N0))

}

if(myfile != NULL ) { fclose(myfile); myfile = NULL;}

}

0 0