jess
/
EIS-BLE-S3
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add linear/circle Nyquist fit resolver with cumulative turning detection

This commit is contained in:
jess 2026-04-02 17:13:35 -07:00
parent 9e9410a78e
commit 6a09782d30
1 changed files with 116 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

144
cue/src/plot.rs
View File

@ -153,15 +153,60 @@ fn kasa_fit(pts: &[(f64, f64)]) -> Option<(f64, f64, f64)> {
Some((cx, cy, r_sq.sqrt())) Some((cx, cy, r_sq.sqrt()))
} }
/// Fit the dominant Nyquist semicircle, trimming first-arc points from the fn cumulative_turning(pts: &[(f64, f64)]) -> f64 {
/// low-frequency end before falling back to outlier removal within each subset. if pts.len() < 3 { return 0.0; }
fn fit_nyquist_circle(points: &[EisPoint]) -> Option<CircleFit> { let mut total = 0.0;
for i in 1..pts.len() - 1 {
let (dx1, dy1) = (pts[i].0 - pts[i-1].0, pts[i].1 - pts[i-1].1);
let (dx2, dy2) = (pts[i+1].0 - pts[i].0, pts[i+1].1 - pts[i].1);
let cross = dx1 * dy2 - dy1 * dx2;
let dot = dx1 * dx2 + dy1 * dy2;
total += cross.atan2(dot).abs();
}
total
}
struct LinearFit {
slope: f32,
y_intercept: f32,
rs: f32,
}
fn fit_linear(pts: &[(f64, f64)]) -> Option<LinearFit> {
if pts.len() < 2 { return None; }
let n = pts.len() as f64;
let sx: f64 = pts.iter().map(|p| p.0).sum();
let sy: f64 = pts.iter().map(|p| p.1).sum();
let sx2: f64 = pts.iter().map(|p| p.0 * p.0).sum();
let sxy: f64 = pts.iter().map(|p| p.0 * p.1).sum();
let denom = n * sx2 - sx * sx;
if denom.abs() < 1e-20 { return None; }
let slope = (n * sxy - sx * sy) / denom;
let y_int = (sy - slope * sx) / n;
let rs = if slope.abs() > 1e-10 { -y_int / slope } else { sx / n };
Some(LinearFit {
slope: slope as f32,
y_intercept: y_int as f32,
rs: rs as f32,
})
}
enum NyquistFit {
Circle(CircleFit),
Linear(LinearFit),
}
fn fit_nyquist(points: &[EisPoint]) -> Option<NyquistFit> {
let all: Vec<(f64, f64)> = points.iter() let all: Vec<(f64, f64)> = points.iter()
.filter(|p| p.z_real.is_finite() && p.z_imag.is_finite()) .filter(|p| p.z_real.is_finite() && p.z_imag.is_finite())
.map(|p| (p.z_real as f64, -p.z_imag as f64)) .map(|p| (p.z_real as f64, -p.z_imag as f64))
.collect(); .collect();
if all.len() < 4 { return None; } if all.len() < 4 { return None; }
if cumulative_turning(&all) < 0.524 {
return fit_linear(&all).map(NyquistFit::Linear);
}
let min_pts = 4.max(all.len() / 3); let min_pts = 4.max(all.len() / 3);
let mut best: Option<CircleFit> = None; let mut best: Option<CircleFit> = None;
let mut best_score = f64::MAX; let mut best_score = f64::MAX;
@ -210,7 +255,17 @@ fn fit_nyquist_circle(points: &[EisPoint]) -> Option<CircleFit> {
} }
} }
} }
best
if let Some(circle) = best {
if best_score > 0.15 {
if let Some(lin) = fit_linear(&all) {
return Some(NyquistFit::Linear(lin));
}
}
Some(NyquistFit::Circle(circle))
} else {
fit_linear(&all).map(NyquistFit::Linear)
}
} }
/* ---- Bode ---- */ /* ---- Bode ---- */
@ -676,33 +731,66 @@ impl<'a> canvas::Program<Message> for NyquistPlot<'a> {
draw_polyline(&mut frame, &pts, COL_NYQ, 2.0); draw_polyline(&mut frame, &pts, COL_NYQ, 2.0);
draw_dots(&mut frame, &pts, COL_NYQ, 3.0); draw_dots(&mut frame, &pts, COL_NYQ, 3.0);
if let Some(fit) = fit_nyquist_circle(self.points) { match fit_nyquist(self.points) {
let theta_r = (-fit.cy).atan2((fit.r * fit.r - fit.cy * fit.cy).sqrt()); Some(NyquistFit::Circle(fit)) => {
let mut theta_l = (-fit.cy).atan2(-(fit.r * fit.r - fit.cy * fit.cy).sqrt()); let theta_r = (-fit.cy).atan2((fit.r * fit.r - fit.cy * fit.cy).sqrt());
if theta_l < theta_r { theta_l += std::f32::consts::TAU; } let mut theta_l = (-fit.cy).atan2(-(fit.r * fit.r - fit.cy * fit.cy).sqrt());
if theta_l < theta_r { theta_l += std::f32::consts::TAU; }
let n_arc = 120; let n_arc = 120;
let arc_pts: Vec<Point> = (0..=n_arc).map(|i| { let arc_pts: Vec<Point> = (0..=n_arc).map(|i| {
let t = theta_r + (theta_l - theta_r) * i as f32 / n_arc as f32; let t = theta_r + (theta_l - theta_r) * i as f32 / n_arc as f32;
let x = fit.cx + fit.r * t.cos(); let x = fit.cx + fit.r * t.cos();
let y = fit.cy + fit.r * t.sin(); let y = fit.cy + fit.r * t.sin();
Point::new( Point::new(
lerp(x, xv.lo, xv.hi, xl, xr), lerp(x, xv.lo, xv.hi, xl, xr),
lerp(y, yv.hi, yv.lo, yt, yb), lerp(y, yv.hi, yv.lo, yt, yb),
) )
}).collect(); }).collect();
draw_polyline(&mut frame, &arc_pts, COL_FIT, 1.5); draw_polyline(&mut frame, &arc_pts, COL_FIT, 1.5);
let y0_scr = lerp(0.0, yv.hi, yv.lo, yt, yb); let y0_scr = lerp(0.0, yv.hi, yv.lo, yt, yb);
let rs_scr = Point::new(lerp(fit.rs, xv.lo, xv.hi, xl, xr), y0_scr); let rs_scr = Point::new(lerp(fit.rs, xv.lo, xv.hi, xl, xr), y0_scr);
let rp_scr = Point::new(lerp(fit.rs + fit.rp, xv.lo, xv.hi, xl, xr), y0_scr); let rp_scr = Point::new(lerp(fit.rs + fit.rp, xv.lo, xv.hi, xl, xr), y0_scr);
frame.fill(&Path::circle(rs_scr, 5.0), COL_FIT_PT); frame.fill(&Path::circle(rs_scr, 5.0), COL_FIT_PT);
frame.fill(&Path::circle(rp_scr, 5.0), COL_FIT_PT); frame.fill(&Path::circle(rp_scr, 5.0), COL_FIT_PT);
dt(&mut frame, Point::new(rs_scr.x, rs_scr.y + 6.0), dt(&mut frame, Point::new(rs_scr.x, rs_scr.y + 6.0),
&format!("Rs={:.0}", fit.rs), COL_FIT_PT, 10.0); &format!("Rs={:.0}", fit.rs), COL_FIT_PT, 10.0);
dt(&mut frame, Point::new(rp_scr.x - 30.0, rp_scr.y + 6.0), dt(&mut frame, Point::new(rp_scr.x - 30.0, rp_scr.y + 6.0),
&format!("Rp={:.0}", fit.rp), COL_FIT_PT, 10.0); &format!("Rp={:.0}", fit.rp), COL_FIT_PT, 10.0);
}
Some(NyquistFit::Linear(fit)) => {
let x_min = self.points.iter()
.filter(|p| p.z_real.is_finite())
.map(|p| p.z_real)
.fold(f32::INFINITY, f32::min);
let x_max = self.points.iter()
.filter(|p| p.z_real.is_finite())
.map(|p| p.z_real)
.fold(f32::NEG_INFINITY, f32::max);
let pad = (x_max - x_min) * 0.1;
let x0 = x_min - pad;
let x1 = x_max + pad;
let y0 = fit.slope * x0 + fit.y_intercept;
let y1 = fit.slope * x1 + fit.y_intercept;
let p0 = Point::new(
lerp(x0, xv.lo, xv.hi, xl, xr),
lerp(y0, yv.hi, yv.lo, yt, yb),
);
let p1 = Point::new(
lerp(x1, xv.lo, xv.hi, xl, xr),
lerp(y1, yv.hi, yv.lo, yt, yb),
);
dl(&mut frame, p0, p1, COL_FIT, 1.5);
let y0_scr = lerp(0.0, yv.hi, yv.lo, yt, yb);
let rs_scr = Point::new(lerp(fit.rs, xv.lo, xv.hi, xl, xr), y0_scr);
frame.fill(&Path::circle(rs_scr, 5.0), COL_FIT_PT);
dt(&mut frame, Point::new(rs_scr.x, rs_scr.y + 6.0),
&format!("Rs={:.0}", fit.rs), COL_FIT_PT, 10.0);
}
None => {}
} }
if let Some(pos) = cursor.position_in(bounds) { if let Some(pos) = cursor.position_in(bounds) {