7 #include "focusalgorithms.h"
10 #include "polynomialfit.h"
13 #include "fitsviewer/fitsstardetector.h"
14 #include "focusstars.h"
15 #include <ekos_focus_debug.h>
26 class LinearFocusAlgorithm :
public FocusAlgorithmInterface
35 LinearFocusAlgorithm(
const FocusParams ¶ms);
40 int initialPosition()
override
42 return requestedPosition;
48 int newMeasurement(
int position,
double value,
51 FocusAlgorithmInterface *
Copy()
override;
62 *pos = pass1Positions;
67 double latestHFR()
const override
74 QString getTextStatus(
double R2 = 0)
const override;
79 int completeIteration(
int step,
bool foundFit,
double minPos,
double minVal);
82 int setupSolution(
int position,
double value,
double sigma);
87 bool setupPendingSolution(
int position);
90 void computeInitialPosition();
95 int setupSecondPass(
int position,
double value,
double margin = 2.0);
98 int finishFirstPass(
int position,
double value);
104 bool bestSamplesHeuristic();
111 double relativeHFR(
double origHFR,
const QList<Edge*> *stars);
114 double calculateStarSigma(
const bool useWeights,
const QList<Edge*> *stars);
130 int requestedPosition;
132 int passStartPosition;
137 double firstPassBestValue;
139 int firstPassBestPosition;
144 int minPositionLimit;
146 int maxPositionLimit;
149 int numPolySolutionsFound;
152 int numRestartSolutionsFound;
154 int secondPassStartIndex;
158 bool solutionPending;
164 double solutionPendingValue = 0;
165 int solutionPendingPosition = 0;
168 bool relativeHFREnabled =
false;
170 double bestRelativeHFR = 0;
171 int bestRelativeHFRIndex = 0;
176 FocusAlgorithmInterface *LinearFocusAlgorithm::Copy()
178 LinearFocusAlgorithm *alg =
new LinearFocusAlgorithm(params);
180 return dynamic_cast<FocusAlgorithmInterface*
>(alg);
183 FocusAlgorithmInterface *MakeLinearFocuser(
const FocusAlgorithmInterface::FocusParams ¶ms)
185 return new LinearFocusAlgorithm(params);
188 LinearFocusAlgorithm::LinearFocusAlgorithm(
const FocusParams &focusParams)
189 : FocusAlgorithmInterface(focusParams)
194 maxPositionLimit = std::min(params.maxPositionAllowed, params.startPosition + params.maxTravel);
195 minPositionLimit = std::max(params.minPositionAllowed, params.startPosition - params.maxTravel);
196 computeInitialPosition();
199 QString LinearFocusAlgorithm::getTextStatus(
double R2)
const
201 if (params.focusAlgorithm == Focus::FOCUS_LINEAR)
205 if (focusSolution > 0)
206 return QString(
"Solution: %1").
arg(focusSolution);
211 return QString(
"Pending: %1, %2").
arg(solutionPendingPosition).
arg(solutionPendingValue, 0,
'f', 2);
215 return QString(
"2nd Pass. 1st: %1, %2")
216 .
arg(firstPassBestPosition).
arg(firstPassBestValue, 0,
'f', 2);
221 if (params.curveFit == CurveFitting::FOCUS_QUADRATIC)
223 else if (params.curveFit == CurveFitting::FOCUS_HYPERBOLA)
226 text.
append(params.useWeights ?
" (W)" :
" (U)");
228 else if (params.curveFit == CurveFitting::FOCUS_PARABOLA)
231 text.
append(params.useWeights ?
" (W)" :
" (U)");
237 return text.
append(
". Moving to Solution");
240 if (focusSolution > 0)
242 if (params.curveFit == CurveFitting::FOCUS_QUADRATIC)
243 return text.
append(
" Solution: %1").
arg(focusSolution);
246 return text.
append(
" Solution: %1, R²=%2").
arg(focusSolution).
arg(trunc(R2 * 100.0) / 100.0, 0,
'f', 2);
249 return text.
append(
" Failed");
254 void LinearFocusAlgorithm::computeInitialPosition()
257 stepSize = params.initialStepSize;
259 solutionPending =
false;
261 firstPassBestValue = -1;
262 firstPassBestPosition = 0;
263 numPolySolutionsFound = 0;
264 numRestartSolutionsFound = 0;
265 secondPassStartIndex = -1;
267 qCDebug(KSTARS_EKOS_FOCUS)
268 <<
QString(
"Linear: Travel %1 initStep %2 pos %3 min %4 max %5 maxIters %6 tolerance %7 minlimit %8 maxlimit %9 init#steps %10 algo %11 backlash %12 curvefit %13 useweights %14")
269 .
arg(params.maxTravel).
arg(params.initialStepSize).
arg(params.startPosition).
arg(params.minPositionAllowed)
270 .
arg(params.maxPositionAllowed).
arg(params.maxIterations).
arg(params.focusTolerance).
arg(minPositionLimit)
271 .
arg(maxPositionLimit).
arg(params.initialOutwardSteps).
arg(params.focusAlgorithm).
arg(params.backlash)
272 .
arg(params.curveFit).
arg(params.useWeights);
274 requestedPosition = std::min(maxPositionLimit,
275 static_cast<int>(params.startPosition + params.initialOutwardSteps * params.initialStepSize));
276 passStartPosition = requestedPosition;
277 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: initialPosition %1 sized %2")
278 .
arg(requestedPosition).
arg(params.initialStepSize);
301 double LinearFocusAlgorithm::relativeHFR(
double origHFR,
const QList<Edge*> *stars)
303 constexpr
double minHFR = 0.3;
304 if (origHFR < minHFR)
return origHFR;
306 const int currentIndex =
values.size();
307 const bool isFirstSample = (currentIndex == 0);
308 double relativeHFR = origHFR;
310 if (isFirstSample && stars !=
nullptr)
311 relativeHFREnabled =
true;
313 if (relativeHFREnabled && stars ==
nullptr)
316 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: Error: Inconsistent relativeHFR, disabling.");
317 relativeHFREnabled =
false;
320 if (!relativeHFREnabled)
323 if (starLists.size() != positions.size())
326 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: Error: Inconsistent relativeHFR data structures, disabling.");
327 relativeHFREnabled =
false;
334 constexpr
int starDistanceThreshold = 5;
335 FocusStars fs(*stars, starDistanceThreshold);
336 starLists.push_back(fs);
337 auto ¤tStars = starLists.back();
341 relativeHFR = currentStars.getHFR();
342 if (relativeHFR <= 0)
345 relativeHFREnabled =
false;
349 bestRelativeHFR = relativeHFR;
350 bestRelativeHFRIndex = currentIndex;
355 auto &bestStars = starLists[bestRelativeHFRIndex];
356 double hfr = currentStars.relativeHFR(bestStars, bestRelativeHFR);
363 relativeHFR = currentStars.getHFR();
364 if (relativeHFR <= 0)
370 if (inFirstPass && relativeHFR <= bestRelativeHFR)
372 bestRelativeHFR = relativeHFR;
373 bestRelativeHFRIndex = currentIndex;
377 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"RelativeHFR: orig %1 computed %2 relative %3")
378 .
arg(origHFR).
arg(currentStars.getHFR()).
arg(relativeHFR);
385 double LinearFocusAlgorithm::calculateStarSigma(
const bool useWeights,
const QList<Edge*> *stars)
387 if (stars ==
nullptr || stars->
size() <= 0 || !useWeights)
391 const int n = stars->
size();
393 for (
int i = 0; i < n; ++i)
394 sum += stars->
value(i)->HFR;
395 const double mean = sum / n;
397 for(
int i = 0; i < n; ++i)
398 variance += pow(stars->
value(i)->HFR - mean, 2.0);
399 variance = variance / n;
400 return sqrt(variance);
403 int LinearFocusAlgorithm::newMeasurement(
int position,
double value,
const QList<Edge*> *stars)
405 double minPos = -1.0, minVal = 0;
406 bool foundFit =
false;
408 double starSigma = 1.0, origValue = value;
413 if (params.curveFit == CurveFitting::FOCUS_QUADRATIC)
414 value = relativeHFR(value, stars);
418 starSigma = calculateStarSigma(params.useWeights, stars);
422 if (params.focusAlgorithm == Focus::FOCUS_LINEAR1PASS && !inFirstPass)
424 return setupSolution(position, value, starSigma);
427 int thisStepSize = stepSize;
430 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: step %1, newMeasurement(%2, %3 -> %4, %5)")
432 .
arg(stars ==
nullptr ? 0 : stars->
size());
434 qCDebug(KSTARS_EKOS_FOCUS)
435 <<
QString(
"Linear: step %1, newMeasurement(%2, %3 -> %4, %5) 1stBest %6 %7").
arg(numSteps)
436 .
arg(position).
arg(origValue).
arg(value).
arg(stars ==
nullptr ? 0 : stars->
size())
437 .
arg(firstPassBestPosition).
arg(firstPassBestValue, 0,
'f', 3);
439 const int LINEAR_POSITION_TOLERANCE = params.initialStepSize;
440 if (abs(position - requestedPosition) > LINEAR_POSITION_TOLERANCE)
442 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: error didn't get the requested position");
443 return requestedPosition;
446 if (focusSolution != -1)
448 doneString =
i18n(
"Called newMeasurement after a solution was found.");
449 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: error %1").
arg(doneString);
456 positions.push_back(position);
457 sigmas.push_back(starSigma);
460 pass1Values.push_back(value);
461 pass1Positions.push_back(position);
462 pass1Sigmas.push_back(starSigma);
469 if (solutionPending ||
470 (!inFirstPass && (value < firstPassBestValue * (1.0 + params.focusTolerance))))
472 if (setupPendingSolution(position))
474 return completeIteration(retryNumber > 0 ? 0 : stepSize,
false, -1.0f, -1.0f);
477 return setupSolution(position, value, starSigma);
482 constexpr
int kMinPolynomialPoints = 5;
483 constexpr
int kNumPolySolutionsRequired = 2;
484 constexpr
int kNumRestartSolutionsRequired = 3;
485 constexpr
double kDecentValue = 2.5;
487 if (
values.size() >= kMinPolynomialPoints)
489 if (params.curveFit == CurveFitting::FOCUS_QUADRATIC)
491 PolynomialFit fit(2, positions, values);
492 foundFit = fit.findMinimum(position, 0, params.maxPositionAllowed, &minPos, &minVal);
497 if (
values.size() > kMinPolynomialPoints)
499 PolynomialFit fit2(2, positions.mid(1),
values.mid(1));
500 foundFit = fit2.findMinimum(position, 0, params.maxPositionAllowed, &minPos, &minVal);
507 curveFit.fitCurve(positions, values, sigmas, params.curveFit, params.useWeights);
508 foundFit = curveFit.findMin(position, 0, params.maxPositionAllowed, &minPos, &minVal,
509 static_cast<CurveFitting::CurveFit
>(params.curveFit));
514 const int distanceToMin =
static_cast<int>(position - minPos);
515 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: fit(%1): %2 = %3 @ %4 distToMin %5")
516 .
arg(positions.size()).
arg(minPos).
arg(minVal).
arg(position).
arg(distanceToMin);
517 if (distanceToMin >= 0)
520 numPolySolutionsFound = 0;
521 numRestartSolutionsFound = 0;
522 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: Solutions reset %1 = %2").
arg(minPos).
arg(minVal);
523 if (value > kDecentValue)
526 const int stepsToMin = distanceToMin / stepSize;
529 thisStepSize = stepSize * 4;
530 else if (stepsToMin >= 4)
531 thisStepSize = stepSize * 2;
534 else if (!bestSamplesHeuristic())
538 if (minPos > passStartPosition)
540 numRestartSolutionsFound++;
541 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: RESTART Solution #%1 %2 = %3 @ %4")
542 .
arg(numRestartSolutionsFound).
arg(minPos).
arg(minVal).
arg(position);
546 numPolySolutionsFound++;
547 numRestartSolutionsFound = 0;
548 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: Solution #%1: %2 = %3 @ %4")
549 .
arg(numPolySolutionsFound).
arg(minPos).
arg(minVal).
arg(position);
560 if (params.focusAlgorithm == Focus::FOCUS_LINEAR)
561 howFarToGo = kNumPolySolutionsRequired;
563 howFarToGo = std::max(1,
static_cast<int> (params.initialOutwardSteps) - 1);
565 if (numPolySolutionsFound >= howFarToGo)
570 double minMeasurement = *std::min_element(
values.begin(),
values.end());
571 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: 1stPass solution @ %1: pos %2 val %3, min measurement %4")
572 .
arg(position).
arg(minPos).
arg(minVal).
arg(minMeasurement);
575 if (params.focusAlgorithm == Focus::FOCUS_LINEAR)
576 return setupSecondPass(
static_cast<int>(minPos), minMeasurement, 2.0);
582 return finishFirstPass(
static_cast<int>(round(minPos)), minMeasurement);
584 else if (numRestartSolutionsFound >= kNumRestartSolutionsRequired)
590 const double highestStartPosition = params.startPosition + params.initialOutwardSteps * params.initialStepSize;
591 params.startPosition = std::min(minPos, highestStartPosition);
592 computeInitialPosition();
593 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: Restart. Current pos %1, min pos %2, min val %3, re-starting at %4")
594 .
arg(position).
arg(minPos).
arg(minVal).
arg(requestedPosition);
595 return requestedPosition;
605 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: ******** No poly min: Poly must be inverted");
614 else if (gettingWorse())
618 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: getting worse, re-running 2nd pass");
619 return setupSecondPass(firstPassBestPosition, firstPassBestValue);
622 return completeIteration(thisStepSize, foundFit, minPos, minVal);
625 int LinearFocusAlgorithm::setupSolution(
int position,
double value,
double sigma)
627 focusSolution = position;
630 doneString =
i18n(
"Solution found.");
631 if (params.focusAlgorithm == Focus::FOCUS_LINEAR)
632 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: solution @ %1 = %2 (best %3)")
633 .
arg(position).
arg(value).
arg(firstPassBestValue);
636 double delta = fabs(value - firstPassBestValue);
637 QString str(
"Linear: solution @ ");
638 str.append(
QString(
"%1 = %2 (expected %3) delta=%4").arg(position).arg(value).arg(firstPassBestValue).arg(delta));
640 if (params.useWeights && sigma > 0.0)
642 double numSigmas = delta / sigma;
643 str.append(
QString(
" or %1 sigma %2 than expected")
644 .arg(numSigmas).arg(value <= firstPassBestValue ?
"better" :
"worse"));
645 if (value <= firstPassBestValue || numSigmas < 1)
646 str.append(
QString(
". GREAT result"));
647 else if (numSigmas < 3)
648 str.append(
QString(
". OK result"));
650 str.append(
QString(
". POOR result. If this happens repeatedly it may be a sign of poor backlash compensation."));
653 qCInfo(KSTARS_EKOS_FOCUS) << str;
659 int LinearFocusAlgorithm::completeIteration(
int step,
bool foundFit,
double minPos,
double minVal)
661 if (params.focusAlgorithm == Focus::FOCUS_LINEAR && numSteps == params.maxIterations - 2)
666 const int minIndex =
static_cast<int>(std::min_element(
values.begin(),
values.end()) -
values.begin());
667 return setupSecondPass(positions[minIndex], values[minIndex], 0.5);
669 else if (numSteps > params.maxIterations)
673 doneString =
i18n(
"Too many steps.");
674 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: error %1").
arg(doneString);
680 requestedPosition = requestedPosition - step;
683 if (requestedPosition < minPositionLimit)
686 if (params.focusAlgorithm == Focus::FOCUS_LINEAR)
688 const int minIndex =
static_cast<int>(std::min_element(
values.begin(),
values.end()) -
values.begin());
689 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: reached end without Vmin. Restarting %1 pos %2 value %3")
690 .
arg(minIndex).
arg(positions[minIndex]).
arg(values[minIndex]);
691 return setupSecondPass(positions[minIndex], values[minIndex]);
695 if (foundFit && minPos >= minPositionLimit)
697 return finishFirstPass(
static_cast<int>(round(minPos)), minVal);
702 doneString =
i18n(
"Solution lies outside max travel.");
703 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: error %1").
arg(doneString);
709 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: requesting position %1").
arg(requestedPosition);
710 return requestedPosition;
722 bool LinearFocusAlgorithm::setupPendingSolution(
int position)
724 const int length =
values.size();
725 const int secondPassIndex = length - secondPassStartIndex;
726 const double thisValue =
values[length - 1];
730 double referenceValue = (secondPassIndex <= 1) ? 1e6 : values[length - 2];
731 if (retryNumber > 0 && length - (2 + retryNumber) >= 0)
732 referenceValue = values[length - (2 + retryNumber)];
735 const bool notGettingWorse = (secondPassIndex <= 1) || (thisValue <= referenceValue);
738 const bool couldGoFather = (numSteps < params.maxIterations - 2) && (position - stepSize > minPositionLimit);
741 int maxNumRetries = 3;
742 if (position - stepSize / 2 < firstPassBestPosition)
744 stepSize = std::max(2, params.initialStepSize / 4);
748 if (notGettingWorse && couldGoFather)
750 qCDebug(KSTARS_EKOS_FOCUS)
751 <<
QString(
"Linear: %1: Position(%2) & HFR(%3) -- Pass1: %4 %5, solution pending, searching further")
752 .
arg(length).
arg(position).
arg(thisValue, 0,
'f', 3).
arg(firstPassBestPosition).
arg(firstPassBestValue, 0,
'f', 3);
753 solutionPending =
true;
754 solutionPendingPosition = position;
755 solutionPendingValue = thisValue;
759 else if (solutionPending && couldGoFather && retryNumber < maxNumRetries &&
760 (secondPassIndex > 1) && (thisValue >= referenceValue))
762 qCDebug(KSTARS_EKOS_FOCUS)
763 <<
QString(
"Linear: %1: Position(%2) & HFR(%3) -- Pass1: %4 %5, solution pending, got worse, retrying")
764 .
arg(length).
arg(position).
arg(thisValue, 0,
'f', 3).
arg(firstPassBestPosition).
arg(firstPassBestValue, 0,
'f', 3);
769 qCDebug(KSTARS_EKOS_FOCUS)
770 <<
QString(
"Linear: %1: Position(%2) & HFR(%3) -- Pass1: %4 %5, finishing, can't go further")
771 .
arg(length).
arg(position).
arg(thisValue, 0,
'f', 3).
arg(firstPassBestPosition).
arg(firstPassBestValue, 0,
'f', 3);
776 void LinearFocusAlgorithm::debugLog()
778 QString str(
"Linear: points=[");
779 for (
int i = 0; i < positions.size(); ++i)
781 str.append(
QString(
"(%1, %2, %3)").arg(positions[i]).arg(values[i]).arg(sigmas[i]));
782 if (i < positions.size() - 1)
785 str.append(
QString(
"];iterations=%1").arg(numSteps));
786 str.append(
QString(
";duration=%1").arg(stopWatch.elapsed() / 1000));
787 str.append(
QString(
";solution=%1").arg(focusSolution));
788 str.append(
QString(
";HFR=%1").arg(focusHFR));
789 str.append(
QString(
";filter='%1'").arg(params.filterName));
790 str.append(
QString(
";temperature=%1").arg(params.temperature));
791 str.append(
QString(
";focusalgorithm=%1").arg(params.focusAlgorithm));
792 str.append(
QString(
";backlash=%1").arg(params.backlash));
793 str.append(
QString(
";curvefit=%1").arg(params.curveFit));
794 str.append(
QString(
";useweights=%1").arg(params.useWeights));
796 qCDebug(KSTARS_EKOS_FOCUS) << str;
800 int LinearFocusAlgorithm::setupSecondPass(
int position,
double value,
double margin)
802 firstPassBestPosition = position;
803 firstPassBestValue = value;
805 solutionPending =
false;
806 secondPassStartIndex =
values.size();
810 requestedPosition = std::min(
static_cast<int>(firstPassBestPosition + stepSize * margin), maxPositionLimit);
811 stepSize = params.initialStepSize / 2;
812 qCDebug(KSTARS_EKOS_FOCUS) <<
QString(
"Linear: 2ndPass starting at %1 step %2").
arg(requestedPosition).
arg(stepSize);
813 return requestedPosition;
817 int LinearFocusAlgorithm::finishFirstPass(
int position,
double value)
819 firstPassBestPosition = position;
820 firstPassBestValue = value;
822 requestedPosition = position;
823 return requestedPosition;
827 bool LinearFocusAlgorithm::bestSamplesHeuristic()
829 const int length =
values.size();
830 if (length < 5)
return true;
832 std::nth_element(tempValues.
begin(), tempValues.
begin() + 2, tempValues.
end());
833 double secondBest = tempValues[1];
834 if ((values[length - 1] <= secondBest) || (values[length - 2] <= secondBest))
840 bool LinearFocusAlgorithm::gettingWorse()
843 constexpr
int streak = 3;
844 const int length =
values.size();
845 if (secondPassStartIndex < 0)
847 if (length < streak + 1)
850 if (length - secondPassStartIndex < streak + 1)
852 for (
int i = length - 1; i >= length - streak; --i)
853 if (values[i] <= values[i - 1])