44#include <gsl/gsl_matrix.h>
45#include <gsl/gsl_vector.h>
46#include <gsl/gsl_multimin.h>
47#include <gsl/gsl_statistics.h>
57#define STEPS_PER_SECOND 500
58#define MAX_SECONDS_PER_CHUNK 100
60#define PI 3.14159265359
61#define TWOPI 6.28318530718
79 i = (side ==
FT) ? 0 : 16;
80 r = i + (4 * tele) + diode;
94 -0.0039217663072871976, -0.005375368693370768, -0.0039996565553508424, -0.0025503151499257103,
95 -0.004993987370698725, -0.0025982105556817733, -0.0033492901476214598, -0.0024666122995294915,
96 -0.004914056163432626, -0.004605706229646769, -0.0024913748841215717, -0.0025406836901383087,
97 -0.0035045471990661765, -0.004864493604480354, -0.0038971489167063875, -0.002645429318536674,
98 -0.0031186035580824016, -0.0030236901965512925, -0.00455348514695007, -0.0027973126198521715,
99 -0.00329832479577174, -0.004717880979213343, -0.0038105012008050844, -0.0034806513678800706,
100 -0.004843769955531035, -0.0038383890934418244, -0.003413267406206906, -0.0032447329693567305,
101 -0.0019067813627941682, -0.003741090151381262, -0.0031293194990934074, -0.002917132698761452,
102 -0.003846563742841961, -0.0016110031116351089, -0.004055586210967794, -0.004665653836460701,
103 -0.0035651245279117107, -0.003703624441150295, -0.0042027507769680774, -0.003685527333212408,
104 -0.0018717967368672794, -0.004309454414641543, -0.004161530517996172, -0.003603457935318008,
105 -0.0031196452749965883, -0.003456638711821881, -0.005237079636108828, -0.0029119219882156274,
106 -0.002883279151359765, -0.0044843120806118035, -0.0033190561101935586, -0.004244083297006716,
107 -0.005334829455674041, -0.004158489195313494, -0.00242824997808483, -0.0018624941393818074,
108 -0.0029737356896519826, -0.004348274099950943, -0.002654502952395225, -0.0030576540586404475,
109 -0.0032698514683164475, -0.003638017106725761, -0.0029787962274195018, -0.002629800558439812,
110 0.0005056591585058203, 0.0006414248583206548, 0.0005120370824436637, 0.0006245251138490539,
111 0.0002781845526483057, -0.00026739218921257777, 0.0005758898707200935, 0.0003888211264503715,
112 0.00036289398311494447, 6.419787480760673e-05, 0.0008198190512220566, 0.0007714407655275396,
113 0.0008010985218159154, 9.667686713004861e-05, 0.0008396378442216361, 0.0005619108431658763
116static double model_x (
const gsl_vector *X,
int step)
119 double a = gsl_vector_get(X, 0);
120 double b = gsl_vector_get(X, 1);
121 double pha1 = gsl_vector_get(X, 2);
122 double pha2 = gsl_vector_get(X, 3);
124 return a * sin(b * sin(fstep *
TWOPI + pha1) + pha2);
127static double model_y (
const gsl_vector *X,
int step)
130 double a = gsl_vector_get(X, 0);
131 double b = gsl_vector_get(X, 1);
132 double pha1 = gsl_vector_get(X, 2);
133 double pha2 = gsl_vector_get(X, 3);
135 return a * cos(b * sin(fstep *
TWOPI + pha1) + pha2);
144static cpl_error_code
fit_model_modulation (
const gsl_vector *vx,
const gsl_vector *vy, gsl_vector *Xsolve);
163 cpl_ensure_code(met_data, CPL_ERROR_NULL_INPUT);
165 cpl_boolean modulation_active;
167 modulation_active = cpl_propertylist_get_bool(
gravi_data_get_header(met_data),
"ESO INS PMC1 MODULATE");
169 modulation_active = 0;
170 cpl_msg_warning (cpl_func,
"Can't find modulation keyword, use false");
176 if (modulation_active) {
178 if (!zero_subtracted)
179 cpl_msg_info(cpl_func,
"Demodulating metrology with no DARK. Using hardcoded diode zero offsets");
181 cpl_msg_info (cpl_func,
"Metrology modulation is not enabled");
183 return CPL_ERROR_NONE;
189 cpl_size ndata = cpl_table_get_nrow(met_data_table);
190 cpl_size ncol = cpl_table_get_column_dimension(met_data_table,
"VOLT", 0);
195 cpl_size seconds_per_chunk = floor(exptime / nchunks);
199 for (
int ichunk = 0; ichunk < nchunks; ichunk++) {
201 cpl_size start = ichunk * chunk_size;
202 cpl_size count = (ichunk < nchunks - 1) ? chunk_size : ndata - start;
203 cpl_table * chunk_data = cpl_table_extract(met_data_table, start, count);
205 cpl_size nrow = cpl_table_get_nrow(chunk_data);
207 cpl_msg_debug(cpl_func,
"chunk %d has %lld rows (%lld-%lld)\n", ichunk, nrow, start, start+count);
208 cpl_msg_debug(cpl_func,
"seconds in this chunk: %lld\n", nsec);
209 cpl_msg_debug(cpl_func,
"leftover portion of a second: %lld\n", nrow % seconds_per_chunk);
212 gsl_matrix * volts = gsl_matrix_alloc(nrow, ncol);
213 cpl_table_cast_column(chunk_data,
"VOLT",
"VOLTf64", CPL_TYPE_DOUBLE);
214 const cpl_array ** volt_arrs = cpl_table_get_data_array_const(chunk_data,
"VOLTf64");
215 for (
int irow = 0; irow < nrow; irow++) {
216 gsl_vector_const_view row_view = gsl_vector_const_view_array(
217 cpl_array_get_data_double_const(volt_arrs[irow]), ncol
219 gsl_matrix_set_row(volts, irow, &row_view.vector);
225 gsl_matrix * volts_zeroed = gsl_matrix_alloc(nrow, ncol);
226 if (zero_subtracted) {
227 gsl_matrix_memcpy(volts_zeroed, volts);
230 gsl_vector_const_view zero_view = gsl_vector_const_view_array(
diode_zeros, ncol);
231 for (
int i = 0; i < nrow; i++) {
232 gsl_matrix_set_row(volts_zeroed, i, &zero_view.vector);
234 gsl_matrix_scale(volts_zeroed, -1.0);
235 gsl_matrix_add(volts_zeroed, volts);
239 gsl_matrix * volts_zeroed_averaged = gsl_matrix_alloc(
STEPS_PER_SECOND, ncol);
241 for (
int icol = 0; icol < ncol; icol++) {
243 double *start = volts_zeroed->data + (step * ncol + icol);
244 gsl_vector_const_view step_col_view = gsl_vector_const_view_array_with_stride(
247 double mean = gsl_stats_mean(
248 step_col_view.vector.data, step_col_view.vector.stride, step_col_view.vector.size);
249 gsl_matrix_set(volts_zeroed_averaged, step, icol, mean);
256 for (
int tel = 0; tel <
ntel; tel++) {
257 for (
int diode = 0; diode <
ndiode; diode++) {
258 for (
int side =
FT; side <=
SC; side++) {
261 gsl_vector_const_view vx = gsl_matrix_const_column(volts_zeroed_averaged, ix);
262 gsl_vector_const_view vy = gsl_matrix_const_column(volts_zeroed_averaged, iy);
264 gsl_vector_view Xsolve_row = gsl_matrix_row(Xsolve, isolve);
267 cpl_error_set(cpl_func, status);
277 for (
int tel = 0; tel <
ntel; tel++) {
278 for (
int diode = 0; diode <
ndiode; diode++) {
279 for (
int side =
FT; side <=
SC; side++) {
280 double pha2 = gsl_matrix_get(Xsolve, icol, 3);
283 gsl_vector_const_view vx = gsl_matrix_const_column(volts_zeroed_averaged, ix);
284 gsl_vector_const_view vy = gsl_matrix_const_column(volts_zeroed_averaged, iy);
286 double phase = atan2(
287 gsl_vector_get(&vy.vector, step), gsl_vector_get(&vx.vector, step));
288 gsl_matrix_set(volts_phase, step, icol, -phase - pha2);
297 for (
int tel = 0; tel <
ntel; tel++) {
298 for (
int diode = 0; diode <
ndiode; diode++) {
299 for (
int side =
FT; side <=
SC; side++) {
302 gsl_vector_view vx = gsl_matrix_column(volts_zeroed, ix);
303 gsl_vector_view vy = gsl_matrix_column(volts_zeroed, iy);
305 double phase = gsl_matrix_get(volts_phase, step, icol);
306 double s = sin(phase), c = cos(phase);
310 double vx_orig = gsl_vector_get(&vx.vector, idx);
311 double vy_orig = gsl_vector_get(&vy.vector, idx);
312 double vx_demod = c * vx_orig - s * vy_orig;
313 double vy_demod = s * vx_orig + c * vy_orig;
314 gsl_vector_set(&vx.vector, idx, vx_demod);
315 gsl_vector_set(&vy.vector, idx, vy_demod);
325 cpl_array * cpl_row = cpl_array_new(ncol, CPL_TYPE_FLOAT);
326 for (
int irow = 0; irow < nrow; irow++) {
327 gsl_vector_const_view row_view = gsl_matrix_const_row(volts_zeroed, irow);
328 for (cpl_size i = 0; i < ncol; i++) {
329 cpl_array_set_float(cpl_row, i, (
float) gsl_vector_get(&row_view.vector, i));
331 cpl_table_set_array(met_data_table,
"VOLT", start + irow, cpl_row);
333 cpl_array_delete(cpl_row);
334 CPLCHECK_INT(
"Cannot store demodulated metrology data");
337 FREE(gsl_matrix_free, volts_phase);
338 FREE(gsl_matrix_free, volts_zeroed_averaged);
339 FREE(gsl_matrix_free, volts_zeroed);
340 FREE(gsl_matrix_free, Xsolve);
341 FREE(gsl_matrix_free, volts);
342 FREE(cpl_table_delete, chunk_data);
346 return CPL_ERROR_NONE;
365 gsl_vector_set(model_vx, i,
model_x(X, i));
366 gsl_vector_set(model_vy, i,
model_y(X, i));
371 gsl_vector_memcpy(chi2_x, p->
volts_x);
372 gsl_vector_sub(chi2_x, model_vx);
373 gsl_vector_mul(chi2_x, chi2_x);
376 gsl_vector_memcpy(chi2_y, p->
volts_y);
377 gsl_vector_sub(chi2_y, model_vy);
378 gsl_vector_mul(chi2_y, chi2_y);
382 chi2 += gsl_vector_get(chi2_x, i) + gsl_vector_get(chi2_y, i);
384 gsl_vector_free(model_vx);
385 gsl_vector_free(model_vy);
386 gsl_vector_free(chi2_x);
387 gsl_vector_free(chi2_y);
403 cpl_ensure_code(vx, CPL_ERROR_NULL_INPUT);
404 cpl_ensure_code(vy, CPL_ERROR_NULL_INPUT);
405 cpl_ensure_code(Xsolve, CPL_ERROR_NULL_INPUT);
410 const int MAX_ITERATIONS = 1000;
411 const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2rand;
412 gsl_multimin_fminimizer *mini = gsl_multimin_fminimizer_alloc(T, dim);
413 int status, iterations;
421 gsl_multimin_function func = {
427 double best_chi2 = INFINITY;
428 double a_guess = gsl_stats_sd(vx->data, vx->stride, vx->size);
429 double b_guess = 0.25;
431 double phase_range =
PI;
433 double phase_step = phase_range / (2 * nphase);
436 for (
int iph1 = -nphase; iph1 < nphase; iph1++) {
437 for (
int iph2 = -nphase; iph2 < nphase; iph2++) {
438 double pha1_guess = iph1 * phase_step;
439 double pha2_guess = iph2 * phase_step;
441 double initial_guess[] = {a_guess, b_guess, pha1_guess, pha2_guess};
442 gsl_vector_view guess_view = gsl_vector_view_array(initial_guess, dim);
443 double step_size[] = {0.1, 0.1, 1, 1};
444 gsl_vector_view step_view = gsl_vector_view_array(step_size, dim);
445 gsl_multimin_fminimizer_set(mini, &func, &guess_view.vector, &step_view.vector);
450 status = gsl_multimin_fminimizer_iterate(mini);
456 size = gsl_multimin_fminimizer_size(mini);
457 status = gsl_multimin_test_size(size, 1e-3);
458 }
while (status == GSL_CONTINUE && iterations < MAX_ITERATIONS);
460 if (status == GSL_SUCCESS && mini->fval < best_chi2) {
461 best_chi2 = mini->fval;
462 gsl_vector_memcpy(Xsolve, mini->x);
468 double a, pha1, pha2;
469 if ((a = gsl_vector_get(Xsolve, 0)) < 0) {
470 gsl_vector_set(Xsolve, 0, -a);
471 pha2 = gsl_vector_get(Xsolve, 3);
472 gsl_vector_set(Xsolve, 3, pha2 +
PI);
476 if (fabs(pha1 = gsl_vector_get(Xsolve, 2)) >
PI)
477 gsl_vector_set(Xsolve, 2, pha1 -
SIGN(
TWOPI, pha1));
478 if (fabs(pha2 = gsl_vector_get(Xsolve, 3)) >
PI)
479 gsl_vector_set(Xsolve, 3, pha2 -
SIGN(
TWOPI, pha2));
481 if (status == GSL_CONTINUE)
482 cpl_msg_warning(cpl_func,
"model-fitting did not converge");
483 else if (status != GSL_SUCCESS)
484 cpl_msg_error(cpl_func,
"model-fitting failed with error code %d (%s)", status, gsl_strerror(status));
486 cpl_msg_debug(cpl_func,
"converged with best-fit:\na\tb\tpha1\tpha2\n%.4f\t%.4f\t%.4f\t%.4f\n", mini->x->data[0], mini->x->data[1], mini->x->data[2], mini->x->data[3]);
488 FREE(gsl_multimin_fminimizer_free, mini);
489 return status ? CPL_ERROR_ILLEGAL_OUTPUT : CPL_ERROR_NONE;
typedefCPL_BEGIN_DECLS struct _gravi_data_ gravi_data
#define gravi_data_get_header(data)
static cpl_error_code fit_model_modulation(const gsl_vector *vx, const gsl_vector *vy, gsl_vector *Xsolve)
Fit the modulation model to voltage data for a single diode.
static cpl_size diode_column_index(int tele, int diode, int side)
Return column index in VOLT table for specific diode.
#define MAX_SECONDS_PER_CHUNK
static double modulation_model_chi2(const gsl_vector *x, void *params)
Calculate chi^2 statistic for modulation model.
cpl_error_code gravi_metrology_demodulate(gravi_data *met_data, cpl_boolean zero_subtracted)
Demodulate the metrology.
static double model_x(const gsl_vector *X, int step)
const double diode_zeros[]
struct _demodulation_model_params_ model_params
static double model_y(const gsl_vector *X, int step)
cpl_msg_debug(cpl_func, "Spectra has <50 pixels -> don't flat")
cpl_msg_info(cpl_func, "Compute WAVE_SCAN for %s", GRAVI_TYPE(type_data))
#define GRAVI_METROLOGY_EXT
#define CPLCHECK_INT(msg)
#define gravi_msg_function_exit(flag)
#define FREE(function, variable)
#define gravi_msg_function_start(flag)
cpl_table * gravi_data_get_table(gravi_data *self, const char *extname)
Return a pointer on a table extension by its EXTNAME.
const gsl_vector * volts_y
const gsl_vector * volts_x