diff --git a/photon-client/src/components/dashboard/tabs/TargetsTab.vue b/photon-client/src/components/dashboard/tabs/TargetsTab.vue
index 11f20f0810..ff624fb77e 100644
--- a/photon-client/src/components/dashboard/tabs/TargetsTab.vue
+++ b/photon-client/src/components/dashboard/tabs/TargetsTab.vue
@@ -3,12 +3,32 @@ import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
import { PipelineType } from "@/types/PipelineTypes";
import { useStateStore } from "@/stores/StateStore";
+const wrapToPi = (delta: number): number => {
+ let ret = delta;
+ while (ret < -Math.PI) ret += Math.PI * 2;
+ while (ret > Math.PI) ret -= Math.PI * 2;
+ return ret;
+};
+
const calculateStdDev = (values: number[]): number => {
if (values.length < 2) return 0;
- const mean = values.reduce((sum, number) => sum + number, 0) / values.length;
+ // Use mean of cosine/sine components to handle angle wrapping
+ const cosines = values.map((it) => Math.cos(it));
+ const sines = values.map((it) => Math.sin(it));
+ const cosmean = cosines.reduce((sum, number) => sum + number, 0) / values.length;
+ const sinmean = sines.reduce((sum, number) => sum + number, 0) / values.length;
+
+ // Borrowed from WPILib's Rotation2d
+ const hypot = Math.hypot(cosmean, sinmean);
+ let mean;
+ if (hypot > 1e-6) {
+ mean = Math.atan2(sinmean / hypot, cosmean / hypot);
+ } else {
+ mean = 0;
+ }
- return Math.sqrt(values.map((x) => Math.pow(x - mean, 2)).reduce((a, b) => a + b) / values.length);
+ return Math.sqrt(values.map((x) => Math.pow(wrapToPi(x - mean), 2)).reduce((a, b) => a + b) / values.length);
};
const resetCurrentBuffer = () => {
// Need to clear the array in place
@@ -119,7 +139,7 @@ const resetCurrentBuffer = () => {
{{
(
- useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_x *
+ (useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_x || 0) *
(180.0 / Math.PI)
).toFixed(2)
}}°
@@ -127,7 +147,7 @@ const resetCurrentBuffer = () => {
|
{{
(
- useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_y *
+ (useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_y || 0) *
(180.0 / Math.PI)
).toFixed(2)
}}°
@@ -135,7 +155,7 @@ const resetCurrentBuffer = () => {
|
{{
(
- useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z *
+ (useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z || 0) *
(180.0 / Math.PI)
).toFixed(2)
}}°
@@ -146,8 +166,8 @@ const resetCurrentBuffer = () => {
Multi-tag pose standard deviation over the last {{ useStateStore().currentMultitagBuffer.length }}/100
- samples
+ >Multi-tag pose standard deviation over the last
+ {{ useStateStore().currentMultitagBuffer?.length || "NaN" }}/100 samples
Reset Samples {
{{
- calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.x)).toFixed(5)
+ calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.x) || []).toFixed(5)
}} m
|
{{
- calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.y)).toFixed(5)
+ calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.y) || []).toFixed(5)
}} m
|
{{
- calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.z)).toFixed(5)
+ calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.z) || []).toFixed(5)
}} m
|
{{
calculateStdDev(
- useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_x * (180.0 / Math.PI))
+ useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_x * (180.0 / Math.PI)) || []
).toFixed(5)
}}°
|
{{
calculateStdDev(
- useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_y * (180.0 / Math.PI))
+ useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_y * (180.0 / Math.PI)) || []
).toFixed(5)
}}°
|
{{
calculateStdDev(
- useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_z * (180.0 / Math.PI))
+ useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_z * (180.0 / Math.PI)) || []
).toFixed(5)
}}°
|
|