From 3ef23b37a3ffa5ceddfb6be6d01d1aae58fcfff1 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 6 Apr 2026 17:45:26 -0400 Subject: [PATCH] probe_eddy_current: Rework internal formulas for "tap" analysis Use a different base frequency and base z value for the internal "tap" least squares analysis to improve the numerical stability of the calculations. Rework the formulas being solved so that only the depressed slope calculation is relative to the estimated z contact point. This results in less variables that are dependent on the z contact. Signed-off-by: Kevin O'Connor --- klippy/extras/probe_eddy_current.py | 64 ++++++++++++++++------------- 1 file changed, 36 insertions(+), 28 deletions(-) diff --git a/klippy/extras/probe_eddy_current.py b/klippy/extras/probe_eddy_current.py index de71a5ceb..9c1fe1de6 100644 --- a/klippy/extras/probe_eddy_current.py +++ b/klippy/extras/probe_eddy_current.py @@ -561,18 +561,26 @@ class EddyTap: s.get_past_mcu_position(pos_time)) for s in kin.get_steppers()} return kin.calc_position(kin_spos) - def _calc_least_squares(self, eqs, ans, est_z_contact): + def _calc_least_squares(self, samples, est_z_contact): # XXX - this implementation is not efficient - len_data = len(eqs) + len_samples = len(samples) import numpy - for i in range(len_data): + eqs = numpy.zeros((len_samples, 4)) + ans = numpy.zeros((len_samples,)) + for i, (step_z, sensor_freq) in enumerate(samples): + ans[i] = sensor_freq eq = eqs[i] - step_z = eq[1] - if step_z < est_z_contact: - eq[2] = eq[3] = 0. - continue - eq[2] = (step_z - est_z_contact) - eq[3] = (step_z - est_z_contact)**2 + eq[0] = 1. + if step_z <= est_z_contact: + # 1*c0 + (z-ezc)*c1 + ezc*c2 + ezc*ezc*c3 = freq + eq[1] = step_z - est_z_contact + eq[2] = est_z_contact + eq[3] = est_z_contact * est_z_contact + else: + # 1*c0 + 0*c1 + z*c2 + z*z*c3 = freq + eq[1] = 0. + eq[2] = step_z + eq[3] = step_z * step_z res = numpy.linalg.lstsq(eqs, ans, rcond=None) coeffs = list(res[0]) if coeffs[3] < 0.: @@ -586,20 +594,15 @@ class EddyTap: #logging.info("z=%.6f err=%.3f coeffs=%s", est_z_contact, err, coeffs) return err, coeffs def _find_least_squares(self, data): - len_data = len(data) - import numpy - # Populate initial numpy linear least squares arrays - eqs = numpy.zeros((len_data, 4)) - ans = numpy.zeros((len_data,)) - for i, (sensor_freq, tool_pos) in enumerate(data): - ans[i] = sensor_freq - eq = eqs[i] - eq[0] = 1. - eq[1] = tool_pos[2] - #logging.info("sample: freq=%.3f z=%.6f", sensor_freq, tool_pos[2]) + #for d in data: + # logging.info("sample: freq=%.3f z=%.6f", d[0], d[1][2]) + # Change base of freq/z measurements to improve numerical stability + base_z = .5 * (data[0][1][2] + data[-1][1][2]) + base_freq = .5 * (data[0][0] + data[-1][0]) + samples = [(d[1][2] - base_z, d[0] - base_freq) for d in data] # Run least squares with various z values to reduce residual error - min_z = best_z = eqs[0][1] - max_z = eqs[-1][1] + min_z = best_z = samples[0][0] + max_z = samples[-1][0] best_err = sys.float_info.max best_coeffs = [0., 0., 0., 0.] while max_z - min_z > 0.000250: @@ -610,7 +613,7 @@ class EddyTap: else: guess_z = (min_z + best_z) * .5 # Calculate least squares error for given z - guess_err, coeffs = self._calc_least_squares(eqs, ans, guess_z) + guess_err, coeffs = self._calc_least_squares(samples, guess_z) # Update search bounds if guess_err < best_err: if guess_z > best_z: @@ -625,17 +628,22 @@ class EddyTap: max_z = guess_z else: min_z = guess_z - best_coeffs = [float(v) for v in best_coeffs] - #logging.info("best: z=%.6f err=%.6f coeffs=%s", - # best_z, best_err, best_coeffs) - return float(best_z), best_coeffs + # Return to original freq/z measurement base + best_z = float(best_z) + bc = [float(v) for v in best_coeffs] + final_coeffs = (base_z + best_z, + base_freq + bc[0] + best_z*bc[2] + best_z*best_z*bc[3], + bc[1], bc[2] + 2.*best_z*bc[3], bc[3]) + #logging.info("probe_analysis: coeffs=%s", final_coeffs) + return final_coeffs def _analyze_pullback(self, measures, start_time, end_time): self._validate_samples_time(measures, start_time, end_time) # Correlate measurements to toolhead position at time of measurement data = [(sensor_freq, self._lookup_toolhead_pos(samp_time)) for samp_time, sensor_freq, sensor_z in measures] # Find best fit for extracted measurements - z_contact, coeffs = self._find_least_squares(data) + coeffs = self._find_least_squares(data) + z_contact, freq_contact, depress_slope, slope, slope2 = coeffs # Report probe position trig_idx = len(data)-1 while trig_idx > 0 and data[trig_idx-1][1][2] > z_contact: