/* Perform one cycle of post refinement on 'image' against 'full' */
static double pr_iterate(Crystal *cr, const RefList *full,
PartialityModel pmodel, int *n_filtered)
{
gsl_matrix *M;
gsl_vector *v;
gsl_vector *shifts;
int param;
Reflection *refl;
RefListIterator *iter;
RefList *reflections;
double max_shift;
int nref = 0;
const int verbose = 0;
int num_params = 0;
enum gparam rv[32];
*n_filtered = 0;
/* If partiality model is anything other than "unity", refine all the
* geometrical parameters */
if ( pmodel != PMODEL_UNITY ) {
rv[num_params++] = GPARAM_ASX;
rv[num_params++] = GPARAM_ASY;
rv[num_params++] = GPARAM_ASZ;
rv[num_params++] = GPARAM_BSX;
rv[num_params++] = GPARAM_BSY;
rv[num_params++] = GPARAM_BSZ;
rv[num_params++] = GPARAM_CSX;
rv[num_params++] = GPARAM_CSY;
rv[num_params++] = GPARAM_CSZ;
}
STATUS("Refining %i parameters.\n", num_params);
reflections = crystal_get_reflections(cr);
M = gsl_matrix_calloc(num_params, num_params);
v = gsl_vector_calloc(num_params);
/* Construct the equations, one per reflection in this image */
for ( refl = first_refl(reflections, &iter);
refl != NULL;
refl = next_refl(refl, iter) )
{
signed int ha, ka, la;
double I_full, delta_I;
double w;
double I_partial;
int k;
double p, l;
Reflection *match;
double gradients[num_params];
/* Find the full version */
get_indices(refl, &ha, &ka, &la);
match = find_refl(full, ha, ka, la);
if ( match == NULL ) continue;
if ( (get_intensity(refl) < 3.0*get_esd_intensity(refl))
|| (get_partiality(refl) < MIN_PART_REFINE)
|| (get_redundancy(match) < 2) ) continue;
I_full = get_intensity(match);
/* Actual measurement of this reflection from this pattern? */
I_partial = get_intensity(refl) / crystal_get_osf(cr);
p = get_partiality(refl);
l = get_lorentz(refl);
/* Calculate the weight for this reflection */
w = pow(get_esd_intensity(refl), 2.0);
w += l * p * I_full * pow(get_esd_intensity(match), 2.0);
w = pow(w, -1.0);
/* Calculate all gradients for this reflection */
for ( k=0; k<num_params; k++ ) {
gradients[k] = p_gradient(cr, rv[k], refl, pmodel) * l;
}
for ( k=0; k<num_params; k++ ) {
int g;
double v_c, v_curr;
for ( g=0; g<num_params; g++ ) {
double M_c, M_curr;
/* Matrix is symmetric */
if ( g > k ) continue;
M_c = gradients[g] * gradients[k];
M_c *= w * pow(I_full, 2.0);
M_curr = gsl_matrix_get(M, k, g);
gsl_matrix_set(M, k, g, M_curr + M_c);
gsl_matrix_set(M, g, k, M_curr + M_c);
}
delta_I = I_partial - (l * p * I_full);
v_c = w * delta_I * I_full * gradients[k];
v_curr = gsl_vector_get(v, k);
gsl_vector_set(v, k, v_curr + v_c);
}
nref++;
}
if ( verbose ) {
STATUS("The original equation:\n");
show_matrix_eqn(M, v);
}
//STATUS("%i reflections went into the equations.\n", nref);
if ( nref == 0 ) {
crystal_set_user_flag(cr, 2);
gsl_matrix_free(M);
gsl_vector_free(v);
return 0.0;
}
max_shift = 0.0;
shifts = solve_svd(v, M, n_filtered, verbose);
if ( shifts != NULL ) {
for ( param=0; param<num_params; param++ ) {
double shift = gsl_vector_get(shifts, param);
apply_shift(cr, rv[param], shift);
//STATUS("Shift %i: %e\n", param, shift);
if ( fabs(shift) > max_shift ) max_shift = fabs(shift);
}
} else {
crystal_set_user_flag(cr, 3);
}
gsl_matrix_free(M);
gsl_vector_free(v);
gsl_vector_free(shifts);
return max_shift;
}
/*
* Create the matrix and vector for the circuit elements
*/
int create_mna(LIST *list , gsl_matrix **matrix , gsl_vector** vector, int transient, gsl_matrix **c_matrix){
LIST_NODE* curr;
gsl_matrix* tmp_matrix;
gsl_vector* tmp_vector;
gsl_matrix* tmp_c_matrix;
//int group1 = list->len - list->m2;
//int group2 = list->m2;
int m2_elements_found = 0;
int rows;
int columns;
if( !matrix || !vector || !list)
return 0;
if(transient && !c_matrix)
return 0;
/* allocate matrix and vector */
rows = list->hashtable->num_nodes + list->m2;
columns = list->hashtable->num_nodes + list->m2;
printf("Creating matrix: rows = %d columns = %d\n",rows,columns);
tmp_matrix = gsl_matrix_calloc(rows , columns);
if( !tmp_matrix )
return 0;
tmp_c_matrix = gsl_matrix_calloc(rows , columns);
if( !tmp_c_matrix )
return 0;
tmp_vector = gsl_vector_calloc( rows);
if( !tmp_vector )
return 0;
/* compute mna */
for( curr = list->head ; curr ; curr = curr->next){
/*
* RESISTANCE ELEMENT
*/
if( curr->type == NODE_RESISTANCE_TYPE ){
double conductance = 1 / curr->node.resistance.value ;
int plus_node = curr->node.resistance.node1 - 1 ;
int minus_node = curr->node.resistance.node2 - 1;
/* <+> is ground */
if( plus_node == -1 ){
double value = gsl_matrix_get(tmp_matrix , minus_node , minus_node);
value += conductance ;
gsl_matrix_set( tmp_matrix , minus_node , minus_node , value );
//printf("Adding to matrix element (%d,%d) value:%f\n\n",minus_node,minus_node,value);
}
else if( minus_node == -1 ){
/* <-> is ground */
double value = gsl_matrix_get(tmp_matrix , plus_node , plus_node);
value += conductance;
gsl_matrix_set( tmp_matrix , plus_node ,plus_node , value );
//printf("Adding to matrix element (%d,%d) value:%f\n\n",plus_node,plus_node,value);
}
else {
/* set <+> <+> matrix element */
double value;
value = gsl_matrix_get(tmp_matrix , plus_node , plus_node);
value += conductance ;
gsl_matrix_set(tmp_matrix , plus_node , plus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n",plus_node,plus_node,value);
/* set <+> <-> */
value = gsl_matrix_get(tmp_matrix , plus_node , minus_node);
value -= conductance ;
gsl_matrix_set(tmp_matrix , plus_node , minus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n",plus_node,minus_node,value);
/* set <-> <+> */
value = gsl_matrix_get(tmp_matrix , minus_node , plus_node);
value -= conductance ;
gsl_matrix_set(tmp_matrix , minus_node , plus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n",minus_node,plus_node,value);
/* set <-> <-> */
value = gsl_matrix_get(tmp_matrix , minus_node , minus_node);
value += conductance ;
gsl_matrix_set(tmp_matrix , minus_node , minus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n\n",minus_node,minus_node,value);
}
}
/*
* CAPACITY ELEMENT
*/
else if( curr->type == NODE_CAPACITY_TYPE && transient){
double capacity = curr->node.capacity.value ;
int plus_node = curr->node.capacity.node1 - 1 ;
int minus_node = curr->node.capacity.node2 - 1;
/* <+> is ground */
if( plus_node == -1 ){
double value = gsl_matrix_get(tmp_c_matrix , minus_node , minus_node);
value += capacity ;
gsl_matrix_set( tmp_c_matrix , minus_node , minus_node , value );
//printf("Adding to matrix element (%d,%d) value:%f\n\n",minus_node,minus_node,value);
}
else if( minus_node == -1 ){
/* <-> is ground */
double value = gsl_matrix_get(tmp_c_matrix , plus_node , plus_node);
value += capacity;
gsl_matrix_set( tmp_c_matrix , plus_node ,plus_node , value );
//printf("Adding to matrix element (%d,%d) value:%f\n\n",plus_node,plus_node,value);
}
else {
/* set <+> <+> matrix element */
double value;
value = gsl_matrix_get(tmp_c_matrix , plus_node , plus_node);
value += capacity ;
gsl_matrix_set(tmp_c_matrix , plus_node , plus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n",plus_node,plus_node,value);
/* set <+> <-> */
value = gsl_matrix_get(tmp_c_matrix , plus_node , minus_node);
value -= capacity ;
gsl_matrix_set(tmp_c_matrix , plus_node , minus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n",plus_node,minus_node,value);
/* set <-> <+> */
value = gsl_matrix_get(tmp_c_matrix , minus_node , plus_node);
value -= capacity ;
gsl_matrix_set(tmp_c_matrix , minus_node , plus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n",minus_node,plus_node,value);
/* set <-> <-> */
value = gsl_matrix_get(tmp_c_matrix , minus_node , minus_node);
value += capacity ;
gsl_matrix_set(tmp_c_matrix , minus_node , minus_node , value);
//printf("Adding to matrix element (%d,%d) value:%f\n\n",minus_node,minus_node,value);
}
}
/*
* CURRENT SOURCE
*/
else if( curr->type == NODE_SOURCE_I_TYPE ){
/* change only the vector */
double current = curr->node.source_i.value;
double value;
if( curr->node.source_i.node1 != 0 ){
/* ste <+> */
value = gsl_vector_get(tmp_vector , curr->node.source_i.node1 - 1 );
value -= current;
gsl_vector_set(tmp_vector , curr->node.source_i.node1 -1 , value );
}
if( curr->node.source_i.node2 != 0 ){
/* <-> */
value = gsl_vector_get(tmp_vector , curr->node.source_i.node2 - 1 );
value += current;
gsl_vector_set(tmp_vector , curr->node.source_i.node2 - 1 , value);
}
}
/*
* VOLTAGE SOURCE
*/
else if ( curr->type == NODE_SOURCE_V_TYPE ){
m2_elements_found++;
int matrix_row = list->hashtable->num_nodes + m2_elements_found - 1 ;
curr->node.source_v.mna_row = matrix_row;
double value;
double c_value;
/* set vector value */
value = gsl_vector_get(tmp_vector , matrix_row );
value += curr->node.source_v.value;
c_value = value;
gsl_vector_set(tmp_vector, matrix_row , value);
/* Change the matrix */
int plus_node = curr->node.source_v.node1 - 1 ;
int minus_node = curr->node.source_v.node2 - 1;
/* <+> */
if( plus_node != -1 ){
value = gsl_matrix_get(tmp_matrix , matrix_row , plus_node);
//value++;
gsl_matrix_set(tmp_matrix , matrix_row , plus_node , 1);
//printf("VOLTAGE SOURCE : (%d,%d) +1\n",matrix_row,plus_node);
value = gsl_matrix_get(tmp_matrix , plus_node , matrix_row);
//value++;
gsl_matrix_set(tmp_matrix , plus_node , matrix_row , 1);
//printf("VOLTAGE SOURCE : (%d,%d) + 1 \n",plus_node,matrix_row);
}
/* <->*/
if( minus_node != -1 ){
//value = gsl_matrix_get(tmp_matrix , matrix_row , minus_node);
//value++;
gsl_matrix_set(tmp_matrix , matrix_row , minus_node , -1);
//value = gsl_matrix_get(tmp_matrix , minus_node , matrix_row);
//value--;
gsl_matrix_set(tmp_matrix , minus_node , matrix_row , -1);
}
}
/*
* Inductance
*/
else if ( curr->type == NODE_INDUCTANCE_TYPE ){
m2_elements_found++;
int matrix_row = list->hashtable->num_nodes + m2_elements_found - 1 ;
double value;
double c_value = curr->node.inductance.value;
/* Change the matrix */
int plus_node = curr->node.inductance.node1 - 1 ;
int minus_node = curr->node.inductance.node2 - 1;
/* <+> */
if( plus_node != -1 ){
value = gsl_matrix_get(tmp_matrix , matrix_row , plus_node);
//value++;
gsl_matrix_set(tmp_matrix , matrix_row , plus_node , 1);
//printf("VOLTAGE SOURCE : (%d,%d) +1\n",matrix_row,plus_node);
value = gsl_matrix_get(tmp_matrix , plus_node , matrix_row);
//value++;
gsl_matrix_set(tmp_matrix , plus_node , matrix_row , 1);
//printf("VOLTAGE SOURCE : (%d,%d) + 1 \n",plus_node,matrix_row);
}
/* <->*/
if( minus_node != -1 ){
//value = gsl_matrix_get(tmp_matrix , matrix_row , minus_node);
//value++;
gsl_matrix_set(tmp_matrix , matrix_row , minus_node , -1);
//value = gsl_matrix_get(tmp_matrix , minus_node , matrix_row);
//value--;
gsl_matrix_set(tmp_matrix , minus_node , matrix_row , -1);
}
if(transient)
{
//value = gsl_matrix_get(tmp_matrix , matrix_row , minus_node);
c_value = c_value * (-1);
gsl_matrix_set(tmp_matrix , matrix_row , matrix_row , c_value);
}
}
}
*matrix = tmp_matrix;
*c_matrix = tmp_c_matrix;
*vector = tmp_vector;
/* return */
return 1;
}
gsl_multifit_fsolver *
gsl_multifit_fsolver_alloc (const gsl_multifit_fsolver_type * T,
size_t n, size_t p)
{
int status;
gsl_multifit_fsolver * s;
if (n < p)
{
GSL_ERROR_VAL ("insufficient data points, n < p", GSL_EINVAL, 0);
}
s = (gsl_multifit_fsolver *) malloc (sizeof (gsl_multifit_fsolver));
if (s == 0)
{
GSL_ERROR_VAL ("failed to allocate space for multifit solver struct",
GSL_ENOMEM, 0);
}
s->x = gsl_vector_calloc (p);
if (s->x == 0)
{
free (s);
GSL_ERROR_VAL ("failed to allocate space for x", GSL_ENOMEM, 0);
}
s->f = gsl_vector_calloc (n);
if (s->f == 0)
{
gsl_vector_free (s->x);
free (s);
GSL_ERROR_VAL ("failed to allocate space for f", GSL_ENOMEM, 0);
}
s->dx = gsl_vector_calloc (p);
if (s->dx == 0)
{
gsl_vector_free (s->x);
gsl_vector_free (s->f);
free (s);
GSL_ERROR_VAL ("failed to allocate space for dx", GSL_ENOMEM, 0);
}
s->state = malloc (T->size);
if (s->state == 0)
{
gsl_vector_free (s->dx);
gsl_vector_free (s->x);
gsl_vector_free (s->f);
free (s); /* exception in constructor, avoid memory leak */
GSL_ERROR_VAL ("failed to allocate space for multifit solver state",
GSL_ENOMEM, 0);
}
s->type = T ;
status = (s->type->alloc)(s->state, n, p);
if (status != GSL_SUCCESS)
{
(s->type->free)(s->state);
free (s->state);
gsl_vector_free (s->dx);
gsl_vector_free (s->x);
gsl_vector_free (s->f);
free (s); /* exception in constructor, avoid memory leak */
GSL_ERROR_VAL ("failed to set solver", status, 0);
}
s->function = NULL;
return s;
}
int prepare(const size_t Ntracks, gsl_vector * v_alpha, gsl_vector * v_d, gsl_matrix * m_D) {
size_t i;
gsl_matrix * ma_D[Ntracks];
gsl_matrix * ma_E[Ntracks];
gsl_vector * va_d[Ntracks];
gsl_vector_view v_alpha_vertex_view = gsl_vector_subvector(v_alpha,0,6);
gsl_vector * v_vertex = &v_alpha_vertex_view.vector;
for(i = 0; i < Ntracks; i++) {
gsl_vector_view v_alpha_track_view = gsl_vector_subvector(v_alpha,3+i*6,6);
gsl_vector * v_track = &v_alpha_track_view.vector;
ma_D[i] = gsl_matrix_calloc(2,6);
ma_E[i] = gsl_matrix_calloc(2,3);
va_d[i] = gsl_vector_calloc(2);
Di(v_vertex,v_track,ma_D[i]);
Ei(v_vertex,v_track,ma_E[i]);
di(v_vertex,v_track,va_d[i]);
}
/* Build generalised D matrix */
gsl_matrix * m_E = gsl_matrix_calloc(2*Ntracks,3);
stack_matrix_array(ma_E,Ntracks,m_E);
gsl_matrix * m_zero = gsl_matrix_calloc(2,6); /* Temporary matrix, used for padding */
/* First build cols */
gsl_matrix * ma_Dcols[Ntracks+1]; /* +1 First col is for E */
ma_Dcols[0] = m_E;
size_t j,k;
gsl_matrix * ma_Dpieces[Ntracks];
/* Allocate the memory */
for(i = 0; i < Ntracks; i++) {
ma_Dcols[i+1] = gsl_matrix_calloc(2*Ntracks,6); /* +1 First col is for E */
}
for(i = 0; i < Ntracks; i++) {
/* Compute blocs order */
for(k = 0; k < Ntracks; k++) {
if(i == k) ma_Dpieces[k] = ma_D[k];
else ma_Dpieces[k] = m_zero;
}
/* Build a column */
stack_matrix_array(ma_Dpieces,Ntracks,ma_Dcols[i+1]); /* +1 First col is for E */
}
/* Juxtapose the cols. */
juxtapose_matrix_array(ma_Dcols,Ntracks+1,m_D); /* +1 First col is for E */
/* Build generalised d matrix */
stack_vector_array(va_d,Ntracks,v_d);
/* Clean the mess */
for(i = 0; i < Ntracks; i++) {
gsl_matrix_free(ma_D[i]);
gsl_matrix_free(ma_E[i]);
gsl_vector_free(va_d[i]);
gsl_matrix_free(ma_Dcols[i+1]); /* +1 First col is for E (bloc pointed by [0] will be freed later) */
}
gsl_matrix_free(m_E);
gsl_matrix_free(m_zero);
return 0;
}
int vertex(struct track trk[4],double * x,double * y,double * z, double * chi2, double init[3]) {
gsl_vector * v_vertex = gsl_vector_calloc(3);
gsl_vector * va_tracks[TRACK_NBR];
unsigned int i;
for(i = 0; i < TRACK_NBR; i++) {
va_tracks[i] = gsl_vector_calloc(6);
}
/*
* pos. [mm]
* mom. [MeV/c]
*/
/* Initial conditions */
gsl_vector_set(v_vertex,0,43.0);
gsl_vector_set(v_vertex,1,0.0);
gsl_vector_set(v_vertex,2,137728.0);
unsigned int j;
for(i=0;i<TRACK_NBR;i++) {
for(j=0;j<6;j++) {
gsl_vector_set(va_tracks[i],j,trk[i].param[j]);
}
}
/* Covariance matricies */
gsl_matrix * m_V_alpha_zero = gsl_matrix_calloc(6*TRACK_NBR+3,6*TRACK_NBR+3);
//VTX
gsl_matrix_set(m_V_alpha_zero, 0, 0, init[0]);
gsl_matrix_set(m_V_alpha_zero, 1, 1, init[1]);
gsl_matrix_set(m_V_alpha_zero, 2, 2, init[2]);
unsigned int k;
for(i=0;i < TRACK_NBR; i++) {
for(j=0;j<6;j++) {
for(k=0;k<6;k++) {
gsl_matrix_set(m_V_alpha_zero, 3+i*6+j, 3+i*6+k, trk[i].cov[j][k]);
//printf("%i,%i,%g\n",3+i*6+j,3+i*6+k,trk[i].cov[j][k]);
}
}
}
/* Prepare inputs */
gsl_vector * v_alpha_zero = gsl_vector_calloc(6*TRACK_NBR+3);
/* Ntracks + 1 : vector + tracks */
gsl_vector * va_inputs[1+TRACK_NBR];
va_inputs[0] = v_vertex;
/* ! Merge this loop ! */
for(i = 0; i < TRACK_NBR; i++) {
va_inputs[i+1] = va_tracks[i];
va_inputs[i+1] = va_tracks[i];
}
stack_vector_array(va_inputs,1+TRACK_NBR,v_alpha_zero);
gsl_vector * v_d = gsl_vector_calloc(CONSTRAINTS*TRACK_NBR);
gsl_matrix * m_D = gsl_matrix_calloc(CONSTRAINTS*TRACK_NBR,6*TRACK_NBR+3);
/* LOOP START HERE */
prepare(TRACK_NBR,v_alpha_zero,v_d,m_D);
/* Ok now minimise ! */
/* Allocate memory for the updated results */
gsl_vector * v_alpha = gsl_vector_calloc(6*TRACK_NBR+3);
gsl_matrix * m_V_alpha = gsl_matrix_calloc(6*TRACK_NBR+3,6*TRACK_NBR+3);
minimise(v_alpha_zero,v_alpha_zero,m_V_alpha_zero,v_d,m_D,v_alpha,m_V_alpha,chi2);
/* Compute D (back proj. to GTK) */
double gtk_pos = 102400.0;
double xp = -1*(gsl_vector_get(v_alpha,2) - gtk_pos)*(gsl_vector_get(v_alpha,6)/gsl_vector_get(v_alpha,8)) + gsl_vector_get(v_alpha,0);
double yp = -1*(gsl_vector_get(v_alpha,2) - gtk_pos)*(gsl_vector_get(v_alpha,7)/gsl_vector_get(v_alpha,8)) + gsl_vector_get(v_alpha,1);
//printf("%g %g %g %g\n",chi2,gsl_vector_get(v_alpha,0),gsl_vector_get(v_alpha,1),gsl_vector_get(v_alpha,2));
*x = gsl_vector_get(v_alpha,0);
*y = gsl_vector_get(v_alpha,1);
*z = gsl_vector_get(v_alpha,2);
//printf("*** %g %g %g\n",*x,*y,*z);
/* LOOP END HERE */
/* Clear the memory */
gsl_matrix_free(m_V_alpha_zero);
gsl_vector_free(v_alpha_zero);
gsl_matrix_free(m_V_alpha);
gsl_vector_free(v_alpha);
gsl_vector_free(v_d);
gsl_matrix_free(m_D);
gsl_vector_free(v_vertex);
for(i=0;i<TRACK_NBR;i++) {
gsl_vector_free(va_tracks[i]);
}
return 0;
}
int minimise(gsl_vector * v_alpha_zero, gsl_vector * v_alpha_A, gsl_matrix * m_V_alpha_zero, gsl_vector * v_d, gsl_matrix * m_D, gsl_vector * v_alpha, gsl_matrix * m_V_alpha, double * chi2) {
size_t height = v_alpha_zero->size;
gsl_vector * v_delta_alpha = gsl_vector_calloc(height);
gsl_vector_memcpy(v_delta_alpha,v_alpha_zero); /* delta_alpha = alpha_zero - alpha_A*/
gsl_vector_sub(v_delta_alpha,v_alpha_A);
gsl_matrix * m_T1 = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,height); /* Temporary matrix */
gsl_matrix * m_T2 = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,TRACK_NBR*CONSTRAINTS); /* Temporary matrix */
gsl_vector * v_T3 = gsl_vector_calloc(TRACK_NBR*CONSTRAINTS); /* Temporary vector */
gsl_matrix * m_V_D = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,TRACK_NBR*CONSTRAINTS);
/* V_D */
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_D,m_V_alpha_zero,0.0,m_T1); /* T1 = D*V_alpha_zero */
gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,m_T1,m_D,0.0,m_T2); /* T2 = T1*D' */
/* Ok */
gsl_blas_dgemv(CblasNoTrans,1.0,m_D,v_delta_alpha,0.0,v_T3); /* T3 = D*delta_alpha */
gsl_vector_add(v_T3,v_d); /* T3 = T3 + d */
/* Ok */
gsl_vector * v_lambda = gsl_vector_calloc(TRACK_NBR*CONSTRAINTS);
/* lambda = inv(T2)*T3 */
int s;
gsl_permutation * p = gsl_permutation_alloc (TRACK_NBR*CONSTRAINTS);
gsl_linalg_LU_decomp (m_T2, p, &s);
gsl_linalg_LU_solve(m_T2,p,v_T3,v_lambda);
/* Ok */
gsl_matrix * m_T4 = gsl_matrix_calloc(height,TRACK_NBR*CONSTRAINTS);
gsl_vector * v_T5 = gsl_vector_calloc(height);
gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,m_V_alpha_zero,m_D,0.0,m_T4); /* T4 = V_alpha_zero*m_D' */
gsl_blas_dgemv(CblasNoTrans,1.0,m_T4,v_lambda,0.0,v_T5); /* T5 = T4*lambda */
gsl_vector_memcpy(v_alpha,v_alpha_A); /* alpha = alpha_A */
gsl_vector_sub(v_alpha,v_T5); /* alpha = alpha_A - T5*/
/* Ok */
/* Compute V_alpha */
gsl_matrix * m_T6 = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,TRACK_NBR*CONSTRAINTS);
gsl_matrix * m_T7 = gsl_matrix_calloc(height,TRACK_NBR*CONSTRAINTS);
gsl_matrix * m_T8 = gsl_matrix_calloc(height,height);
gsl_matrix * m_T9 = gsl_matrix_calloc(height,height);
gsl_matrix_memcpy(m_V_alpha,m_V_alpha_zero); /* V_alpha = V_alpha_zero */
/* T6 = inv(T2) */
gsl_linalg_LU_invert(m_T2,p,m_T6); /* See before for LU_decomp(m_T2,...)*/
/* T7 = T4 * T6 */
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_T4,m_T6,0.0,m_T7);
/* T8 = T7 * D */
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_T7,m_D,0.0,m_T8);
/* T9 = T8 * V_alpha_zero */
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_T8,m_V_alpha_zero,0.0,m_T9);
gsl_matrix_sub(m_V_alpha,m_T9);
/* Ok */
/* Compute chi2 */
/* chi2 = lambda' * T3 */
gsl_blas_ddot(v_lambda,v_T3,chi2);
gsl_vector_free(v_T5);
/* Clean the mess */
gsl_matrix_free(m_T1);
gsl_matrix_free(m_T2);
gsl_vector_free(v_T3);
gsl_matrix_free(m_T4);
gsl_matrix_free(m_T6);
gsl_matrix_free(m_T7);
gsl_matrix_free(m_T8);
gsl_matrix_free(m_T9);
gsl_permutation_free(p);
gsl_matrix_free(m_V_D);
gsl_vector_free(v_delta_alpha);
gsl_vector_free(v_lambda);
}
static int
hybridj_alloc (void *vstate, size_t n)
{
hybridj_state_t *state = (hybridj_state_t *) vstate;
gsl_matrix *q, *r;
gsl_vector *tau, *diag, *qtf, *newton, *gradient, *x_trial, *f_trial,
*df, *qtdf, *rdx, *w, *v;
q = gsl_matrix_calloc (n, n);
if (q == 0)
{
GSL_ERROR ("failed to allocate space for q", GSL_ENOMEM);
}
state->q = q;
r = gsl_matrix_calloc (n, n);
if (r == 0)
{
gsl_matrix_free (q);
GSL_ERROR ("failed to allocate space for r", GSL_ENOMEM);
}
state->r = r;
tau = gsl_vector_calloc (n);
if (tau == 0)
{
gsl_matrix_free (q);
gsl_matrix_free (r);
GSL_ERROR ("failed to allocate space for tau", GSL_ENOMEM);
}
state->tau = tau;
diag = gsl_vector_calloc (n);
if (diag == 0)
{
gsl_matrix_free (q);
gsl_matrix_free (r);
gsl_vector_free (tau);
GSL_ERROR ("failed to allocate space for diag", GSL_ENOMEM);
}
state->diag = diag;
qtf = gsl_vector_calloc (n);
if (qtf == 0)
{
gsl_matrix_free (q);
gsl_matrix_free (r);
gsl_vector_free (tau);
gsl_vector_free (diag);
GSL_ERROR ("failed to allocate space for qtf", GSL_ENOMEM);
}
state->qtf = qtf;
newton = gsl_vector_calloc (n);
if (newton == 0)
{
gsl_matrix_free (q);
gsl_matrix_free (r);
gsl_vector_free (tau);
gsl_vector_free (diag);
gsl_vector_free (qtf);
GSL_ERROR ("failed to allocate space for newton", GSL_ENOMEM);
}
state->newton = newton;
gradient = gsl_vector_calloc (n);
if (gradient == 0)
{
gsl_matrix_free (q);
gsl_matrix_free (r);
gsl_vector_free (tau);
gsl_vector_free (diag);
gsl_vector_free (qtf);
gsl_vector_free (newton);
GSL_ERROR ("failed to allocate space for gradient", GSL_ENOMEM);
}
state->gradient = gradient;
x_trial = gsl_vector_calloc (n);
if (x_trial == 0)
{
gsl_matrix_free (q);
gsl_matrix_free (r);
gsl_vector_free (tau);
gsl_vector_free (diag);
gsl_vector_free (qtf);
gsl_vector_free (newton);
gsl_vector_free (gradient);
GSL_ERROR ("failed to allocate space for x_trial", GSL_ENOMEM);
}
state->x_trial = x_trial;
f_trial = gsl_vector_calloc (n);
if (f_trial == 0)
{
gsl_matrix_free (q);
gsl_matrix_free (r);
gsl_vector_free (tau);
gsl_vector_free (diag);
gsl_vector_free (qtf);
gsl_vector_free (newton);
gsl_vector_free (gradient);
gsl_vector_free (x_trial);
GSL_ERROR ("failed to allocate space for f_trial", GSL_ENOMEM);
}
state->f_trial = f_trial;
df = gsl_vector_calloc (n);
if (df == 0)
{
gsl_matrix_free (q);
gsl_matrix_free (r);
gsl_vector_free (tau);
gsl_vector_free (diag);
gsl_vector_free (qtf);
gsl_vector_free (newton);
gsl_vector_free (gradient);
gsl_vector_free (x_trial);
gsl_vector_free (f_trial);
GSL_ERROR ("failed to allocate space for df", GSL_ENOMEM);
}
state->df = df;
qtdf = gsl_vector_calloc (n);
if (qtdf == 0)
{
gsl_matrix_free (q);
gsl_matrix_free (r);
gsl_vector_free (tau);
gsl_vector_free (diag);
gsl_vector_free (qtf);
gsl_vector_free (newton);
gsl_vector_free (gradient);
gsl_vector_free (x_trial);
gsl_vector_free (f_trial);
gsl_vector_free (df);
GSL_ERROR ("failed to allocate space for qtdf", GSL_ENOMEM);
}
state->qtdf = qtdf;
rdx = gsl_vector_calloc (n);
if (rdx == 0)
{
gsl_matrix_free (q);
gsl_matrix_free (r);
gsl_vector_free (tau);
gsl_vector_free (diag);
gsl_vector_free (qtf);
gsl_vector_free (newton);
gsl_vector_free (gradient);
gsl_vector_free (x_trial);
gsl_vector_free (f_trial);
gsl_vector_free (df);
gsl_vector_free (qtdf);
GSL_ERROR ("failed to allocate space for rdx", GSL_ENOMEM);
}
state->rdx = rdx;
w = gsl_vector_calloc (n);
if (w == 0)
{
gsl_matrix_free (q);
gsl_matrix_free (r);
gsl_vector_free (tau);
gsl_vector_free (diag);
gsl_vector_free (qtf);
gsl_vector_free (newton);
gsl_vector_free (gradient);
gsl_vector_free (x_trial);
gsl_vector_free (f_trial);
gsl_vector_free (df);
gsl_vector_free (qtdf);
gsl_vector_free (rdx);
GSL_ERROR ("failed to allocate space for w", GSL_ENOMEM);
}
state->w = w;
v = gsl_vector_calloc (n);
if (v == 0)
{
gsl_matrix_free (q);
gsl_matrix_free (r);
gsl_vector_free (tau);
gsl_vector_free (diag);
gsl_vector_free (qtf);
gsl_vector_free (newton);
gsl_vector_free (gradient);
gsl_vector_free (x_trial);
gsl_vector_free (f_trial);
gsl_vector_free (df);
gsl_vector_free (qtdf);
gsl_vector_free (rdx);
gsl_vector_free (w);
GSL_ERROR ("failed to allocate space for v", GSL_ENOMEM);
}
state->v = v;
return GSL_SUCCESS;
}
int main (void)
{
/* Local variables */
int i, j, k, l; /* Indices and counters */
int s; /* Sign of the permutation */
int nr = 3; /* Matrix dimensions, rows */
int nc = 3; /* Matrix dimensions, columns */
/* Declare and allocate matrix and vector variables */
gsl_matrix *A = gsl_matrix_calloc(nr,nc); /* A */
gsl_vector *b = gsl_vector_calloc(nr); /* b */
gsl_vector *x = gsl_vector_calloc(nc); /* x */
gsl_permutation *p = gsl_permutation_alloc(nr); /* Permutation Vector for LU */
/* Simple Example */
/* A 3x3 matrix */
double a_data[] = { 2.00, 1.00, 1.00,
4.00, 0.00, 0.00,
-2.00, 7.00, 2.00 } ;
/* b 3-vector */
double b_data[] = { 5.00, -2.00, 9.00 };
/* Initialize coefficient matrix A and vector b */
/* use gsl_matrix_set and gsl_vector_set */
k = 0 ; l = 0 ; /* set counters to zero */
for (i=0;i<nr;i++) {
for (j=0;j<nc;j++) {
gsl_matrix_set(A,i,j,a_data[k]);
k++ ;
} /* for j */
gsl_vector_set(b,i,b_data[l]);
l++ ;
} /* for i */
/* Print entries of A use gsl_matrix_get and printf */
/* do not use gsl_matrix_fprintf */
printf("Solution of the system Ax=b via PLU factorizations\n");
printf("Matrix A:\n");
for (i = 0; i < nr; i++) {
for (j = 0; j < nc; j++)
printf ("%7.2g ", gsl_matrix_get(A, i, j));
putchar('\n');
} /* for i */
/* Print entries of vector b */
printf("Vector b:\n");
gsl_vector_fprintf(stdout, b,"%7.2g");
/* Perform (in place) PLU factorization */
gsl_linalg_LU_decomp(A, p, &s); /* A is overwritten, p is permutation */
/* Find solution using the PLU factors found above and b */
gsl_linalg_LU_solve(A, p, b, x);
/* Print solution x */
printf("Solution x:\n");
gsl_vector_fprintf(stdout, x, "%7.2g");
/* Clean up - free heap memory */
gsl_matrix_free(A);
gsl_vector_free(b);
gsl_permutation_free(p);
gsl_vector_free(x);
return 0;
} /* main */
