pcrb.c

本文件采用Matlab软件

/* 

    Compute the Posterior Cramer Rao Bound associated with the fowolling discrete time dynamic systems for altimetric navigation:

    X_1       = N(X1 , Q1)
	  
	X_{k+1}   = FkXk + Uk + N(0 , Qk)

    Zk        = h(Xk) + N(0 , Rk(Xk))


    Usage
	-----

  
	[tr , FIM]         = PCRB(map , covmap , X1 , Q1 , F , U , Q , [M] , [Y_min] , [Y_max] , [X_min] , [X_max] , [Hp], [h]);

    or

  	[tr , FIM , J]     = PCRB(map , covmap , X1 , Q1 , F , U , Q , [M] , [Y_min] , [Y_max] , [X_min] , [X_max] , [Hp], [h]);

     
	if compiled with -Dfulloutput


  Inputs
  -------

  map               Measurements map (nR x nC x m)
  covmap            Measurements covariance map (nR x nC x m x m)
  X1                Initial state vector (d x 1 x N)
  Q1                Initial state covariance (d x d)
  F                 State transition matrix (d x d x K-1 x N)
  U                 Control command (d x K-1 x N)
  Q                 State covariance matrix (d x d x K-1 x N)
  M                 Number of paths to evaluate PCRB (default M = 1000)
  Y_min             Minimum value of y axis (default Y_min = 1)
  Y_max             Maximum value of y axis (default Y_max = nR)
  X_min             Minimum value of x axis (default Y_min = 1)
  X_max             Maximum value of x axis (default Y_max = nC)
  Hp                Transfert matrix (2 x d) (default )
  h                 Differential step (2 x 1)



 Ouputs
 -------


 tr                 Trace of a posteriori FIM versus time (1 x K x N)
 FIM                FIM versus time (d x K x N)
 J                  SubFisher matrix (d x d x K x N)


 Example 1
 ----------

 clear, clc , close all hidden;
 load colorado.mat
 map           = double(colorado);
 clear colorado;
 [nR , nC]     = size(map);      
 
 d             = 4;
 m             = 1;
 beta          = 0.15;                 % K = round(Ks*(1 + beta)) 
 N             = 100;
 M             = 1000;
 Ny            = 160;
 Nx            = 160;
 R             = sqrt(chi2inv(0.95 , 2));
 Ne            = 30;


 Y_min         = 0.0 ;                 % m
 Y_max         = 10000;                % m
 X_min         = 0.0 ;                 % m 
 X_max         = 10000;                % m
 Z_min         = 0;                    % Minimum elevation  (m)
 Z_max         = 300;                  % Maximum elevation  (m)

 pas_x         = 100; 
 pas_y         = 100;

 cov_ry        = (150)^2;              %m^2 %200
 cov_vy        = (0.3)^2;              %(m/s)^2 %0.2
 cov_rx        = (150)^2;              %m^2
 cov_vx        = (0.3)^2;              %(m/s)^2
 cov_R         = (10)^2;               %m^2

 cov_q         = (5*10e-4)^2;          % (m/s)^2
 v             = 40/3.6;               % m/s


 
 Q1            = [cov_ry 0 0 0 ; 0 cov_vy 0 0 ; 0 0 cov_rx 0; 0 0 0 cov_vx];

 Fmodel        = @(dt) [1 dt 0 0 ; 0 1 0 0 ; 0 0 1 dt ; 0 0 0 1 ];
 Qmodel        = @(dt,cov_q) [dt.^3/3 dt.^2/2 0 0 ;  dt.^2/2 dt 0 0 ; 0 0 dt.^3/3 dt.^2/2 ; 0 0 dt.^2/2 dt ].*cov_q;
 
 covmap        = cov_R*ones(nR , nC);
 Hp            = [1 0 0 0 ; 0 0 1 0 ];

 diff_Y        = abs(Y_max - Y_min);
 diff_X        = abs(X_max - X_min);
 pente_Y       = diff_Y/(Ny - 1);
 pente_X       = diff_X/(Nx - 1);
 vect_X        = (X_min : pente_X : X_max);
 vect_Y        = (Y_min : pente_Y : Y_max);
 [x , y]       = ndgrid(vect_X , vect_Y );
 V             = [x(:)' ; y(:)'];
 hrx           = diff_X/pas_x;            %m 
 hry           = diff_Y/pas_y;            %m 
 h             = [hry ; hrx ];
 mini          = min(map(:));
 maxi          = max(map(:));
 map           = Z_min + (Z_max - Z_min)*(map - mini)/(maxi - mini);

 S             = Nx*Ny;

 I             = indice_ij(Nx , Ny);
 delta         = circulant([1 1 0 0 0 0 0 1 ]);


 figure(1)
%set(gcf , 'renderer' , 'opengl')
 imagesc(vect_X , vect_Y , map)
 colormap(flipud(pink))
 axis xy
 hold on
 plot(V(1 , :) , V(2 , :) , '+')

 hh            = title('Select the first exlusion zone');
 set(hh  , 'fontsize' , 13 , 'fontname' , 'times' , 'FontWeight' , 'bold')

 [xx , yy]     = getline(gcf , 'closed');
 node          = [xx' ; yy'];

 hold on
 plot(xx , yy , 'r', 'linewidth' , 2)
 [in , bnd]    = inpoly(V , node);
 exclu         = find(in)';
 Xexclu        = V(1,exclu);
 Yexclu        = V(2,exclu);

%bnd                 = convhull(Xexclu, Yexclu);
%plot(Xexclu(bnd) , Yexclu(bnd) , 'ro', 'linewidth' , 2 )
 plot(Xexclu , Yexclu , 'ro', 'linewidth' , 2 )

 I             = exclusion(I , exclu);
 delta         = circulant([1 1 0 0 0 0 0 1 ]);


 hh            = title('Select the second exlusion zone');
 set(hh  , 'fontsize' , 13 , 'fontname' , 'times' , 'FontWeight' , 'bold')
 [xx , yy]     = getline(gcf , 'closed');
 node          = [xx' ; yy'];

 hold on
 plot(xx , yy , 'r', 'linewidth' , 2)
 [in , bnd]    = inpoly(V , node);
 exclu         = find(in)';
 Xexclu        = V(1,exclu);
 Yexclu        = V(2,exclu);
 plot(Xexclu , Yexclu , 'ro', 'linewidth' , 2 )

 I             = exclusion(I , exclu);


 hh            = title('Select start zone');
 set(hh  , 'fontsize' , 13 , 'fontname' , 'times' , 'FontWeight' , 'bold')


 [xx , yy]     = getline(gcf , 'closed');
 node          = [xx' ; yy'];
 plot(xx , yy , 'k', 'linewidth' , 2)
 [in , bnd]    = inpoly(V , node);
 plot(V(1 , in) , V(2 , in) , 'y*', 'linewidth' , 2 )
 start         = find(in)';


 hh            = title('Select finish zone');
 set(hh  , 'fontsize' , 13 , 'fontname' , 'times' , 'FontWeight' , 'bold')


 [xx , yy]     = getline(gcf , 'closed');
 node          = [xx' ; yy'];
 plot(xx , yy , 'k', 'linewidth' , 2)
 [in , bnd]    = inpoly(V , node);
 plot(V(1 , in) , V(2 , in) , 'm^', 'linewidth' , 2 )
 finish        = find(in)';
 drawnow


 %%%% Shortest path %%%%

 C             = mat_dist(V,I);
 index_Y_spath = mdp_contraintes3(start, finish, I , -C , delta);
 Y_spath       = V(: , index_Y_spath);
 plot(Y_spath(1 , :) , Y_spath(2 , :) , 'y' , V(1 , start) ,  V(2 , start) , '^' , V(1 , finish) , V(2 , finish ) , 'sg'  , 'linewidth' , 2 )
 drawnow
 K             = max(1,round(length(Y_spath)*(1 + beta)));


%%%% Random paths %%%%

  
 C            = mat_uni(I);
 index_Y_rand = path_generator2(start, finish,I, C, K, N, delta);
 Y_rand       = reshape(V(: , index_Y_rand) , [2 , K , N]);

%%%% PCRB %%%%


 [dt , cap_t] = dt_cap(Y_rand , v , 1);
 [X1 , U]     = commande(Y_rand , v , dt  , cap_t);
 [F , Q]      = modelFQ(Fmodel , Qmodel , dt , cov_q , d);


 [tr, FIM, J] = pcrb(map , covmap , X1 , Q1 , F , U , Q , M , Y_min , Y_max , X_min , X_max , Hp , h);

 [xe , ye]    = ndellipse(reshape(V(: , index_Y_rand) , [2 , 1 , K , N]) , ndyRyt(Hp , J) , eye(2) , R , Ne);

 dist_rand    = squeeze(sum(sqrt(sum(Y_rand.^2 , 1)) , 2));

 figure(1)

%  imagesc(vect_X , vect_Y , map)
%  colormap(flipud(pink))
%  axis xy
%  hold on
%  plot(V(1 , :) , V(2 , :) , '+')
  plot(reshape(Y_rand(1 , : , :) , K , N) , reshape(Y_rand(2 , : , :) , K , N) , 'linewidth' , 2)
%  plot(V(1 , start) , V(2 , start) , 'yo' , 'markersize' , 10 , 'linewidth' , 2)
%  plot(V(1 , finish) , V(2 , finish) , 'g^' , 'markersize' , 10 , 'linewidth' , 2)
  plot(reshape(cat(1 , xe , repmat(nan , [1 , K , N])) , (Ne+1)*K , N) , reshape(cat(1 , ye , repmat(nan , [1 , K , N])) , (Ne+1)*K , N) , 'linewidth' , 2)
  hold off
  hh          = title(sprintf('N = %d, S = %d, K = %d, \\beta = %3.2f, v = %4.2f (m/s), \\sigma_q = %4.4f (m/s), \\sigma_R = %4.2f (m)', N, S , K , beta , v, sqrt(cov_q), sqrt(cov_R)));
  set(hh  , 'fontsize' , 13 , 'fontname' , 'times' , 'FontWeight' , 'bold')
 
  
  figure(2)
  semilogy((1:K),squeeze(sqrt(tr(1 , : , :))), 'linewidth' , 2)
  grid on
  hh          = xlabel('number of legs');
  set(hh  , 'fontsize' , 12 , 'fontname' , 'times' , 'FontWeight' , 'bold')
  hh          = ylabel('sqrt(Tr(FIM_{1:K}))');
  set(hh  , 'fontsize' , 12 , 'fontname' , 'times' , 'FontWeight' , 'bold')
  axis tight

  figure(3)
  plot(squeeze(sum(tr , 2)), 'linewidth' , 2)
  grid on
  hh          = xlabel('number of paths');
  set(hh  , 'fontsize' , 12 , 'fontname' , 'times' , 'FontWeight' , 'bold')
  hh          = ylabel('sum(sqrt(Tr(FIM_{1:K})))');
  set(hh  , 'fontsize' , 12 , 'fontname' , 'times' , 'FontWeight' , 'bold')



 Compile with:
 ------------

 mex -g -output pcrb.dll pcrb.c

 mex -f mexopts_intel10amd.bat -output pcrb.dll pcrb.c

 or compile with the fulloutput option to output J matrices

 mex -f mexopts_intel10amd.bat -Dfulloutput -output pcrb.dll pcrb.c


 Author          S閎astien PARIS ([e-mail]) (5/4/08)
 -------

	
*/

