From 9c7066da2ca03d4674d305927622641c72448c39 Mon Sep 17 00:00:00 2001 From: j-w9 <295470691+j-w9@users.noreply.github.com> Date: Sat, 24 Oct 2026 11:42:27 -0400 Subject: [PATCH 1/3] MAGFit: add GSF and GPS yaw attitude sources --- MAGFit/index.html | 4 +- MAGFit/magfit.js | 113 +++++++++++++++++++++++++++++++++++++++++++++- 2 files changed, 114 insertions(+), 3 deletions(-) diff --git a/MAGFit/index.html b/MAGFit/index.html index 4da50d67..bb1d86ab 100644 --- a/MAGFit/index.html +++ b/MAGFit/index.html @@ -91,10 +91,10 @@

-
+
Attitude source
diff --git a/MAGFit/magfit.js b/MAGFit/magfit.js index 7f312942..4ff9672e 100644 --- a/MAGFit/magfit.js +++ b/MAGFit/magfit.js @@ -1715,7 +1715,62 @@ function set_need_calc(b) { document.getElementById('SaveParams').disabled = b } -function add_attitude_source(quaternion, name) { +// Make a new attitude using the roll and pitch from quaternion and the yaw (deg) from another source. +// Only the times where yaw is available are used, gaps of more than 1 second in the yaw are skipped. +function replace_yaw(quaternion, yaw_time, yaw_deg) { + const yaw_len = yaw_time.length + if (yaw_len < 2) { + return + } + + // Unwrap so yaw can be interpolated + let yaw = new Array(yaw_len) + yaw[0] = yaw_deg[0] + for (let i = 1; i < yaw_len; i++) { + let delta = yaw_deg[i] - yaw_deg[i-1] + delta -= 360 * Math.round(delta / 360) + yaw[i] = yaw[i-1] + delta + } + + let ret = { time: [], q1: [], q2: [], q3: [], q4: [] } + let q = new Quaternion() + let yaw_index = 0 + for (let i = 0; i < quaternion.time.length; i++) { + const time = quaternion.time[i] + if ((time < yaw_time[0]) || (time > yaw_time[yaw_len - 1])) { + continue + } + while ((yaw_index < yaw_len - 2) && (yaw_time[yaw_index + 1] < time)) { + yaw_index++ + } + const dt = yaw_time[yaw_index + 1] - yaw_time[yaw_index] + if (dt > 1.0) { + continue + } + const ratio = (dt > 0) ? (time - yaw_time[yaw_index]) / dt : 0 + const new_yaw = yaw[yaw_index] + (yaw[yaw_index + 1] - yaw[yaw_index]) * ratio + + q.q1 = quaternion.q1[i] + q.q2 = quaternion.q2[i] + q.q3 = quaternion.q3[i] + q.q4 = quaternion.q4[i] + q.from_euler(q.get_euler_roll(), q.get_euler_pitch(), new_yaw * (Math.PI / 180)) + + ret.time.push(time) + ret.q1.push(q.q1) + ret.q2.push(q.q2) + ret.q3.push(q.q3) + ret.q4.push(q.q4) + } + + if (ret.time.length < 2) { + return + } + return ret +} + +// time_range is optional, if given the analysis time is limited to it when this source is selected +function add_attitude_source(quaternion, name, time_range) { // Add check box for this attitude source let section = document.getElementById("ATTITUDE") @@ -1730,6 +1785,15 @@ function add_attitude_source(quaternion, name) { radio.addEventListener('change', function() { set_need_calc(true) source = null + + if (time_range != null) { + // Limit analysis time to the period this source is valid for + let start = document.getElementById("TimeStart") + let end = document.getElementById("TimeEnd") + start.value = Math.min(Math.max(parseFloat(start.value), time_range[0]), time_range[1]) + end.value = Math.max(Math.min(parseFloat(end.value), time_range[1]), time_range[0]) + time_range_changed() + } }) let label = document.createElement("label") @@ -2064,6 +2128,53 @@ async function load(log_file) { field.select.checked = true } body_frame_earth_field.push(field) + + // EKF roll and pitch with yaw from the EKF's Gaussian sum filter (GSF) yaw estimator, this does not use the compass + if (("XKY0" in log.messageTypes) && ("instances" in log.messageTypes.XKY0) && (primary in log.messageTypes.XKY0.instances)) { + const time = TimeUS_to_seconds(log.get_instance("XKY0", primary, "TimeUS")) + const yaw = log.get_instance("XKY0", primary, "YC") + const yaw_sd = log.get_instance("XKY0", primary, "YCS") + + // Use the same accuracy threshold as the EKF does for a yaw reset (GSF_YAW_ACCURACY_THRESHOLD_DEG) + // YCS is logged as sqrt(degrees(variance)) so convert the threshold to match + const threshold = Math.sqrt((15.0 * (Math.PI / 180))**2 * (180 / Math.PI)) + let valid_time = [] + let valid_yaw = [] + for (let i = 0; i < time.length; i++) { + if (yaw_sd[i] < threshold) { + valid_time.push(time[i]) + valid_yaw.push(yaw[i]) + } + } + + const GSF_quaternion = replace_yaw(quaternion, valid_time, valid_yaw) + if (GSF_quaternion != null) { + const range = [GSF_quaternion.time[0], GSF_quaternion.time[GSF_quaternion.time.length - 1]] + body_frame_earth_field.push(add_attitude_source(GSF_quaternion, "EKF 3 IMU " + (primary + 1) + " GSF yaw", range)) + } + } + + // EKF roll and pitch with yaw from GPS (for example moving baseline), yaw is logged as 0 when not available + if (("GPS" in log.messageTypes) && ("instances" in log.messageTypes.GPS)) { + for (const inst of Object.keys(log.messageTypes.GPS.instances)) { + const time = TimeUS_to_seconds(log.get_instance("GPS", inst, "TimeUS")) + const yaw = log.get_instance("GPS", inst, "Yaw") + let valid_time = [] + let valid_yaw = [] + for (let i = 0; i < time.length; i++) { + if (yaw[i] != 0) { + valid_time.push(time[i]) + valid_yaw.push(yaw[i]) + } + } + + const GPS_quaternion = replace_yaw(quaternion, valid_time, valid_yaw) + if (GPS_quaternion != null) { + const range = [GPS_quaternion.time[0], GPS_quaternion.time[GPS_quaternion.time.length - 1]] + body_frame_earth_field.push(add_attitude_source(GPS_quaternion, "EKF 3 IMU " + (primary + 1) + " GPS " + (parseFloat(inst) + 1) + " yaw", range)) + } + } + } } } From e19a75669af1a7e449a738c04138de5d5bb83fba Mon Sep 17 00:00:00 2001 From: j-w9 <295470691+j-w9@users.noreply.github.com> Date: Sun, 8 Nov 2026 23:16:57 -0500 Subject: [PATCH 2/3] MAGFit: leave compass samples in yaw gaps out of the fit GSF and GPS yaw sources can have gaps and only cover part of the log. Compass samples in a gap or outside the source's time are now given zero weight in the fit, error and orientation check, rather than using an attitude interpolated across the gap. Other attitude sources have no gaps and are unchanged. --- MAGFit/magfit.js | 98 +++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 84 insertions(+), 14 deletions(-) diff --git a/MAGFit/magfit.js b/MAGFit/magfit.js index 4ff9672e..16adec5d 100644 --- a/MAGFit/magfit.js +++ b/MAGFit/magfit.js @@ -821,6 +821,11 @@ function calculate() { select_body_frame_attitude() + if (!have_valid_samples()) { + alert("The selected attitude source has no data in the analysis time") + return + } + calculate_bins() check_orientation() @@ -904,6 +909,14 @@ function get_yaw(mag_field, quaternion) { return yaw } +// Validity of each compass sample for the selected attitude source between the given indexes, null if all are valid +function get_valid(i, start_index, end_index) { + if (MAG_Data[i].valid == null) { + return null + } + return MAG_Data[i].valid.slice(start_index, end_index) +} + // Calculate error weights based on attitude binning const num_bins = 80 function calculate_bins() { @@ -957,7 +970,8 @@ function calculate_bins() { console.log(`Binning took: ${end - start} ms`); } -function get_weights(bins) { +// valid is optional, samples that are not valid get zero weight +function get_weights(bins, valid) { const count = new Array(num_bins).fill(0) @@ -965,6 +979,9 @@ function get_weights(bins) { let num_unique_bins = 0 let total_bins = 0 for (let i = 0; i < len; i++) { + if ((valid != null) && !valid[i]) { + continue + } if (count[bins[i]] == 0) { num_unique_bins++ } @@ -977,11 +994,15 @@ function get_weights(bins) { let weights = new Array(len).fill(1) for (let i = 0; i < len; i++) { + if ((valid != null) && !valid[i]) { + weights[i] = 0 + continue + } // Scale by mean_bin_size so that the average weight is 1, this give comparable error magnitude to the un-weighted case weights[i] = mean_bin_size / count[bins[i]] } - return { weights, coverage } + return { weights, coverage, num_valid: total_bins } } function check_orientation() { @@ -1003,20 +1024,25 @@ function check_orientation() { const num_samples = end_index - start_index // Get weighting based on bins - const weights = get_weights(MAG_Data[i].expected.bins.slice(start_index, end_index)).weights + const weight_obj = get_weights(MAG_Data[i].expected.bins.slice(start_index, end_index), get_valid(i, start_index, end_index)) + const weights = weight_obj.weights + const num_valid = weight_obj.num_valid // Calculate average earth filed to match sensor to let ef_mean = { x:0.0, y:0.0, z:0.0 } for (let j = 0; j < num_samples; j++) { const data_index = start_index + j + if (weights[j] == 0) { + continue + } ef_mean.x += MAG_Data[i].expected.x[data_index] ef_mean.y += MAG_Data[i].expected.y[data_index] ef_mean.z += MAG_Data[i].expected.z[data_index] } - ef_mean.x /= num_samples - ef_mean.y /= num_samples - ef_mean.z /= num_samples + ef_mean.x /= num_valid + ef_mean.y /= num_valid + ef_mean.z /= num_valid let rotation = new Quaternion() @@ -1056,13 +1082,16 @@ function check_orientation() { y[j] = tmp[1] z[j] = tmp[2] + if (weights[j] == 0) { + continue + } mean.x += x[j] mean.y += y[j] mean.z += z[j] } - mean.x /= num_samples - mean.y /= num_samples - mean.z /= num_samples + mean.x /= num_valid + mean.y /= num_valid + mean.z /= num_valid const offsets = { x: ef_mean.x - mean.x, @@ -1079,7 +1108,7 @@ function check_orientation() { (z[j] - MAG_Data[i].expected.z[data_index] + offsets.z)**2) * weights[j] } - rot_error.push({ rotation: rot, error: error_sum / num_samples }) + rot_error.push({ rotation: rot, error: error_sum / num_valid }) } @@ -1171,6 +1200,42 @@ function get_body_frame_ef(quaternion) { return ret } +// For attitude sources with gaps, a compass sample is valid if it is within the source time and not in a gap +// Returns null if the source has no gaps +function get_attitude_valid(quaternion, time) { + if (quaternion.max_gap == null) { + return null + } + const q_time = quaternion.time + const last = q_time.length - 1 + let valid = new Array(time.length).fill(false) + let k = 0 + for (let j = 0; j < time.length; j++) { + if ((time[j] < q_time[0]) || (time[j] > q_time[last])) { + continue + } + while ((k < last - 1) && (q_time[k + 1] < time[j])) { + k++ + } + valid[j] = (q_time[k + 1] - q_time[k]) <= quaternion.max_gap + } + return valid +} + +// True if any compass has valid samples in the analysis time +function have_valid_samples() { + for (let i = 0; i < 3; i++) { + if ((MAG_Data[i] == null) || (MAG_Data[i].valid == null)) { + continue + } + const valid = get_valid(i, find_start_index(MAG_Data[i].time), find_end_index(MAG_Data[i].time) + 1) + if (!valid.includes(true)) { + return false + } + } + return true +} + let source function select_body_frame_attitude() { @@ -1252,6 +1317,7 @@ function select_body_frame_attitude() { } MAG_Data[i].quaternion = array_slerp(source.quaternion, source.quaternion.time, MAG_Data[i].time) + MAG_Data[i].valid = get_attitude_valid(source.quaternion, MAG_Data[i].time) // Get yaw from quaternion for comparison later let quat = new Quaternion() @@ -1307,8 +1373,9 @@ function fit() { const num_samples = end_index - start_index // Get weighting based on bins - const weight_obj = get_weights(MAG_Data[i].expected.bins.slice(start_index, end_index)) + const weight_obj = get_weights(MAG_Data[i].expected.bins.slice(start_index, end_index), get_valid(i, start_index, end_index)) const weights = weight_obj.weights + const num_valid = weight_obj.num_valid const sqrt_weight = array_sqrt(weights) // Update coverage graphic @@ -1319,7 +1386,7 @@ function fit() { for (let j = 0; j < num_samples; j++) { error_sum += weights[j] * MAG_Data[i].orig.error[start_index + j]**2 } - MAG_Data[i].orig.mean_error = Math.sqrt(error_sum / num_samples) + MAG_Data[i].orig.mean_error = Math.sqrt(error_sum / num_valid) let rot = {} let orientation @@ -1526,7 +1593,7 @@ function fit() { for (let j = 0; j < num_samples; j++) { error_sum += weights[j] * ret.error[start_index + j]**2 } - ret.mean_error = Math.sqrt(error_sum / num_samples) + ret.mean_error = Math.sqrt(error_sum / num_valid) ret.yaw = get_yaw(ret, MAG_Data[i].quaternion) @@ -1715,6 +1782,7 @@ function set_need_calc(b) { document.getElementById('SaveParams').disabled = b } +const max_yaw_gap = 1.0 // Make a new attitude using the roll and pitch from quaternion and the yaw (deg) from another source. // Only the times where yaw is available are used, gaps of more than 1 second in the yaw are skipped. function replace_yaw(quaternion, yaw_time, yaw_deg) { @@ -1744,7 +1812,7 @@ function replace_yaw(quaternion, yaw_time, yaw_deg) { yaw_index++ } const dt = yaw_time[yaw_index + 1] - yaw_time[yaw_index] - if (dt > 1.0) { + if (dt > max_yaw_gap) { continue } const ratio = (dt > 0) ? (time - yaw_time[yaw_index]) / dt : 0 @@ -1766,6 +1834,8 @@ function replace_yaw(quaternion, yaw_time, yaw_deg) { if (ret.time.length < 2) { return } + // Compass samples in gaps longer than this are not used + ret.max_gap = max_yaw_gap return ret } From 4f4ae977dbad33e606dd3cefbd7926374a7628e7 Mon Sep 17 00:00:00 2001 From: j-w9 <295470691+j-w9@users.noreply.github.com> Date: Mon, 9 Nov 2026 07:28:09 -0500 Subject: [PATCH 3/3] MAGFit: leave a gap in the error and yaw plots where the attitude source has no data --- MAGFit/magfit.js | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/MAGFit/magfit.js b/MAGFit/magfit.js index 16adec5d..53b8477a 100644 --- a/MAGFit/magfit.js +++ b/MAGFit/magfit.js @@ -422,6 +422,15 @@ function redraw() { const name = "Mag " + (i + 1) MAG_Data[i].param_selection = [] + // Leave a gap in the plot where the attitude source has no data + const valid = MAG_Data[i].valid + function mask_invalid(values) { + if ((valid == null) || !Array.isArray(values)) { + return values + } + return values.map((v, j) => valid[j] ? v : null) + } + function setup_plot(data, group_index, fit_name) { const show = data.show.checked const index = mag_plot.x.data.length @@ -452,7 +461,7 @@ function redraw() { legendgrouptitle: { text: group_name }, hovertemplate: gauss_hovertemplate, x: MAG_Data[i].time, - y: data.error + y: mask_invalid(data.error) }) // Calculate yaw change from existing calibration @@ -479,7 +488,7 @@ function redraw() { hoverinfo : existing ? "none" : "all", hovertemplate: existing ? "" : yaw_change_hover, x: MAG_Data[i].time, - y: mag_yaw + y: mask_invalid(mag_yaw) }) let att_yaw = 0 @@ -496,7 +505,7 @@ function redraw() { legendgrouptitle: { text: group_name }, hovertemplate: yaw_change_hover, x: MAG_Data[i].time, - y: att_yaw + y: mask_invalid(att_yaw) }) // Calculate field length @@ -1222,7 +1231,7 @@ function get_attitude_valid(quaternion, time) { return valid } -// True if any compass has valid samples in the analysis time +// True if every compass with a validity mask has valid samples in the analysis time function have_valid_samples() { for (let i = 0; i < 3; i++) { if ((MAG_Data[i] == null) || (MAG_Data[i].valid == null)) {