Kstars

gaussian_process_guider.cpp
1 /*
2  PHD2 Guiding
3 
4  @file
5  @date 2014-2017
6  @copyright Max Planck Society
7 
8  @brief Provides a Gaussian process based guiding algorithm
9 
10  SPDX-FileCopyrightText: Edgar D. Klenske <[email protected]>
11  SPDX-FileCopyrightText: Stephan Wenninger <[email protected]>
12  SPDX-FileCopyrightText: Raffi Enficiaud <[email protected]>
13 
14  SPDX-License-Identifier: BSD-3-Clause
15 */
16 
17 #include "gaussian_process_guider.h"
18 
19 #include <cmath>
20 #include <ctime>
21 #include <iostream>
22 #include <iomanip>
23 #include <fstream>
24 
25 // HY: Added this include and a few qCDebug EKos logging calls below.
26 #include "ekos_guide_debug.h"
27 
28 #define SAVE_FFT_DATA_ 0
29 #define PRINT_TIMINGS_ 0
30 
31 #define CIRCULAR_BUFFER_SIZE 8192 // for the raw data storage
32 #define REGULAR_BUFFER_SIZE 2048 // for the regularized data storage
33 #define FFT_SIZE 4096 // for zero-padding the FFT, >= REGULAR_BUFFER_SIZE!
34 #define GRID_INTERVAL 5.0
35 #define MAX_DITHER_STEPS 10 // for our fallback dithering
36 
37 #define DEFAULT_LEARNING_RATE 0.01 // for a smooth parameter adaptation
38 
39 #define HYSTERESIS 0.1 // for the hybrid mode
40 
41 GaussianProcessGuider::GaussianProcessGuider(guide_parameters parameters) :
42  start_time_(std::chrono::system_clock::now()),
43  last_time_(std::chrono::system_clock::now()),
44  control_signal_(0),
45  prediction_(0),
46  last_prediction_end_(0),
47  dither_steps_(0),
48  dithering_active_(false),
49  dither_offset_(0.0),
50  circular_buffer_data_(CIRCULAR_BUFFER_SIZE),
51  covariance_function_(),
52  output_covariance_function_(),
53  gp_(covariance_function_),
54  learning_rate_(DEFAULT_LEARNING_RATE),
55  parameters(parameters)
56 {
57  circular_buffer_data_.push_front(data_point()); // add first point
58  circular_buffer_data_[0].control = 0; // set first control to zero
59  gp_.enableExplicitTrend(); // enable the explicit basis function for the linear drift
60  gp_.enableOutputProjection(output_covariance_function_); // for prediction
61 
62  std::vector<double> hyperparameters(NumParameters);
63  hyperparameters[SE0KLengthScale] = parameters.SE0KLengthScale_;
64  hyperparameters[SE0KSignalVariance] = parameters.SE0KSignalVariance_;
65  hyperparameters[PKLengthScale] = parameters.PKLengthScale_;
66  hyperparameters[PKSignalVariance] = parameters.PKSignalVariance_;
67  hyperparameters[SE1KLengthScale] = parameters.SE1KLengthScale_;
68  hyperparameters[SE1KSignalVariance] = parameters.SE1KSignalVariance_;
69  hyperparameters[PKPeriodLength] = parameters.PKPeriodLength_;
70  SetGPHyperparameters(hyperparameters);
71 
72  qCDebug(KSTARS_EKOS_GUIDE) << QString("GPG Parameters: control_gain %1 min_move %2 pred_gain %3 min_for_inf %4 min_for_period %5 pts %6 cpd %7 -- se0L %8 se0V %9 PL %10 PV %11 Se1L %12 se1V %13 ppd %14")
73  .arg(parameters.control_gain_, 6, 'f', 3)
74  .arg(parameters.min_move_, 6, 'f', 3)
75  .arg(parameters.prediction_gain_, 6, 'f', 3)
76  .arg(parameters.min_periods_for_inference_, 6, 'f', 3)
77  .arg(parameters.min_periods_for_period_estimation_, 6, 'f', 3)
78  .arg(parameters.points_for_approximation_)
79  .arg(parameters.compute_period_)
80  .arg(parameters.SE0KLengthScale_, 6, 'f', 3)
81  .arg(parameters.SE0KSignalVariance_, 6, 'f', 3)
82  .arg(parameters.PKLengthScale_, 6, 'f', 3)
83  .arg(parameters.PKSignalVariance_, 6, 'f', 3)
84  .arg(parameters.SE1KLengthScale_, 6, 'f', 3)
85  .arg(parameters.SE1KSignalVariance_, 6, 'f', 3)
86  .arg(parameters.PKPeriodLength_, 6, 'f', 3);
87 }
88 
89 GaussianProcessGuider::~GaussianProcessGuider()
90 {
91 }
92 
93 void GaussianProcessGuider::SetTimestamp()
94 {
95  auto current_time = std::chrono::system_clock::now();
96  double delta_measurement_time = std::chrono::duration<double>(current_time - last_time_).count();
97  last_time_ = current_time;
98  get_last_point().timestamp = std::chrono::duration<double>(current_time - start_time_).count()
99  - (delta_measurement_time / 2.0) // use the midpoint as time stamp
100  + dither_offset_; // correct for the gear time offset from dithering
101 }
102 
103 // adds a new measurement to the circular buffer that holds the data.
104 void GaussianProcessGuider::HandleGuiding(double input, double SNR)
105 {
106  SetTimestamp();
107  get_last_point().measurement = input;
108  get_last_point().variance = CalculateVariance(SNR);
109 
110  // we don't want to predict for the part we have measured!
111  // therefore, don't use the past when a measurement is available.
112  last_prediction_end_ = get_last_point().timestamp;
113 }
114 
115 void GaussianProcessGuider::HandleDarkGuiding()
116 {
117  SetTimestamp();
118  get_last_point().measurement = 0; // we didn't actually measure
119  get_last_point().variance = 1e4; // add really high noise
120 }
121 
122 void GaussianProcessGuider::HandleControls(double control_input)
123 {
124  get_last_point().control = control_input;
125 }
126 
127 double GaussianProcessGuider::CalculateVariance(double SNR)
128 {
129  SNR = std::max(SNR, 3.4); // limit the minimal SNR
130 
131  // this was determined by simulated experiments
132  double standard_deviation = 2.1752 * 1 / (SNR - 3.3) + 0.5;
133 
134  return standard_deviation * standard_deviation;
135 }
136 
137 void GaussianProcessGuider::UpdateGP(double prediction_point /*= std::numeric_limits<double>::quiet_NaN()*/)
138 {
139 #if PRINT_TIMINGS_
140  clock_t begin = std::clock(); // this is for timing the method in a simple way
141 #endif
142 
143  size_t N = get_number_of_measurements();
144 
145  // initialize the different vectors needed for the GP
146  Eigen::VectorXd timestamps(N-1);
147  Eigen::VectorXd measurements(N-1);
148  Eigen::VectorXd variances(N-1);
149  Eigen::VectorXd sum_controls(N-1);
150 
151  double sum_control = 0;
152 
153  // transfer the data from the circular buffer to the Eigen::Vectors
154  for (size_t i = 0; i < N-1; i++)
155  {
156  sum_control += circular_buffer_data_[i].control; // sum over the control signals
157  timestamps(i) = circular_buffer_data_[i].timestamp;
158  measurements(i) = circular_buffer_data_[i].measurement;
159  variances(i) = circular_buffer_data_[i].variance;
160  sum_controls(i) = sum_control; // store current accumulated control signal
161  }
162 
163  Eigen::VectorXd gear_error(N-1);
164  Eigen::VectorXd linear_fit(N-1);
165 
166  // calculate the accumulated gear error
167  gear_error = sum_controls + measurements; // for each time step, add the residual error
168 
169 #if PRINT_TIMINGS_
170  clock_t end = std::clock();
171  double time_init = double(end - begin) / CLOCKS_PER_SEC;
172  begin = std::clock();
173 #endif
174 
175  // regularize the measurements
176  Eigen::MatrixXd result = regularize_dataset(timestamps, gear_error, variances);
177 
178  // the three vectors are returned in a matrix, we need to extract them
179  timestamps = result.row(0);
180  gear_error = result.row(1);
181  variances = result.row(2);
182 
183 #if PRINT_TIMINGS_
184  end = std::clock();
185  double time_regularize = double(end - begin) / CLOCKS_PER_SEC;
186  begin = std::clock();
187 #endif
188 
189  // linear least squares regression for offset and drift to de-trend the data
190  Eigen::MatrixXd feature_matrix(2, timestamps.rows());
191  feature_matrix.row(0) = Eigen::MatrixXd::Ones(1, timestamps.rows()); // timestamps.pow(0)
192  feature_matrix.row(1) = timestamps.array(); // timestamps.pow(1)
193 
194  // this is the inference for linear regression
195  Eigen::VectorXd weights = (feature_matrix*feature_matrix.transpose()
196  + 1e-3*Eigen::Matrix<double, 2, 2>::Identity()).ldlt().solve(feature_matrix*gear_error);
197 
198  // calculate the linear regression for all datapoints
199  linear_fit = weights.transpose()*feature_matrix;
200 
201  // subtract polynomial fit from the data points
202  Eigen::VectorXd gear_error_detrend = gear_error - linear_fit;
203 
204 #if PRINT_TIMINGS_
205  end = std::clock();
206  double time_detrend = double(end - begin) / CLOCKS_PER_SEC;
207  begin = std::clock();
208  double time_fft = 0; // need to initialize in case the FFT isn't calculated
209 #endif
210 
211  // calculate period length if we have enough points already
212  double period_length = GetGPHyperparameters()[PKPeriodLength];
213  if (GetBoolComputePeriod() && get_last_point().timestamp > parameters.min_periods_for_period_estimation_ * period_length)
214  {
215  // find periodicity parameter with FFT
216  period_length = EstimatePeriodLength(timestamps, gear_error_detrend);
217  UpdatePeriodLength(period_length);
218 
219 #if PRINT_TIMINGS_
220  end = std::clock();
221  time_fft = double(end - begin) / CLOCKS_PER_SEC;
222 #endif
223  }
224 
225 #if PRINT_TIMINGS_
226  begin = std::clock();
227 #endif
228 
229  // inference of the GP with the new points, maximum accuracy should be reached around current time
230  gp_.inferSD(timestamps, gear_error, parameters.points_for_approximation_, variances, prediction_point);
231 
232 #if PRINT_TIMINGS_
233  end = std::clock();
234  double time_gp = double(end - begin) / CLOCKS_PER_SEC;
235 
236  printf("timings: init: %f, regularize: %f, detrend: %f, fft: %f, gp: %f, total: %f\n",
237  time_init, time_regularize, time_detrend, time_fft, time_gp,
238  time_init + time_regularize + time_detrend + time_fft + time_gp);
239 #endif
240 }
241 
242 double GaussianProcessGuider::PredictGearError(double prediction_location)
243 {
244  // in the first step of each sequence, use the current time stamp as last prediction end
245  if (last_prediction_end_ < 0.0)
246  {
247  last_prediction_end_ = std::chrono::duration<double>(std::chrono::system_clock::now() - start_time_).count();
248  }
249 
250  // prediction from the last endpoint to the prediction point
251  Eigen::VectorXd next_location(2);
252  next_location << last_prediction_end_, prediction_location + dither_offset_;
253  Eigen::VectorXd prediction = gp_.predictProjected(next_location);
254 
255  double p1 = prediction(1);
256  double p0 = prediction(0);
257 
258  assert(!math_tools::isNaN(p1 - p0));
259 
260  last_prediction_end_ = next_location(1); // store current endpoint
261 
262  // we are interested in the error introduced by the gear over the next time step
263  return (p1 - p0);
264 }
265 
266 double GaussianProcessGuider::result(double input, double SNR, double time_step, double prediction_point /*= -1*/)
267 {
268  /*
269  * Dithering behaves differently from pausing. During dithering, the mount
270  * is moved and thus we can assume that we applied the perfect control, but
271  * we cannot trust the measurement. Once dithering has settled, we can trust
272  * the measurement again and we can pretend nothing has happend.
273  */
274  double hyst_percentage = 0.0;
275  double period_length;
276 
277  if (dithering_active_)
278  {
279  qCDebug(KSTARS_EKOS_GUIDE) << QString("GPG::result(in=%1,snr=%2,ts=%3,ppt=%4) dithering active")
280  .arg(input, 6, 'f', 2).arg(SNR, 5, 'f', 1).arg(time_step, 6, 'f', 2).arg(prediction_point, 5, 'f', 1);
281  if (--dither_steps_ <= 0)
282  {
283  dithering_active_ = false;
284  }
285  deduceResult(time_step); // just pretend we would do dark guiding...
286 
287  GPDebug->Log("PPEC rslt(dithering): input = %.2f, final = %.2f",
288  input, parameters.control_gain_ * input);
289 
290  qCDebug(KSTARS_EKOS_GUIDE)
291  << QString("PPEC rslt(dithering): input = %1, final = %2")
292  .arg(input, 5, 'f', 2)
293  .arg(parameters.control_gain_ * input, 5, 'f', 2);
294 
295  return parameters.control_gain_ * input; // ...but apply proportional control
296  }
297 
298  qCDebug(KSTARS_EKOS_GUIDE) << QString("GPG::result(in=%1,snr=%2,ts=%3,ppt=%4)")
299  .arg(input, 6, 'f', 2).arg(SNR, 5, 'f', 1).arg(time_step, 6, 'f', 2).arg(prediction_point, 5, 'f', 1);
300  // the starting time is set at the first call of result after startup or reset
301  if (get_number_of_measurements() == 1)
302  {
303  start_time_ = std::chrono::system_clock::now();
304  last_time_ = start_time_; // this is OK, since last_time_ only provides a minor correction
305  }
306 
307  // collect data point content, except for the control signal
308  HandleGuiding(input, SNR);
309 
310  // calculate hysteresis result, too, for hybrid control
311  double last_control = 0.0;
312  if (get_number_of_measurements() > 1)
313  {
314  last_control = get_second_last_point().control;
315  }
316  double hysteresis_control = (1.0 - HYSTERESIS) * input + HYSTERESIS * last_control;
317  hysteresis_control *= parameters.control_gain_;
318 
319  control_signal_ = parameters.control_gain_ * input; // start with proportional control
320  if (std::abs(input) < parameters.min_move_)
321  {
322  control_signal_ = 0.0; // don't make small moves
323  hysteresis_control = 0.0;
324  }
325  assert(std::abs(control_signal_) == 0.0 || std::abs(input) >= parameters.min_move_);
326 
327  // calculate GP prediction
328  if (get_number_of_measurements() > 10)
329  {
330  if (prediction_point < 0.0)
331  {
332  prediction_point = std::chrono::duration<double>(std::chrono::system_clock::now() - start_time_).count();
333  }
334  // the point of highest precision shoud be between now and the next step
335  UpdateGP(prediction_point + 0.5 * time_step);
336 
337  // the prediction should end after one time step
338  prediction_ = PredictGearError(prediction_point + time_step);
339  control_signal_ += parameters.prediction_gain_ * prediction_; // add the prediction
340 
341  // smoothly blend over between hysteresis and GP
342  period_length = GetGPHyperparameters()[PKPeriodLength];
343  if (get_last_point().timestamp < parameters.min_periods_for_inference_ * period_length)
344  {
345  double percentage = get_last_point().timestamp / (parameters.min_periods_for_inference_ * period_length);
346  percentage = std::min(percentage, 1.0); // limit to 100 percent GP
347  hyst_percentage = 1.0 - percentage;
348  control_signal_ = percentage * control_signal_ + (1.0 - percentage) * hysteresis_control;
349  }
350  }
351  else
352  {
353  period_length = GetGPHyperparameters()[PKPeriodLength]; // for logging
354  }
355 
356  // assert for the developers...
357  assert(!math_tools::isNaN(control_signal_));
358 
359  // ...safeguard for the users
360  if (math_tools::isNaN(control_signal_))
361  {
362  control_signal_ = hysteresis_control;
363  }
364 
365  add_one_point(); // add new point here, since the control is for the next point in time
366  HandleControls(control_signal_); // already store control signal
367 
368  GPDebug->Log("PPEC rslt: input = %.2f, final = %.2f, react = %.2f, pred = %.2f, hyst = %.2f, hyst_pct = %.2f, period_length = %.2f",
369  input, control_signal_, parameters.control_gain_ * input, parameters.prediction_gain_ * prediction_, hysteresis_control,
370  hyst_percentage, period_length);
371 
372  qCDebug(KSTARS_EKOS_GUIDE) << QString("PPEC rslt: input = %1, final = %2, react = %3, pred = %4, hyst = %5, hyst_pct = %6, period_length = %7")
373  .arg(input, 5, 'f', 2)
374  .arg(control_signal_, 5, 'f', 2)
375  .arg(parameters.control_gain_ * input, 5, 'f', 2)
376  .arg(parameters.prediction_gain_ * prediction_, 5, 'f', 2)
377  .arg(hysteresis_control, 5, 'f', 2)
378  .arg(hyst_percentage, 5, 'f', 2)
379  .arg(period_length, 5, 'f', 2);
380  return control_signal_;
381 }
382 
383 double GaussianProcessGuider::deduceResult(double time_step, double prediction_point /*= -1.0*/)
384 {
385  HandleDarkGuiding();
386 
387  control_signal_ = 0; // no measurement!
388  // check if we are allowed to use the GP
389  if (get_number_of_measurements() > 10
390  && get_last_point().timestamp > parameters.min_periods_for_inference_ * GetGPHyperparameters()[PKPeriodLength])
391  {
392  if (prediction_point < 0.0)
393  {
394  prediction_point = std::chrono::duration<double>(std::chrono::system_clock::now() - start_time_).count();
395  }
396  // the point of highest precision should be between now and the next step
397  UpdateGP(prediction_point + 0.5 * time_step);
398 
399  // the prediction should end after one time step
400  prediction_ = PredictGearError(prediction_point + time_step);
401  control_signal_ += prediction_; // control based on prediction
402  }
403 
404  add_one_point(); // add new point here, since the control is for the next point in time
405  HandleControls(control_signal_); // already store control signal
406 
407  // assert for the developers...
408  assert(!math_tools::isNaN(control_signal_));
409 
410  // ...safeguard for the users
411  if (math_tools::isNaN(control_signal_))
412  {
413  control_signal_ = 0.0;
414  }
415 
416  return control_signal_;
417 }
418 
420 {
421  qCDebug(KSTARS_EKOS_GUIDE) << QString("GPG::reset()");
422  circular_buffer_data_.clear();
423  gp_.clearData();
424 
425  // We need to add a first data point because the measurements are always relative to the control.
426  // For the first measurement, we therefore need to add a point with zero control.
427  circular_buffer_data_.push_front(data_point()); // add first point
428  circular_buffer_data_[0].control = 0; // set first control to zero
429 
430  last_prediction_end_ = -1.0; // the negative value signals we didn't predict yet
431  start_time_ = std::chrono::system_clock::now();
432  last_time_ = std::chrono::system_clock::now();
433 
434  dither_offset_ = 0.0;
435  dither_steps_ = 0;
436  dithering_active_ = false;
437 }
438 
439 void GaussianProcessGuider::GuidingDithered(double amt, double rate)
440 {
441  qCDebug(KSTARS_EKOS_GUIDE) << QString("GPG::GuidingDithered(amt=%1,rate=%2)")
442  .arg(amt, 6, 'f', 2).arg(rate, 5, 'f', 1);
443 
444  // we store the amount of dither in seconds of gear time
445  dither_offset_ += amt / rate; // this is the amount of time offset
446 
447  dithering_active_ = true;
448  dither_steps_ = MAX_DITHER_STEPS;
449 }
450 
452 {
453  qCDebug(KSTARS_EKOS_GUIDE) << QString("GPG::GuidingDitherSettleDone(%1)")
454  .arg(success ? "true" : "false");
455  if (success)
456  {
457  dither_steps_ = 1; // the last dither step should always be executed by
458  // result(), since it corrects for the time difference
459  }
460 }
461 
462 void GaussianProcessGuider::DirectMoveApplied(double amt, double rate)
463 {
464  (void) amt; // silence compiler warning while the code below is commented out (HY).
465  (void) rate;
466  // we store the amount of dither in seconds of gear time
467 // todo: validate this:
468  // dither_offset_ += amt / rate; // this is the amount of time offset
469 }
470 
471 double GaussianProcessGuider::GetControlGain(void) const
472 {
473  return parameters.control_gain_;
474 }
475 
476 bool GaussianProcessGuider::SetControlGain(double control_gain)
477 {
478  parameters.control_gain_ = control_gain;
479  return false;
480 }
481 
482 bool GaussianProcessGuider::GetBoolComputePeriod() const {
483  return parameters.compute_period_;
484 }
485 
486 bool GaussianProcessGuider::SetBoolComputePeriod(bool active) {
487  parameters.compute_period_ = active;
488  return false;
489 }
490 
491 std::vector<double> GaussianProcessGuider::GetGPHyperparameters() const
492 {
493  // since the GP class works in log space, we have to exp() the parameters first.
494  Eigen::VectorXd hyperparameters_full = gp_.getHyperParameters().array().exp();
495  // remove first parameter, which is unused here
496  Eigen::VectorXd hyperparameters = hyperparameters_full.tail(NumParameters);
497 
498  // converts the length-scale of the periodic covariance from standard notation to natural units
499  hyperparameters(PKLengthScale) = std::asin(hyperparameters(PKLengthScale)/4.0)*hyperparameters(PKPeriodLength)/M_PI;
500 
501  // we need to map the Eigen::vector into a std::vector.
502  return std::vector<double>(hyperparameters.data(), // the first element is at the array address
503  hyperparameters.data() + NumParameters);
504 }
505 
506 bool GaussianProcessGuider::SetGPHyperparameters(std::vector<double> const &hyperparameters)
507 {
508  Eigen::VectorXd hyperparameters_eig = Eigen::VectorXd::Map(&hyperparameters[0], hyperparameters.size());
509 
510  // prevent length scales from becoming too small (makes GP unstable)
511  hyperparameters_eig(SE0KLengthScale) = std::max(hyperparameters_eig(SE0KLengthScale), 1.0);
512  hyperparameters_eig(PKLengthScale) = std::max(hyperparameters_eig(PKLengthScale), 1.0);
513  hyperparameters_eig(SE1KLengthScale) = std::max(hyperparameters_eig(SE1KLengthScale), 1.0);
514 
515  // converts the length-scale of the periodic covariance from natural units to standard notation
516  hyperparameters_eig(PKLengthScale) = 4*std::sin(hyperparameters_eig(PKLengthScale)
517  *M_PI/hyperparameters_eig(PKPeriodLength));
518 
519  // safeguard all parameters from being too small (log conversion)
520  hyperparameters_eig = hyperparameters_eig.array().max(1e-10);
521 
522  // need to convert to GP parameters
523  Eigen::VectorXd hyperparameters_full(NumParameters + 1); // the GP has one more parameter!
524  hyperparameters_full << 1.0, hyperparameters_eig;
525 
526  // the GP works in log space, therefore we need to convert
527  gp_.setHyperParameters(hyperparameters_full.array().log());
528  return false;
529 }
530 
531 double GaussianProcessGuider::GetMinMove() const {
532  return parameters.min_move_;
533 }
534 
535 bool GaussianProcessGuider::SetMinMove(double min_move) {
536  parameters.min_move_ = min_move;
537  return false;
538 }
539 
540 int GaussianProcessGuider::GetNumPointsForApproximation() const {
541  return parameters.points_for_approximation_;
542 }
543 
544 bool GaussianProcessGuider::SetNumPointsForApproximation(int num_points) {
545  parameters.points_for_approximation_ = num_points;
546  return false;
547 }
548 
549 double GaussianProcessGuider::GetPeriodLengthsInference() const {
550  return parameters.min_periods_for_inference_;
551 }
552 
553 bool GaussianProcessGuider::SetPeriodLengthsInference(double num_periods) {
554  parameters.min_periods_for_inference_ = num_periods;
555  return false;
556 }
557 
558 double GaussianProcessGuider::GetPeriodLengthsPeriodEstimation() const {
559  return parameters.min_periods_for_period_estimation_;
560 }
561 
562 bool GaussianProcessGuider::SetPeriodLengthsPeriodEstimation(double num_periods) {
563  parameters.min_periods_for_period_estimation_ = num_periods;
564  return false;
565 }
566 
567 double GaussianProcessGuider::GetPredictionGain() const {
568  return parameters.prediction_gain_;
569 }
570 
571 bool GaussianProcessGuider::SetPredictionGain(double prediction_gain) {
572  parameters.prediction_gain_ = prediction_gain;
573  return false;
574 }
575 
576 void GaussianProcessGuider::inject_data_point(double timestamp, double input, double SNR, double control) {
577  // collect data point content, except for the control signal
578  HandleGuiding(input, SNR);
579  last_prediction_end_ = timestamp;
580  get_last_point().timestamp = timestamp; // overrides the usual HandleTimestamps();
581 
582  start_time_ = std::chrono::system_clock::now() - std::chrono::seconds((int) timestamp);
583 
584  add_one_point(); // add new point here, since the control is for the next point in time
585  HandleControls(control); // already store control signal
586 }
587 
588 double GaussianProcessGuider::EstimatePeriodLength(const Eigen::VectorXd& time, const Eigen::VectorXd& data) {
589  // compute Hamming window to reduce spectral leakage
590  Eigen::VectorXd windowed_data = data.array() * math_tools::hamming_window(data.rows()).array();
591 
592  // compute the spectrum
593  std::pair<Eigen::VectorXd, Eigen::VectorXd> result = math_tools::compute_spectrum(windowed_data, FFT_SIZE);
594 
595  Eigen::ArrayXd amplitudes = result.first;
596  Eigen::ArrayXd frequencies = result.second;
597 
598  double dt = (time(time.rows()-1) - time(0))/(time.rows()-1); // (t_end - t_begin) / num_t
599 
600  frequencies /= dt; // correct for the average time step width
601 
602  Eigen::ArrayXd periods = 1/frequencies.array();
603  amplitudes = (periods > 1500.0).select(0,amplitudes); // set amplitudes to zero for too large periods
604 
605  assert(amplitudes.size() == frequencies.size());
606 
607  Eigen::VectorXd::Index maxIndex;
608  amplitudes.maxCoeff(&maxIndex);
609 
610  double max_frequency = frequencies(maxIndex);
611 
612  // quadratic interpolation to find maximum
613  // check if we can interpolate
614  if (maxIndex < frequencies.size() - 1 && maxIndex > 0)
615  {
616  double spread = std::abs(frequencies(maxIndex - 1) - frequencies(maxIndex + 1));
617 
618  Eigen::VectorXd interp_loc(3);
619  interp_loc << frequencies(maxIndex - 1), frequencies(maxIndex), frequencies(maxIndex + 1);
620  interp_loc = interp_loc.array() - max_frequency; // centering for numerical stability
621  interp_loc = interp_loc.array() / spread; // normalize for numerical stability
622 
623  Eigen::VectorXd interp_dat(3);
624  interp_dat << amplitudes(maxIndex - 1), amplitudes(maxIndex), amplitudes(maxIndex + 1);
625  interp_dat = interp_dat.array() / amplitudes(maxIndex); // normalize for numerical stability
626 
627  // we need to handle the case where all amplitudes are equal
628  // the linear regression would be unstable in this case
629  if (interp_dat.maxCoeff() - interp_dat.minCoeff() < 1e-10)
630  {
631  return 1 / max_frequency; // don't do the linear regression
632  }
633 
634 
635  // building feature matrix
636  Eigen::MatrixXd phi(3,3);
637  phi.row(0) = interp_loc.array().pow(2);
638  phi.row(1) = interp_loc.array().pow(1);
639  phi.row(2) = interp_loc.array().pow(0);
640 
641  // standard equation for linear regression
642  Eigen::VectorXd w = (phi*phi.transpose()).ldlt().solve(phi*interp_dat);
643 
644  // recovering the maximum from the weights relative to the frequency of the maximum
645  max_frequency = max_frequency - w(1)/(2*w(0))*spread; // note the de-normalization
646  }
647 
648 #if SAVE_FFT_DATA_
649  {
650  std::ofstream outfile;
651  outfile.open("spectrum_data.csv", std::ios_base::out);
652  if (outfile) {
653  outfile << "period, amplitude\n";
654  for (int i = 0; i < amplitudes.size(); ++i) {
655  outfile << std::setw(8) << periods[i] << "," << std::setw(8) << amplitudes[i] << "\n";
656  }
657  }
658  else {
659  std::cout << "unable to write to file" << std::endl;
660  }
661  outfile.close();
662  }
663 #endif
664 
665  double period_length = 1 / max_frequency; // we return the period length!
666  return period_length;
667 }
668 
670 {
671  std::vector<double> hypers = GetGPHyperparameters();
672 
673  // assert for the developers...
674  assert(!math_tools::isNaN(period_length));
675 
676  // ...and save the day for the users
677  if (math_tools::isNaN(period_length))
678  {
679  period_length = hypers[PKPeriodLength]; // just use the old value instead
680  }
681 
682  // we just apply a simple learning rate to slow down parameter jumps
683  hypers[PKPeriodLength] = (1 - learning_rate_) * hypers[PKPeriodLength] + learning_rate_ * period_length;
684 
685  SetGPHyperparameters(hypers); // the setter function is needed to convert parameters
686 }
687 
688 Eigen::MatrixXd GaussianProcessGuider::regularize_dataset(const Eigen::VectorXd& timestamps,
689  const Eigen::VectorXd& gear_error, const Eigen::VectorXd& variances)
690 {
691  size_t N = get_number_of_measurements();
692  double grid_interval = GRID_INTERVAL;
693  double last_cell_end = -grid_interval;
694  double last_timestamp = -grid_interval;
695  double last_gear_error = 0.0;
696  double last_variance = 0.0;
697  double gear_error_sum = 0.0;
698  double variance_sum = 0.0;
699  int grid_size = static_cast<int>(std::ceil(timestamps(timestamps.size() - 1) / grid_interval)) + 1;
700  assert(grid_size > 0);
701  Eigen::VectorXd reg_timestamps(grid_size);
702  Eigen::VectorXd reg_gear_error(grid_size);
703  Eigen::VectorXd reg_variances(grid_size);
704  int j = 0;
705  for (size_t i = 0; i < N-1; ++i)
706  {
707  if (timestamps(i) < last_cell_end + grid_interval)
708  {
709  gear_error_sum += (timestamps(i) - last_timestamp) * 0.5 * (last_gear_error + gear_error(i));
710  variance_sum += (timestamps(i) - last_timestamp) * 0.5 * (last_variance + variances(i));
711  last_timestamp = timestamps(i);
712  }
713  else
714  {
715  while (timestamps(i) >= last_cell_end + grid_interval)
716  {
717  double inter_timestamp = last_cell_end + grid_interval;
718 
719  double proportion = (inter_timestamp-last_timestamp)/(timestamps(i)-last_timestamp);
720  double inter_gear_error = proportion*gear_error(i) + (1-proportion)*last_gear_error;
721  double inter_variance = proportion*variances(i) + (1-proportion)*last_variance;
722 
723  gear_error_sum += (inter_timestamp - last_timestamp) * 0.5 * (last_gear_error + inter_gear_error);
724  variance_sum += (inter_timestamp - last_timestamp) * 0.5 * (last_variance + inter_variance);
725 
726  reg_timestamps(j) = last_cell_end + 0.5 * grid_interval;
727  reg_gear_error(j) = gear_error_sum / grid_interval;
728  reg_variances(j) = variance_sum / grid_interval;
729 
730  last_timestamp = inter_timestamp;
731  last_gear_error = inter_gear_error;
732  last_variance = inter_variance;
733  last_cell_end = inter_timestamp;
734 
735  gear_error_sum = 0.0;
736  variance_sum = 0.0;
737  ++j;
738  }
739  }
740  }
741  if (j > REGULAR_BUFFER_SIZE)
742  {
743  j = REGULAR_BUFFER_SIZE;
744  }
745 
746  // We need to output 3 vectors. For simplicity, we join them into a matrix.
747  Eigen::MatrixXd result(3,j);
748  result.row(0) = reg_timestamps.head(j);
749  result.row(1) = reg_gear_error.head(j);
750  result.row(2) = reg_variances.head(j);
751 
752  return result;
753 }
754 
756 {
757  // write the GP output to a file for easy analyzation
758  size_t N = get_number_of_measurements();
759 
760  if (N < 2)
761  {
762  return; // cannot save data before first measurement
763  }
764 
765  // initialize the different vectors needed for the GP
766  Eigen::VectorXd timestamps(N - 1);
767  Eigen::VectorXd measurements(N - 1);
768  Eigen::VectorXd variances(N - 1);
769  Eigen::VectorXd controls(N - 1);
770  Eigen::VectorXd sum_controls(N - 1);
771  Eigen::VectorXd gear_error(N - 1);
772  Eigen::VectorXd linear_fit(N - 1);
773 
774  // transfer the data from the circular buffer to the Eigen::Vectors
775  for (size_t i = 0; i < N-1; i++)
776  {
777  timestamps(i) = circular_buffer_data_[i].timestamp;
778  measurements(i) = circular_buffer_data_[i].measurement;
779  variances(i) = circular_buffer_data_[i].variance;
780  controls(i) = circular_buffer_data_[i].control;
781  sum_controls(i) = circular_buffer_data_[i].control;
782  if(i > 0)
783  {
784  sum_controls(i) += sum_controls(i-1); // sum over the control signals
785  }
786  }
787  gear_error = sum_controls + measurements; // for each time step, add the residual error
788 
789  int M = 512; // number of prediction points
790  Eigen::VectorXd locations = Eigen::VectorXd::LinSpaced(M, 0, get_second_last_point().timestamp + 1500);
791 
792  Eigen::VectorXd vars(locations.size());
793  Eigen::VectorXd means = gp_.predictProjected(locations, &vars);
794  Eigen::VectorXd stds = vars.array().sqrt();
795 
796  {
797  std::ofstream outfile;
798  outfile.open("measurement_data.csv", std::ios_base::out);
799  if(outfile) {
800  outfile << "location, output\n";
801  for( int i = 0; i < timestamps.size(); ++i) {
802  outfile << std::setw(8) << timestamps[i] << "," << std::setw(8) << gear_error[i] << "\n";
803  }
804  } else {
805  std::cout << "unable to write to file" << std::endl;
806  }
807  outfile.close();
808 
809  outfile.open("gp_data.csv", std::ios_base::out);
810  if(outfile) {
811  outfile << "location, mean, std\n";
812  for( int i = 0; i < locations.size(); ++i) {
813  outfile << std::setw(8) << locations[i] << "," << std::setw(8) << means[i] << "," << std::setw(8) << stds[i] << "\n";
814  }
815  } else {
816  std::cout << "unable to write to file" << std::endl;
817  }
818  outfile.close();
819  }
820 }
821 
822 void GaussianProcessGuider::SetLearningRate(double learning_rate)
823 {
824  learning_rate_ = learning_rate;
825  return;
826 }
827 
828 // Debug Log interface ======
829 
830 class NullDebugLog : public GPDebug
831 {
832  void Log(const char *fmt, ...) override { (void) fmt; }
833 };
834 
835 class GPDebug *GPDebug = new NullDebugLog();
836 
837 namespace {
838  // just so the leak checker does not complain
839  struct GPDebugCleanup {
840  ~GPDebugCleanup() { GPDebug::SetGPDebug(nullptr); }
841  } s_cleanup;
842 }
843 
844 void GPDebug::SetGPDebug(GPDebug *logger)
845 {
846  delete ::GPDebug;
847  ::GPDebug = logger;
848 }
849 
850 GPDebug::~GPDebug()
851 {
852 }
void DirectMoveApplied(double amt, double rate)
This method tells the guider that a direct move was issued.
double result(double input, double SNR, double time_step, double prediction_point=-1.0)
Calculates the control value based on the current input.
void reset()
Clears the data from the circular buffer and clears the GP data.
void UpdatePeriodLength(double period_length)
Does filtering and sets the period length of the GPGuider.
void UpdateGP(double prediction_point=std::numeric_limits< double >::quiet_NaN())
Runs the inference machinery on the GP.
double deduceResult(double time_step, double prediction_point=-1.0)
This method provides predictive control if no measurement could be made.
void inject_data_point(double timestamp, double input, double SNR, double control)
This method is needed for automated testing.
QCA_EXPORT Logger * logger()
void GuidingDithered(double amt, double rate)
This method tells the guider that a dither command was issued.
void SetLearningRate(double learning_rate)
Sets the learning rate.
QString arg(qlonglong a, int fieldWidth, int base, QChar fillChar) const const
Eigen::MatrixXd regularize_dataset(const Eigen::VectorXd &timestamps, const Eigen::VectorXd &gear_error, const Eigen::VectorXd &variances)
Takes timestamps, measurements and SNRs and returns them regularized in a matrix.
void save_gp_data() const
Saves the GP data to a csv file for external analysis.
void GuidingDitherSettleDone(bool success)
This method tells the guider that dithering is finished.
This file is part of the KDE documentation.
Documentation copyright © 1996-2022 The KDE developers.
Generated on Fri Aug 19 2022 03:57:51 by doxygen 1.8.17 written by Dimitri van Heesch, © 1997-2006

KDE's Doxygen guidelines are available online.