pcrb.c

本文件采用Matlab软件

	
	else
		
	{
		h     = (double *)mxMalloc(2*sizeof(double));
		
		h[0]  = fabs(Y_max - Y_min)/steph;
		
		h[1]  = fabs(X_max - X_min)/steph;
		
	}
	
	
	/* --- Output 1 ---*/
	
	if(N > 1)
	{
		numdimstr = 3;
	}
	
    dimstr        = (int *)mxMalloc(numdimstr*sizeof(int));
	
	dimstr[0]     = 1;
	
	dimstr[1]     = K;
	
	if(N > 1)
	{
		
		dimstr[2] = N;
		
	}
	
	
	plhs[0]       = mxCreateNumericArray(numdimstr, dimstr, mxDOUBLE_CLASS, mxREAL);
	
	tr            = mxGetPr(plhs[0]);
	
	
	
	/* --- Output 2 ---*/
	
	if(N > 1)
	{
		numdimsFIM = 3;
	}
	
    dimsFIM        = (int *)mxMalloc(numdimsFIM*sizeof(int));
	
	dimsFIM[0]     = d;
	
	dimsFIM[1]     = K;
	
	if(N > 1)
	{
		
		dimsFIM[2] = N;
		
	}
	
	
	plhs[1]       = mxCreateNumericArray(numdimsFIM, dimsFIM, mxDOUBLE_CLASS, mxREAL);
	
	FIM           = mxGetPr(plhs[1]);
	


#ifdef fulloutput

	if(N > 1)
	{
		
		numdimsJ  = 4;

	}


    dimsJ         = (int *)mxMalloc(numdimsJ*sizeof(int));
	
	dimsJ[0]      = d;
	
	dimsJ[1]      = d;

	dimsJ[2]      = K;


	if(N > 1)
	{
		
		dimsJ[3]  = N;

	}


	plhs[2]       = mxCreateNumericArray(numdimsJ , dimsJ , mxDOUBLE_CLASS, mxREAL);
	
	J             = mxGetPr(plhs[2]);



#endif
	
	/*------------------------------ */
	
	randini();
	
	randnini();


#ifdef fulloutput

    PCRB(map , covmap , X1 , U , Q1 , F , Q , M , Y_min , Y_max , X_min , X_max , Hp, h,
		tr , FIM, J,
		d , m , K , N, nR, nC);


#else 

    PCRB(map , covmap , X1 , U , Q1 , F , Q , M , Y_min , Y_max , X_min , X_max , Hp, h,
		tr , FIM,
		d , m , K , N, nR, nC);
	
#endif
	
	
	/*------------------------------ */
	
	if (nrhs < 13)
		
	{
		
		mxFree(Hp);
		
		mxFree(dimsHp);
			
	}
	
	
	if (nrhs < 14)
		
	{
		
		mxFree(h);
		
		mxFree(dimsh);
		
	}
	
	
}


/*-------------------------------------------------------------------------------------------------*/

#ifdef fulloutput

           void  PCRB(double *map , double *covmap , double *X1 , double *U , double *Q1 , double *F , double *Q , int M , double Y_min , double Y_max , double X_min , double X_max , double *Hp, double *h,
		              double *tr , double *FIM, double *J,
		              int d , int m , int K , int N, int nR, int nC)
		   
		   
#else 
		   
		   
		   void  PCRB(double *map , double *covmap , double *X1 , double *U , double *Q1 , double *F , double *Q , int M , double Y_min , double Y_max , double X_min , double X_max , double *Hp, double *h,
		              double *tr , double *FIM,
		              int d , int m , int K , int N, int nR, int nC)
		   
		   
