MAGFit: add GSF and GPS yaw attitude sources

MAGFit/index.html

@@ -91,10 +91,10 @@ <h1 style="text-align:center"><a href="" style="color: #000000; text-decoration:
         </td>
         <td style="width: 10px;"></td>
         <td>
-            <fieldset style="width:200px;height:80px" id="ATTITUDE">
+            <fieldset style="width:200px;min-height:80px" id="ATTITUDE">
                 <legend>Attitude source
                     <img id="TT" src="../images/question-circle.svg" style="width: 1em; vertical-align: bottom" 
-                    data-tippy-content='Select the attitude source that will be used for the MAGFit, this should best represent the true attitude of the vehicle, typically the default is fine, if you know there were EKF issues using DCM may give better results. The Calculate button must be clicked after any change of source.'
+                    data-tippy-content='Select the attitude source that will be used for the MAGFit, this should best represent the true attitude of the vehicle, typically the default is fine, if you know there were EKF issues using DCM may give better results. GSF yaw and GPS yaw use the EKF roll and pitch with yaw from the EKF&#39;s GSF yaw estimator or from a GPS that provides yaw, these do not use the compass so can be used if a bad compass has upset the EKF yaw. They are only available for part of the flight, selecting one limits the analysis time to that part. The Calculate button must be clicked after any change of source.'
                     data-tippy-maxWidth='750px'/>
                 </legend>
             </fieldset>

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,14 +979,18 @@ 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)
 
     const len = bins.length
     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))
+                    }
+                }
+            }
         }
     }