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

KDE's Doxygen guidelines are available online.