#endif

		   
{
	
	int i, j, k , n, nK, dK = d*K, ndK, dM = d*M, nd, jd;
	
	int d2 = d*d, K1 = K - 1, d2K1 = d2*K1, nd2K1, dK1 = d*K1, m2 = m*m;
	
	int kd2 , kd , ndK1, kdd;


#ifdef fulloutput

	int nd2K, d2K = d2*K, kdd2;
	
#endif
	
	double  *vect , *vv, *invQ1, *cholQ1, *cholQ;
	
	
	
	double *Jnew,  *Jold, *invJnew, *invJold, *Ftemp, *Qtemp, *Dtemp, *invDtemp, *FX, *Ft;
	
	double *Xold, *Xnew, *W;
	
	
	double *E_PI , *Y , *grad_Zi , *Ri , *Riinv , *gRiinv , *gRiinvgt , *EgRiinvgt, *yR, *FinvJFt;
	
	double *vectZ , *vvZ , *HptEgRiinvgt;
	
	
	int *indx, *indxZ;
	
	double sum, co;
	
	

	// Usefull vector & matrices allocation //
	
	
	vect          =  (double *)mxMalloc(d*sizeof(double));
	
	vv            =  (double *)mxMalloc(d*sizeof(double));
	
	invQ1         =  (double *)mxMalloc(d2*sizeof(double));
	
	cholQ1        =  (double *)mxMalloc(d2*sizeof(double));
	
	cholQ         =  (double *)mxMalloc(d2*sizeof(double));
	
	Jnew          =  (double *)mxMalloc(d2*sizeof(double));
	
	Jold          =  (double *)mxMalloc(d2*sizeof(double));
	
	invJnew       =  (double *)mxMalloc(d2*sizeof(double));
	
	invJold       =  (double *)mxMalloc(d2*sizeof(double));
	
	Ftemp         =  (double *)mxMalloc(d2*sizeof(double));
	
	Qtemp         =  (double *)mxMalloc(d2*sizeof(double));
	
	Dtemp         =  (double *)mxMalloc(d2*sizeof(double));
	
	invDtemp      =  (double *)mxMalloc(d2*sizeof(double));
	
	Ft            =  (double *)mxMalloc(d2*sizeof(double));
	
	
	Xnew          =  (double *)mxMalloc(dM*sizeof(double));
	
	Xold          =  (double *)mxMalloc(dM*sizeof(double));
	
	W             =  (double *)mxMalloc(dM*sizeof(double));
	
	FX            =  (double *)mxMalloc(dM*sizeof(double));
	
	
	
	Y             =  (double *)mxMalloc(2*sizeof(double));
	
	grad_Zi       =  (double *)mxMalloc((2*m)*sizeof(double));
	
	gRiinv        =  (double *)mxMalloc((2*m)*sizeof(double));


	Ri            =  (double *)mxMalloc((m2)*sizeof(double));
	
	Riinv         =  (double *)mxMalloc((m2)*sizeof(double));
	
	vectZ         =  (double *)mxMalloc(m*sizeof(double));
	
	vvZ           =  (double *)mxMalloc(m*sizeof(double));
	
	E_PI          =  (double *)mxMalloc(d2*sizeof(double));
	
	yR            =  (double *)mxMalloc(d2*sizeof(double));
	
	FinvJFt       =  (double *)mxMalloc(d2*sizeof(double));
	
	gRiinvgt      =  (double *)mxMalloc(4*sizeof(double));
	
	EgRiinvgt     =  (double *)mxMalloc(4*sizeof(double));
		
	HptEgRiinvgt  =  (double *)mxMalloc((2*d)*sizeof(double));
	
	
	
	
	indx          =  (int *)mxMalloc(d*sizeof(int));
	
	indxZ         =  (int *)mxMalloc(m*sizeof(int));
	
	
	
	
	/*---------- invQ1 = inv(Q1), cholQ1  = chol(Q1) -------------- */
	
	
	inv(Q1 , invQ1 , indx , vect , vv , d); 
	
	chol(Q1 , cholQ1, d);
	
	
	/*---------- Main loop  -------------- */
	
	
	for (n = 0 ; n < N ; n++)
	{
		
		nK        = n*K;
		
		ndK       = n*dK;
		
		nd        = n*d;
		
		nd2K1     = n*d2K1;
		
		ndK1      = n*dK1;

#ifdef fulloutput

		nd2K      = n*d2K;

#endif
		
		// 1) Initialisation //
		
		for(i = 0 ; i < d2 ; i++)
		{
			
			invJold[i] = Q1[i];
			
			Jold[i]    = invQ1[i];

#ifdef fulloutput

						
			J[i + nd2K] = invJold[i];

#endif
			
		}
		
		sum  = 0.0;
		
		for(i = 0 ; i < d ; i++)
		{
			
			FIM[i + ndK] = Q1[i + i*d];
			
			sum         += FIM[i + ndK];
			
		}
		
		tr[0 + nK]       = sum;
		
		
		
		// Generate multivariate samples W ~ N(0 , Q) //
		
		mvg(cholQ1 , W, d , M);
		
		// X_k ~ N(X1 , Q1) //
		
		
		for (j = 0 ; j < M ; j++)
		{
			jd = j*d;
			
			for(i = 0 ; i < d ; i++)
			{
				
				Xold[i + jd] = X1[i + nd] + W[i + jd];
				
			}
			
		}

				
		// 2) Iteration //
		
		for (k = 0 ; k < K1 ; k++)
		{
			
			kd     = k*d + ndK1;
			
			kdd    = (k+1)*d  + ndK;
			
			kd2    = k*d2 + nd2K1;

#ifdef fulloutput

			kdd2   = (k+1)*d2 + nd2K;

#endif
			
			for(i = 0 ; i < d2 ; i++)
			{
				
				Ftemp[i] = F[i + kd2];
				
				Qtemp[i] = Q[i + kd2];
				
			}
			
			
			// Xnew = FXold + U + W
			
			chol(Qtemp , cholQ , d);
			
			mvg(cholQ , W , d , M);
			
						
			transpose(Ftemp , Ft , d);
			
			mult_trans(Ft , Xold , FX , d , d, M);
			
			
			// X_{k+1} ~ FkX_{k} + Uk + N(0 , Qk) //

			for (j = 0 ; j < M ; j++)
			{
				jd = j*d;
				
				for(i = 0 ; i < d ; i++)
				{
					
					Xnew[i + jd] = FX[i + jd] + U[i + kd] + W[i + jd]; 
					
				}
				
			}
			
			
			// E_PI 
			
			
			co   = esperance_PI(map , covmap , Xnew , Hp , h , X_min , X_max , Y_min , Y_max , 
				E_PI , 
				nR , nC , m , d , M , 
				Y , Ri , grad_Zi , Riinv , gRiinv , gRiinvgt , EgRiinvgt , vectZ , vvZ , indxZ , HptEgRiinvgt);
			
			
			
            // Jnew      = E_PI + inv( Qk + Fk*invJold*Fk'); //
			
			
			YRYt(Ftemp , invJold , d , d , FinvJFt, yR);
			
			for(i = 0 ; i < d2 ; i++)
			{
				
				Dtemp[i] = Qtemp[i] + FinvJFt[i];
				
			}
			
			inv(Dtemp , invDtemp , indx , vect , vv , d); 
			
			
			for(i = 0 ; i < d2 ; i++)
			{
				
				Jnew[i] = E_PI[i] + invDtemp[i];
				
			}
			
			
			inv(Jnew , invJnew , indx , vect , vv , d); 
					
			sum  = 0.0;
			
			for(i = 0 ; i < d ; i++)
			{
				
				FIM[i + kdd] = invJnew[i + i*d];
				
				sum         += FIM[i + kdd];
				
			}
			
			tr[k + 1 + nK]       = sum;
			
			
			for(i = 0 ; i < d2 ; i++)
			{
				
				Jold[i]       = Jnew[i];
				
				invJold[i]    = invJnew[i];

#ifdef fulloutput

				J[i + kdd2]   = invJold[i];

#endif
				
			}

			
			for(i = 0 ; i < dM ; i++)
			{
				
				Xold[i]       =  Xnew[i];
				
			}
						
			
		}
		
	}
	
	
	
	mxFree(vect);
	
	mxFree(vv);
	
	mxFree(invQ1);
	
	mxFree(cholQ1);
	
	mxFree(cholQ);
	
	mxFree(Jnew);
	
	mxFree(Jold);
	
	mxFree(invJnew);
	
	mxFree(invJold);
	
	mxFree(Xnew);
	
	mxFree(Xold);
	
	mxFree(W);
	
	mxFree(Ftemp);
	
	mxFree(Qtemp);
	
	mxFree(Dtemp);
	
	mxFree(invDtemp);
	
	mxFree(Ft);
	
	mxFree(FX);
	
	mxFree(Y);
	
	mxFree(Ri);
	
	mxFree(Riinv);
	
	mxFree(grad_Zi);
	
	mxFree(gRiinv);
	
	mxFree(gRiinvgt);
	
	mxFree(EgRiinvgt);
	
	mxFree(vectZ);
	
	mxFree(vvZ);
	
	mxFree(HptEgRiinvgt);
	
	mxFree(E_PI);
	
	mxFree(yR);
	
	mxFree(FinvJFt);
	
	mxFree(indx);
	
	mxFree(indxZ);
	
}



/*------------------------------------------------------------------------------------*/

void    YRYt(double *y , double *R , int d , int m , 
			 double *Z , 
			 double *yR)
			 
{
	
	//     Z           = Y*R*Y', Y(d x m), R(m x d);
	
	int i  , t  , l , i2 , lm;

	register double temp;
	
	
	/* b ) --------- yR = y*R (d x m) -----------*/
	
	
	for (t = 0 ; t < d ; t++)
		
	{	
		
		for(l = 0 ; l < m ; l++)
		{
			
			lm          = l*m;
			
			temp        = 0.0;
						
			for(i = 0 ; i < m ; i++)
				
			{
				
				temp   += y[t + i*d]*R[i + lm];

				
			}

			yR[t + l*d]  = temp;

		}
	}  
	
	
	/* c ) --------- Z = yR*y^T (d x d) -----------*/
	
	
	
	for (t = 0 ; t < d ; t++)
		
	{
				
		for(l = 0 ; l < d  ; l++)
			
		{
			
			temp    = 0.0;
					
			for(i = 0 ; i < m ; i++)
				
			{
				
				i2            = i*d ;	
				
				temp         += yR[t + i2]*y[l + i2];
			
			}

			Z[t + l*d]   = temp;
			
		}
		
	}  
	
}

/* ----------------------------------------------------------------------- */