#include <math.h>
#include <time.h>
#include "mex.h"


#define max(a , b) ((a) >= (b) ? (a) : (b))

#define NUMERICS_FLOAT_MIN 1.0E-37


#define mix(a , b , c) \
{ \
	a -= b; a -= c; a ^= (c>>13); \
	b -= c; b -= a; b ^= (a<<8); \
	c -= a; c -= b; c ^= (b>>13); \
	a -= b; a -= c; a ^= (c>>12);  \
	b -= c; b -= a; b ^= (a<<16); \
	c -= a; c -= b; c ^= (b>>5); \
	a -= b; a -= c; a ^= (c>>3);  \
	b -= c; b -= a; b ^= (a<<10); \
	c -= a; c -= b; c ^= (b>>15); \
}

#define zigstep 128 // Number of Ziggurat'Steps


#define znew   (z = 36969*(z&65535) + (z>>16) )

#define wnew   (w = 18000*(w&65535) + (w>>16) )

#define MWC    ((znew<<16) + wnew )

#define SHR3   ( jsr ^= (jsr<<17), jsr ^= (jsr>>13), jsr ^= (jsr<<5) )

#define CONG   (jcong = 69069*jcong + 1234567)

#define KISS   ((MWC^CONG) + SHR3)


#define randint KISS

