EXT pmaxlike_wk* pmaxlike_wkalloc
(int a, int b, marginal* m, int N)
{
pmaxlike_wk* wk = (pmaxlike_wk*) malloc( sizeof(pmaxlike_wk) );
int K = b - a;
wk-> K = K;
wk-> oper = dmatrix_alloc(N);
wk-> proj = dmatrix_alloc(N);
wk-> quad = (Real*) malloc( K * sizeof(Real) );
wk-> c_lup = (Real*) malloc( K * sizeof(Real) );
wk-> s_lup = (Real*) malloc( K * sizeof(Real) );
int i;
for (i = 0; i < K; i++) {
wk-> quad[i] = m-> quad[i + a];
wk-> c_lup[i] = cos( m-> phase[i + a]);
wk-> s_lup[i] = sin( m-> phase[i + a]);
}
wk-> h_lup = (Real*) malloc( ( N + 1 ) * sizeof(Real) );
return wk;
}
EXT maxlike_wk* maxlike_alloc
(int N, histogram* h)
{
maxlike_wk* max = (maxlike_wk*) malloc(sizeof(maxlike_wk));
Check(max,"max");
max-> print = 0;
max-> iteration = 0;
max-> data = h;
max-> state = dmatrix_alloc(N);
max-> tester = NULL;
dmatrix_to_Id(max-> state);
int theta, x, i;
int M = N + 1;
int T = h-> gd-> x-> n_val;
int X = h-> gd-> y-> n_val;
max-> proj_re = (Real****) malloc( sizeof(Real***) * T );
max-> proj_im = (Real****) malloc( sizeof(Real***) * T );
Check(max-> proj_re,"proj_r****");
Check(max-> proj_im,"proj_i****");
for(theta = 0; theta < T; theta++) {
max-> proj_re[theta] = (Real***) malloc( sizeof(Real**) * X );
max-> proj_im[theta] = (Real***) malloc( sizeof(Real**) * X );
Check(max-> proj_re[theta],"proj_r***");
Check(max-> proj_im[theta],"proj_i***");
for(x = 0; x < X; x++) {
max-> proj_re[theta][x] = (Real**) malloc( sizeof(Real*) * M );
max-> proj_im[theta][x] = (Real**) malloc( sizeof(Real*) * M );
Check(max-> proj_re[theta][x],"proj_r**");
Check(max-> proj_im[theta][x],"proj_i**");
for(i = 0; i < M; i++) {
max-> proj_re[theta][x][i] = (Real*) malloc( sizeof(Real) * M );
max-> proj_im[theta][x][i] = (Real*) malloc( sizeof(Real) * M );
Check(max-> proj_re[theta][x][i],"proj_r*");
Check(max-> proj_im[theta][x][i],"proj_i*");
}
}
}
return max;
}
int lob_scaat(SCAATState *out,
STORAGE_TYPE z, STORAGE_TYPE s_x, STORAGE_TYPE s_y, STORAGE_TYPE dt,
SCAATState *in,
STORAGE_TYPE q, STORAGE_TYPE R){
STORAGE_TYPE **A;
STORAGE_TYPE **Q;
STORAGE_TYPE **Q1; // temporary matrix
STORAGE_TYPE opt_z=0.0;
STORAGE_TYPE *H;
STORAGE_TYPE *K; //Kalman gain
STORAGE_TYPE **I; //unit matrix
STORAGE_TYPE **v; // for the eigenvectors
int *nrot; // for the Jacobi method of eigenvalue computation
STORAGE_TYPE *d;
SCAATState pred; // prediction
int i, j;
int add_noise = 0;
// section for temporary variables
STORAGE_TYPE *t_v1; // vector
STORAGE_TYPE **t_m1; // matrix
STORAGE_TYPE **t_m2; // matrix
STORAGE_TYPE **t_m3; // matrix
STORAGE_TYPE t_s; //temporary scalar value
STORAGE_TYPE temp_sum = 0.0;
STORAGE_TYPE min_d = 0.0;
// program start
//A=[1 0 dt 0 dt*dt/2 0
// 0 1 0 dt 0 dt*dt/2
// 0 0 1 0 dt 0
// 0 0 0 1 0 dt
// 0 0 0 0 1 0
// 0 0 0 0 0 1];
pred.x = VECTOR(1, STATE_NUM);
//pred.P = MATRIX(1, STATE_NUM, 1, STATE_NUM);
pred.P = dmatrix_alloc(STATE_NUM,STATE_NUM);
t_v1 = VECTOR(1, STATE_NUM);
//t_m1 = MATRIX(1, STATE_NUM, 1, STATE_NUM);
//t_m2 = MATRIX(1, STATE_NUM, 1, STATE_NUM);
//t_m3 = MATRIX(1, STATE_NUM, 1, STATE_NUM);
t_m1 = dmatrix_alloc(STATE_NUM,STATE_NUM);
t_m2 = dmatrix_alloc(STATE_NUM,STATE_NUM);
t_m3 = dmatrix_alloc(STATE_NUM,STATE_NUM);
//v = MATRIX(1, STATE_NUM, 1, STATE_NUM);
v = dmatrix_alloc(STATE_NUM,STATE_NUM);
nrot = ivector(1, STATE_NUM);
d = VECTOR(1, STATE_NUM);
//A = MATRIX(1, STATE_NUM, 1, STATE_NUM);
//Q = MATRIX(1, STATE_NUM, 1, STATE_NUM);
//Q1 = MATRIX(1, STATE_NUM, 1, STATE_NUM);
A = dmatrix_alloc(STATE_NUM,STATE_NUM);
Q = dmatrix_alloc(STATE_NUM,STATE_NUM);
Q1 = dmatrix_alloc(STATE_NUM,STATE_NUM);
H = VECTOR(1, STATE_NUM);
K = VECTOR(1, STATE_NUM);
//I = MATRIX(1, STATE_NUM, 1, STATE_NUM);
I = dmatrix_alloc(STATE_NUM,STATE_NUM);
// Initialize matrices A and Q
for (i=1;i<=STATE_NUM;i++){
K[i] = 0.0;
for (j=1;j<=STATE_NUM;j++){
A[i][i] = 0;
Q[i][i] = 0;
Q1[i][i] = 0;
if (i==j)
I[i][j]= 1.0;
else
I[i][j]= 0.0;
}
}
for (i=1;i<=STATE_NUM;i++){
A[i][i] = 1;
if (i+2<=STATE_NUM)
A[i][i+2] = dt;
if (i+4<=STATE_NUM)
A[i][i+4] = dt*dt/2.0;
}
//% The state error covariance matrix is a function of the driving
//% error which is assumed to only hit the accelaration directly.
//% Indirectly this noise affects the velocity and position estimate
//% through the dynamics model.
//Q=q*[(dt^5)/20 0 (dt^4)/8 0 (dt^3)/6 0
// 0 (dt^5)/20 0 (dt^4)/8 0 (dt^3)/6
// (dt^4)/8 0 (dt^3)/3 0 (dt^2)/2 0
// 0 (dt^4)/8 0 (dt^3)/3 0 (dt^2)/2
// (dt^3)/6 0 (dt^2)/2 0 dt 0
// 0 (dt^3)/6 0 (dt^2)/2 0 dt];
Q[1][1] = q*pow(dt, 5)/20.0;
Q[1][3] = q*pow(dt, 4)/8.0;
Q[1][5] = q*pow(dt, 3)/6.0;
Q[2][2] = q*pow(dt, 5)/20.0;
Q[2][4] = q*pow(dt, 4)/8.0;
Q[2][6] = q*pow(dt, 3)/6.0;
Q[3][1] = q*pow(dt, 4)/8.0;
Q[3][3] = q*pow(dt, 3)/3.0;
Q[3][5] = q*pow(dt, 2)/2.0;
Q[4][2] = q*pow(dt, 4)/8.0;
Q[4][4] = q*pow(dt, 3)/3.0;
Q[4][6] = q*pow(dt, 2)/2.0;
Q[5][1] = q*pow(dt, 3)/6.0;
Q[5][3] = q*pow(dt, 2)/2.0;
Q[5][5] = q*dt;
Q[6][2] = q*pow(dt, 3)/6.0;
Q[6][4] = q*pow(dt, 2)/2.0;
Q[6][6] = q*dt;
/*for (i=1;i<=STATE_NUM;i++) {
for (j=1;j<=STATE_NUM;j++)
printf("%f ",Q[i][j]);
printf("\n");
}*/
//%step b
//pred_x=A*pre_x; %6 x 1
mat_vec_mult(pred.x, A, in->x, STATE_NUM, 0);
//pred_P=A*pre_P*(A')+Q; %6 x 6
// A is transpose
mat_mat_mult(t_m1, in->P, A, STATE_NUM, 0, 1);
mat_mat_mult(t_m2, A, t_m1, STATE_NUM, 0, 0);
mat_add(pred.P, t_m2, Q, STATE_NUM, 0);
// %step c
// % assume that 0<=z<2*pi
// opt_z = 2*pi + atan2((pred_x(2)-s_y), (pred_x(1)-s_x)); %1 x 1
// if (opt_z>2*pi)
// opt_z = opt_z - 2*pi;
// end
// %H is 1 x 6
// % measurement function is nonlinear
// %
// % z = atan(x(2) - s_y, x(1) - s_x);
// % In order to linearize the problem we find the jacobian
// % noticing that
// %
// % d(atan(x))/dx = 1/(1+x^2)
// %
// H=[ -(pred_x(2)-s_y)/(( (pred_x(1)-s_x)^2+(pred_x(2)-s_y)^2 )), (pred_x(1)-s_x)/(( (pred_x(1)-s_x)^2+(pred_x(2)-s_y)^2 )), 0, 0, 0, 0];
opt_z = 2*PI + atan2((pred.x[2]-s_y), (pred.x[1]-s_x));
H[1] = -(pred.x[2]-s_y)/ ( (pred.x[1]-s_x)*(pred.x[1]-s_x)+(pred.x[2]-s_y)*(pred.x[2]-s_y) );
H[2] = (pred.x[1]-s_x) / ( (pred.x[1]-s_x)*(pred.x[1]-s_x)+(pred.x[2]-s_y)*(pred.x[2]-s_y) );
H[3] = 0.0; H[4] = 0.0; H[5] = 0.0; H[6] = 0.0;
//test=find(isnan(H)==1);
//if (~isempty(test))
// disp('H goes to infinity due to node position is the same as the predicted value')
// return;
//end
if (isnan(H[1]) || isnan(H[1])){
fprintf(stderr, "H goes to infinity due to node position is the same as the predicted value\n");
return(-1);
}
//%step d
//%also test if P is still a positive definite matrix. If not, add some small noise to it to
//%make it still positive.
//n1=1;
//Q1=n1*eye(6);
//Q1(1,1)=0.1;
//Q1(2,2)=0.1;
//Q1(3,3)=5;
//Q1(4,4)=5;
Q1[1][1] = 0.1;
Q1[2][2] = 0.1;
Q1[3][3] = 5.0;
Q1[4][4] = 5.0;
Q1[5][5] = 1.0;
Q1[6][6] = 1.0;
//if isempty(find(eig(pred_P)<0.001))
// K=pred_P*H'*inv(H*pred_P*H' + R); %6 x 1
//else
// pred_P=pred_P+Q1;
// K=pred_P*H'*inv(H*pred_P*H' + R); %6 x 1
//end
add_noise=0;
// jacobi destroys the input vector
mat_copy(t_m2, pred.P, STATE_NUM, 0);
jacobi(t_m2, STATE_NUM, d, v, nrot);
for(i=1;i<=STATE_NUM;i++){
if (d[i]< 0.001)
add_noise=1;
}
if (add_noise){
mat_add(t_m1, pred.P, Q1, STATE_NUM, 0);
mat_copy(pred.P, t_m1, STATE_NUM, 0);
}
mat_vec_mult(t_v1, pred.P, H, STATE_NUM, 1);
inner_prod(&t_s, t_v1, H, STATE_NUM);
t_s+= R;
scal_vec_mult(K, 1.0/t_s, t_v1, STATE_NUM);
//%step e and f
//I=eye(6);
//opt_x=pred_x+K*(z-opt_z); %6 x 1
scal_vec_mult(t_v1, z - opt_z, K, STATE_NUM);
vec_add(out->x, pred.x, t_v1, STATE_NUM);
// H: 1 x 6, K: 6 x 1
//%Joseph form to ward off round off problem
//opt_P=(I-K*H)*pred_P*(I-K*H)'+K*R*K'; %6 x 6
scal_vec_mult(t_v1, R, K, STATE_NUM); // R*K'
outer_prod(t_m1, K, t_v1, STATE_NUM); // K*(R*K')
outer_prod(t_m2, K, H, STATE_NUM); // K*H
mat_add(t_m3, I, t_m2, STATE_NUM, 1); // I-K*H
mat_mat_mult(t_m2, pred.P, t_m3, STATE_NUM, 0, 1);
mat_mat_mult(out->P, t_m3, t_m2, STATE_NUM, 0, 0);
mat_add(t_m3, out->P, t_m1, STATE_NUM, 0);
mat_copy(out->P, t_m3, STATE_NUM, 0);
//% PRECAUTIONARY APPROACH. EVEN THOUGH IT HASN'T HAPPEN IN THE SIMULATION SO FAR
//% also test if opt_P is still a positive definite matrix. If not, add some small noise to it to
//% make it still positive.
//% d: a diagonal matrix of eigenvalues
//% v: matrix whose vectors are the eigenvectors
//% X*V = V*D
//if isempty(find(eig(opt_P)<0.001))
//else
//[v d]=eig(opt_P);
//c=find(d==min(diag(d)));
//d(c)=0.001;
//opt_P=v*d*v';
//end
add_noise=0;
// jacobi destroys the input vector
mat_copy(t_m1, out->P, STATE_NUM, 0);
jacobi(t_m1, STATE_NUM, d, v, nrot);
min_d = d[1] ;
for(i=1;i<=STATE_NUM;i++){
if ( min_d > d[i])
min_d = d[i];
if (d[i]< 0.001)
add_noise=1;
}
// CAUTION - maybe the 2nd minimum is quite far from 0.001
if (add_noise){
// change diagonal matrix d
for (i=1;i<STATE_NUM;i++){
if (d[i] == min_d)
d[i] = 0.001;
}
// opt_P=v*d*v';
mat_copy(t_m1, v, STATE_NUM, 1);
convert_vec_diag(t_m2, d, STATE_NUM);
mat_mat_mult(t_m3, t_m2, t_m1, STATE_NUM, 0, 0);
mat_mat_mult(out->P, v, t_m3, STATE_NUM, 0, 0);
}
//printf("The pred are x = %f y = %f\n",pred.x[1],pred.x[2]);
// Free all MALLOCed matrices
FREE_VECTOR_FUNCTION(pred.x, 1, STATE_NUM);
FREE_MATRIX_FUNCTION(pred.P, 1, STATE_NUM, 1, STATE_NUM);
FREE_VECTOR_FUNCTION(t_v1, 1, STATE_NUM);
FREE_MATRIX_FUNCTION(t_m1, 1, STATE_NUM, 1, STATE_NUM);
FREE_MATRIX_FUNCTION(t_m2, 1, STATE_NUM, 1, STATE_NUM);
FREE_MATRIX_FUNCTION(t_m3, 1, STATE_NUM, 1, STATE_NUM);
FREE_MATRIX_FUNCTION(v, 1, STATE_NUM, 1, STATE_NUM);
free_ivector(nrot, 1, STATE_NUM);
FREE_VECTOR_FUNCTION(d, 1, STATE_NUM);
FREE_MATRIX_FUNCTION(A, 1, STATE_NUM, 1, STATE_NUM);
FREE_MATRIX_FUNCTION(Q, 1, STATE_NUM, 1, STATE_NUM);
FREE_MATRIX_FUNCTION(Q1, 1, STATE_NUM, 1, STATE_NUM);
FREE_VECTOR_FUNCTION(H, 1, STATE_NUM);
FREE_VECTOR_FUNCTION(K, 1, STATE_NUM);
FREE_MATRIX_FUNCTION(I, 1, STATE_NUM, 1, STATE_NUM);
return(0);
}
EXT Real* pmaxlike_margi_iteration
(maxlike_margi_wk* max, int J)
{
int a, i, j, k;
int K = max-> data-> samples, N = max-> state-> n, M = N + 1;
Real re, im;
Real norm = 1.0 / (max-> data-> samples);
dmatrix* operator = dmatrix_alloc(max->state->n);
dmatrix* prod = dmatrix_alloc(max->state->n);
dmatrix* proj = dmatrix_alloc(max->state->n);
dmatrix* last = dmatrix_alloc(max->state->n);
dmatrix* state = max-> state;
Real* err = (Real*) malloc( J * sizeof(Real) );
int iter = max-> iteration;
max-> iteration += J;
J = max-> iteration;
void *status;
pthread_t threads[NUM_THREADS];
pthread_attr_t attr;
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
pmaxlike_wk* data[NUM_THREADS];
j = K / NUM_THREADS;
#pragma unroll
for (a = 0; a < NUM_THREADS; a++) {
data[a] = pmaxlike_wkalloc(a * j, (a +1) * j, max-> data, N);
data[a]-> state = state;
}
k = 0;
while(J - iter) {
i = 0;
while(M - i) {
j = 0;
while(M - j) {
last->re[i][j] = state->re[i][j];
last->im[i][j] = state->im[i][j];
j++;
}
i++;
}
#pragma unroll
for (a = 0; a < NUM_THREADS; a++)
pthread_create(&threads[a], &attr, pmaxlike_thread_getproj, (void *) data[a]);
#pragma unroll
for (a = 0; a < NUM_THREADS; a++)
pthread_join(threads[a], &status);
i = 0;
while(M - i) {
j = 0;
while(M - j) {
re = 0.0;
im = 0.0;
#pragma unroll
for (a = 0; a < NUM_THREADS; a++)
re += data[a]-> oper-> re[i][j];
#pragma unroll
for (a = 0; a < NUM_THREADS; a++)
im += data[a]-> oper-> im[i][j];
operator-> re[i][j] = norm * re;
operator-> im[i][j] = norm * im;
j++;
}
i++;
}
dmatrix_transpose(operator);
dmatrix_prod(prod, operator, state);
dmatrix_prod(state, prod, operator);
dmatrix_renorm(state);
err[k] = dmatrix_dist_max(state, last);
k++;
if(max-> print)
printf("err(%i):%f\n",iter,err[k]);
if(max-> tester != NULL)
(max-> tester)(iter,state);
iter++;
if( iter % 10 == 0)
;//printf("%i iterations done\n",iter);
}
dmatrix_free(operator);
dmatrix_free(prod);
dmatrix_free(proj);
dmatrix_free(last);
#pragma unroll
for (a = 0; a < NUM_THREADS; a++)
pmaxlike_wkfree( data[a] );
pthread_attr_destroy(&attr);
return err;
}
EXT Real* pmaxlike_iteration
(maxlike_wk* max, int J)
{
int i, j, k, theta, x;
int M = max-> state-> n + 1;
int T = max-> data-> gd-> x-> n_val;
int X = max-> data-> gd-> y-> n_val;
Real trace, reg;
dmatrix* operator = dmatrix_alloc(max->state->n);
dmatrix* prod = dmatrix_alloc(max->state->n);
dmatrix* last = dmatrix_alloc(max->state->n);
dmatrix* state = max->state;
Real* err = (Real*) malloc( J * sizeof(Real) );
int iter = max-> iteration;
max-> iteration += J;
J = max-> iteration;
k = 0;
while(J-iter) {
i = 0;
while(M - i) {
j = 0;
while(M - j) {
last->re[i][j] = state->re[i][j];
last->im[i][j] = state->im[i][j];
j++;
}
i++;
}
dmatrix_to_zero(operator);
theta = 0;
while(T-theta) {
x = 0;
while(X-x) {
reg = max-> data-> val[theta][x];
if ( reg != 0.0 ) {
trace = 0.0;
i = 0;
while(M-i) {
j = 0;
while(M-j) {
trace += max-> proj_re[theta][x][i][j] * state-> re[i][j]
- max-> proj_im[theta][x][i][j] * state-> im[i][j];
j++;
}
i++;
}
trace = reg / trace;
i = 0;
while(M-i) {
j = 0;
while(M-j) {
operator-> re[i][j] += trace * max-> proj_re[theta][x][i][j];
operator-> im[i][j] += trace * max-> proj_im[theta][x][i][j];
j++;
}
i++;
}
}
x++;
}
theta++;
}
dmatrix_transpose(operator);
dmatrix_prod(prod, operator, state);
dmatrix_prod(state, prod, operator);
dmatrix_renorm(state);
err[k] = dmatrix_dist_max(state, last);
k++;
if(max-> print)
printf("err(%i):%f\n",iter,err[k]);
if(max-> tester != NULL)
(max-> tester)(iter,state);
iter++;
if( iter % 50 == 0)
printf("%i iterations done\n",iter);
}
dmatrix_free(operator);
dmatrix_free(prod);
dmatrix_free(last);
return err;
}
EXT void maxlike_precalculate
(maxlike_wk* max)
{
#define H_Amp 10.0
#define H_Step 65535
#define H_Rez (H_Amp/H_Step)
int i = 0, j = 0, theta = 0, x = 0, y = 0, q = 0;
int M = max-> state-> n + 1;
int T = max-> data-> gd-> x-> n_val;
int X = max-> data-> gd-> y-> n_val;
int Q = H_Step;
int sign = 1;
Real accum, thres, a, b;
Real rez = max-> data-> gd-> y-> resolution;
range* r = range_single(H_Step, H_Amp);
hermite_workspace* h = hermite_alloc(max-> state-> n, r);
hermite_calculate(h);
Real** c_lup = (Real**) malloc( sizeof(Real*) * M );
Real** s_lup = (Real**) malloc( sizeof(Real*) * M );
i = 0;
while(M-i) {
c_lup[i] = (Real*) malloc( sizeof(Real) * T );
s_lup[i] = (Real*) malloc( sizeof(Real) * T );
theta = 0;
while(T-theta) {
c_lup[i][theta] = cos( i * (max-> data-> gd-> x-> val[theta]) );
s_lup[i][theta] = sin( i * (max-> data-> gd-> x-> val[theta]) );
theta++;
}
i++;
}
i = 0;
while(M - i) {
j = 0;
while(i + 1 - j) {
accum = 0.0;
thres = rez/2;
q = 0;
x = (max-> data-> gd-> y-> n_val)/2;
while(X - x && Q - q) {
accum += h-> harmonics[i][q] * h-> harmonics[j][q];
if ( r-> val[q] >= thres) {
thres += rez;
accum *= r-> resolution;
theta = 0;
while(T-theta) {
max-> proj_re[theta][x][i][j] = c_lup[i-j][theta] * accum;
max-> proj_im[theta][x][i][j] = s_lup[i-j][theta] * accum;
theta++;
}
x++;
accum = 0.0;
}
q++;
}
j++;
}
i++;
}
theta = 0;
while(T-theta) {
x = (max-> data-> gd-> y-> n_val)/2;
y = x;
while(X - x ) {
i = 0;
while(M - i) {
j = 0;
while(i + 1 - j) {
sign = ( (i+j)%2 )? -1 : 1;
max-> proj_re[theta][y][i][j] = sign * max-> proj_re[theta][x][i][j];
max-> proj_im[theta][y][i][j] = sign * max-> proj_im[theta][x][i][j];
j++;
}
i++;
}
x++;
y--;
}
theta++;
}
dmatrix* buf = dmatrix_alloc(M-1);
theta = 0;
while(T-theta) {
x = 0;
while(X - x ) {
i = 0;
while(M - i) {
j = 0;
while(i + 1 - j) {
buf-> re[i][j] = max-> proj_re[theta][x][i][j];
buf-> im[i][j] = max-> proj_im[theta][x][i][j];
j++;
}
i++;
}
i = 0;
while(M - i) {
j = 0;
while(i + 1 - j) {
max-> proj_re[theta][x][j][i] = buf-> re[i][j];
max-> proj_im[theta][x][j][i] =-buf-> im[i][j];
j++;
}
i++;
}
x++;
}
theta++;
}
dmatrix_free(buf);
for (i = 0; i < M; i++) {
free( c_lup[i] );
free( s_lup[i] );
}
free(c_lup);
free(s_lup);
hermite_free(h);
range_free(r);
#undef H_Amp
#undef H_Step
#undef H_Rez
}