double esperance_PI(double *map , double *R , double *X , double *Hp , double *h ,
					double X_min , double X_max , double Y_min , double Y_max ,
					double *E_PI , int nR , int nC , int m , int d , int N ,
					double *Y , double *Ri , double *grad_Zi , double *Riinv,
					double *gRiinv , double *gRiinvgt , double *EgRiinvgt , 
					double *vect, double *vv , int *indx , double *HptEgRiinvgt)
					
					
{
	
	double temp , pente_Y , pente_X , cte_pente_Y , cte_pente_X;
	
	double Yx , Yy , Yhx, Yxh , Yhy , Yyh , floorY , floorX;
	
	double fhx_y , fxh_y , fx_hy , fx_yh;
	
	double floorYhy , floorYyh , floorYhx , floorYxh , hy , hx , hyy , hxx;
	

	double Y_min_h0 = Y_min + h[0], Y_max_h0 = Y_max - h[0], X_min_h1 = X_min + h[1], X_max_h1 = X_max - h[1]; 
		
    double Y_min_pente_Y, X_min_pente_X;  

	double cte_compteur = 1.0 , compteur = 0.0;	
	
	
	int i , j , l , k , id , i2 , lm ,  nm = nR*nC , r , v , t , d2 = 2; 
	
	int indexY , indexYhy , indexYyh , indexYhx , indexYxh;
	
	
	
	
    pente_Y       = fabs(Y_max - Y_min)/(nR - 1);
	
    pente_X       = fabs(X_max - X_min)/(nC - 1);


    Y_min_pente_Y = Y_min - pente_Y;
	
	X_min_pente_X = X_min - pente_X;
	
	
    cte_pente_Y   = 1.0/pente_Y;
	
    cte_pente_X   = 1.0/pente_X;
	
	
	hy            = h[0]*cte_pente_Y;
	
	hx            = h[1]*cte_pente_X;
	
	
	hyy           = 1.0/(2.0*h[0]);
	
	hxx           = 1.0/(2.0*h[1]);
	
	
	
	for (l = 0 ; l < 4 ; l++)
		
	{
		
		EgRiinvgt[l] = 0.0;
		
	}
	
	
	/*-------- Loop over N values of Yi ----------*/
	
	
	for (i = 0 ; i < N ; i++)
		
	{
		i2 = i*2;
		
		/*-------- Y = Hp*X ----------*/
		
		id = i*d;
		
		
		for(j = 0 ; j < d2 ; j++)
			
		{
			
			temp = 0.0;
			
			for(l = 0 ; l < d ; l++)
				
			{
				
				temp += Hp[j + l*2]*X[l + id];
				
			}
			
			
			Y[j] = temp;
			
		}
		
        if ( (Y[0] > Y_min_h0) && (Y[0] < Y_max_h0)  && (Y[1] > X_min_h1) && (Y[1] < X_max_h1) )
			
		{
			
			compteur += 1.0;

					
			/*-------- Normalized values laying in [Ymin Ymax Xmin Xmax] ----------*/
			
			
            Yy                  = (Y[0] - Y_min_pente_Y)*cte_pente_Y;
            
            Yx                  = (Y[1] - X_min_pente_X)*cte_pente_X;
					
			
			Yhy                 = Yy  - hy;
		
			Yyh                 = Yy  + hy;
				
			
			Yhx                 = Yx   - hx;
			
			Yxh                 = Yx   + hx;
			
			
			/* f(x , y) */
			
			
			floorY              = floor(Yy);
					
			floorX              = floor(Yx);
			
			indexY              = floorX + (floorY - 1)*nR - 1;
			
			Yy                  = (Yy - floorY);
			
			Yx                  = (Yx - floorX);
			
						
			/* f(x , y - h) */
			
			
			floorYhy            = floor(Yhy);
			
			indexYhy            = floorX + (floorYhy - 1)*nR - 1;
			
			Yhy                 = (Yhy - floorYhy);
			
			
			/* f(x , y + h) */
			
			floorYyh            = floor(Yyh);
			
			indexYyh            = floorX + (floorYyh - 1)*nR - 1;
			
			Yyh                 = (Yyh - floorYyh);
			
			
			/* f(x - h , y ) */
			
			
			floorYhx            = floor(Yhx);
			
			indexYhx            = floorYhx + (floorY - 1)*nR - 1;
			
			Yhx                 = (Yhx - floorYhx);
			
			
			/* f(x + h , y ) */
			
			
			floorYxh            = floor(Yxh);
			
			indexYxh            = floorYxh + (floorY - 1)*nR - 1;
			
			Yxh                 = (Yxh - floorYxh);
			
			
			/*-------- Gradient evaluation and measurement covariance ----------*/
			
			
			for (l = 0 ; l < m ; l++)
				
			{
					
				i2              = l*2;