Kstars

curvefit.cpp
1#include "curvefit.h"
2#include "ekos/ekos.h"
3#include <ekos_focus_debug.h>
4
5// Constants used to identify the number of parameters used for different curve types
6constexpr int NUM_HYPERBOLA_PARAMS = 4;
7constexpr int NUM_PARABOLA_PARAMS = 3;
8constexpr int NUM_GAUSSIAN_PARAMS = 7;
9constexpr int NUM_PLANE_PARAMS = 3;
10// Parameters for the solver
11// MAX_ITERATIONS is used to limit the number of iterations for the solver.
12// A value needs to be entered that allows a solution to be found without iterating unnecessarily
13// There is a relationship with the tolerance parameters that follow.
14constexpr int MAX_ITERATIONS_CURVE = 5000;
15constexpr int MAX_ITERATIONS_STARS = 1000;
16constexpr int MAX_ITERATIONS_PLANE = 5000;
17// The next 3 parameters are used as tolerance for convergence
18// convergence is achieved if for each datapoint i
19// dx_i < INEPSABS + (INEPSREL * x_i)
20constexpr double INEPSXTOL = 1e-5;
21const double INEPSGTOL = pow(GSL_DBL_EPSILON, 1.0 / 3.0);
22constexpr double INEPSFTOL = 1e-5;
23
24// The functions here fit a number of different curves to the incoming data points using the Lehvensberg-Marquart
25// solver with geodesic acceleration as provided the Gnu Science Library (GSL). The following sources of information are useful:
26// www.wikipedia.org/wiki/Levenberg–Marquardt_algorithm - overview of the mathematics behind the algo
27// www.gnu.org/software/gsl/doc/html/nls.html - GSL manual describing Nonlinear Least-Squares fitting.
28//
29// Levensberg-Marquart (LM)
30// ------------------------
31// The Levensberg-Marquart algorithm is a non-linear least-squares solver and thus suitable for mnay
32// different equations. The basic is idea is to adjust the equation y = f(x,P) so that the computed
33// y values are as close as possible to the y values of the datapoints provided, so that the resultant
34// curve fits the data as best as it can. P is a set of parameters that are varied by the solver in order
35// to find the best fit. The solver measures how far away the curve is at each data point, squares the
36// result and adds them all up. This is the number to be minimised, lets call is S.
37//
38// The solver is supplied with an initial guess for the parameters, P. It calculates S, makes an adjustment
39// P and calculates a new S1. Provided S1 < S the solver has moved in the right direction and reduced S.
40// It iterates through this procedure until S1 - S is:
41// 1. less than a supplied limit (convergence has been reached), or
42// 2. The maximum number of iterations has been reached, or
43// 3. The solver encountered an error.
44//
45// The solver is capable of solving either an unweighted or weighted set of datapoints. In essence, an
46// unweighted set of data gives equal weight to each datapoint when trying to fit a curve. An alternative is to
47// weight each datapoint with a measure that corresponds to how accurate the measurement of the datapoint
48// actually is. Given we are calculating HFRs of stars we can calculate the variance (standard deviation
49// squared) in the measurement of HFR and use 1 / SD^2 as weighting. What this means is that if we have a
50// datapoint where the SD in HFR measurements is small we would give this datapoint a relatively high
51// weighting when fitting the curve, and if there was a datapoint with a higher SD in HFR measurements
52// it would receive a lower weighting when trying to fit curve to that point.
53//
54// There are several optimisations to LM that speed up convergence for certain types of equations. This
55// code uses the LM solver with geodesic acceleration; a technique to accelerate convergence by using the
56// second derivative of the fitted function. An optimisation that has been implemented is that since, in normal
57// operation, the solver is run multiple times with the same or similar parameters, the initial guess for
58// a solver run is set the solution from the previous run.
59//
60// The documents referenced provide the maths of the LM algorithm. What is required is the function to be
61// used f(x) and the derivative of f(x) with respect to the parameters of f(x). This partial derivative forms
62// a matrix called the Jacobian, J(x). LM then finds a minimum. Mathematically it will find a minimum local
63// to the initial guess, but not necessarily the global minimum but for the equations used this is not a
64// problem under normal operation as there will only be 1 minimum. If the data is poor, however, the
65// solver may find an "n" solution rather than a "u" especially at the start of the process where there
66// are a small number of points and the error in measurement is high. We'll ignore and continue and this
67// should correct itself.
68//
69// The original algorithm used in Ekos is a quadratic equation, which represents a parabolic curve. In order
70// not to disturb historic code the original solution will be called Quadratic. It uses a linear least-squares
71// model, not LM. The LM solver has been applied to a hyperbolic and a parabolic curve.
72//
73// In addition, the solver has been applied to:
74// 1. 3D Gaussian in order to fit star profiles and calculate FWHM.
75// 2. 3D Plane surface used by Aberration Inspector.
76//
77// Hyperbola
78// ---------
79// Equation y = f(x) = b * sqrt(1 + ((x - c) / a) ^2) + d
80// This can be re-written as:
81//
82// f(x) = b * phi + d
83// where phi = sqrt(1 + ((x - c) / a) ^2)
84//
85// Jacobian J = {df/da, df/db, df/dc, df/db}
86// df/da = b * (1 / 2) * (1 / phi) * -2 * ((x - c) ^2) / (a ^3)
87// = -b * ((x - c) ^2) / ((a ^ 3) * phi)
88// df/db = phi
89// df/dc = b * (1 / 2) * (1 / phi) * 2 * (x - c) / (a ^2) * (-1)
90// = -b * (x - c) / ((a ^2) * phi)
91// df/dd = 1
92//
93// Second directional derivative:
94// {
95// {ddf/dada,ddf/dadb,ddf/dadc,ddf/dadd},
96// {ddf/dbda,ddf/dbdb,ddf/dbdc,ddf/dbdd},
97// {ddf/dcda,ddf/dcdb,ddf/dcdc,ddf/dcdd},
98// {ddf/ddda,ddf/dddb,ddf/dddc,ddf/dddd}
99// }
100// Note the matrix is symmetric, as ddf/dxdy = ddf/dydx.
101//
102// ddf/dada = (b*(c-x)^2*(3*a^2+a*(c-x)^2))/(a^4*(a^2 + (c-x)^2)^3/2))
103// ddf/dadb = -((x - c) ^2) / ((a ^ 3) * phi)
104// ddf/dadc = -(b*(c-x)*(2a^2+(c-x)^2))/(a^3*(a^2 + (c-x)^2)^3/2))
105// ddf/dadd = 0
106// ddf/dbdb = 0
107// ddf/dbdc = -(x-c)/(a^2*phi)
108// ddf/dbdd = 0
109// ddf/dcdc = b/((a^2+(c-x)^2) * phi)
110// ddf/dcdd = 0
111// ddf/dddd = 0
112//
113// For a valid solution:
114// 1. c must be > 0 and within the range of travel of the focuser.
115// 2. b must be > 0 for a V shaped curve (b < 0 for an inverted V and b = 0 for a horizontal line).
116// 3. b + d > 0. The minimum solution when x = c gives f(x) = b + d which must be > 0.
117//
118// Parabola
119// --------
120// Equation y = f(x) = a + b((x - c) ^2)
121// Jacobian J = {df/da, df/db, df/dc}
122// df/da = 1
123// df/db = (x - c) ^2
124// df/dc = -2 * b * (x - c)
125//
126// For a valid solution:
127// 1. c must be > 0 and within the range of travel of the focuser.
128// 2. b must be > 0 for a V shaped curve (b < 0 for an inverted V and b = 0 for a horizontal line).
129// 3. a > 0. The minimum solution when x = c gives f(x) = a which must be > 0.
130//
131// Gaussian
132// --------
133// Generalised equation for a 2D gaussian.
134// Equation z = f(x,y) = b + a.exp-(A((x-x0)^2) + 2B(x-x0)(y-y0) + 2C((y-y0)^2))
135//
136// with parameters
137// b = background
138// a = Peak Intensity
139// x0 = Star centre in x dimension
140// y0 = Star centre in y dimension
141// A, B, C = relate to the shape of the elliptical gaussian and the orientation of the major and
142// minor ellipse axes wrt x and y
143//
144// Jacobian J = {df/db, df/da, df/dx0, df/dy0, df/dA, df/dB, df/dC}
145// Let phi = exp-(A((x-x0)^2) + 2B(x-x0)(y-y0) + C((y-y0)^2))
146// df/db = 1
147// df/da = phi
148// df/dx0 = [2A(x-x0)+2B(y-y0)].a.phi
149// df/dy0 = [2B(x-x0)+2C(y-y0)].a.phi
150// df/dA = -(x-x0)^2.a.phi
151// df/DB = -2(x-x0)(y-y0).a.phi
152// df/dC = -(y-y0)^2.a.phi
153//
154// 2nd derivatives
155// {
156// {ddf/dbdb,ddf/dbda,ddf/dbdx0,ddf/dbdy0,ddf/dbdA,ddf/dbdB,ddf/dbdC},
157// {ddf/dadb,ddf/dada,ddf/dadx0,ddf/dady0,ddf/dadA,ddf/dadB,ddf/dadC},
158// {ddf/dx0db,ddf/dx0da,ddf/dx0dx0,ddf/dx0dy0,ddf/dx0dA,ddf/dx0dB,ddf/dx0dC},
159// {ddf/dy0db,ddf/dy0da,ddf/dy0dx0,ddf/dy0dy0,ddf/dy0dA,ddf/dy0dB,ddf/dy0dC},
160// {ddf/dAdb,ddf/dAda,ddf/dAdx0,ddf/dAdy0,ddf/dAdA,ddf/dAdB,ddf/dAdC},
161// {ddf/dBdb,ddf/dBda,ddf/dBdx0,ddf/dBdy0,ddf/dBdA,ddf/dBdB,ddf/dBdC},
162// {ddf/dCdb,ddf/dCda,ddf/dCdx0,ddf/dCdy0,ddf/dCdA,ddf/dCdB,ddf/dCdC},
163// }
164// The matrix is symmetric, as ddf/dParm1dParm2 = ddf/dParm2dParm1 so we only need to differentiate
165// one half (above or below the diagonal) of the matrix elements.
166// ddf/dbdb = 0
167// ddf/dbda = 0
168// ddf/dbdx0 = 0
169// ddf/dbdy0 = 0
170// ddf/dbdA = 0
171// ddf/dbdB = 0
172// ddf/dbdC = 0
173//
174// ddf/dada = 0
175// ddf/dadx0 = [2A(x-x0) + 2B(y-y0)].phi
176// ddf/dady0 = [2B(x-x0) + 2C(y-y0)].phi
177// ddf/dadA = -(x-x0)^2.phi
178// ddf/dadB = -2(x-x0)(y-y0).phi
179// ddf/dadC = -Cy-y0)^2.phi
180//
181// ddf/dxodx0 = -2A.a.phi + [2A(x-x0)+2B(y-y0)]^2.a.phi
182// ddf/dx0dy0 = -2B.a.phi - [2A(x-x0)+2B(y-y0)].[2B(x-x0)+2C(y-y0)].a.phi
183// ddf/dx0dA = 2(x-x0).a.phi - [2A(x-x0)+2B(y-y0)].(x-x0)^2.a.phi
184// ddf/dx0dB = 2(y-y0).a.phi - [2A(x-x0)+2B(y-y0)].2(x-x0).(y-y0).a.phi
185// ddf/dx0dC = -[2A(x-x0)+2B(y-y0)].2(y-y0)^2.a.phi
186//
187// ddf/dy0dy0 = -2c.a.phi + [2B(x-x0)+2C(y-y0)]^2.a.phi
188// ddf/dy0dA = -[2B(x-x0)+2C(y-y0)].(x-x0)^2.a.phi
189// ddf/dy0dB = 2(x-x0).a.phi - [2B(x-x0)+2C(y-y0)].2(x-x0)(y-y0).a.phi
190// ddf/dy0dC = 2(y-y0).a.phi - [2B(x-x0)+2C(y-y0)].(y-y0)^2.a.phi
191//
192// ddf/dAdA = (x-x0)^4.a.phi
193// ddf/dAdB = 2(x-x0)^3.(y-y0).a.phi
194// ddf/dAdC = (x-x0)^2.(y-y0)^2.a.phi
195//
196// ddf/dBdB = 4(x-x0)^2.(y-y0)^2.a.phi
197// ddf/dBdC = 2(x-x0).(y-y0)^3.a.phi
198//
199// ddf/dCdC = (y-y0)^4.a.phi
200//
201// 3D Plane
202// Generalised equation for a 3D plane going through the origin.
203// Equation z = f(x,y) = -(A.x + B.y) / C
204// Jacobian J = {df/dA, df/dB, df/dC}
205// df/dA = -x / C
206// df/dB = -y / C
207// df/dC = (A.x + B.y) / C^2
208//
209// 2nd derivatives
210// {ddf/dAdA,ddf/dAdB,ddf/dAdC}
211// {ddf/dBdA,ddf/dBdB,ddf/dBdC}
212// {ddf/dCdA,ddf/dCdB,ddf/dCdC}
213//
214// The matrix is symmetric, as ddf/dParm1dParm2 = ddf/dParm2dParm1 so we only need to differentiate
215// one half (above or below the diagonal) of the matrix elements.
216//
217// ddf/dAdA = 0
218// ddf/dAdB = 0
219// ddf/dAdC = x / C^2
220//
221// ddf/dBdB = 0
222// ddf/dBdC = y / C^2
223//
224// ddf/dCdC = -2.(A.x + B.y) / C^3
225//
226//
227// Convergence
228// -----------
229// There following are key parameters that drive the LM solver. Currently these settings are coded into
230// the program. It would be better to store them in a configuration file somewhere, but they aren't the
231// sort of parameters users should have ready access to.
232// MAX_ITERATIONS - This is how many iterations to do before aborting. The game here is to set a
233// reasonable number to allow for convergence. In my testing with good data and a
234// good start point <10 iterations are required. With good data and a poor start point
235// <200 iterations are required. With poor data and a tight "tolerance" convergence
236// may need >1000 iterations. Setting to 500 seems a good compromise.
237// tolerance - Conceptually tolerance could be done in 2 ways:
238// - check the gradient, would be 0 at the curve minimum
239// - check the residuals (and their gradient), will minimise at the curve minimum
240// Currently we will check on residuals.
241// This is supported by 2 parameters INEPSABS and INEPSREL.
242// convergence is achieved if for each datapoint i, where:
243// dx_i < INEPSABS + (INEPSREL * x_i)
244// Setting a slack tolerance will mean a range of x (focus positions) that are deemed
245// valid solutions. Not good.
246// Setting a tighter tolerance will require more iterations to solve, but setting too
247// tight a tolerance is just wasting time if it doesn't improve the focus position, and
248// if too tight the solver may not be able to find the solution, so a balance needs to
249// be struck. For now lets just make these values constant and see where this gets to.
250// If this turns out not to work for some equipment, the next step would be to adjust
251// these parameters based on the equipment profile, or to adapt the parameters starting
252// with a loose tolerance and tightening as the curve gets nearer to a complete solution.
253// If we inadvertently overtighten the tolerance and fail to converge, the tolerance
254// could be slackened or more iterations used.
255// I have found that the following work well on my equipment and the simulator
256// for both hyperbola and parabola. The advice in the GSL documentation is to start
257// with an absolute tolerance of 10^-d where d is the number of digits required in the
258// solution precision of x (focuser position). The gradient tolerance starting point is
259// (machine precision)^1/3 which we're using a relative tolerance.
260// INEPSABS = 1e-5
261// INEPSREL = GSL_DBL_EPSILON ^ 1/3
262
263namespace Ekos
264{
265
266namespace
267{
268// Constants used to index m_coefficient arrays
269enum { A_IDX = 0, B_IDX, C_IDX, D_IDX, E_IDX, F_IDX, G_IDX };
270
271// hypPhi() is a repeating part of the function calculations for Hyperbolas.
272double hypPhi(double x, double a, double c)
273{
274 return sqrt(1.0 + pow(((x - c) / a), 2.0));
275}
276
277// Function to calculate f(x) for a hyperbola
278// y = b * hypPhi(x, a, c) + d
279double hypfx(double x, double a, double b, double c, double d)
280{
281 return b * hypPhi(x, a, c) + d;
282}
283
284// Calculates F(x) for each data point on the hyperbola
285int hypFx(const gsl_vector * X, void * inParams, gsl_vector * outResultVec)
286{
287 CurveFitting::DataPointT * DataPoints = ((CurveFitting::DataPointT *)inParams);
288
289 double a = gsl_vector_get (X, A_IDX);
290 double b = gsl_vector_get (X, B_IDX);
291 double c = gsl_vector_get (X, C_IDX);
292 double d = gsl_vector_get (X, D_IDX);
293
294 for(int i = 0; i < DataPoints->dps.size(); ++i)
295 {
296 // Hyperbola equation
297 double yi = hypfx(DataPoints->dps[i].x, a, b, c, d);
298
299 gsl_vector_set(outResultVec, i, (yi - DataPoints->dps[i].y));
300 }
301
302 return GSL_SUCCESS;
303}
304
305// Calculates the Jacobian (derivative) matrix for the hyperbola
306int hypJx(const gsl_vector * X, void * inParams, gsl_matrix * J)
307{
308 CurveFitting::DataPointT * DataPoints = ((struct CurveFitting::DataPointT *)inParams);
309
310 // Store current coefficients
311 double a = gsl_vector_get(X, A_IDX);
312 double b = gsl_vector_get(X, B_IDX);
313 double c = gsl_vector_get(X, C_IDX);
314
315 // Store non-changing calculations
316 const double a2 = a * a;
317 const double a3 = a * a2;
318
319 for(int i = 0; i < DataPoints->dps.size(); ++i)
320 {
321 // Calculate the Jacobian Matrix
322 const double x = DataPoints->dps[i].x;
323 const double x_minus_c = x - c;
324
325 gsl_matrix_set(J, i, A_IDX, -b * (x_minus_c * x_minus_c) / (a3 * hypPhi(x, a, c)));
326 gsl_matrix_set(J, i, B_IDX, hypPhi(x, a, c));
327 gsl_matrix_set(J, i, C_IDX, -b * x_minus_c / (a2 * hypPhi(x, a, c)));
328 gsl_matrix_set(J, i, D_IDX, 1);
329 }
330
331 return GSL_SUCCESS;
332}
333
334
335// ddf/dada = (b*(c-x)^2*(3*a^2+a*(c-x)^2))/(a^4*(a^2 + (c-x)^2)*phi)
336// ddf/dadb = -((x - c) ^2) / ((a ^ 3) * phi)
337// ddf/dadc = -(b*(c-x)*(2a^2+(c-x)^2))/(a^3*(a^2 + (c-x)^2)*phi))
338// ddf/dadd = 0
339// ddf/dbdb = 0
340// ddf/dbdc = -(x-c)/(a^2*phi)
341// ddf/dbdd = 0
342// ddf/dcdc = b/((a^2+(c-x)^2) * phi)
343// ddf/dcdd = 0
344// ddf/dddd = 0
345
346// ddf/dada = (b*(c-x)^2*/(a^4*phi).[3 - (x-c)^2/(a * phi)^2]
347// ddf/dadb = -((x - c)^2) / ((a^3) * phi)
348// ddf/dadc = -(b*(x-c)/((a * phi)^3).[2 - (x-c)^2/((a * phi)^2)]
349// ddf/dadd = 0
350// ddf/dbdb = 0
351// ddf/dbdc = -(x-c)/(a^2*phi)
352// ddf/dbdd = 0
353// ddf/dcdc = b/(phi^3 * a^2).[1 - ((x - c)^2)/((a * phi)^2)]
354// ddf/dcdd = 0
355// ddf/dddd = 0
356int hypFxx(const gsl_vector* X, const gsl_vector* v, void* inParams, gsl_vector* fvv)
357{
358 CurveFitting::DataPointT * DataPoint = ((struct CurveFitting::DataPointT *)inParams);
359
360 // Store current coefficients
361 const double a = gsl_vector_get(X, A_IDX);
362 const double b = gsl_vector_get(X, B_IDX);
363 const double c = gsl_vector_get(X, C_IDX);
364
365 const double a2 = pow(a, 2);
366 const double a4 = pow(a2, 2);
367
368 const double va = gsl_vector_get(v, A_IDX);
369 const double vb = gsl_vector_get(v, B_IDX);
370 const double vc = gsl_vector_get(v, C_IDX);
371
372 for(int i = 0; i < DataPoint->dps.size(); ++i)
373 {
374 const double x = DataPoint->dps[i].x;
375 const double xmc = x - c;
376 const double xmc2 = pow(xmc, 2);
377 const double phi = hypPhi(x, a, c);
378 const double aphi = a * phi;
379 const double aphi2 = pow(aphi, 2);
380
381 const double Daa = b * xmc2 * (3 - xmc2 / aphi2) / (a4 * phi);
382 const double Dab = -xmc2 / (a2 * aphi);
383 const double Dac = b * xmc * (2 - (xmc2 / aphi2)) / (aphi2 * aphi);
384 const double Dbc = -xmc / (a2 * phi);
385 const double Dcc = b * (1 - (xmc2 / aphi2)) / (phi * aphi2);
386
387 double sum = va * va * Daa + 2 * (va * vb * Dab + va * vc * Dac + vb * vc * Dbc) + vc * vc * Dcc;
388
389 gsl_vector_set(fvv, i, sum);
390
391 }
392
393 return GSL_SUCCESS;
394}
395
396// Function to calculate f(x) for a parabola.
397double parfx(double x, double a, double b, double c)
398{
399 return a + b * pow((x - c), 2.0);
400}
401
402// Calculates f(x) for each data point in the parabola.
403int parFx(const gsl_vector * X, void * inParams, gsl_vector * outResultVec)
404{
405 CurveFitting::DataPointT * DataPoint = ((struct CurveFitting::DataPointT *)inParams);
406
407 double a = gsl_vector_get (X, A_IDX);
408 double b = gsl_vector_get (X, B_IDX);
409 double c = gsl_vector_get (X, C_IDX);
410
411 for(int i = 0; i < DataPoint->dps.size(); ++i)
412 {
413 // Parabola equation
414 double yi = parfx(DataPoint->dps[i].x, a, b, c);
415
416 gsl_vector_set(outResultVec, i, (yi - DataPoint->dps[i].y));
417 }
418
419 return GSL_SUCCESS;
420}
421
422// Calculates the Jacobian (derivative) matrix for the parabola equation f(x) = a + b*(x-c)^2
423// dy/da = 1
424// dy/db = (x - c)^2
425// dy/dc = -2b * (x - c)
426int parJx(const gsl_vector * X, void * inParams, gsl_matrix * J)
427{
428 CurveFitting::DataPointT * DataPoint = ((struct CurveFitting::DataPointT *)inParams);
429
430 // Store current coefficients
431 double b = gsl_vector_get(X, B_IDX);
432 double c = gsl_vector_get(X, C_IDX);
433
434 for(int i = 0; i < DataPoint->dps.size(); ++i)
435 {
436 // Calculate the Jacobian Matrix
437 const double x = DataPoint->dps[i].x;
438 const double xmc = x - c;
439
440 gsl_matrix_set(J, i, A_IDX, 1);
441 gsl_matrix_set(J, i, B_IDX, xmc * xmc);
442 gsl_matrix_set(J, i, C_IDX, -2 * b * xmc);
443 }
444
445 return GSL_SUCCESS;
446}
447// Calculates the second directional derivative vector for the parabola equation f(x) = a + b*(x-c)^2
448int parFxx(const gsl_vector* X, const gsl_vector* v, void* inParams, gsl_vector* fvv)
449{
450 CurveFitting::DataPointT * DataPoint = ((struct CurveFitting::DataPointT *)inParams);
451
452 const double b = gsl_vector_get(X, B_IDX);
453 const double c = gsl_vector_get(X, C_IDX);
454
455 const double vb = gsl_vector_get(v, B_IDX);
456 const double vc = gsl_vector_get(v, C_IDX);
457
458 for(int i = 0; i < DataPoint->dps.size(); ++i)
459 {
460 const double x = DataPoint->dps[i].x;
461 double Dbc = -2 * (x - c);
462 double Dcc = 2 * b;
463 double sum = 2 * vb * vc * Dbc + vc * vc * Dcc;
464
465 gsl_vector_set(fvv, i, sum);
466
467 }
468
469 return GSL_SUCCESS;
470}
471
472// Function to calculate f(x,y) for a 2-D gaussian.
473double gaufxy(double x, double y, double a, double x0, double y0, double A, double B, double C, double b)
474{
475 return b + a * exp(-(A * (pow(x - x0, 2.0)) + 2.0 * B * (x - x0) * (y - y0) + C * (pow(y - y0, 2.0))));
476}
477
478// Calculates f(x,y) for each data point in the gaussian.
479int gauFxy(const gsl_vector * X, void * inParams, gsl_vector * outResultVec)
480{
481 CurveFitting::DataPoint3DT * DataPoint = ((struct CurveFitting::DataPoint3DT *)inParams);
482
483 double a = gsl_vector_get (X, A_IDX);
484 double x0 = gsl_vector_get (X, B_IDX);
485 double y0 = gsl_vector_get (X, C_IDX);
486 double A = gsl_vector_get (X, D_IDX);
487 double B = gsl_vector_get (X, E_IDX);
488 double C = gsl_vector_get (X, F_IDX);
489 double b = gsl_vector_get (X, G_IDX);
490
491 for(int i = 0; i < DataPoint->dps.size(); ++i)
492 {
493 // Gaussian equation
494 double zij = gaufxy(DataPoint->dps[i].x, DataPoint->dps[i].y, a, x0, y0, A, B, C, b);
495 gsl_vector_set(outResultVec, i, (zij - DataPoint->dps[i].z));
496 }
497
498 return GSL_SUCCESS;
499}
500
501// Calculates the Jacobian (derivative) matrix for the gaussian
502// Jacobian J = {df/db, df/da, df/dx0, df/dy0, df/dA, df/dB, df/dC}
503// Let phi = exp-(A((x-x0)^2) + 2B(x-x0)(y-y0) + C((y-y0)^2))
504// df/db = 1
505// df/da = phi
506// df/dx0 = [2A(x-x0)+2B(y-y0)].a.phi
507// df/dy0 = [2B(x-x0)+2C(y-y0)].a.phi
508// df/dA = -(x-x0)^2.a.phi
509// df/DB = -2(x-x0)(y-y0).a.phi
510// df/dC = -(y-y0)^2.a.phi
511int gauJxy(const gsl_vector * X, void * inParams, gsl_matrix * J)
512{
513 CurveFitting::DataPoint3DT * DataPoint = ((struct CurveFitting::DataPoint3DT *)inParams);
514
515 // Get current coefficients
516 const double a = gsl_vector_get (X, A_IDX);
517 const double x0 = gsl_vector_get (X, B_IDX);
518 const double y0 = gsl_vector_get (X, C_IDX);
519 const double A = gsl_vector_get (X, D_IDX);
520 const double B = gsl_vector_get (X, E_IDX);
521 const double C = gsl_vector_get (X, F_IDX);
522 // b is not used ... const double b = gsl_vector_get (X, G_IDX);
523
524 for(int i = 0; i < DataPoint->dps.size(); ++i)
525 {
526 // Calculate the Jacobian Matrix
527 const double x = DataPoint->dps[i].x;
528 const double xmx0 = x - x0;
529 const double xmx02 = xmx0 * xmx0;
530 const double y = DataPoint->dps[i].y;
531 const double ymy0 = y - y0;
532 const double ymy02 = ymy0 * ymy0;
533 const double phi = exp(-((A * xmx02) + (2.0 * B * xmx0 * ymy0) + (C * ymy02)));
534 const double aphi = a * phi;
535
537 gsl_matrix_set(J, i, B_IDX, 2.0 * aphi * ((A * xmx0) + (B * ymy0)));
538 gsl_matrix_set(J, i, C_IDX, 2.0 * aphi * ((B * xmx0) + (C * ymy0)));
539 gsl_matrix_set(J, i, D_IDX, -1.0 * aphi * xmx02);
540 gsl_matrix_set(J, i, E_IDX, -2.0 * aphi * xmx0 * ymy0);
541 gsl_matrix_set(J, i, F_IDX, -1.0 * aphi * ymy02);
542 gsl_matrix_set(J, i, G_IDX, 1.0);
543 }
544
545 return GSL_SUCCESS;
546}
547
548// Calculates the second directional derivative vector for the gaussian equation
549// The matrix is symmetric, as ddf/dParm1dParm2 = ddf/dParm2dParm1 so we only need to differentiate
550// one half (above or below the diagonal) of the matrix elements.
551// ddf/dbdb = 0
552// ddf/dbda = 0
553// ddf/dbdx0 = 0
554// ddf/dbdy0 = 0
555// ddf/dbdA = 0
556// ddf/dbdB = 0
557// ddf/dbdC = 0
558//
559// ddf/dada = 0
560// ddf/dadx0 = [2A(x-x0) + 2B(y-y0)].phi
561// ddf/dady0 = [2B(x-x0) + 2C(y-y0)].phi
562// ddf/dadA = -(x-x0)^2.phi
563// ddf/dadB = -2(x-x0)(y-y0).phi
564// ddf/dadC = -Cy-y0)^2.phi
565//
566// ddf/dxodx0 = -2A.a.phi + [2A(x-x0)+2B(y-y0)]^2.a.phi
567// ddf/dx0dy0 = -2B.a.phi - [2A(x-x0)+2B(y-y0)].[2B(x-x0)+2C(y-y0)].a.phi
568// ddf/dx0dA = 2(x-x0).a.phi - [2A(x-x0)+2B(y-y0)].(x-x0)^2.a.phi
569// ddf/dx0dB = 2(y-y0).a.phi - [2A(x-x0)+2B(y-y0)].2(x-x0).(y-y0).a.phi
570// ddf/dx0dC = -[2A(x-x0)+2B(y-y0)].2(y-y0)^2.a.phi
571//
572// ddf/dy0dy0 = -2c.a.phi + [2B(x-x0)+2C(y-y0)]^2.a.phi
573// ddf/dy0dA = -[2B(x-x0)+2C(y-y0)].(x-x0)^2.a.phi
574// ddf/dy0dB = 2(x-x0).a.phi - [2B(x-x0)+2C(y-y0)].2(x-x0)(y-y0).a.phi
575// ddf/dy0dC = 2(y-y0).a.phi - [2B(x-x0)+2C(y-y0)].(y-y0)^2.a.phi
576//
577// ddf/dAdA = (x-x0)^4.a.phi
578// ddf/dAdB = 2(x-x0)^3.(y-y0).a.phi
579// ddf/dAdC = (x-x0)^2.(y-y0)^2.a.phi
580//
581// ddf/dBdB = 4(x-x0)^2.(y-y0)^2.a.phi
582// ddf/dBdC = 2(x-x0).(y-y0)^3.a.phi
583//
584// ddf/dCdC = (y-y0)^4.a.phi
585//
586int gauFxyxy(const gsl_vector* X, const gsl_vector* v, void* inParams, gsl_vector* fvv)
587{
588 CurveFitting::DataPoint3DT * DataPoint = ((struct CurveFitting::DataPoint3DT *)inParams);
589
590 // Get current coefficients
591 const double a = gsl_vector_get (X, A_IDX);
592 const double x0 = gsl_vector_get (X, B_IDX);
593 const double y0 = gsl_vector_get (X, C_IDX);
594 const double A = gsl_vector_get (X, D_IDX);
595 const double B = gsl_vector_get (X, E_IDX);
596 const double C = gsl_vector_get (X, F_IDX);
597 // b not used ... const double b = gsl_vector_get (X, G_IDX);
598
599 const double va = gsl_vector_get(v, A_IDX);
600 const double vx0 = gsl_vector_get(v, B_IDX);
601 const double vy0 = gsl_vector_get(v, C_IDX);
602 const double vA = gsl_vector_get(v, D_IDX);
603 const double vB = gsl_vector_get(v, E_IDX);
604 const double vC = gsl_vector_get(v, F_IDX);
605 // vb not used ... const double vb = gsl_vector_get(v, G_IDX);
606
607 for(int i = 0; i < DataPoint->dps.size(); ++i)
608 {
609 double x = DataPoint->dps[i].x;
610 double xmx0 = x - x0;
611 double xmx02 = xmx0 * xmx0;
612 double y = DataPoint->dps[i].y;
613 double ymy0 = y - y0;
614 double ymy02 = ymy0 * ymy0;
615 double phi = exp(-((A * xmx02) + (2.0 * B * xmx0 * ymy0) + (C * ymy02)));
616 double aphi = a * phi;
617 double AB = 2.0 * ((A * xmx0) + (B * ymy0));
618 double BC = 2.0 * ((B * xmx0) + (C * ymy0));
619
620 double Dax0 = AB * phi;
621 double Day0 = BC * phi;
622 double DaA = -xmx02 * phi;
623 double DaB = -2.0 * xmx0 * ymy0 * phi;
624 double DaC = -ymy02 * phi;
625
626 double Dx0x0 = aphi * ((-2.0 * A) + (AB * AB));
627 double Dx0y0 = -aphi * ((2.0 * B) + (AB * BC));
628 double Dx0A = aphi * ((2.0 * xmx0) - (AB * xmx02));
629 double Dx0B = 2.0 * aphi * (ymy0 - (AB * xmx0 * ymy0));
630 double Dx0C = -2.0 * aphi * AB * ymy02;
631
632 double Dy0y0 = aphi * ((-2.0 * C) + (BC * BC));
633 double Dy0A = -aphi * BC * xmx02;
634 double Dy0B = 2.0 * aphi * (xmx0 - (BC * xmx0 * ymy0));
635 double Dy0C = aphi * ((2.0 * ymy0) - (BC * ymy02));
636
637 double DAA = aphi * xmx02 * xmx02;
638 double DAB = 2.0 * aphi * xmx02 * xmx0 * ymy0;
639 double DAC = aphi * xmx02 * ymy02;
640
641 double DBB = 4.0 * aphi * xmx02 * ymy02;
642 double DBC = 2.0 * aphi * xmx0 * ymy02 * ymy0;
643
644 double DCC = aphi * ymy02 * ymy02;
645
646 double sum = 2 * va * ((vx0 * Dax0) + (vy0 * Day0) + (vA * DaA) + (vB * DaB) + (vC * DaC)) + // a diffs
647 vx0 * ((vx0 * Dx0x0) + 2 * ((vy0 * Dx0y0) + (vA * Dx0A) + (vB * Dx0B) + (vC * Dx0C))) + // x0 diffs
648 vy0 * ((vy0 * Dy0y0) + 2 * ((vA * Dy0A) + (vB * Dy0B) + (vC * Dy0C))) + // y0 diffs
649 vA * ((vA * DAA) + 2 * ((vB * DAB) + (vC * DAC))) + // A diffs
650 vB * ((vB * DBB) + 2 * (vC * DBC)) + // B diffs
651 vC * vC * DCC; // C diffs
652
653 gsl_vector_set(fvv, i, sum);
654 }
655
656 return GSL_SUCCESS;
657}
658
659// Function to calculate f(x,y) for a 2-D plane.
660// f(x,y) = (A.x + B.y) / -C
661double plafxy(double x, double y, double A, double B, double C)
662{
663 return -(A * x + B * y) / C;
664}
665
666// Calculates f(x,y) for each data point in the gaussian.
667int plaFxy(const gsl_vector * X, void * inParams, gsl_vector * outResultVec)
668{
669 CurveFitting::DataPoint3DT * DataPoint = ((struct CurveFitting::DataPoint3DT *)inParams);
670
671 double A = gsl_vector_get (X, A_IDX);
672 double B = gsl_vector_get (X, B_IDX);
673 double C = gsl_vector_get (X, C_IDX);
674
675 for(int i = 0; i < DataPoint->dps.size(); ++i)
676 {
677 // Plane equation
678 double zi = plafxy(DataPoint->dps[i].x, DataPoint->dps[i].y, A, B, C);
679 gsl_vector_set(outResultVec, i, (zi - DataPoint->dps[i].z));
680 }
681
682 return GSL_SUCCESS;
683}
684
685// Calculates the Jacobian (derivative) matrix for the plane
686// Jacobian J = {df/dA, df/dB, df/dC}
687// df/dA = -x / C
688// df/dB = -y / C
689// df/dC = (A.x + B.y) / C^2
690int plaJxy(const gsl_vector * X, void * inParams, gsl_matrix * J)
691{
692 CurveFitting::DataPoint3DT * DataPoint = ((struct CurveFitting::DataPoint3DT *)inParams);
693
694 // Get current coefficients
695 const double A = gsl_vector_get (X, A_IDX);
696 const double B = gsl_vector_get (X, B_IDX);
697 const double C = gsl_vector_get (X, C_IDX);
698
699 for(int i = 0; i < DataPoint->dps.size(); ++i)
700 {
701 // Calculate the Jacobian Matrix
702 const double x = DataPoint->dps[i].x;
703 const double y = DataPoint->dps[i].y;
704
705 gsl_matrix_set(J, i, A_IDX, -x / C);
706 gsl_matrix_set(J, i, B_IDX, -y / C);
707 gsl_matrix_set(J, i, C_IDX, (A * x + B * y) / (C * C));
708 }
709
710 return GSL_SUCCESS;
711}
712
713// Calculates the second directional derivative vector for the plane equation
714// The matrix is symmetric, as ddf/dParm1dParm2 = ddf/dParm2dParm1 so we only need to differentiate
715// one half (above or below the diagonal) of the matrix elements.
716//
717// ddf/dAdA = 0
718// ddf/dAdB = 0
719// ddf/dAdC = x / C^2
720
721// ddf/dBdB = 0
722// ddf/dBdC = y / C^2
723//
724// ddf/dCdC = -2.(A.x + B.y) / C^3
725//
726int plaFxyxy(const gsl_vector* X, const gsl_vector* v, void* inParams, gsl_vector* fvv)
727{
728 CurveFitting::DataPoint3DT * DataPoint = ((struct CurveFitting::DataPoint3DT *)inParams);
729
730 // Get current coefficients
731 const double A = gsl_vector_get (X, A_IDX);
732 const double B = gsl_vector_get (X, B_IDX);
733 const double C = gsl_vector_get (X, C_IDX);
734
735 const double vA = gsl_vector_get(v, A_IDX);
736 const double vB = gsl_vector_get(v, B_IDX);
737 const double vC = gsl_vector_get(v, C_IDX);
738
739 for(int i = 0; i < DataPoint->dps.size(); ++i)
740 {
741 double x = DataPoint->dps[i].x;
742 double y = DataPoint->dps[i].y;
743 double C2 = C * C;
744 double C3 = C * C2;
745 double dAdC = x / C2;
746 double dBdC = y / C2;
747 double dCdC = -2 * (A * x + B * y) / C3;
748
749 double sum = 2 * (vA * vC * dAdC) + 2 * (vB * vC * dBdC) + vC * vC * dCdC;
750
751 gsl_vector_set(fvv, i, sum);
752 }
753
754 return GSL_SUCCESS;
755}
756} // namespace
757
758CurveFitting::CurveFitting()
759{
760 // Constructor just initialises variables
761 m_FirstSolverRun = true;
762}
763
764CurveFitting::CurveFitting(const QString &serialized)
765{
766 recreateFromQString(serialized);
767}
768
769void CurveFitting::fitCurve(const FittingGoal goal, const QVector<int> &x_, const QVector<double> &y_,
771 const CurveFit curveFit, const bool useWeights, const OptimisationDirection optDir)
772{
773 if ((x_.size() != y_.size()) || (x_.size() != weight_.size()) || (x_.size() != outliers_.size()))
774 qCDebug(KSTARS_EKOS_FOCUS) << QString("CurveFitting::fitCurve inconsistent parameters. x=%1, y=%2, weight=%3, outliers=%4")
775 .arg(x_.size()).arg(y_.size()).arg(weight_.size()).arg(outliers_.size());
776
777 m_x.clear();
778 m_y.clear();
779 m_scale.clear();
780 m_outliers.clear();
781 for (int i = 0; i < x_.size(); ++i)
782 {
783 m_x.push_back(static_cast<double>(x_[i]));
784 m_y.push_back(y_[i]);
785 m_scale.push_back(weight_[i]);
786 m_outliers.push_back(outliers_[i]);
787 }
788
789 m_useWeights = useWeights;
790 m_CurveType = curveFit;
791
792 switch (m_CurveType)
793 {
794 case FOCUS_QUADRATIC :
795 m_coefficients = polynomial_fit(m_x.data(), m_y.data(), m_x.count(), 2);
796 break;
797 case FOCUS_HYPERBOLA :
798 m_coefficients = hyperbola_fit(goal, m_x, m_y, m_scale, m_outliers, useWeights, optDir);
799 break;
800 case FOCUS_PARABOLA :
801 m_coefficients = parabola_fit(goal, m_x, m_y, m_scale, m_outliers, useWeights, optDir);
802 break;
803 default :
804 // Something went wrong, log an error and reset state so solver starts from scratch if called again
805 qCDebug(KSTARS_EKOS_FOCUS) << QString("CurveFitting::fitCurve called with curveFit=%1").arg(curveFit);
806 m_FirstSolverRun = true;
807 return;
808 }
809 m_LastCoefficients = m_coefficients;
810 m_LastCurveType = m_CurveType;
811 m_FirstSolverRun = false;
812}
813
814void CurveFitting::fitCurve3D(const DataPoint3DT data, const CurveFit curveFit)
815{
816 if (data.useWeights)
817 {
818 qCDebug(KSTARS_EKOS_FOCUS) << QString("CurveFitting::fitCurve3D called with useWeights = true. Not currently supported");
819 m_FirstSolverRun = true;
820 return;
821 }
822
823 m_useWeights = data.useWeights;
824 m_CurveType = curveFit;
825 m_dataPoints = data;
826
827 switch (m_CurveType)
828 {
829 case FOCUS_PLANE :
830 m_coefficients = plane_fit(data);
831 break;
832 default :
833 // Something went wrong, log an error and reset state so solver starts from scratch if called again
834 qCDebug(KSTARS_EKOS_FOCUS) << QString("CurveFitting::fitCurve3D called with curveFit=%1").arg(curveFit);
835 m_FirstSolverRun = true;
836 return;
837 }
838 m_LastCoefficients = m_coefficients;
839 m_LastCurveType = m_CurveType;
840 m_FirstSolverRun = false;
841}
842
843double CurveFitting::curveFunction(double x, void *params)
844{
845 CurveFitting *instance = static_cast<CurveFitting *>(params);
846
847 if (instance && !instance->m_coefficients.empty())
848 return instance->f(x);
849 else
850 return -1;
851}
852
853double CurveFitting::f(double x)
854{
855 const int order = m_coefficients.size() - 1;
856 double y = 0;
857 if (m_CurveType == FOCUS_QUADRATIC)
858 {
859 for (int i = 0; i <= order; ++i)
860 y += m_coefficients[i] * pow(x, i);
861 }
862 else if (m_CurveType == FOCUS_HYPERBOLA && m_coefficients.size() == NUM_HYPERBOLA_PARAMS)
863 y = hypfx(x, m_coefficients[A_IDX], m_coefficients[B_IDX], m_coefficients[C_IDX], m_coefficients[D_IDX]);
864 else if (m_CurveType == FOCUS_PARABOLA && m_coefficients.size() == NUM_PARABOLA_PARAMS)
865 y = parfx(x, m_coefficients[A_IDX], m_coefficients[B_IDX], m_coefficients[C_IDX]);
866 else
867 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error: CurveFitting::f called with m_CurveType = %1 m_coefficients.size = %2")
868 .arg(m_CurveType).arg(m_coefficients.size());
869
870 return y;
871}
872
873double CurveFitting::f3D(double x, double y)
874{
875 double z = 0;
876 if (m_CurveType == FOCUS_GAUSSIAN && m_coefficients.size() == NUM_GAUSSIAN_PARAMS)
877 z = gaufxy(x, y, m_coefficients[A_IDX], m_coefficients[B_IDX], m_coefficients[C_IDX], m_coefficients[D_IDX],
878 m_coefficients[E_IDX], m_coefficients[F_IDX], m_coefficients[G_IDX]);
879 else if (m_CurveType == FOCUS_PLANE && m_coefficients.size() == NUM_PLANE_PARAMS)
880 z = plafxy(x, y, m_coefficients[A_IDX], m_coefficients[B_IDX], m_coefficients[C_IDX]);
881 else
882 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error: CurveFitting::f3D called with m_CurveType = %1 m_coefficients.size = %2")
883 .arg(m_CurveType).arg(m_coefficients.size());
884
885 return z;
886}
887
888QVector<double> CurveFitting::polynomial_fit(const double *const data_x, const double *const data_y, const int n,
889 const int order)
890{
891 int status = 0;
892 double chisq = 0;
894 gsl_vector *y, *c;
895 gsl_matrix *X, *cov;
896 y = gsl_vector_alloc(n);
897 c = gsl_vector_alloc(order + 1);
898 X = gsl_matrix_alloc(n, order + 1);
899 cov = gsl_matrix_alloc(order + 1, order + 1);
900
901 for (int i = 0; i < n; i++)
902 {
903 for (int j = 0; j < order + 1; j++)
904 {
905 gsl_matrix_set(X, i, j, pow(data_x[i], j));
906 }
907 gsl_vector_set(y, i, data_y[i]);
908 }
909
910 // Must turn off error handler or it aborts on error
912
914 status = gsl_multifit_linear(X, y, c, cov, &chisq, work);
915
916 if (status != GSL_SUCCESS)
917 qDebug() << Q_FUNC_INFO << "GSL multifit error:" << gsl_strerror(status);
918 else
919 {
921
922 for (int i = 0; i < order + 1; i++)
923 {
924 vc.push_back(gsl_vector_get(c, i));
925 }
926 }
927
932
933 return vc;
934}
935
936QVector<double> CurveFitting::hyperbola_fit(FittingGoal goal, const QVector<double> data_x, const QVector<double> data_y,
937 const QVector<double> data_weights, const QVector<bool> outliers, const bool useWeights, const OptimisationDirection optDir)
938{
940 DataPointT dataPoints;
941
942 // Fill in the data to which the curve will be fitted
943 dataPoints.useWeights = useWeights;
944 dataPoints.dir = optDir;
945 for (int i = 0; i < data_x.size(); i++)
946 if (!outliers[i])
947 dataPoints.push_back(data_x[i], data_y[i], data_weights[i]);
948
949 auto weights = gsl_vector_alloc(dataPoints.dps.size());
950 // Set the gsl error handler off as it aborts the program on error.
952
953 // Setup variables to be used by the solver
956 NUM_HYPERBOLA_PARAMS);
958 gsl_vector *guess = gsl_vector_alloc(NUM_HYPERBOLA_PARAMS);
959 int numIters;
960 double xtol, gtol, ftol;
961
962 // Fill in function info
963 fdf.f = hypFx;
964 fdf.df = hypJx;
965 fdf.fvv = hypFxx;
966 fdf.n = dataPoints.dps.size();
967 fdf.p = NUM_HYPERBOLA_PARAMS;
968 fdf.params = &dataPoints;
969
970 // This is the callback used by the LM solver to allow some introspection of each iteration
971 // Useful for debugging but clogs up the log
972 // To activate, uncomment the callback lambda and change the call to gsl_multifit_nlinear_driver
973 /*auto callback = [](const size_t iter, void* _params, const auto * w)
974 {
975 gsl_vector *f = gsl_multifit_nlinear_residual(w);
976 gsl_vector *x = gsl_multifit_nlinear_position(w);
977
978 // compute reciprocal condition number of J(x)
979 double rcond;
980 gsl_multifit_nlinear_rcond(&rcond, w);
981
982 // ratio of accel component to velocity component
983 double avratio = gsl_multifit_nlinear_avratio(w);
984
985 qCDebug(KSTARS_EKOS_FOCUS) << QString("iter %1: A=%2, B=%3, C=%4, D=%5 rcond(J)=%6, avratio=%7, |f(x)|=%8")
986 .arg(iter)
987 .arg(gsl_vector_get(x, A_IDX))
988 .arg(gsl_vector_get(x, B_IDX))
989 .arg(gsl_vector_get(x, C_IDX))
990 .arg(gsl_vector_get(x, D_IDX))
991 .arg(rcond)
992 .arg(avratio)
993 .arg(gsl_blas_dnrm2(f));
994 };*/
995
996 // Start a timer to see how long the solve takes.
997 QElapsedTimer timer;
998 timer.start();
999
1000 // We can sometimes have several attempts to solve based on "goal" and why the solver failed.
1001 // If the goal is STANDARD and we fail to solve then so be it. If the goal is BEST, then retry
1002 // with different parameters to really try and get a solution. A special case is if the solver
1003 // fails on its first step where we will always retry after adjusting parameters. It helps with
1004 // a situation where the solver gets "stuck" failing on first step repeatedly.
1005 for (int attempt = 0; attempt < 5; attempt++)
1006 {
1007 // Make initial guesses
1008 hypMakeGuess(attempt, dataPoints, guess);
1009
1010 // Load up the weights and guess vectors
1011 if (useWeights)
1012 {
1013 for (int i = 0; i < dataPoints.dps.size(); i++)
1014 gsl_vector_set(weights, i, dataPoints.dps[i].weight);
1015 gsl_multifit_nlinear_winit(guess, weights, &fdf, w);
1016 }
1017 else
1019
1020 // Tweak solver parameters from default values
1021 hypSetupParams(goal, &params, &numIters, &xtol, &gtol, &ftol);
1022
1023 qCDebug(KSTARS_EKOS_FOCUS) << QString("Starting LM solver, fit=hyperbola, solver=%1, scale=%2, trs=%3, iters=%4, xtol=%5,"
1024 "gtol=%6, ftol=%7")
1025 .arg(params.solver->name).arg(params.scale->name).arg(params.trs->name).arg(numIters)
1026 .arg(xtol).arg(gtol).arg(ftol);
1027
1028 int info = 0;
1030
1031 if (status != 0)
1032 {
1033 // Solver failed so determine whether a retry is required.
1034 bool retry = false;
1035 if (goal == BEST)
1036 {
1037 // Pull out all the stops to get a solution
1038 retry = true;
1039 goal = BEST_RETRY;
1040 }
1041 else if (status == GSL_EMAXITER && info == GSL_ENOPROG && gsl_multifit_nlinear_niter(w) <= 1)
1042 // This is a special case where the solver can't take a first step
1043 // So, perturb the initial conditions and have another go.
1044 retry = true;
1045
1046 qCDebug(KSTARS_EKOS_FOCUS) <<
1047 QString("LM solver (Hyperbola): Failed after %1ms iters=%2 [attempt=%3] with status=%4 [%5] and info=%6 [%7], retry=%8")
1048 .arg(timer.elapsed()).arg(gsl_multifit_nlinear_niter(w)).arg(attempt + 1).arg(status).arg(gsl_strerror(status))
1049 .arg(info).arg(gsl_strerror(info)).arg(retry);
1050 if (!retry)
1051 break;
1052 }
1053 else
1054 {
1055 // All good so store the results - parameters A, B, C and D
1056 auto solution = gsl_multifit_nlinear_position(w);
1057 for (int j = 0; j < NUM_HYPERBOLA_PARAMS; j++)
1058 vc.push_back(gsl_vector_get(solution, j));
1059
1060 qCDebug(KSTARS_EKOS_FOCUS) <<
1061 QString("LM Solver (Hyperbola): Solution found after %1ms %2 iters (%3). A=%4, B=%5, C=%6, D=%7")
1062 .arg(timer.elapsed()).arg(gsl_multifit_nlinear_niter(w)).arg(getLMReasonCode(info)).arg(vc[A_IDX]).arg(vc[B_IDX])
1063 .arg(vc[C_IDX]).arg(vc[D_IDX]);
1064 break;
1065 }
1066 }
1067
1068 // Free GSL memory
1071 gsl_vector_free(weights);
1072
1073 // Restore old GSL error handler
1075
1076 return vc;
1077}
1078
1079QString CurveFitting::getLMReasonCode(int info)
1080{
1081 QString reason;
1082
1083 if (info == 1)
1084 reason = QString("small step size");
1085 else if(info == 2)
1086 reason = QString("small gradient");
1087 else
1088 reason = QString("unknown reason");
1089
1090 return reason;
1091}
1092
1093// Setup the parameters for hyperbola curve fitting
1094void CurveFitting::hypSetupParams(FittingGoal goal, gsl_multifit_nlinear_parameters *params, int *numIters, double *xtol,
1095 double *gtol, double *ftol)
1096{
1097 // Trust region subproblem
1098 // - gsl_multifit_nlinear_trs_lm is the Levenberg-Marquardt algorithm without acceleration
1099 // - gsl_multifit_nlinear_trs_lmaccel is the Levenberg-Marquardt algorithm with acceleration
1100 // - gsl_multilarge_nlinear_trs_dogleg is the dogleg algorithm
1101 // - gsl_multifit_nlinear_trs_ddogleg is the double dogleg algorithm
1102 // - gsl_multifit_nlinear_trs_subspace2D is the 2D subspace algorithm
1104
1105 // Scale
1106 // - gsl_multifit_nlinear_scale_more uses. More strategy. Good for problems with parameters with widely different scales
1107 // - gsl_multifit_nlinear_scale_levenberg. Levensberg strategy. Can be good but not scale invariant.
1108 // - gsl_multifit_nlinear_scale_marquardt. Marquardt strategy. Considered inferior to More and Levensberg
1109 params->scale = gsl_multifit_nlinear_scale_more;
1110
1111 // Solver
1112 // - gsl_multifit_nlinear_solver_qr produces reliable results but needs more iterations than Cholesky
1113 // - gsl_multifit_nlinear_solver_cholesky fast but not as stable as QR
1114 // - gsl_multifit_nlinear_solver_mcholesky modified Cholesky more stable than Cholesky
1115
1116 // avmax is the max allowed ratio of 2nd order term (acceleration, a) to the 1st order term (velocity, v)
1117 // GSL defaults to 0.75, but suggests reducing it in the case of convergence problems
1118 switch (goal)
1119 {
1120 case STANDARD:
1121 params->solver = gsl_multifit_nlinear_solver_qr;
1122 params->avmax = 0.75;
1123
1124 *numIters = MAX_ITERATIONS_CURVE;
1125 *xtol = INEPSXTOL;
1126 *gtol = INEPSGTOL;
1127 *ftol = INEPSFTOL;
1128 break;
1129 case BEST:
1130 params->solver = gsl_multifit_nlinear_solver_cholesky;
1131 params->avmax = 0.75;
1132
1133 *numIters = MAX_ITERATIONS_CURVE * 2.0;
1134 *xtol = INEPSXTOL / 10.0;
1135 *gtol = INEPSGTOL / 10.0;
1136 *ftol = INEPSFTOL / 10.0;
1137 break;
1138 case BEST_RETRY:
1139 params->solver = gsl_multifit_nlinear_solver_qr;
1140 params->avmax = 0.5;
1141
1142 *numIters = MAX_ITERATIONS_CURVE * 2.0;
1143 *xtol = INEPSXTOL;
1144 *gtol = INEPSGTOL;
1145 *ftol = INEPSFTOL;
1146 break;
1147 default:
1148 break;
1149 }
1150}
1151
1152// Initialise parameters before starting the solver. Its important to start with a guess as near to the solution as possible
1153// If we found a solution before and we're just adding more datapoints use the last solution as the guess.
1154// If we don't have a solution use the datapoints to approximate. Work out the min and max points and use these
1155// to find "close" values for the starting point parameters
1156void CurveFitting::hypMakeGuess(const int attempt, const DataPointT &dataPoints, gsl_vector * guess)
1157{
1158 // If we are retrying then perturb the initial conditions. The hope is that by doing this the solver
1159 // will be nudged to find a solution this time
1160 double perturbation = 1.0 + pow(-1, attempt) * (attempt * 0.1);
1161
1162 if (!m_FirstSolverRun && (m_LastCurveType == FOCUS_HYPERBOLA) && (m_LastCoefficients.size() == NUM_HYPERBOLA_PARAMS))
1163 {
1164 // Last run of the solver was a Hyperbola and the solution was good, so use that solution
1165 gsl_vector_set(guess, A_IDX, m_LastCoefficients[A_IDX] * perturbation);
1166 gsl_vector_set(guess, B_IDX, m_LastCoefficients[B_IDX] * perturbation);
1167 gsl_vector_set(guess, C_IDX, m_LastCoefficients[C_IDX] * perturbation);
1168 gsl_vector_set(guess, D_IDX, m_LastCoefficients[D_IDX] * perturbation);
1169 }
1170 else
1171 {
1172 double minX = dataPoints.dps[0].x;
1173 double minY = dataPoints.dps[0].y;
1174 double maxX = minX;
1175 double maxY = minY;
1176 for(int i = 0; i < dataPoints.dps.size(); i++)
1177 {
1178 if (dataPoints.dps[i].y <= 0.0)
1179 continue;
1180 if(minY <= 0.0 || dataPoints.dps[i].y < minY)
1181 {
1182 minX = dataPoints.dps[i].x;
1183 minY = dataPoints.dps[i].y;
1184 }
1185 if(maxY <= 0.0 || dataPoints.dps[0].y > maxY)
1186 {
1187 maxX = dataPoints.dps[i].x;
1188 maxY = dataPoints.dps[i].y;
1189 }
1190 }
1191 double A, B, C, D;
1192 if (dataPoints.dir == OPTIMISATION_MAXIMISE)
1193 {
1194 // Hyperbola equation: y = f(x) = b * sqrt(1 + ((x - c) / a) ^2) + d
1195 // For a maximum: c = maximum x = x(max)
1196 // b < 0 and b + d > 0
1197 // For the array of data, set:
1198 // => c = x(max)
1199 // Now assume maximum x is near the real curve maximum, so y = b + d
1200 // Set b = -d/2. So y(max) = -d/2 + d = d/2.
1201 // => d = 2.y(max)
1202 // => b = -y(max)
1203 // Now look at the minimum y value in the array of datapoints
1204 // y(min) = b * sqrt(1 + ((x(min) - c) / a) ^2) + d
1205 // (y(min) - d) / b) ^ 2) = 1 + ((x(min) - c) / a) ^2
1206 // sqrt((((y(min) - d) / b) ^ 2) - 1) = (x(min) - c) / a
1207 // a = (x(min) - c) / sqrt((((y(min) - d) / b) ^ 2) - 1)
1208 // use the values for b, c, d obtained above to get:
1209 // => a = (x(min) - x(max)) / sqrt((((y(min) - (2.y(max)) / (-y(max))) ^ 2) - 1)
1210 double num = minX - maxX;
1211 double denom = sqrt(pow((2.0 * maxY - minY) / maxY, 2.0) - 1.0);
1212 if(denom <= 0.0)
1213 denom = 1.0;
1214 A = num / denom * perturbation;
1215 B = -maxY * perturbation;
1216 C = maxX * perturbation;
1217 D = 2.0 * maxY * perturbation;
1218 }
1219 else
1220 {
1221 // For a minimum: c = minimum x; b > 0 and b + d > 0
1222 // For the array of data, set:
1223 // => c = x(min)
1224 // Now assume minimum x is near the real curve minimum, so y = b + d
1225 // => Set b = d = y(min) / 2
1226 // Now look at the maximum y value in the array of datapoints
1227 // y(max) = b * sqrt(1 + ((x(max) - c) / a) ^2) + d
1228 // ((y(max) - d) / b) ^2 = 1 + ((x(max) - c) / a) ^2
1229 // a = (x(max) - c) / sqrt((((y(max) - d) / b) ^ 2) - 1)
1230 // use the values for b, c, d obtained above to get:
1231 // a = (x(max) - x(min)) / sqrt((((y(max) - (y(min) / 2)) / (y(min) / 2)) ^ 2) - 1)
1232 double minYdiv2 = (minY / 2.0 <= 0.0) ? 1.0 : minY / 2.0;
1233 double num = maxX - minX;
1234 double denom = sqrt(pow((maxY - minYdiv2) / minYdiv2, 2.0) - 1.0);
1235 if(denom <= 0.0)
1236 denom = 1.0;
1237 A = num / denom * perturbation;
1238 B = minYdiv2 * perturbation;
1239 C = minX * perturbation;
1240 D = B;
1241 }
1242 qCDebug(KSTARS_EKOS_FOCUS) << QString("LM solver (hyp) initial params: perturbation=%1, A=%2, B=%3, C=%4, D=%5")
1243 .arg(perturbation).arg(A).arg(B).arg(C).arg(D);
1244
1249 }
1250}
1251
1252QVector<double> CurveFitting::parabola_fit(FittingGoal goal, const QVector<double> data_x, const QVector<double> data_y,
1253 const QVector<double> data_weights, const QVector<bool> outliers, bool useWeights, const OptimisationDirection optDir)
1254{
1256 DataPointT dataPoints;
1257
1258 // Fill in the data to which the curve will be fitted
1259 dataPoints.useWeights = useWeights;
1260 dataPoints.dir = optDir;
1261 for (int i = 0; i < data_x.size(); i++)
1262 if (!outliers[i])
1263 dataPoints.push_back(data_x[i], data_y[i], data_weights[i]);
1264
1265 auto weights = gsl_vector_alloc(dataPoints.dps.size());
1266 // Set the gsl error handler off as it aborts the program on error.
1268
1269 // Setup variables to be used by the solver
1272 NUM_PARABOLA_PARAMS);
1274 gsl_vector * guess = gsl_vector_alloc(NUM_PARABOLA_PARAMS);
1275 int numIters;
1276 double xtol, gtol, ftol;
1277
1278 // Fill in function info
1279 fdf.f = parFx;
1280 fdf.df = parJx;
1281 fdf.fvv = parFxx;
1282 fdf.n = dataPoints.dps.size();
1283 fdf.p = NUM_PARABOLA_PARAMS;
1284 fdf.params = &dataPoints;
1285
1286 // This is the callback used by the LM solver to allow some introspection of each iteration
1287 // Useful for debugging but clogs up the log
1288 // To activate, uncomment the callback lambda and change the call to gsl_multifit_nlinear_driver
1289 /*auto callback = [](const size_t iter, void* _params, const auto * w)
1290 {
1291 gsl_vector *f = gsl_multifit_nlinear_residual(w);
1292 gsl_vector *x = gsl_multifit_nlinear_position(w);
1293
1294 // compute reciprocal condition number of J(x)
1295 double rcond;
1296 gsl_multifit_nlinear_rcond(&rcond, w);
1297
1298 // ratio of accel component to velocity component
1299 double avratio = gsl_multifit_nlinear_avratio(w);
1300
1301 qCDebug(KSTARS_EKOS_FOCUS) << QString("iter %1: A=%2, B=%3, C=%4, rcond(J)=%5, avratio=%6 |f(x)|=%7")
1302 .arg(iter)
1303 .arg(gsl_vector_get(x, A_IDX))
1304 .arg(gsl_vector_get(x, B_IDX))
1305 .arg(gsl_vector_get(x, C_IDX))
1306 .arg(rcond)
1307 .arg(gsl_blas_dnrm2(f));
1308 };*/
1309
1310 // Start a timer to see how long the solve takes.
1311 QElapsedTimer timer;
1312 timer.start();
1313
1314 // We can sometimes have several attempts to solve based on "goal" and why the solver failed.
1315 // If the goal is STANDARD and we fail to solve then so be it. If the goal is BEST, then retry
1316 // with different parameters to really try and get a solution. A special case is if the solver
1317 // fails on its first step where we will always retry after adjusting parameters. It helps with
1318 // a situation where the solver gets "stuck" failing on first step repeatedly.
1319 for (int attempt = 0; attempt < 5; attempt++)
1320 {
1321 // Make initial guesses - here we just set all parameters to 1.0
1322 parMakeGuess(attempt, dataPoints, guess);
1323
1324 // Load up the weights and guess vectors
1325 if (useWeights)
1326 {
1327 for (int i = 0; i < dataPoints.dps.size(); i++)
1328 gsl_vector_set(weights, i, dataPoints.dps[i].weight);
1329 gsl_multifit_nlinear_winit(guess, weights, &fdf, w);
1330 }
1331 else
1333
1334 // Tweak solver parameters from default values
1335 parSetupParams(goal, &params, &numIters, &xtol, &gtol, &ftol);
1336
1337 qCDebug(KSTARS_EKOS_FOCUS) << QString("Starting LM solver, fit=parabola, solver=%1, scale=%2, trs=%3, iters=%4, xtol=%5,"
1338 "gtol=%6, ftol=%7")
1339 .arg(params.solver->name).arg(params.scale->name).arg(params.trs->name).arg(numIters)
1340 .arg(xtol).arg(gtol).arg(ftol);
1341
1342 int info = 0;
1344
1345 if (status != 0)
1346 {
1347 // Solver failed so determine whether a retry is required.
1348 bool retry = false;
1349 if (goal == BEST)
1350 {
1351 // Pull out all the stops to get a solution
1352 retry = true;
1353 goal = BEST_RETRY;
1354 }
1355 else if (status == GSL_EMAXITER && info == GSL_ENOPROG && gsl_multifit_nlinear_niter(w) <= 1)
1356 // This is a special case where the solver can't take a first step
1357 // So, perturb the initial conditions and have another go.
1358 retry = true;
1359
1360 qCDebug(KSTARS_EKOS_FOCUS) <<
1361 QString("LM solver (Parabola): Failed after %1ms iters=%2 [attempt=%3] with status=%4 [%5] and info=%6 [%7], retry=%8")
1362 .arg(timer.elapsed()).arg(gsl_multifit_nlinear_niter(w)).arg(attempt + 1).arg(status).arg(gsl_strerror(status))
1363 .arg(info).arg(gsl_strerror(info)).arg(retry);
1364 if (!retry)
1365 break;
1366 }
1367 else
1368 {
1369 // All good so store the results - parameters A, B, and C
1370 auto solution = gsl_multifit_nlinear_position(w);
1371 for (int j = 0; j < NUM_PARABOLA_PARAMS; j++)
1372 vc.push_back(gsl_vector_get(solution, j));
1373
1374 qCDebug(KSTARS_EKOS_FOCUS) << QString("LM Solver (Parabola): Solution found after %1ms %2 iters (%3). A=%4, B=%5, C=%6")
1375 .arg(timer.elapsed()).arg(gsl_multifit_nlinear_niter(w)).arg(getLMReasonCode(info)).arg(vc[A_IDX]).arg(vc[B_IDX])
1376 .arg(vc[C_IDX]);
1377 break;
1378 }
1379 }
1380
1381 // Free GSL memory
1384 gsl_vector_free(weights);
1385
1386 // Restore old GSL error handler
1388
1389 return vc;
1390}
1391
1392// Setup the parameters for parabola curve fitting
1393void CurveFitting::parSetupParams(FittingGoal goal, gsl_multifit_nlinear_parameters *params, int *numIters, double *xtol,
1394 double *gtol, double *ftol)
1395{
1396 // Trust region subproblem
1397 // - gsl_multifit_nlinear_trs_lm is the Levenberg-Marquardt algorithm without acceleration
1398 // - gsl_multifit_nlinear_trs_lmaccel is the Levenberg-Marquardt algorithm with acceleration
1399 // - gsl_multilarge_nlinear_trs_dogleg is the dogleg algorithm
1400 // - gsl_multifit_nlinear_trs_ddogleg is the double dogleg algorithm
1401 // - gsl_multifit_nlinear_trs_subspace2D is the 2D subspace algorithm
1403
1404 // Scale
1405 // - gsl_multifit_nlinear_scale_more uses. More strategy. Good for problems with parameters with widely different scales
1406 // - gsl_multifit_nlinear_scale_levenberg. Levensberg strategy. Can be good but not scale invariant.
1407 // - gsl_multifit_nlinear_scale_marquardt. Marquardt strategy. Considered inferior to More and Levensberg
1408 params->scale = gsl_multifit_nlinear_scale_more;
1409
1410 // Solver
1411 // - gsl_multifit_nlinear_solver_qr produces reliable results but needs more iterations than Cholesky
1412 // - gsl_multifit_nlinear_solver_cholesky fast but not as stable as QR
1413 // - gsl_multifit_nlinear_solver_mcholesky modified Cholesky more stable than Cholesky
1414
1415 // avmax is the max allowed ratio of 2nd order term (acceleration, a) to the 1st order term (velocity, v)
1416 // GSL defaults to 0.75, but suggests reducing it in the case of convergence problems
1417 switch (goal)
1418 {
1419 case STANDARD:
1420 params->solver = gsl_multifit_nlinear_solver_cholesky;
1421 params->avmax = 0.75;
1422
1423 *numIters = MAX_ITERATIONS_CURVE;
1424 *xtol = INEPSXTOL;
1425 *gtol = INEPSGTOL;
1426 *ftol = INEPSFTOL;
1427 break;
1428 case BEST:
1429 params->solver = gsl_multifit_nlinear_solver_cholesky;
1430 params->avmax = 0.75;
1431
1432 *numIters = MAX_ITERATIONS_CURVE * 2.0;
1433 *xtol = INEPSXTOL / 10.0;
1434 *gtol = INEPSGTOL / 10.0;
1435 *ftol = INEPSFTOL / 10.0;
1436 break;
1437 case BEST_RETRY:
1438 params->solver = gsl_multifit_nlinear_solver_qr;
1439 params->avmax = 0.5;
1440
1441 *numIters = MAX_ITERATIONS_CURVE * 2.0;
1442 *xtol = INEPSXTOL;
1443 *gtol = INEPSGTOL;
1444 *ftol = INEPSFTOL;
1445 break;
1446 default:
1447 break;
1448 }
1449}
1450
1451// Initialise parameters before starting the solver. Its important to start with a guess as near to the solution as possible
1452// If we found a solution before and we're just adding more datapoints use the last solution as the guess.
1453// If we don't have a solution use the datapoints to approximate. Work out the min and max points and use these
1454// to find "close" values for the starting point parameters
1455void CurveFitting::parMakeGuess(const int attempt, const DataPointT &dataPoints, gsl_vector * guess)
1456{
1457 // If we are retrying then perturb the initial conditions. The hope is that by doing this the solver
1458 // will be nudged to find a solution this time
1459 double perturbation = 1.0 + pow(-1, attempt) * (attempt * 0.1);
1460
1461 if (!m_FirstSolverRun && (m_LastCurveType == FOCUS_PARABOLA) && (m_LastCoefficients.size() == NUM_PARABOLA_PARAMS))
1462 {
1463 // Last run of the solver was a Parabola and that solution was good, so use that solution
1464 gsl_vector_set(guess, A_IDX, m_LastCoefficients[A_IDX] * perturbation);
1465 gsl_vector_set(guess, B_IDX, m_LastCoefficients[B_IDX] * perturbation);
1466 gsl_vector_set(guess, C_IDX, m_LastCoefficients[C_IDX] * perturbation);
1467 }
1468 else
1469 {
1470 double minX = dataPoints.dps[0].x;
1471 double minY = dataPoints.dps[0].y;
1472 double maxX = minX;
1473 double maxY = minY;
1474 for(int i = 0; i < dataPoints.dps.size(); i++)
1475 {
1476 if (dataPoints.dps[i].y <= 0.0)
1477 continue;
1478 if(minY <= 0.0 || dataPoints.dps[i].y < minY)
1479 {
1480 minX = dataPoints.dps[i].x;
1481 minY = dataPoints.dps[i].y;
1482 }
1483 if(maxY <= 0.0 || dataPoints.dps[i].y > maxY)
1484 {
1485 maxX = dataPoints.dps[i].x;
1486 maxY = dataPoints.dps[i].y;
1487 }
1488 }
1489 double A, B, C;
1490 if (dataPoints.dir == OPTIMISATION_MAXIMISE)
1491 {
1492 // Equation y = f(x) = a + b((x - c) ^2)
1493 // For a maximum b < 0 and a > 0
1494 // At the maximum, Xmax = c, Ymax = a
1495 // Far from the maximum, b = (Ymin - a) / ((Xmin - c) ^2)
1496 A = maxY * perturbation;
1497 B = ((minY - maxY) / pow(minX - maxX, 2.0)) * perturbation;
1498 C = maxX * perturbation;
1499 }
1500 else
1501 {
1502 // For a minimum b > 0 and a > 0
1503 // At the minimum, Xmin = c, Ymin = a
1504 // Far from the minimum, b = (Ymax - a) / ((Xmax - c) ^2)
1505 A = minY * perturbation;
1506 B = ((maxY - minY) / pow(maxX - minX, 2.0)) * perturbation;
1507 C = minX * perturbation;
1508 }
1509 qCDebug(KSTARS_EKOS_FOCUS) << QString("LM solver (par) initial params: perturbation=%1, A=%2, B=%3, C=%4")
1510 .arg(perturbation).arg(A).arg(B).arg(C);
1511
1515 }
1516}
1517
1518QVector<double> CurveFitting::gaussian_fit(DataPoint3DT data, const StarParams &starParams)
1519{
1521
1522 // Set the gsl error handler off as it aborts the program on error.
1524
1525 // Setup variables to be used by the solver
1528 NUM_GAUSSIAN_PARAMS);
1530 int numIters;
1531 double xtol, gtol, ftol;
1532
1533 // Fill in function info
1534 fdf.f = gauFxy;
1535 fdf.df = gauJxy;
1536 fdf.fvv = gauFxyxy;
1537 fdf.n = data.dps.size();
1538 fdf.p = NUM_GAUSSIAN_PARAMS;
1539 fdf.params = &data;
1540
1541 // Allocate the guess vector
1542 gsl_vector * guess = gsl_vector_alloc(NUM_GAUSSIAN_PARAMS);
1543 // Allocate weights vector
1544 auto weights = gsl_vector_alloc(data.dps.size());
1545
1546 // Setup a timer to see how long the solve takes
1547 QElapsedTimer timer;
1548 timer.start();
1549
1550 // Setup for multiple solve attempts. We won't worry too much if the solver fails as there should be
1551 // plenty of stars, but if the solver fails on its first step then adjust parameters and retry as this
1552 // is a fast thing to do.
1553 for (int attempt = 0; attempt < 5; attempt++)
1554 {
1555 // Make initial guesses
1556 gauMakeGuess(attempt, starParams, guess);
1557
1558 // If using weights load up the GSL vector
1559 if (data.useWeights)
1560 {
1561 QVectorIterator<DataPT3D> dp(data.dps);
1562 int i = 0;
1563 while (dp.hasNext())
1564 gsl_vector_set(weights, i++, dp.next().weight);
1565
1566 gsl_multifit_nlinear_winit(guess, weights, &fdf, w);
1567 }
1568 else
1570
1571 // This is the callback used by the LM solver to allow some introspection of each iteration
1572 // Useful for debugging but clogs up the log
1573 // To activate, uncomment the callback lambda and change the call to gsl_multifit_nlinear_driver
1574 /*auto callback = [](const size_t iter, void* _params, const gsl_multifit_nlinear_workspace * w)
1575 {
1576 gsl_vector *f = gsl_multifit_nlinear_residual(w);
1577 gsl_vector *x = gsl_multifit_nlinear_position(w);
1578 double rcond;
1579
1580 // compute reciprocal condition number of J(x)
1581 gsl_multifit_nlinear_rcond(&rcond, w);
1582
1583 // ratio of accel component to velocity component
1584 double avratio = gsl_multifit_nlinear_avratio(w);
1585
1586 qCDebug(KSTARS_EKOS_FOCUS) <<
1587 QString("iter %1: A = %2, B = %3, C = %4, D = %5, E = %6, F = %7, G = %8 "
1588 "rcond(J) = %9, avratio = %10, |f(x)| = %11")
1589 .arg(iter)
1590 .arg(gsl_vector_get(x, A_IDX))
1591 .arg(gsl_vector_get(x, B_IDX))
1592 .arg(gsl_vector_get(x, C_IDX))
1593 .arg(gsl_vector_get(x, D_IDX))
1594 .arg(gsl_vector_get(x, E_IDX))
1595 .arg(gsl_vector_get(x, F_IDX))
1596 .arg(gsl_vector_get(x, G_IDX))
1597 //.arg(1.0 / rcond)
1598 .arg(rcond)
1599 .arg(avratio)
1600 .arg(gsl_blas_dnrm2(f));
1601 };*/
1602
1603 gauSetupParams(&params, &numIters, &xtol, &gtol, &ftol);
1604
1605 qCDebug(KSTARS_EKOS_FOCUS) << QString("Starting LM solver, fit=gaussian, solver=%1, scale=%2, trs=%3, iters=%4, xtol=%5,"
1606 "gtol=%6, ftol=%7")
1607 .arg(params.solver->name).arg(params.scale->name).arg(params.trs->name).arg(numIters)
1608 .arg(xtol).arg(gtol).arg(ftol);
1609
1610 int info = 0;
1612
1613 if (status != 0)
1614 {
1615 qCDebug(KSTARS_EKOS_FOCUS) <<
1616 QString("LM solver (Gaussian): Failed after %1ms iters=%2 [attempt=%3] with status=%4 [%5] and info=%6 [%7]")
1617 .arg(timer.elapsed()).arg(gsl_multifit_nlinear_niter(w)).arg(attempt + 1).arg(status).arg(gsl_strerror(status))
1618 .arg(info).arg(gsl_strerror(info));
1619 if (status != GSL_EMAXITER || info != GSL_ENOPROG || gsl_multifit_nlinear_niter(w) > 1)
1620 break;
1621 }
1622 else
1623 {
1624 // All good so store the results - parameters A, B, C, D, E, F, G
1625 auto solution = gsl_multifit_nlinear_position(w);
1626 for (int j = 0; j < NUM_GAUSSIAN_PARAMS; j++)
1627 vc.push_back(gsl_vector_get(solution, j));
1628
1629 qCDebug(KSTARS_EKOS_FOCUS) << QString("LM Solver (Gaussian): Solution found after %1ms %2 iters (%3). A=%4, B=%5, C=%6, "
1630 "D=%7, E=%8, F=%9, G=%10").arg(timer.elapsed()).arg(gsl_multifit_nlinear_niter(w)).arg(getLMReasonCode(info))
1632 .arg(vc[F_IDX]).arg(vc[G_IDX]);
1633 break;
1634 }
1635 }
1636
1637 // Free GSL memory
1640 gsl_vector_free(weights);
1641
1642 // Restore old GSL error handler
1644
1645 return vc;
1646}
1647
1648// Initialise parameters before starting the solver. Its important to start with a guess as near to the solution as possible
1649// Since we have already run some HFR calcs on the star, use these values to calculate the guess
1650void CurveFitting::gauMakeGuess(const int attempt, const StarParams &starParams, gsl_vector * guess)
1651{
1652 // If we are retrying then perturb the initial conditions. The hope is that by doing this the solver
1653 // will be nudged to find a solution this time
1654 double perturbation = 1.0 + pow(-1, attempt) * (attempt * 0.1);
1655
1656 // Default from the input star details
1657 const double a = std::max(starParams.peak, 0.0) * perturbation; // Peak value
1658 const double x0 = std::max(starParams.centroid_x, 0.0) * perturbation; // Centroid x
1659 const double y0 = std::max(starParams.centroid_y, 0.0) * perturbation; // Centroid y
1660 const double b = std::max(starParams.background, 0.0) * perturbation; // Background
1661
1662 double A = 1.0, B = 0.0, C = 1.0;
1663 if (starParams.HFR > 0.0)
1664 {
1665 // Use 2*HFR value as FWHM along with theta to calc A, B, C
1666 // FWHM = 2.sqrt(2.ln(2)).sigma
1667 // Assume circular symmetry so B = 0
1668 const double sigma2 = pow(starParams.HFR / (sqrt(2.0 * log(2.0))), 2.0);
1669 const double costheta2 = pow(cos(starParams.theta), 2.0);
1670 const double sintheta2 = pow(sin(starParams.theta), 2.0);
1671
1672 A = C = (costheta2 + sintheta2) / (2 * sigma2) * perturbation;
1673 }
1674
1675 qCDebug(KSTARS_EKOS_FOCUS) <<
1676 QString("LM Solver (Gaussian): Guess perturbation=%1, A=%2, B=%3, C=%4, D=%5, E=%6, F=%7, G=%8")
1677 .arg(perturbation).arg(a).arg(x0).arg(y0).arg(A).arg(B).arg(C).arg(b);
1678
1686}
1687
1688// Setup the parameters for gaussian curve fitting
1689void CurveFitting::gauSetupParams(gsl_multifit_nlinear_parameters *params, int *numIters, double *xtol, double *gtol,
1690 double *ftol)
1691{
1692 // Trust region subproblem
1693 // - gsl_multifit_nlinear_trs_lm is the Levenberg-Marquardt algorithm without acceleration
1694 // - gsl_multifit_nlinear_trs_lmaccel is the Levenberg-Marquardt algorithm with acceleration
1695 // - gsl_multilarge_nlinear_trs_dogleg is the dogleg algorithm
1696 // - gsl_multifit_nlinear_trs_ddogleg is the double dogleg algorithm
1697 // - gsl_multifit_nlinear_trs_subspace2D is the 2D subspace algorithm
1699
1700 // avmax is the max allowed ratio of 2nd order term (acceleration, a) to the 1st order term (velocity, v)
1701 // GSL defaults to 0.75
1702 params->avmax = 0.75;
1703
1704 // Scale
1705 // - gsl_multifit_nlinear_scale_more uses. More strategy. Good for problems with parameters with widely different scales
1706 // - gsl_multifit_nlinear_scale_levenberg. Levensberg strategy. Can be good but not scale invariant.
1707 // - gsl_multifit_nlinear_scale_marquardt. Marquardt strategy. Considered inferior to More and Levensberg
1708 params->scale = gsl_multifit_nlinear_scale_more;
1709
1710 // Solver
1711 // - gsl_multifit_nlinear_solver_qr produces reliable results but needs more iterations than Cholesky
1712 // - gsl_multifit_nlinear_solver_cholesky fast but not as stable as QR
1713 // - gsl_multifit_nlinear_solver_mcholesky modified Cholesky more stable than Cholesky
1714 params->solver = gsl_multifit_nlinear_solver_qr;
1715
1716 *numIters = MAX_ITERATIONS_STARS;
1717 *xtol = 1e-5;
1718 *gtol = INEPSGTOL;
1719 *ftol = 1e-5;
1720}
1721
1722// Curve fit a 3D plane
1723QVector<double> CurveFitting::plane_fit(const DataPoint3DT data)
1724{
1726
1727 // Set the gsl error handler off as it aborts the program on error.
1729
1730 // Setup variables to be used by the solver
1733 NUM_PLANE_PARAMS);
1735 int numIters;
1736 double xtol, gtol, ftol;
1737
1738 // Fill in function info
1739 fdf.f = plaFxy;
1740 fdf.df = plaJxy;
1741 fdf.fvv = plaFxyxy;
1742 fdf.n = data.dps.size();
1743 fdf.p = NUM_PLANE_PARAMS;
1744 fdf.params = (void *) &data;
1745
1746 // Allocate the guess vector
1747 gsl_vector * guess = gsl_vector_alloc(NUM_PLANE_PARAMS);
1748 // Allocate weights vector
1749 auto weights = gsl_vector_alloc(data.dps.size());
1750
1751 // Setup a timer to see how long the solve takes
1752 QElapsedTimer timer;
1753 timer.start();
1754
1755 // Setup for multiple solve attempts.
1756 for (int attempt = 0; attempt < 5; attempt++)
1757 {
1758 // Make initial guesses
1759 plaMakeGuess(attempt, guess);
1760
1761 // If using weights load up the GSL vector
1762 if (data.useWeights)
1763 {
1764 QVectorIterator<DataPT3D> dp(data.dps);
1765 int i = 0;
1766 while (dp.hasNext())
1767 gsl_vector_set(weights, i++, dp.next().weight);
1768
1769 gsl_multifit_nlinear_winit(guess, weights, &fdf, w);
1770 }
1771 else
1773
1774 // This is the callback used by the LM solver to allow some introspection of each iteration
1775 // Useful for debugging but clogs up the log
1776 // To activate, uncomment the callback lambda and change the call to gsl_multifit_nlinear_driver
1777 /*auto callback = [](const size_t iter, void* _params, const gsl_multifit_nlinear_workspace * w)
1778 {
1779 gsl_vector *f = gsl_multifit_nlinear_residual(w);
1780 gsl_vector *x = gsl_multifit_nlinear_position(w);
1781 double rcond;
1782
1783 // compute reciprocal condition number of J(x)
1784 gsl_multifit_nlinear_rcond(&rcond, w);
1785
1786 // ratio of accel component to velocity component
1787 double avratio = gsl_multifit_nlinear_avratio(w);
1788
1789 qCDebug(KSTARS_EKOS_FOCUS) <<
1790 QString("iter %1: A = %2, B = %3, C = %4"
1791 "rcond(J) = %5, avratio = %6, |f(x)| = %7")
1792 .arg(iter)
1793 .arg(gsl_vector_get(x, A_IDX))
1794 .arg(gsl_vector_get(x, B_IDX))
1795 .arg(gsl_vector_get(x, C_IDX))
1796 //.arg(1.0 / rcond)
1797 .arg(rcond)
1798 .arg(avratio)
1799 .arg(gsl_blas_dnrm2(f));
1800 };*/
1801
1802 plaSetupParams(&params, &numIters, &xtol, &gtol, &ftol);
1803
1804 qCDebug(KSTARS_EKOS_FOCUS) << QString("Starting LM solver, fit=plane, solver=%1, scale=%2, trs=%3, iters=%4, xtol=%5,"
1805 "gtol=%6, ftol=%7")
1806 .arg(params.solver->name).arg(params.scale->name).arg(params.trs->name).arg(numIters)
1807 .arg(xtol).arg(gtol).arg(ftol);
1808
1809 int info = 0;
1811
1812 if (status != 0)
1813 {
1814 qCDebug(KSTARS_EKOS_FOCUS) <<
1815 QString("LM solver (Plane): Failed after %1ms iters=%2 [attempt=%3] with status=%4 [%5] and info=%6 [%7]")
1816 .arg(timer.elapsed()).arg(gsl_multifit_nlinear_niter(w)).arg(attempt + 1).arg(status).arg(gsl_strerror(status))
1817 .arg(info).arg(gsl_strerror(info));
1818 if (status != GSL_EMAXITER || info != GSL_ENOPROG || gsl_multifit_nlinear_niter(w) > 1)
1819 break;
1820 }
1821 else
1822 {
1823 // All good so store the results - parameters A, B, C
1824 auto solution = gsl_multifit_nlinear_position(w);
1825 for (int j = 0; j < NUM_PLANE_PARAMS; j++)
1826 vc.push_back(gsl_vector_get(solution, j));
1827
1828 qCDebug(KSTARS_EKOS_FOCUS) << QString("LM Solver (Plane): Solution found after %1ms %2 iters (%3). A=%4, B=%5, C=%6")
1829 .arg(timer.elapsed()).arg(gsl_multifit_nlinear_niter(w)).arg(getLMReasonCode(info))
1830 .arg(vc[A_IDX]).arg(vc[B_IDX]).arg(vc[C_IDX]);
1831 break;
1832 }
1833 }
1834
1835 // Free GSL memory
1838 gsl_vector_free(weights);
1839
1840 // Restore old GSL error handler
1842
1843 return vc;
1844}
1845
1846// Initialise parameters before starting the solver. Its important to start with a guess as near to the solution as possible
1847void CurveFitting::plaMakeGuess(const int attempt, gsl_vector * guess)
1848{
1849 double A, B, C;
1850 // If we are retrying then perturb the initial conditions. The hope is that by doing this the solver
1851 // will be nudged to find a solution this time
1852 const double perturbation = 1.0 + pow(-1, attempt) * (attempt * 0.1);
1853
1854 if (!m_FirstSolverRun && (m_LastCurveType == FOCUS_PLANE) && (m_LastCoefficients.size() == NUM_PLANE_PARAMS) && attempt < 3)
1855 {
1856 // Last run of the solver was a Plane and that solution was good, so use that solution
1857 A = m_LastCoefficients[A_IDX] * perturbation;
1858 B = m_LastCoefficients[B_IDX] * perturbation;
1859 C = m_LastCoefficients[C_IDX] * perturbation;
1860 }
1861 else
1862 {
1863 A = perturbation;
1864 B = perturbation;
1865 C = perturbation;
1866 }
1867
1868 qCDebug(KSTARS_EKOS_FOCUS) << QString("LM Solver (Plane): Guess perturbation=%1, A=%2, B=%3, C=%4")
1869 .arg(perturbation).arg(A).arg(B).arg(C);
1870
1874}
1875
1876// Setup the parameters for plane curve fitting
1877void CurveFitting::plaSetupParams(gsl_multifit_nlinear_parameters *params, int *numIters, double *xtol, double *gtol,
1878 double *ftol)
1879{
1880 // Trust region subproblem
1881 // - gsl_multifit_nlinear_trs_lm is the Levenberg-Marquardt algorithm without acceleration
1882 // - gsl_multifit_nlinear_trs_lmaccel is the Levenberg-Marquardt algorithm with acceleration
1883 // - gsl_multilarge_nlinear_trs_dogleg is the dogleg algorithm
1884 // - gsl_multifit_nlinear_trs_ddogleg is the double dogleg algorithm
1885 // - gsl_multifit_nlinear_trs_subspace2D is the 2D subspace algorithm
1886 params->trs = gsl_multifit_nlinear_trs_lm;
1887
1888 // avmax is the max allowed ratio of 2nd order term (acceleration, a) to the 1st order term (velocity, v)
1889 // GSL defaults to 0.75
1890 // params->avmax = 0.75;
1891
1892 // Scale
1893 // - gsl_multifit_nlinear_scale_more uses. More strategy. Good for problems with parameters with widely different scales
1894 // - gsl_multifit_nlinear_scale_levenberg. Levensberg strategy. Can be good but not scale invariant.
1895 // - gsl_multifit_nlinear_scale_marquardt. Marquardt strategy. Considered inferior to More and Levensberg
1896 params->scale = gsl_multifit_nlinear_scale_more;
1897
1898 // Solver
1899 // - gsl_multifit_nlinear_solver_qr produces reliable results but needs more iterations than Cholesky
1900 // - gsl_multifit_nlinear_solver_cholesky fast but not as stable as QR
1901 // - gsl_multifit_nlinear_solver_mcholesky modified Cholesky more stable than Cholesky
1902 params->solver = gsl_multifit_nlinear_solver_qr;
1903
1904 *numIters = MAX_ITERATIONS_PLANE;
1905 *xtol = 1e-5;
1906 *gtol = INEPSGTOL;
1907 *ftol = 1e-5;
1908}
1909
1910bool CurveFitting::findMinMax(double expected, double minPosition, double maxPosition, double *position, double *value,
1911 CurveFit curveFit, const OptimisationDirection optDir)
1912{
1913 bool foundFit;
1914 switch (curveFit)
1915 {
1916 case FOCUS_QUADRATIC :
1917 foundFit = minimumQuadratic(expected, minPosition, maxPosition, position, value);
1918 break;
1919 case FOCUS_HYPERBOLA :
1920 foundFit = minMaxHyperbola(expected, minPosition, maxPosition, position, value, optDir);
1921 break;
1922 case FOCUS_PARABOLA :
1923 foundFit = minMaxParabola(expected, minPosition, maxPosition, position, value, optDir);
1924 break;
1925 default :
1926 foundFit = false;
1927 break;
1928 }
1929 if (!foundFit)
1930 // If we couldn't fit a curve then something's wrong so reset coefficients which will force the next run of the LM solver to start from scratch
1931 m_LastCoefficients.clear();
1932 return foundFit;
1933}
1934
1935bool CurveFitting::minimumQuadratic(double expected, double minPosition, double maxPosition, double *position,
1936 double *value)
1937{
1938 int status;
1939 int iter = 0, max_iter = 100;
1940 const gsl_min_fminimizer_type *T;
1942 double m = expected;
1943
1944 gsl_function F;
1945 F.function = &CurveFitting::curveFunction;
1946 F.params = this;
1947
1948 // Must turn off error handler or it aborts on error
1950
1954
1955 if (status != GSL_SUCCESS)
1956 {
1957 qCWarning(KSTARS_EKOS_FOCUS) << "Focus GSL error:" << gsl_strerror(status);
1958 return false;
1959 }
1960
1961 do
1962 {
1963 iter++;
1965
1969
1971
1972 if (status == GSL_SUCCESS)
1973 {
1974 *position = m;
1975 *value = curveFunction(m, this);
1976 }
1977 }
1978 while (status == GSL_CONTINUE && iter < max_iter);
1979
1981 return (status == GSL_SUCCESS);
1982}
1983
1984bool CurveFitting::minMaxHyperbola(double expected, double minPosition, double maxPosition, double *position,
1985 double *value, const OptimisationDirection optDir)
1986{
1988 if (m_coefficients.size() != NUM_HYPERBOLA_PARAMS)
1989 {
1990 if (m_coefficients.size() != 0)
1991 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::minimumHyperbola coefficients.size()=%1").arg(
1992 m_coefficients.size());
1993 return false;
1994 }
1995
1996 double b = m_coefficients[B_IDX];
1997 double c = m_coefficients[C_IDX];
1998 double d = m_coefficients[D_IDX];
1999
2000 // We need to check that the solution found is in the correct form.
2001 // Check 1: The hyperbola minimum (=c) lies within the bounds of the focuser (and is > 0)
2002 // Check 2: At the minimum position (=c), the value of f(x) (which is the HFR) given by b+d is > 0
2003 // Check 3: For a minimum: we have a "u" shaped curve, not an "n" shape. b > 0.
2004 // maximum: we have an "n" shaped curve, not a "U" shape. b < 0;
2005 if ((c >= minPosition) && (c <= maxPosition) && (b + d > 0.0) &&
2006 ((optDir == OPTIMISATION_MINIMISE && b > 0.0) || (optDir == OPTIMISATION_MAXIMISE && b < 0.0)))
2007 {
2008 *position = c;
2009 *value = b + d;
2010 return true;
2011 }
2012 else
2013 return false;
2014}
2015
2016bool CurveFitting::minMaxParabola(double expected, double minPosition, double maxPosition, double *position,
2017 double *value, const OptimisationDirection optDir)
2018{
2020 if (m_coefficients.size() != NUM_PARABOLA_PARAMS)
2021 {
2022 if (m_coefficients.size() != 0)
2023 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::minimumParabola coefficients.size()=%1").arg(
2024 m_coefficients.size());
2025 return false;
2026 }
2027
2028 double a = m_coefficients[A_IDX];
2029 double b = m_coefficients[B_IDX];
2030 double c = m_coefficients[C_IDX];
2031
2032 // We need to check that the solution found is in the correct form.
2033 // Check 1: The parabola minimum (=c) lies within the bounds of the focuser (and is > 0)
2034 // Check 2: At the minimum position (=c), the value of f(x) (which is the HFR) given by a is > 0
2035 // Check 3: For a minimum: we have a "u" shaped curve, not an "n" shape. b > 0.
2036 // maximum: we have an "n" shaped curve, not a "U" shape. b < 0;
2037 if ((c >= minPosition) && (c <= maxPosition) && (a > 0.0) &&
2038 ((optDir == OPTIMISATION_MINIMISE && b > 0.0) || (optDir == OPTIMISATION_MAXIMISE && b < 0.0)))
2039 {
2040 *position = c;
2041 *value = a;
2042 return true;
2043 }
2044 else
2045 return false;
2046}
2047
2048bool CurveFitting::getStarParams(const CurveFit curveFit, StarParams *starParams)
2049{
2050 bool foundFit;
2051 switch (curveFit)
2052 {
2053 case FOCUS_GAUSSIAN :
2054 foundFit = getGaussianParams(starParams);
2055 break;
2056 default :
2057 foundFit = false;
2058 break;
2059 }
2060 if (!foundFit)
2061 // If we couldn't fit a curve then something's wrong so reset coefficients which will force the next run of the LM solver to start from scratch
2062 m_LastCoefficients.clear();
2063 return foundFit;
2064}
2065
2066// Having solved for a Gaussian return params
2067bool CurveFitting::getGaussianParams(StarParams *starParams)
2068{
2069 if (m_coefficients.size() != NUM_GAUSSIAN_PARAMS)
2070 {
2071 if (m_coefficients.size() != 0)
2072 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::getGaussianParams coefficients.size()=%1").arg(
2073 m_coefficients.size());
2074 return false;
2075 }
2076
2077 const double a = m_coefficients[A_IDX];
2078 const double x0 = m_coefficients[B_IDX];
2079 const double y0 = m_coefficients[C_IDX];
2080 const double A = m_coefficients[D_IDX];
2081 const double B = m_coefficients[E_IDX];
2082 const double C = m_coefficients[F_IDX];
2083 const double b = m_coefficients[G_IDX];
2084
2085 // Sanity check the coefficients in case the solver produced a bad solution
2086 if (a <= 0.0 || b <= 0.0 || x0 <= 0.0 || y0 <= 0.0)
2087 {
2088 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::getGaussianParams a=%1 b=%2 x0=%3 y0=%4").arg(a).arg(b)
2089 .arg(x0).arg(y0);
2090 return false;
2091 }
2092
2093 const double AmC = A - C;
2094 double theta;
2095 abs(AmC) < 1e-10 ? theta = 0.0 : theta = 0.5 * atan(2 * B / AmC);
2096 const double costheta = cos(theta);
2097 const double costheta2 = costheta * costheta;
2098 const double sintheta = sin(theta);
2099 const double sintheta2 = sintheta * sintheta;
2100
2101 double sigmax2 = 0.5 / ((A * costheta2) + (2 * B * costheta * sintheta) + (C * sintheta2));
2102 double sigmay2 = 0.5 / ((A * costheta2) - (2 * B * costheta * sintheta) + (C * sintheta2));
2103
2104 double FWHMx = 2 * pow(2 * log(2) * sigmax2, 0.5);
2105 double FWHMy = 2 * pow(2 * log(2) * sigmay2, 0.5);
2106 double FWHM = (FWHMx + FWHMy) / 2.0;
2107
2108 if (isnan(FWHM) || FWHM < 0.0)
2109 {
2110 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::getGaussianParams FWHM=%1").arg(FWHM);
2111 return false;
2112 }
2113
2114 starParams->background = b;
2115 starParams->peak = a;
2116 starParams->centroid_x = x0;
2117 starParams->centroid_y = y0;
2118 starParams->theta = theta;
2119 starParams->FWHMx = FWHMx;
2120 starParams->FWHMy = FWHMy;
2121 starParams->FWHM = FWHM;
2122
2123 return true;
2124}
2125
2126// R2 (R squared) is the coefficient of determination gives a measure of how well the curve fits the datapoints.
2127// It lies between 0 and 1. 1 means that all datapoints will lie on the curve which therefore exactly describes the
2128// datapoints. 0 means that there is no correlation between the curve and the datapoints.
2129// See www.wikipedia.org/wiki/Coefficient_of_determination for more details.
2130double CurveFitting::calculateR2(CurveFit curveFit)
2131{
2132 double R2 = 0.0;
2134 int i;
2135
2136 switch (curveFit)
2137 {
2138 case FOCUS_QUADRATIC :
2139 // Not implemented
2140 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::calculateR2 called for Quadratic");
2141 break;
2142
2143 case FOCUS_HYPERBOLA :
2144 // Calculate R2 for the hyperbola
2145 if (m_coefficients.size() != NUM_HYPERBOLA_PARAMS)
2146 {
2147 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::calculateR2 hyperbola coefficients.size()=").arg(
2148 m_coefficients.size());
2149 break;
2150 }
2151
2152 for (i = 0; i < m_y.size(); i++)
2153 if (!m_outliers[i])
2154 {
2155 // Load up the dataPoints and curvePoints vector, excluding any outliers
2156 dataPoints.push_back(m_y[i]);
2157 curvePoints.push_back(hypfx(m_x[i], m_coefficients[A_IDX], m_coefficients[B_IDX], m_coefficients[C_IDX],
2158 m_coefficients[D_IDX]));
2159 scalePoints.push_back(m_scale[i]);
2160 }
2161
2162 // Do the actual R2 calculation
2163 R2 = calcR2(dataPoints, curvePoints, scalePoints, m_useWeights);
2164 break;
2165
2166 case FOCUS_PARABOLA :
2167 // Calculate R2 for the parabola
2168 if (m_coefficients.size() != NUM_PARABOLA_PARAMS)
2169 {
2170 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::calculateR2 parabola coefficients.size()=").arg(
2171 m_coefficients.size());
2172 break;
2173 }
2174
2175 for (i = 0; i < m_y.size(); i++)
2176 if (!m_outliers[i])
2177 {
2178 dataPoints.push_back(m_y[i]);
2179 // Load up the dataPoints and curvePoints vector, excluding any outliers
2180 curvePoints.push_back(parfx(m_x[i], m_coefficients[A_IDX], m_coefficients[B_IDX], m_coefficients[C_IDX]));
2181 scalePoints.push_back(m_scale[i]);
2182 }
2183
2184 // Do the actual R2 calculation
2185 R2 = calcR2(dataPoints, curvePoints, scalePoints, m_useWeights);
2186 break;
2187
2188 case FOCUS_GAUSSIAN :
2189 // Calculate R2 for the gaussian
2190 if (m_coefficients.size() != NUM_GAUSSIAN_PARAMS)
2191 {
2192 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::calculateR2 gaussian coefficients.size()=").arg(
2193 m_coefficients.size());
2194 break;
2195 }
2196
2197 for (i = 0; i < m_dataPoints.dps.size(); i++)
2198 {
2199 // Load up the curvePoints vector
2200 curvePoints.push_back(gaufxy(m_dataPoints.dps[i].x, m_dataPoints.dps[i].y, m_coefficients[A_IDX],
2201 m_coefficients[B_IDX], m_coefficients[C_IDX], m_coefficients[D_IDX],
2202 m_coefficients[E_IDX], m_coefficients[F_IDX], m_coefficients[G_IDX]));
2203 dataPoints.push_back(static_cast <double> (m_dataPoints.dps[i].z));
2204 }
2205
2206 // Do the actual R2 calculation
2207 R2 = calcR2(dataPoints, curvePoints, m_scale, m_dataPoints.useWeights);
2208 break;
2209
2210 case FOCUS_PLANE :
2211 // Calculate R2 for the plane
2212 if (m_coefficients.size() != NUM_PLANE_PARAMS)
2213 {
2214 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::calculateR2 plane coefficients.size()=").arg(
2215 m_coefficients.size());
2216 break;
2217 }
2218
2219 for (i = 0; i < m_dataPoints.dps.size(); i++)
2220 {
2221 // Load up the curvePoints vector
2222 curvePoints.push_back(plafxy(m_dataPoints.dps[i].x, m_dataPoints.dps[i].y, m_coefficients[A_IDX],
2223 m_coefficients[B_IDX], m_coefficients[C_IDX]));
2224 dataPoints.push_back(static_cast <double> (m_dataPoints.dps[i].z));
2225 }
2226
2227 // Do the actual R2 calculation
2228 R2 = calcR2(dataPoints, curvePoints, m_scale, m_dataPoints.useWeights);
2229 break;
2230
2231 default :
2232 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::calculateR2 curveFit=%1").arg(curveFit);
2233 break;
2234 }
2235 // R2 for linear function range 0<=R2<=1. For non-linear functions it is possible
2236 // for R2 to be negative. This doesn't really mean anything so force 0 in these situations.
2237 return std::max(R2, 0.0);
2238}
2239
2240// Do the maths to calculate R2 - how well the curve fits the datapoints
2241double CurveFitting::calcR2(const QVector<double> dataPoints, const QVector<double> curvePoints,
2242 const QVector<double> scale, const bool useWeights)
2243{
2244 double R2 = 0.0, chisq = 0.0, sum = 0.0, totalSumSquares = 0.0, weight, average;
2245
2246 for (int i = 0; i < dataPoints.size(); i++)
2247 {
2248 sum += dataPoints[i];
2249 useWeights ? weight = scale[i] : weight = 1.0;
2250 chisq += weight * pow((dataPoints[i] - curvePoints[i]), 2.0);
2251 }
2252 average = sum / dataPoints.size();
2253
2254 for (int i = 0; i < dataPoints.size(); i++)
2255 {
2256 useWeights ? weight = scale[i] : weight = 1.0;
2257 totalSumSquares += weight * pow((dataPoints[i] - average), 2.0);
2258 }
2259
2260 if (totalSumSquares > 0.0)
2261 R2 = 1 - (chisq / totalSumSquares);
2262 else
2263 {
2264 R2 = 0.0;
2265 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::calcR2 tss=%1").arg(totalSumSquares);
2266 }
2267
2268 // R2 for linear function range 0<=R2<=1. For non-linear functions it is possible
2269 // for R2 to be negative. This doesn't really mean anything so force 0 in these situations.
2270 return std::max(R2, 0.0);
2271}
2272
2273void CurveFitting::calculateCurveDeltas(CurveFit curveFit, std::vector<std::pair<int, double>> &curveDeltas)
2274{
2275 curveDeltas.clear();
2276
2277 switch (curveFit)
2278 {
2279 case FOCUS_QUADRATIC :
2280 // Not implemented
2281 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::calculateCurveDeltas called for Quadratic");
2282 break;
2283
2284 case FOCUS_HYPERBOLA :
2285 if (m_coefficients.size() != NUM_HYPERBOLA_PARAMS)
2286 {
2287 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::calculateCurveDeltas hyperbola coefficients.size()=").arg(
2288 m_coefficients.size());
2289 break;
2290 }
2291
2292 for (int i = 0; i < m_y.size(); i++)
2293 if (!m_outliers[i])
2294 curveDeltas.push_back(std::make_pair(i, abs(m_y[i] - hypfx(m_x[i], m_coefficients[A_IDX], m_coefficients[B_IDX],
2295 m_coefficients[C_IDX], m_coefficients[D_IDX]))));
2296 break;
2297
2298 case FOCUS_PARABOLA :
2299 if (m_coefficients.size() != NUM_PARABOLA_PARAMS)
2300 {
2301 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::calculateCurveDeltas parabola coefficients.size()=").arg(
2302 m_coefficients.size());
2303 break;
2304 }
2305
2306 for (int i = 0; i < m_y.size(); i++)
2307 if (!m_outliers[i])
2308 curveDeltas.push_back(std::make_pair(i, abs(m_y[i] - parfx(m_x[i], m_coefficients[A_IDX], m_coefficients[B_IDX],
2309 m_coefficients[C_IDX]))));
2310 break;
2311
2312 case FOCUS_GAUSSIAN :
2313 // Not implemented
2314 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::calculateCurveDeltas called for Gaussian");
2315 break;
2316
2317 default :
2318 qCDebug(KSTARS_EKOS_FOCUS) << QString("Error in CurveFitting::calculateCurveDeltas curveFit=%1").arg(curveFit);
2319 break;
2320 }
2321}
2322
2323namespace
2324{
2326{
2327 QString str = QString("%1").arg(vector.size());
2328 for (const double v : vector)
2329 str.append(QString(";%1").arg(v, 0, 'g', 12));
2330 return str;
2331}
2332
2333bool decodeDoubleVector(const QString serialized, QVector<double> *vector)
2334{
2335 vector->clear();
2336 const QStringList parts = serialized.split(';');
2337 if (parts.size() == 0) return false;
2338 bool ok;
2339 int size = parts[0].toInt(&ok);
2340 if (!ok || (size != parts.size() - 1)) return false;
2341 for (int i = 0; i < size; ++i)
2342 {
2343 const double val = parts[i + 1].toDouble(&ok);
2344 if (!ok) return false;
2345 vector->append(val);
2346 }
2347 return true;
2348}
2349}
2350
2351QString CurveFitting::serialize() const
2352{
2353 QString serialized = "";
2354 if (m_FirstSolverRun) return serialized;
2355 serialized = QString("%1").arg(int(m_CurveType));
2356 serialized.append(QString("|%1").arg(serializeDoubleVector(m_x)));
2357 serialized.append(QString("|%1").arg(serializeDoubleVector(m_y)));
2358 serialized.append(QString("|%1").arg(serializeDoubleVector(m_scale)));
2359 serialized.append(QString("|%1").arg(m_useWeights ? "T" : "F"));
2360 // m_dataPoints not implemented. Not needed for graphing.
2361 serialized.append(QString("|m_dataPoints not implemented"));
2362 serialized.append(QString("|%1").arg(serializeDoubleVector(m_coefficients)));
2363 serialized.append(QString("|%1").arg(int(m_LastCurveType)));
2364 serialized.append(QString("|%1").arg(serializeDoubleVector(m_LastCoefficients)));
2365 return serialized;
2366}
2367
2368bool CurveFitting::recreateFromQString(const QString &serialized)
2369{
2370 QStringList parts = serialized.split('|');
2371 bool ok = false;
2372 if (parts.size() != 9) return false;
2373
2374 int val = parts[0].toInt(&ok);
2375 if (!ok) return false;
2376 m_CurveType = static_cast<CurveFit>(val);
2377
2378 if (!decodeDoubleVector(parts[1], &m_x)) return false;
2379 if (!decodeDoubleVector(parts[2], &m_y)) return false;
2380 if (!decodeDoubleVector(parts[3], &m_scale)) return false;
2381
2382 if (parts[4] == "T") m_useWeights = true;
2383 else if (parts[4] == "F") m_useWeights = false;
2384 else return false;
2385
2386 // parts[5]: m_dataPoints not implemented.
2387
2388 if (!decodeDoubleVector(parts[6], &m_coefficients)) return false;
2389
2390 val = parts[7].toInt(&ok);
2391 if (!ok) return false;
2392 m_LastCurveType = static_cast<CurveFit>(val);
2393
2394 if (!decodeDoubleVector(parts[8], &m_LastCoefficients)) return false;
2395
2396 m_FirstSolverRun = false;
2397 return true;
2398}
2399
2400} // namespace
Ekos is an advanced Astrophotography tool for Linux.
Definition align.cpp:78
NETWORKMANAGERQT_EXPORT NetworkManager::Status status()
qsizetype size() const const
QString & append(QChar ch)
QString arg(Args &&... args) const const
QStringList split(QChar sep, Qt::SplitBehavior behavior, Qt::CaseSensitivity cs) const const
This file is part of the KDE documentation.
Documentation copyright © 1996-2024 The KDE developers.
Generated on Tue Mar 26 2024 11:19:02 by doxygen 1.10.0 written by Dimitri van Heesch, © 1997-2006

KDE's Doxygen guidelines are available online.