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

均方根信息滤波（Square Root Information Filter）由于具有更好的数值稳定性而被广泛使用。参考Gerald J.Bierman的Factorization Methods for Discrete Sequential Estimation 对SRIF的基本原理进行了学习，并完成了SRIF的仿真数据测试，该方法在实现中暂未考虑状态方程的过程噪声，因此仅仅适用于不随时间变化的参数估计问题。数据仿真采用的是抛物线y=ax^2 + bx + c；其中a=3.2，b=1.2，c=3.0。对其中的参数a、b和c进行参数估计。仿真噪声服从0均值，标准差为1.2的正态分布。其测试结果如下：


 


其中进行SRIF滤波解算的代码如下：

double randn(double myu, double sig)

{

    double PI = 3.14159265357;

    double a,b;

    a=((double)rand()+1.0)/((double)RAND_MAX+1.0);  

    b=((double)rand()+1.0)/((double)RAND_MAX+1.0);  

    return myu+sqrt(-2.0*log(a))*sin(2.0*PI*b)*sig;

}

void HouseholderUpperTri( double* M,int num_of_row, int num_of_column )

{

    int colcount = 0;

    int m = num_of_row;

    int n = num_of_column;

    double* A = M ; 

    int test = num_of_row<=num_of_column?(m-1):(n-1);

    double *u = new double[num_of_row];

    

    while( A != NULL )

    {

        if( colcount >= test+1 )

        {

            break;

        }

        

        double s =0.0 ,s1 =0.0,b =0.0,r = 0.0 ;

        int t =0 , i =0, j = 0 ;

        if( *A >= 0 ) t = 1;

        else if( *A < 0 ) t =-1;

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

        {

            s1+=  (*(A+i*num_of_column ))*(*(A+i*num_of_column ));

        }

        s = -t*sqrt(s1); 

        memset(u,0,sizeof(double)*num_of_row);

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

        {

            if( i == 0 )

            {

                u[i] = *A - s;

            }

            else