#define rand() (randint*2.328306e-10)



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


typedef unsigned long UL;


/*------------------------------------------ Static Def -----------------------------------------*/


static UL z=362436069, w=521288629, jcong=380116160 , jsrseed = 31340134 , jsr;

static UL iz , kn[zigstep];

static long hz;

static double wn[zigstep] , fn[zigstep];


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


void randnini(void);

void randini(void);  


double nfix(void);

double randn(void); 



int  chol(double *  , double * , int);

void mult_trans(double *, double * , double * , int , int , int);

void YRYt(double * , double * , int  , int  , double * , double *);



void transpose(double *, double * , int);

void mvg(double *, double *, int , int );

void lubksb(double *, int , int *, double *);

int  ludcmp(double *, int , int *, double * , double *);

void inv(double * , double * , int * , double * , double * , int ) ;



void YRinvYt(double * , double * , double * ,
			 double * , double * , double * , double * , 
			 int * ,  int , int); 


void  HptEgRiinvgtHp(double * , double *  , double *  , double * , int , int );



double esperance_PI(double * , double * , double * , double * , double * ,
					double  , double  , double  , double  , double * ,
					int , int , int , int , int , double * , double * ,
					double * , double * , double * , double * , double * , 
					double * , double * , int* , double *);
#ifdef fulloutput

