📄 likelihood_nav.c
字号:
/* likelihood_nav.c
Dans matlab il faut compiler de la mani鑢e suivante : mex likelihood_nav.c
nR = 100;
nC = 120;
m = 2;
N = 100;
map = rand(nR , nC , m);
R = rand(nR , nC , m , m);
Z = randn(m , 1);
X = repmat([10 ; 10 ; 10 ; 10] , 1 , N) + randn(4 , N);
Hp = [1 0 0 0 ; 0 0 1 0];
X_min = 1;
X_max = 500;
Y_min = 1;
Y_max = 1000;
like = likelihood_nav(Z , map , R , X , Hp , X_min , X_max , Y_min , Y_max);
mex likelihood_nav.c
mex -f mexopts_intel10amd.bat -output likelihood_nav.dll likelihood_nav.c
mex -f mexopts_intelamd.bat -L"C:\Program Files\Microsoft Visual Studio\VC98\Lib" -lMSVCRT -output likelihood_nav.dll likelihood_nav.c
*/
#include <math.h>
#include "mex.h"
#define NUMERICS_FLOAT_MIN 1.0E-37
#define PI 3.14159265358979323846
double my_nan(void)
{
double zero = 0.0;
return zero/zero;
}
#define NAN my_nan()
/*-------------------------------------------------------------------------*/
void lubksb(double *, int , int *, double *);
/*-------------------------------------------------------------------------*/
int ludcmp(double *, int , int *, double * , double *);
/* ------------------------------------------------------------------------------------- */
void likelihood_nav(double * , double * , double * , double * , double * , double , double , double , double ,
double *,
int , int , int , int , int , double * , double * , double * , double * , double *,
double *, double *, int *);
/* ------------------------------------------------------------------------------------- */
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
double *Z , *map , *R , *Hp , *X;
double *like;
double *Y , *Ri , *Zi , *res , *Riinv , *vect , *vv;
double X_min , X_max , Y_min , Y_max;
int *dimsZ , *dimsmap , *dimsR , *dimsX , *dimsHp;
int *dimslike , *dimsZi;
int *indx;
int numdimsZ , numdimsmap, numdimsR , numdimsX , numdimsHp;
int numdimslike , numdimsZi;
int nR , nC , m , d , N;
/* Check input */
if(nrhs < 5)
{
mexErrMsgTxt("like = likelihood_nav(map , R , X , Hp , X_min , X_max , Y_min , Y_max)");
}
/* Input 1 */
Z = mxGetPr(prhs[0]);
numdimsZ = mxGetNumberOfDimensions(prhs[0]);
dimsZ = mxGetDimensions(prhs[0]);
if ( (numdimsZ < 2) || (numdimsZ > 3) || (dimsZ[1] != 1) )
{
mexErrMsgTxt("First input must be (m x 1)");
}
m = dimsZ[0];
/* Input 2 */
map = mxGetPr(prhs[1]);
numdimsmap = mxGetNumberOfDimensions(prhs[1]);
dimsmap = mxGetDimensions(prhs[1]);
if ( (numdimsmap < 2) || (numdimsmap > 3) )
{
mexErrMsgTxt("Second input must be (nR x nC x m)");
}
nR = dimsmap[0];
nC = dimsmap[1];
/* Input 3 */
R = mxGetPr(prhs[2]);
numdimsR = mxGetNumberOfDimensions(prhs[2]);
dimsR = mxGetDimensions(prhs[2]);
if (( (nR != dimsR[0] ) || (nC != dimsR[1]) || (numdimsmap == 2) && (numdimsR != 2) ) || (numdimsmap == 3 ) && (numdimsR != 4) )
{
mexErrMsgTxt("Third input must be (nR x nC x m x m)");
}
/* Input 4 */
X = mxGetPr(prhs[3]);
numdimsX = mxGetNumberOfDimensions(prhs[3]);
dimsX = mxGetDimensions(prhs[3]);
if (numdimsX != 2)
{
mexErrMsgTxt("Fourth input must be (d x N)");
}
d = dimsX[0];
N = dimsX[1];
/* Input 5 */
Hp = mxGetPr(prhs[4]);
numdimsHp = mxGetNumberOfDimensions(prhs[4]);
dimsHp = mxGetDimensions(prhs[4]);
if ( (numdimsHp != 2) || (dimsHp[1] != d) )
{
mexErrMsgTxt("Fifth input must be (2 x d)");
}
/* Input 6 */
if (nrhs < 6)
{
X_min = 1.0;
}
else
{
X_min = mxGetScalar(prhs[5]);
}
/* Input 7 */
if (nrhs < 7)
{
X_max = (double) nR;
}
else
{
X_max = mxGetScalar(prhs[6]);
}
/* Input 8 */
if (nrhs < 8)
{
Y_min = 1.0;
}
else
{
Y_min = mxGetScalar(prhs[7]);
}
/* Input 9 */
if (nrhs < 9)
{
Y_max = (double) nC;
}
else
{
Y_max = mxGetScalar(prhs[8]);
}
Y = (double *)mxMalloc((2)*sizeof(double));
res = (double *)mxMalloc(m*sizeof(double));
Ri = (double *)mxMalloc((m*m)*sizeof(double));
Riinv = (double *)mxMalloc((m*m)*sizeof(double));
vect = (double *)mxMalloc(m*sizeof(double));
vv = (double *)mxMalloc(m*sizeof(double));
indx = (int *)mxMalloc(m*sizeof(int));
/*--------------- Output 1------------------ */
numdimslike = 2;
dimslike = (int *)mxMalloc(numdimslike*sizeof(int));
dimslike[0] = 1;
dimslike[1] = N;
plhs[0] = mxCreateNumericArray(numdimslike , dimslike, mxDOUBLE_CLASS, mxREAL);
like = mxGetPr(plhs[0]);
numdimsZi = 2;
dimsZi = (int *)mxMalloc(numdimsZi*sizeof(int));
dimsZi[0] = m;
dimsZi[1] = N;
plhs[1] = mxCreateNumericArray(numdimsZi , dimsZi, mxDOUBLE_CLASS, mxREAL);
Zi = mxGetPr(plhs[1]);
/* --------------------- Main Call ---------------------------------- */
likelihood_nav(Z , map , R , X , Hp , X_min , X_max , Y_min , Y_max ,
like ,
nR , nC , m , d , N , Zi , res , Y , Ri , Riinv ,
vect , vv , indx);
/* -------------------------------------------------------------------- */
mxFree(Y);
mxFree(res);
mxFree(Ri);
mxFree(Riinv);
mxFree(vect);
mxFree(vv);
mxFree(indx);
mxFree(dimslike);
}
/* ----------------------------------------------------------------------- */
void likelihood_nav(double *Z , double *map , double *R , double *X , double *Hp , double X_min , double X_max , double Y_min , double Y_max ,
double *like ,
int nR , int nC , int m , int d , int N ,
double *Zi , double *res , double *Y , double *Ri , double *Riinv , double *vect , double *vv ,
int *indx)
{
double pente_Y , pente_X , cte_pente_Y , cte_pente_X;
double Yx , Yy , floorY , floorX;
double dd , detRi , quad , temp;
register double cte = 1.0/pow(2.0*PI , m/2);
register double NotANumber = NAN , tiny = 10e-50;
int i , j , l , k , id , i2 , lm , jm , r , v , t , d2 = 2 , nm = nR*nC , im;
int indexY;
pente_Y = fabs(Y_max - Y_min)/(nR - 1);
pente_X = fabs(X_max - X_min)/(nC - 1);
cte_pente_Y = 1.0/pente_Y;
cte_pente_X = 1.0/pente_X;
for (i = 0 ; i < N ; i++)
{
i2 = i*2;
im = i*m;
/*----- a) 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 ) && (Y[0] < Y_max) && (Y[1] > X_min) && (Y[1] < X_max ) )
{
/*----- b) Scale Coordinates ------*/
Yy = (Y[0] - Y_min + pente_Y)*cte_pente_Y;
Yx = (Y[1] - X_min + pente_X)*cte_pente_X;
floorY = floor(Yy);
floorX = floor(Yx);
indexY = floorX + (floorY - 1)*nR - 1;
Yy = (Yy - floorY);
Yx = (Yx - floorX);
/*----- c) Zi & Ri bilinear interpolation ------*/
for (l = 0 ; l < m ; l++)
{
r = l*nm;
t = r*m;
lm = l*m;
Zi[l + im ] = (map[indexY + r]*(1.0 - Yx) + map[indexY + 1 + r ]*Yx)*(1.0 - Yy) + (map[indexY + nR + r]*(1.0 - Yx) + map[indexY + nR + 1 + r]*Yx)*Yy;
for (k = 0 ; k < m ; k++)
{
v = k*nm + t;
Ri[k + lm] = (R[indexY + v]*(1.0 - Yx) + R[indexY + 1 + v]*Yx)*(1.0 - Yy) + (R[indexY + nR + v]*(1.0 - Yx) + R[indexY + nR + 1 + v ]*Yx)*Yy;
}
}
/*----- d) Riinv = Ri^(-1) & detRi = det(Ri) ------*/
if (m == 1)
{
Riinv[0] = 1.0/Ri[0];
detRi = Ri[0];
}
else
{
if(ludcmp(Ri , m , indx , &dd , vv))
{
detRi = 1.0;
for(r = 0 ; r < m ; r++)
{
detRi *= Ri[r + r*m];
}
for(r = 0 ; r < m ; r++)
{
for(v = 0 ; v < m ; v++)
{
vect[v] = 0.0;
}
jm = r*m;
vect[r] = 1.0;
lubksb(Ri , m, indx, vect);
for(v = 0 ; v < m ; v++)
{
Riinv[v + jm] = vect[v];
}
}
}
}
/*----- e) like = cte*exp(-0.5*res*Riinv*res^T) ------*/
for (r = 0 ; r < m ; r++)
{
res[r] = (Z[r] - Zi[r + im]);
}
quad = 0.0;
for (r = 0 ; r < m ; r++)
{
temp = 0.0;
id = r*m;
for(v = 0 ; v < m ; v++)
{
temp += res[v]*Riinv[v + id];
}
quad += temp*res[r];
}
like[i] = (cte/sqrt(fabs(detRi)))*exp(-0.5*quad) + tiny;
// like[i] = exp(-0.5*quad) + tiny;
}
else
{
// like[i] = NotANumber;
like[i] = tiny;
}
}
}
/*-------------------------------------------------------------------------*/
void lubksb(double *m, int n, int *indx, double *b)
{
int i, ii = -1, ip, j , nn = n*n , in;
double sum;
for(i = 0; i < n; i++)
{
ip = *(indx + i);
sum = *(b + ip);
*(b + ip) = *(b + i);
if(ii > -1)
{
for(j = ii; j <= i - 1; j++)
{
sum -= m[i + j*n] * *(b + j);
}
}
else if(sum)
{
ii = i;
}
*(b + i) = sum;
}
for(i = n - 1; i >= 0; i--)
{
sum = *(b + i);
in = i*n;
for(j = i + 1; j < n; j++)
{
sum -= m[i + j*n] * *(b + j);
}
*(b + i) = sum / m[i + in];
}
}
/*-------------------------------------------------------------------------*/
int ludcmp(double *m, int n, int *indx, double *d , double *vv)
{
int i, imax, j, k , jn , kn , n1 = n - 1;
double big, dum, sum, temp;
*d = 1.0;
for(i = 0; i < n; i++)
{
big = 0.0;
for(j = 0; j < n; j++)
{
if((temp = fabs(m[i + j*n])) > big)
{
big = temp;
}
}
if(big == 0.0)
{
return 0;
}
vv[i] = 1.0 / big;
}
for(j = 0; j < n; j++)
{
jn = j*n;
for(i = 0; i < j; i++)
{
sum = m[i + jn];
for(k = 0; k < i; k++)
{
sum -= m[i + k*n ] * m[k + jn];
}
m[i + jn] = sum;
}
big = 0.0;
for(i = j; i < n; i++)
{
sum = m[i + jn];
for(k = 0; k < j; k++)
{
sum -= m[i + k*n] * m[k + jn];
}
m[i + jn] = sum;
if((dum = vv[i] * fabs(sum)) >= big)
{
big = dum;
imax = i;
}
}
if(j != imax)
{
for(k = 0; k < n; k++)
{
kn = k*n;
dum = m[imax + kn];
m[imax + kn] = m[j + kn];
m[j + kn] = dum;
}
*d = -(*d);
vv[imax] = vv[j];
}
*(indx + j) = imax;
if(m[j + jn] == 0.0)
{
m[j + jn] = NUMERICS_FLOAT_MIN;
}
if(j != n1)
{
dum = 1.0 / (m[j + jn]);
for(i = j + 1; i < n; i++)
{
m[i + jn] *= dum;
}
}
}
return 1;
};⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -