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..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 @@ -821,6 +830,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 +918,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 +979,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 +988,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 +1003,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 +1033,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 +1091,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 +1117,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 +1209,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 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)) { + 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 +1326,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 +1382,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 +1395,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 +1602,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,7 +1791,65 @@ function set_need_calc(b) { document.getElementById('SaveParams').disabled = b } -function add_attitude_source(quaternion, name) { +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) { + 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 > max_yaw_gap) { + 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 + } + // Compass samples in gaps longer than this are not used + ret.max_gap = max_yaw_gap + 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 +1864,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 +2207,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)) + } + } + } } }