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
43GaussianProcessGuider::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
92GaussianProcessGuider::~GaussianProcessGuider()
93{
94}
95
96void 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.
107void 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
118void 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
125void GaussianProcessGuider::HandleControls(double control_input)
126{
127 get_last_point().control = control_input;
128}
129
130double 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
140void 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
245double 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
269double 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
387double 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
443void 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
466void 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
475double GaussianProcessGuider::GetControlGain(void) const
476{
477 return parameters.control_gain_;
478}
479
480bool GaussianProcessGuider::SetControlGain(double control_gain)
481{
482 parameters.control_gain_ = control_gain;
483 return false;
484}
485
486bool GaussianProcessGuider::GetBoolComputePeriod() const
487{
488 return parameters.compute_period_;
489}
490
491bool GaussianProcessGuider::SetBoolComputePeriod(bool active)
492{
493 parameters.compute_period_ = active;
494 return false;
495}
496
497std::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
512bool 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
537double GaussianProcessGuider::GetMinMove() const
538{
539 return parameters.min_move_;
540}
541
542bool GaussianProcessGuider::SetMinMove(double min_move)
543{
544 parameters.min_move_ = min_move;
545 return false;
546}
547
548int GaussianProcessGuider::GetNumPointsForApproximation() const
549{
550 return parameters.points_for_approximation_;
551}
552
553bool GaussianProcessGuider::SetNumPointsForApproximation(int num_points)
554{
555 parameters.points_for_approximation_ = num_points;
556 return false;
557}
558
559double GaussianProcessGuider::GetPeriodLengthsInference() const
560{
561 return parameters.min_periods_for_inference_;
562}
563
564bool GaussianProcessGuider::SetPeriodLengthsInference(double num_periods)
565{
566 parameters.min_periods_for_inference_ = num_periods;
567 return false;
568}
569
570double GaussianProcessGuider::GetPeriodLengthsPeriodEstimation() const
571{
572 return parameters.min_periods_for_period_estimation_;
573}
574
575bool GaussianProcessGuider::SetPeriodLengthsPeriodEstimation(double num_periods)
576{
577 parameters.min_periods_for_period_estimation_ = num_periods;
578 return false;
579}
580
581double GaussianProcessGuider::GetPredictionGain() const
582{
583 return parameters.prediction_gain_;
584}
585
586bool GaussianProcessGuider::SetPredictionGain(double prediction_gain)
587{
588 parameters.prediction_gain_ = prediction_gain;
589 return false;
590}
591
592void 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
605double 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
709Eigen::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
852{
853 learning_rate_ = learning_rate;
854 return;
855}
856
857// Debug Log interface ======
858
859class NullDebugLog : public GPDebug
860{
861 void Log(const char *fmt, ...) override
862 {
863 (void) fmt;
864 }
865};
866
867class GPDebug *GPDebug = new NullDebugLog();
868
869namespace
870{
871// just so the leak checker does not complain
872struct GPDebugCleanup
873{
874 ~GPDebugCleanup()
875 {
876 GPDebug::SetGPDebug(nullptr);
877 }
878} s_cleanup;
879}
880
881void GPDebug::SetGPDebug(GPDebug *logger)
882{
883 delete ::GPDebug;
884 ::GPDebug = logger;
885}
886
887GPDebug::~GPDebug()
888{
889}
void UpdateGP(double prediction_point=std::numeric_limits< double >::quiet_NaN())
Runs the inference machinery on the GP.
void SetLearningRate(double learning_rate)
Sets the learning rate.
void GuidingDitherSettleDone(bool success)
This method tells the guider that dithering is finished.
void reset()
Clears the data from the circular buffer and clears the GP data.
void save_gp_data() const
Saves the GP data to a csv file for external analysis.
void GuidingDithered(double amt, double rate)
This method tells the guider that a dither command was issued.
void DirectMoveApplied(double amt, double rate)
This method tells the guider that a direct move was issued.
void UpdatePeriodLength(double period_length)
Does filtering and sets the period length of the GPGuider.
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.
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.
double result(double input, double SNR, double time_step, double prediction_point=-1.0)
Calculates the control value based on the current input.
Implementation of Open Astronomy Log (OAL) XML specifications to record observation logs.
QCA_EXPORT Logger * logger()
QString arg(Args &&... args) const const
This file is part of the KDE documentation.
Documentation copyright © 1996-2024 The KDE developers.
Generated on Mon Nov 4 2024 16:38:43 by doxygen 1.12.0 written by Dimitri van Heesch, © 1997-2006

KDE's Doxygen guidelines are available online.