void     PCRB(double * , double * , double * , double * , double * , double * , double * , int , double  , double , double , double , double *, double *,
			  double * , double *, double *,
			  int  , int , int , int, int , int );


#else

void     PCRB(double * , double * , double * , double * , double * , double * , double * , int , double  , double , double , double , double *, double *,
			  double * , double *,
			  int  , int , int , int, int , int );

#endif

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


void mexFunction(int nlhs, mxArray *plhs[],
				 int nrhs,  const mxArray *prhs[])
{
	
	
	// Inputs //
	
	
	double *map , *covmap , *X1 , *U , *Q1 , *F , *Q , *Hp, *h;
	
	double Y_min=1.0 , Y_max , X_min=1.0 , X_max;
	
	int M;
	
	const int *dimsmap , *dimscovmap , *dimsX1, *dimsU, *dimsQ1 , *dimsF, *dimsQ, *dimsHp ,  *dimsh ;
		
	int numdimsmap , numdimscovmap ,  numdimsX1 , numdimsU, numdimsQ1, numdimsF,  numdimsQ ,  numdimsHp ,  numdimsh;
	
	// Outputs //
	
	
	double *FIM , *tr ;
	
	int *dimsFIM, *dimstr;
	
	int numdimsFIM=2 , numdimstr=2;

#ifdef fulloutput

    double *J;

	int *dimsJ;

 	int numdimsJ = 3;
   	
#endif

	
	int nR , nC , m=1 , d , K , N = 1,steph = 1000;
	
	
	if((nrhs < 7) )    
		
	{
#ifdef fulloutput
		
		mexErrMsgTxt("Usage : \n [tr , FIM , J]  = PCRB(map , covmap , X1 , Q1 , F , U , Q , [M] , [Y_min] , [Y_max] , [X_min] , [X_max] , [Hp], [h]);");

#else

		mexErrMsgTxt("Usage : \n [tr , FIM]  = PCRB(map , covmap , X1 , Q1 , F , U , Q , [M] , [Y_min] , [Y_max] , [X_min] , [X_max] , [Hp], [h]);");

#endif
	
	}
	
	/* --- Input 1 ---*/
	
    map        = mxGetPr(prhs[0]);
    
    numdimsmap = mxGetNumberOfDimensions(prhs[0]);
    
	dimsmap    = mxGetDimensions(prhs[0]);
	
	if (numdimsmap > 3)
	{
		
		mexErrMsgTxt("Input 1 must be (nR x nC x m)");
		
	}
	
	nR                 = dimsmap[0];
	
	nC                 = dimsmap[1];
	
	if (numdimsmap == 3)
	{
		
		m                  = dimsmap[2];
		
	}
	
	
	/* --- Input 2 ---*/
	
    covmap             = mxGetPr(prhs[1]);
    
    numdimscovmap      = mxGetNumberOfDimensions(prhs[1]);
    
	dimscovmap         = mxGetDimensions(prhs[1]);
	
	if ((dimscovmap[0] != nR) || (dimscovmap[1] != nC) || (numdimscovmap > 4) || (numdimscovmap == 3) )
		
	{
		
		mexErrMsgTxt("Input 2 must be (nR x nC x m x m)");
		
		
	}
	
	if (numdimscovmap == 4 )
		
	{	
		if ( (dimscovmap[2] != m) || (dimscovmap[3] != m))
		{
			mexErrMsgTxt("Input 2 must be (nR x nC x m x m)");
		}
		
	}
	
	
	
	/* --- Input 3 ---*/
	
    X1                = mxGetPr(prhs[2]);
    
    numdimsX1         = mxGetNumberOfDimensions(prhs[2]);
    
	dimsX1            = mxGetDimensions(prhs[2]);
	
	if ( (numdimsX1 < 2) || (numdimsX1 > 3) )
		
	{	
		
		mexErrMsgTxt("Input 3 must be (d x 1 x N)");
		
		
	}
	
	if (dimsX1[1] != 1)
	{
		
		mexErrMsgTxt("Input 3 must be (d x 1 x N)");
		
	}
	
    d                 = dimsX1[0];
	
	if (numdimsX1 == 3)
	{
		
		N             = dimsX1[2];
		
	}
	
	
	
	/* --- Input 4 ---*/


    Q1                = mxGetPr(prhs[3]);
    
    numdimsQ1         = mxGetNumberOfDimensions(prhs[3]);
    
	dimsQ1            = mxGetDimensions(prhs[3]);
	
	if ((numdimsQ1 != 2))
		
	{	
		
		mexErrMsgTxt("Input 4 must be (d x d)");
		
	}
	
	
	if ((dimsQ1[0] != d) || (dimsQ1[1] != d))
		
	{	
		
		mexErrMsgTxt("Input 4 must be (d x d)");
		
	}

	
	
	/* --- Input 5 ---*/
	
    F                 = mxGetPr(prhs[4]);
    
    numdimsF          = mxGetNumberOfDimensions(prhs[4]);
    
	dimsF             = mxGetDimensions(prhs[4]);
	
	if ((dimsF[0] != d) || (dimsF[1] != d) || (numdimsF != (numdimsX1 + 1)))
		
	{	
		
		mexErrMsgTxt("Input 5 must be (d x d x K-1 x N)");
		
	}

    K                 = dimsF[2] + 1;

	if(numdimsF == 4)
	{
		
		if (dimsF[3] != N)
		{
			
			mexErrMsgTxt("Input 5 must be (d x d x K-1 x N)");
			
		}
		
	}
	
	
	/* --- Input 6 ---*/
	
	

    U                 = mxGetPr(prhs[5]);
    
    numdimsU          = mxGetNumberOfDimensions(prhs[5]);
    
	dimsU             = mxGetDimensions(prhs[5]);
	
	if ((dimsU[0] != d) || (dimsU[1] != (K - 1)) || (numdimsU != numdimsX1))
		
	{	
		
		mexErrMsgTxt("Input 6 must be (d x K-1 x N)");
		
	}
	
	if(numdimsU == 3)
	{
		
		if (dimsU[2] != N)
		{
			
			mexErrMsgTxt("Input 6 must be (d x K-1 x N)");
			
		}
		
		
	}
	

	
	/* --- Input 7 ---*/
	
	
    Q                 = mxGetPr(prhs[6]);
    
    numdimsQ          = mxGetNumberOfDimensions(prhs[6]);
    
	dimsQ             = mxGetDimensions(prhs[6]);
	
	if ((dimsQ[0] != d) || (dimsQ[1] != d) || (numdimsQ != (numdimsX1 + 1)))
		
	{	
		
		mexErrMsgTxt("Input 7 must be (d x d x K-1 x N)");
		
	}
	
	if(numdimsQ > 2)
	{
		
		if (dimsQ[2] != (K - 1))
		{
			
			mexErrMsgTxt("Input 7 must be (d x d x K-1 x N)");
			
		}
		
	}
	
	if(numdimsQ == 4)
	{
		
		if (dimsQ[3] != N)
		{
			
			mexErrMsgTxt("Input 7 must be (d x d x K-1 x N)");
			
		}
		
	}
	
	
	/* --- Input 8 ---*/
	
	if (nrhs > 7)
		
	{
		
		M         = (int) mxGetScalar(prhs[7]);
		
	}
	
	/* --- Input 9 ---*/
	
	
	if (nrhs > 8)
		
	{
		
		Y_min  = mxGetScalar(prhs[8]);
		
	}
	
	
	
	/* --- Input 10 ---*/
	
	
	if (nrhs > 9)
		
	{
		
		Y_max  = mxGetScalar(prhs[9]);
		
	}
	
	else
		
	{
		
		Y_max  = (double) nR;
		
		
	}
	
	/* --- Input 11 ---*/
	
	if (nrhs > 10)
		
	{
		
		X_min  = mxGetScalar(prhs[10]);
		
	}
	
	
	/* --- Input 12 ---*/
	
	
	if (nrhs > 11)
		
	{
		
		X_max  = mxGetScalar(prhs[11]);
		
	}
	
	else
		
	{
		
		X_max  = (double) nC;
		
	}
	
	
	/* --- Input 13 ---*/
	
	if (nrhs > 12)
		
	{
		
		Hp         = mxGetPr(prhs[12]);
		
		numdimsHp  = mxGetNumberOfDimensions(prhs[12]);
		
		dimsHp     = mxGetDimensions(prhs[12]);
		
		if ((numdimsHp != 2))
			
		{	
			
			mexErrMsgTxt("Input 13 must be (2 x d)");
			
		}
		
		if ((dimsHp[0] != 2) || (dimsHp[1] != d))
			
		{	
			
			mexErrMsgTxt("Input 13 must be (2 x d)");
			
		}
		
		
	}
	
	else
		
	{
		
		Hp    = (double *)mxMalloc(2*d*sizeof(double));
		
		Hp[0] = 1.0;
		
		if(d > 3)
		{
			
			Hp[5] = 1.0;
			
		}
		
	}
	
		
	/* --- Input 14 ---*/
	
	if (nrhs > 13)
		
	{
		
		h         = mxGetPr(prhs[13]);
		
		numdimsh  = mxGetNumberOfDimensions(prhs[13]);
		
		dimsh     = mxGetDimensions(prhs[13]);
		
		if ((numdimsh != 2))
			
		{	
			
			mexErrMsgTxt("Input 14 must be (2 x 1)");
			
		}
		
		if ((dimsh[0] != 2) || (dimsh[1] != 1))
			
		{	
			
			mexErrMsgTxt("Input 14 must be (2 x 1)");
			
		}
			
	}