📄 likelihood_power2.c
字号:
/*
Compute the likelihood of RSSI measurements. Model is based on a Raytracing algorithm with nr (nr<=3) reflexions
Usage
------
like = likelihood_power2(z , TXpoint , RXpoint , planes , material , fc , sigma , [nr] , [flp_scale]);
Inputs
-------
z RSSI Measurements (Nt x 1)
TXpoint Transmitter points (3 x Nt)
RXpoint Receiver points (3 x Nr)
planes Planes (22 x nplanes)
material Material (6 x nplanes)
fc Central Frequency
sigma Measurement noise covariance
nr Numbre of reflexion (nr>= 0 and nr < 4, default nr = 3)
flp_scale Scale factor (default = 100)
Outputs
-------
like Likelihood (1 x Nr)
rs_amp Total power (Nt x Nr)
To compile
----------
mex -output likelihood_power2.dll likelihood_power2.c
mex -f mexopts_intel10amd.bat -output likelihood_power2.dll likelihood_power2.c
Example 1
---------
flp = load_flp('norwich01.flp');
nr = 1;
Nt = 5;
sigma = (0.001)^2;
N = 1000;
Cov = [1000000 0 0 ; 0 1000000 0 ; 0 0 100];
temp = flp.geom.planes([1 , 4 , 7] , :);
xmin = min(temp(:));
xmax = max(temp(:));
temp = flp.geom.planes([2 , 5 , 8] , :);
ymin = min(temp(:));
ymax = max(temp(:));
temp = flp.geom.planes([3 , 6 , 9] , :);
zmin = min(temp(:));
zmax = max(temp(:));
vect1 = [xmin ; ymin ; zmin];
vect2 = [xmax-xmin ; ymax-ymin ; zmax-zmin];
Cov = [1000000 0 0 ; 0 1000000 0 ; 0 0 100];
flp.info.TXpoint = flp.info.TXpoint(: , ones(1 , Nt)) + chol(Cov)'*randn(3 , Nt);
ON = ones(1 , N);
rs_amp = total_power3(flp.info.TXpoint , flp.info.RXpoint , flp.geom.planes , flp.geom.material , flp.info.fc , nr);
z = rs_amp + sqrt(sigma)*randn(Nt , 1);
RXpoint = vect1(: , ON) + vect2(: , ON).*rand(3 , N);
like = likelihood_power2(z , flp.info.TXpoint , RXpoint , flp.geom.planes , flp.geom.material , flp.info.fc , sigma , nr);
figure(1)
h = plot_flp(flp);
figure(2)
plot(like)
Example 2
---------
close all, clear
flp = load_flp('norwich01.flp');
flp.info.nr = 2;
nb_pts = 120;
option.TX = 0;
option.RX = 0;
option.path = 0;
sigma = (5*10e-6)^2;
temp = flp.geom.planes([1 , 4 , 7] , :);
xmin = min(temp(:));
xmax = max(temp(:));
temp = flp.geom.planes([2 , 5 , 8] , :);
ymin = min(temp(:));
ymax = max(temp(:));
temp = flp.geom.planes([3 , 6 , 9] , :);
zmin = min(temp(:));
zmax = max(temp(:));
vect1 = [xmin ; ymin ; zmin];
vect2 = [xmax-xmin ; ymax-ymin ; zmax-zmin];
figure(1)
h = plot_flp(flp , option);
hold on
title('Selection Beacons');
[xx , yy] = getpts;
Nt = size(xx , 1);
temp = (zmax-zmin)/2;
flp.info.TXpoint = [xx' ; yy' ; temp(: , ones(1 , Nt))];
plot(flp.info.TXpoint(1 , :) , flp.info.TXpoint(2 , :) , 'c*');
drawnow
title('Selection target');
[xx , yy] = getpts;
flp.info.RXpoint = [xx(1)' ; yy(1)' ; temp];
plot(flp.info.RXpoint(1 , :) , flp.info.RXpoint(2 , :) , 'gp');
hold off
drawnow
pasx = (xmax-xmin)/(nb_pts-1);
vectx = (xmin:pasx:xmax);
pasy = (ymax-ymin)/(nb_pts-1);
vecty = (ymin:pasy:ymax);
[X , Y] = meshgrid(vectx , vecty);
Z = ((zmax-zmin)/2)*ones(nb_pts , nb_pts);
RX = [X(:) , Y(:) , Z(:)]';
rs_amp = total_power3(flp.info.TXpoint , flp.info.RXpoint , flp.geom.planes , flp.geom.material , flp.info.fc , flp.info.nr);
z = rs_amp + sqrt(sigma)*randn(Nt , 1);
like = reshape(likelihood_power2(z , flp.info.TXpoint , RX , flp.geom.planes , flp.geom.material , flp.info.fc , sigma , flp.info.nr), nb_pts , nb_pts);
[val , pos] = max(reshape(like , nb_pts*nb_pts , 1));
[maxy , maxx] = ind2sub([nb_pts , nb_pts] , pos);
figure(2)
imagesc(vectx , vecty , (like))
hold on
h = plot_flp(flp , option);
hold on
plot(flp.info.TXpoint(1 , :) , flp.info.TXpoint(2 , :) , 'c*');
plot(flp.info.RXpoint(1 , :) , flp.info.RXpoint(2 , :) , 'gp');
%plot(maxi*pasx , maxj*pasy , 'yo');
plot(maxx*pasx + xmin , maxy*pasy + ymin , 'yo');
title('Likelihood')
xlabel('x in pixels')
ylabel('y in pixels')
zlabel('z in pixles')
axis xy
hold off
colorbar
figure(3)
surfc(vectx , vecty , like)
shading interp
hold on
plot(flp.info.TXpoint(1 , :) , flp.info.TXpoint(2 , :) , 'c*');
plot(flp.info.RXpoint(1 , :) , flp.info.RXpoint(2 , :) , 'gp');
plot(maxx*pasx + xmin , maxy*pasy + ymin , 'yo');
title('Likelihood')
xlabel('x in pixels')
ylabel('y in pixels')
zlabel('z in pixles')
axis xy
hold off
Author : S閎astien PARIS : sebastien.paris@lsis.org
------- Date : 10/09/2007
Reference : Jacques Beneat, http://www2.norwich.edu/jbeneat/
----------
*/
#include <math.h>
#include <mex.h>
#define PI 3.14159265358979323846
void fct_calimage(double * , double * , int , double *) ;
bool fct_calrpt(double *, double *, double * , int , double * );
double * fct_caltpts(double *, double *, double *, int , double * , int *, double *);
void compute_distloss(double * , int , double * , double * , double , double , int , double * , double *);
void qsindex( double * , int * , int , int );
/*-------------------------------------------------------------------------------------------------------------- */
double c = 3e8 , Ant = 1.0 , loss_factor = 1.0 , t_tol = 1.0;
void mexFunction( int nlhs, mxArray *plhs[] , int nrhs, const mxArray *prhs[] )
{
double *z , *RXpoint , *TXpoint , *planes , *material;
double fc , sigma , flp_scale = 100.0;
int nr = 3;
double *like , *rs_amp;
double *rpath1=NULL , *rpath2=NULL , *rpath3=NULL;
double *path0=NULL , *path1=NULL , *path2=NULL , *path3=NULL;
double temp , res , ctelike , ctesig , sumtemp , tiny = 1.0E-50;
int nplanes , i , j , k , id , jd , kd , indice , ind , Nt , Nr , n , l , nd , ld , nNt;
int npath1 = 0 , npath2 = 0 , npath3 = 0 , npath , currentpath = 0 , nb_pts;
double *RX , *TX , *TX_i , *Phiti , *TX_j , *Phitj , *Phitij , *TX_k , *Phitk , *Phitjk , *Phitijk , *Phit_nt;
double *Thits0=NULL ;
double *Thits11=NULL , *Thits12=NULL;
double *Thits21=NULL , *Thits22=NULL, *Thits23=NULL;
double *Thits31=NULL , *Thits32=NULL, *Thits33=NULL, *Thits34=NULL;
double *distance , *lossfac;
int tflag0 , tflag11 , tflag12 , tflag21 , tflag22 , tflag23 , tflag31 , tflag32 , tflag33 , tflag34;
bool rflagi=false , rflagj=false , rflagij=false , rflagk=false , rflagjk=false , rflagijk=false;
/*-------------------------- Inputs ------------------------ */
if( (nrhs <7) || (nrhs >9) )
{
mexErrMsgTxt("At least 7 inputs are requiered for likelihood_power");
}
/* Inputs 1*/
z = mxGetPr(prhs[0]);
Nt = mxGetM(prhs[0]);
/* Inputs 2*/
TXpoint = mxGetPr(prhs[1]);
if( mxGetNumberOfDimensions(prhs[1]) !=2 || mxGetM(prhs[1]) != 3 || mxGetN(prhs[1]) != Nt )
{
mexErrMsgTxt("RXpoint must be (3 x Nt)");
}
/* Inputs 3 */
RXpoint = mxGetPr(prhs[2]);
if( mxGetNumberOfDimensions(prhs[2]) !=2 || mxGetM(prhs[2]) != 3 )
{
mexErrMsgTxt("RXpoint must be (3 x Nr)");
}
Nr = mxGetN(prhs[2]);
/* Inputs 4 */
planes = mxGetPr(prhs[3]);
if( mxGetNumberOfDimensions(prhs[3]) !=2 || mxGetM(prhs[3]) !=22 )
{
mexErrMsgTxt("planes must be (22 x nplanes)");
}
nplanes = mxGetN(prhs[3]);
/* Inputs 5 */
material = mxGetPr(prhs[4]);
if( mxGetNumberOfDimensions(prhs[4]) !=2 || mxGetM(prhs[4]) !=6 )
{
mexErrMsgTxt("material must be (6 x nplanes)");
}
/* Inputs 6 */
fc = mxGetScalar(prhs[5]);
/* Inputs 7 */
sigma = mxGetScalar(prhs[6]);
ctelike = 1.0/(sqrt(pow(2.0*PI , Nt)*sigma));
ctesig = -0.5/(sigma);
if( nrhs > 7)
{
nr = (int)mxGetScalar(prhs[7]);
if((nr < 0) || (nr > 3))
{
mexErrMsgTxt("nr must be [0,1,2,3]");
}
}
if( nrhs > 8)
{
flp_scale = mxGetScalar(prhs[8]);
}
/* Memory allocation */
RX = mxMalloc(3*sizeof(double));
TX = mxMalloc(3*sizeof(double));
TX_i = mxMalloc(3*sizeof(double));
Phiti = mxMalloc(3*sizeof(double));
TX_j = mxMalloc(3*sizeof(double));
Phitj = mxMalloc(3*sizeof(double));
Phitij = mxMalloc(3*sizeof(double));
TX_k = mxMalloc(3*sizeof(double));
Phitk = mxMalloc(3*sizeof(double));
Phitjk = mxMalloc(3*sizeof(double));
Phitijk = mxMalloc(3*sizeof(double));
Phit_nt = mxMalloc(3*sizeof(double));
/* Outputs */
plhs[0] = mxCreateDoubleMatrix(1 , Nr, mxREAL);
like = mxGetPr(plhs[0]);
plhs[1] = mxCreateDoubleMatrix(Nt , Nr, mxREAL);
rs_amp = mxGetPr(plhs[1]);
for (n = 0 ; n < Nr ; n++)
{
nd = n*3;
nNt = n*Nt;
sumtemp = 0.0;
for (i = 0 ; i < 3 ; i++)
{
RX[i] = RXpoint[i + nd];
}
for (l = 0 ; l < Nt ; l++)
{
ld = l*3;
currentpath = 0;
npath1 = 0;
npath2 = 0;
npath3 = 0;
for (i = 0 ; i < 3 ; i++)
{
TX[i] = TXpoint[i + ld];
}
if(nr > 0)
{
for (i = 0 ; i < nplanes ; i++)
{
fct_calimage(TX , planes , i , TX_i);
rflagi = fct_calrpt(TX_i, RX, planes , i , Phiti);
if (rflagi == true)
{
id = npath1*5;
npath1++;
rpath1 = (double *)mxRealloc((double *)rpath1 , (id + 5)*sizeof(double));
rpath1[0 + id] = Phiti[0];
rpath1[1 + id] = Phiti[1];
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -