Sitting mounting resets

gui/public/i18n/en/translation.ftl

@@ -197,6 +197,7 @@ widget-imu_visualizer-rotation_raw = Raw rotation
 widget-imu_visualizer-rotation_preview = Preview rotation
 widget-imu_visualizer-acceleration = Acceleration
 widget-imu_visualizer-position = Position
+widget-imu_visualizer-stay_aligned = Stay Aligned
 
 ## Widget: Skeleton Visualizer
 widget-skeleton_visualizer-preview = Skeleton preview
@@ -221,6 +222,7 @@ tracker-table-column-temperature = Temp. °C
 tracker-table-column-linear-acceleration = Accel. X/Y/Z
 tracker-table-column-rotation = Rotation X/Y/Z
 tracker-table-column-position = Position X/Y/Z
+tracker-table-column-stay_aligned = Stay Aligned
 tracker-table-column-url = URL
 
 ## Tracker rotation
@@ -350,6 +352,7 @@ mounting_selection_menu-close = Close
 settings-sidebar-title = Settings
 settings-sidebar-general = General
 settings-sidebar-tracker_mechanics = Tracker mechanics
+settings-sidebar-stay_aligned = Stay Aligned
 settings-sidebar-fk_settings = Tracking settings
 settings-sidebar-gesture_control = Gesture control
 settings-sidebar-interface = Interface
@@ -440,6 +443,27 @@ settings-general-tracker_mechanics-use_mag_on_all_trackers-description =
     Can be disabled per tracker in the tracker's settings. <b>Please don't shutdown any of the trackers while toggling this!</b>
 settings-general-tracker_mechanics-use_mag_on_all_trackers-label = Use magnetometer on trackers
 
+settings-stay_aligned = Stay Aligned
+settings-stay_aligned-description = Keeps your trackers aligned by slowly adjusting the yaw of your trackers to correct for drift.
+settings-stay_aligned-warnings-drift_compensation = ⚠ Please disable "Drift Compensation". Stay Aligned and Drift Compensation try to solve the same problem and only one should be enabled.
+settings-stay_aligned-enabled-label = Enabled
+settings-stay_aligned-extra_yaw_correction-label = Extra correction for low quality IMUs (e.g. BMI160, MPU60XX)
+settings-stay_aligned-hide_yaw_correction-label = Hide correction (for comparison)
+settings-stay_aligned-relaxed_poses-label = Relaxed Poses
+settings-stay_aligned-relaxed_poses-description = Stay Aligned needs to know the positions of your trackers when you are relaxed. Use "Setup Stay Aligned" to configure.
+settings-stay_aligned-relaxed_poses-standing = Standing
+settings-stay_aligned-relaxed_poses-sitting = Sitting in chair
+settings-stay_aligned-relaxed_poses-flat = Sitting on floor
+settings-stay_aligned-relaxed_poses-current_angles = Current angles
+settings-stay_aligned-relaxed_poses-upper_leg_angle = Thigh
+settings-stay_aligned-relaxed_poses-lower_leg_angle = Ankle
+settings-stay_aligned-relaxed_poses-foot_angle = Foot
+settings-stay_aligned-relaxed_poses-auto_detect = Detect angles
+settings-stay_aligned-relaxed_poses-reset = Reset angles
+settings-stay_aligned-relaxed_poses-outwards = {$angle} outwards
+settings-stay_aligned-relaxed_poses-inwards = {$angle} inwards
+settings-stay_aligned-setup-label = Setup Stay Aligned
+
 ## FK/Tracking settings
 settings-general-fk_settings = Tracking settings
 
@@ -957,15 +981,30 @@ onboarding-automatic_mounting-prev_step = Previous step
 onboarding-automatic_mounting-done-title = Mounting orientations calibrated.
 onboarding-automatic_mounting-done-description = Your mounting calibration is complete!
 onboarding-automatic_mounting-done-restart = Try again
-onboarding-automatic_mounting-mounting_reset-title = Mounting Reset
-onboarding-automatic_mounting-mounting_reset-step-0 = 1. Squat in a "skiing" pose with your legs bent, your upper body tilted forwards, and your arms bent.
-onboarding-automatic_mounting-mounting_reset-step-1 = 2. Press the "Reset Mounting" button and wait for 3 seconds before the trackers' mounting orientations will reset.
+onboarding-automatic_mounting-mounting_reset-title = Sitting Reset
+onboarding-automatic_mounting-mounting_reset-step-0 = 1. Without moving your feet, sit back down
+onboarding-automatic_mounting-mounting_reset-step-1 = 2. Keep your thighs parallel, and legs straight down.
+onboarding-automatic_mounting-mounting_reset-step-2 = 3. Lean backwards so that your upper body is slanted.
+onboarding-automatic_mounting-mounting_reset-step-3 = 4. If you have arm trackers, position your arms in the mounting reset pose. (You may need to choose a different arm pose in settings, e.g. "Forward".)
 onboarding-automatic_mounting-preparation-title = Preparation
-onboarding-automatic_mounting-preparation-step-0 = 1. Stand upright with your arms to your sides.
-onboarding-automatic_mounting-preparation-step-1 = 2. Press the "Full Reset" button and wait for 3 seconds before the trackers will reset.
+onboarding-automatic_mounting-preparation-step-0 = 1. Sit on a chair or couch.
+onboarding-automatic_mounting-preparation-step-1 = 2. Adjust your thighs to be parallel.
+onboarding-automatic_mounting-preparation-step-2 = 3. Adjust your legs to point straight down.
+onboarding-automatic_mounting-preparation-step-3 = 4. Position your feet comfortably. They can point inwards or outwards if necessary. Do not force them to point forward.
+onboarding-automatic_mounting-standing_reset-title = Standing Reset
+onboarding-automatic_mounting-standing_reset-step-0 = 1. Without moving your feet, stand up straight.
+onboarding-automatic_mounting-standing_reset-step-1 = 2. You can use the edge of your chair to help stand straight.
+onboarding-automatic_mounting-standing_reset-step-2 = 3. If you have arm trackers, position your arms in the full reset pose.
 onboarding-automatic_mounting-put_trackers_on-title = Put on your trackers
 onboarding-automatic_mounting-put_trackers_on-description = To calibrate mounting orientations, we're gonna use the trackers you just assigned. Put on all your trackers, you can see which are which in the figure to the right.
 onboarding-automatic_mounting-put_trackers_on-next = I have all my trackers on
+onboarding-automatic_mounting-preparation_feet-title = Lower Leg Reset (1 of 2)
+onboarding-automatic_mounting-preparation_feet-step-0 = 1. Rotate your feet so that they point forwards.
+onboarding-automatic_mounting-preparation_feet-step-1 = 2. Keep your thighs parallel, and legs straight down.
+onboarding-automatic_mounting-mounting_reset_feet-title = Lower Leg Reset (2 of 2)
+onboarding-automatic_mounting-mounting_reset_feet-step-0 = 1. Slide your feet forward, then lift your feet up slightly.
+onboarding-automatic_mounting-mounting_reset_feet-step-1 = 2. Do not angle your feet inwards or outwards.
+
 
 ## Tracker manual proportions setupa
 onboarding-manual_proportions-back = Go Back to Reset tutorial
@@ -1089,6 +1128,23 @@ onboarding-scaled_proportions-reset_proportion-description = To set your body pr
 onboarding-scaled_proportions-done-title = Body proportions set
 onboarding-scaled_proportions-done-description = Your body proportions should now be configured based on your height.
 
+## Stay Aligned setup
+onboarding-stay_aligned-title = Stay Aligned
+onboarding-stay_aligned-description = Configure Stay Aligned to keep your trackers aligned.
+onboarding-stay_aligned-done-title = Stay Aligned enabled!
+onboarding-stay_aligned-done-description = Your Stay Aligned setup is complete!
+onboarding-stay_aligned-done-restart = Try again
+onboarding-stay_aligned-done-done = Done
+onboarding-stay_aligned-relaxed_poses-standing-title = Relaxed Standing Pose
+onboarding-stay_aligned-relaxed_poses-standing-step-0 = 1. Stand in a comfortable position. Relax!
+onboarding-stay_aligned-relaxed_poses-standing-step-1 = 2. Check that your trackers match your body. If it does not match, you need to restart this flow.
+onboarding-stay_aligned-relaxed_poses-sitting-title = Relaxed Sitting Pose
+onboarding-stay_aligned-relaxed_poses-sitting-step-0 = 1. Sit in a comfortable position. Relax!
+onboarding-stay_aligned-relaxed_poses-sitting-step-1 = 2. Check that your trackers match your body. If it does not match, you need to restart this flow.
+onboarding-stay_aligned-relaxed_poses-flat-title = Relaxed Lying Pose
+onboarding-stay_aligned-relaxed_poses-flat-step-0 = 1. Sit on the floor with your legs in front. Relax!
+onboarding-stay_aligned-relaxed_poses-flat-step-1 = 2. Check that your trackers match your body. If it does not match, you need to restart this flow.
+
 ## Home
 home-no_trackers = No trackers detected or assigned
 

gui/src/App.tsx

@@ -58,6 +58,7 @@ import { ScaledProportionsPage } from './components/onboarding/pages/body-propor
 import { AdvancedSettings } from './components/settings/pages/AdvancedSettings';
 import { FirmwareUpdate } from './components/firmware-update/FirmwareUpdate';
 import { ConnectionLost } from './components/onboarding/pages/ConnectionLost';
+import { StayAlignedSetup } from './components/onboarding/pages/stay-aligned/StayAlignedSetup';
 
 export const GH_REPO = 'SlimeVR/SlimeVR-Server';
 export const VersionContext = createContext('');
@@ -161,6 +162,7 @@ function Layout() {
               path="body-proportions/scaled"
               element={<ScaledProportionsPage />}
             />
+            <Route path="stay-aligned" element={<StayAlignedSetup />} />
             <Route path="done" element={<DonePage />} />
           </Route>
           <Route path="*" element={<TopBar></TopBar>}></Route>

gui/src/components/Navbar.tsx

@@ -121,7 +121,7 @@ export function Navbar() {
       <NavButton
         to="/settings/trackers"
         match="/settings/*"
-        state={{ scrollTo: 'steamvr' }}
+        state={{ scrollTo: 'stayaligned' /* FIXME: DO NOT MERGE */ }}
         icon={<GearIcon></GearIcon>}
       >
         {l10n.getString('navbar-settings')}

gui/src/components/commons/NumberSelector.tsx

@@ -83,7 +83,7 @@ export function NumberSelector({
                 -
               </Button>
             </div>
-            <div className="flex flex-grow justify-center items-center w-10 text-standard">
+            <div className="flex flex-grow justify-center text-center items-center w-10 text-standard">
               {valueLabelFormat ? valueLabelFormat(value) : value}
             </div>
             <div className="flex gap-1">

gui/src/components/home/ResetButton.tsx

@@ -1,6 +1,8 @@
 import { useLocalization } from '@fluent/react';
 import { useEffect, useMemo, useRef, useState } from 'react';
 import {
+  BodyPart,
+  ResetBodyPose,
   ResetRequestT,
   ResetType,
   RpcMessage,
@@ -25,10 +27,16 @@ import classNames from 'classnames';
 
 export function ResetButton({
   type,
+  bodyPose,
+  referenceTrackerPosition,
+  trackerPositions,
   variant = 'big',
   onReseted,
 }: {
   type: ResetType;
+  bodyPose?: ResetBodyPose;
+  referenceTrackerPosition?: BodyPart;
+  trackerPositions?: BodyPart[];
   variant: 'big' | 'small';
   onReseted?: () => void;
 }) {
@@ -51,6 +59,9 @@ export function ResetButton({
   const reset = () => {
     const req = new ResetRequestT();
     req.resetType = type;
+    req.bodyPose = bodyPose || ResetBodyPose.SKIING;
+    req.referenceTracker = referenceTrackerPosition || BodyPart.HEAD;
+    req.trackers = trackerPositions || [];
     sendRPCPacket(RpcMessage.ResetRequest, req);
   };
 

gui/src/components/onboarding/pages/mounting/AutomaticMounting.tsx

@@ -6,19 +6,37 @@ import { MountingResetStep } from './mounting-steps/MountingReset';
 import { PreparationStep } from './mounting-steps/Preparation';
 import { PutTrackersOnStep } from './mounting-steps/PutTrackersOn';
 import { useLocalization } from '@fluent/react';
+import { PreparationStep as SittingModePreparationStep } from './sitting-mounting-steps/Preparation';
+import { SittingLegsRaisedResetStep as SittingModeSittingLegsRaisedResetStep } from './sitting-mounting-steps/SittingLegsRaisedReset';
+import { SittingLegsTogetherResetStep as SittingModeSittingLegsTogetherResetStep } from './sitting-mounting-steps/SittingLegsTogetherReset';
+import { SittingResetStep as SittingModeSittingResetStep } from './sitting-mounting-steps/SittingReset';
+import { StandingResetStep as SittingModeStandingResetStep } from './sitting-mounting-steps/StandingReset';
 
-const steps: Step[] = [
+// Auto mounting steps that can be included within other flows
+export const skiingMountingSteps: Step[] = [
   { type: 'numbered', component: PutTrackersOnStep },
   { type: 'numbered', component: PreparationStep },
   { type: 'numbered', component: MountingResetStep },
-  { type: 'fullsize', component: DoneStep },
 ];
+
+export const sittingMountingSteps: Step[] = [
+  { type: 'numbered', component: SittingModePreparationStep },
+  { type: 'numbered', component: SittingModeStandingResetStep },
+  { type: 'numbered', component: SittingModeSittingResetStep },
+  { type: 'numbered', component: SittingModeSittingLegsTogetherResetStep },
+  { type: 'numbered', component: SittingModeSittingLegsRaisedResetStep },
+];
+
+const doneSteps: Step[] = [{ type: 'fullsize', component: DoneStep }];
+
 export function AutomaticMountingPage() {
   const { l10n } = useLocalization();
   const { applyProgress, state } = useOnboarding();
 
   applyProgress(0.7);
 
+  const steps = [...sittingMountingSteps, ...doneSteps];
+
   return (
     <>
       <div className="flex flex-col gap-2 h-full items-center w-full xs:justify-center relative overflow-y-auto overflow-x-hidden px-4 pb-4">

gui/src/components/onboarding/pages/mounting/sitting-mounting-steps/Preparation.tsx

@@ -0,0 +1,49 @@
+import { Button } from '@/components/commons/Button';
+import { Typography } from '@/components/commons/Typography';
+import { useLocalization } from '@fluent/react';
+
+export function PreparationStep({
+  nextStep,
+}: {
+  nextStep: () => void;
+  prevStep: () => void;
+  variant: 'onboarding' | 'alone';
+}) {
+  const { l10n } = useLocalization();
+
+  return (
+    <div className="flex mobile:flex-col">
+      <div className="flex flex-grow flex-col gap-4 max-w-sm">
+        <Typography variant="main-title" bold>
+          {l10n.getString('onboarding-automatic_mounting-preparation-title')}
+        </Typography>
+        <div className="flex flex-col gap-2">
+          <Typography color="secondary">
+            {l10n.getString('onboarding-automatic_mounting-preparation-step-0')}
+          </Typography>
+          <Typography color="secondary">
+            {l10n.getString('onboarding-automatic_mounting-preparation-step-1')}
+          </Typography>
+          <Typography color="secondary">
+            {l10n.getString('onboarding-automatic_mounting-preparation-step-2')}
+          </Typography>
+          <Typography color="secondary">
+            {l10n.getString('onboarding-automatic_mounting-preparation-step-3')}
+          </Typography>
+        </div>
+        <div className="flex gap-3 mobile:justify-between">
+          <Button variant="primary" onClick={nextStep}>
+            {l10n.getString('onboarding-automatic_mounting-next')}
+          </Button>
+        </div>
+      </div>
+      <div className="flex flex-col pt-1 items-center fill-background-50 justify-center px-12">
+        <img
+          src="/images/reset-sitting-pose.webp"
+          width={200}
+          alt="Reset position"
+        />
+      </div>
+    </div>
+  );
+}

gui/src/components/onboarding/pages/mounting/sitting-mounting-steps/SittingLegsRaisedReset.tsx

@@ -0,0 +1,69 @@
+import { BodyPart, ResetBodyPose, ResetType } from 'solarxr-protocol';
+import { Button } from '@/components/commons/Button';
+import { Typography } from '@/components/commons/Typography';
+import { ResetButton } from '@/components/home/ResetButton';
+import { useLocalization } from '@fluent/react';
+
+export function SittingLegsRaisedResetStep({
+  nextStep,
+  prevStep,
+  variant,
+}: {
+  nextStep: () => void;
+  prevStep: () => void;
+  variant: 'onboarding' | 'alone';
+}) {
+  const { l10n } = useLocalization();
+
+  return (
+    <div className="flex mobile:flex-col">
+      <div className="flex flex-grow flex-col gap-4 max-w-sm">
+        <Typography variant="main-title" bold>
+          {l10n.getString(
+            'onboarding-automatic_mounting-mounting_reset_feet-title'
+          )}
+        </Typography>
+        <div className="flex flex-col gap-2">
+          <Typography color="secondary">
+            {l10n.getString(
+              'onboarding-automatic_mounting-mounting_reset_feet-step-0'
+            )}
+          </Typography>
+          <Typography color="secondary">
+            {l10n.getString(
+              'onboarding-automatic_mounting-mounting_reset_feet-step-1'
+            )}
+          </Typography>
+        </div>
+        <div className="flex gap-3 mobile:justify-between">
+          <Button
+            variant={variant === 'onboarding' ? 'secondary' : 'tertiary'}
+            onClick={prevStep}
+          >
+            {l10n.getString('onboarding-automatic_mounting-prev_step')}
+          </Button>
+          <ResetButton
+            variant="small"
+            type={ResetType.Mounting}
+            bodyPose={ResetBodyPose.SITTING_LEANING_BACK}
+            referenceTrackerPosition={BodyPart.CHEST}
+            trackerPositions={[
+              BodyPart.LEFT_LOWER_LEG,
+              BodyPart.RIGHT_LOWER_LEG,
+              BodyPart.LEFT_FOOT,
+              BodyPart.RIGHT_FOOT,
+            ]}
+            onReseted={nextStep}
+          ></ResetButton>
+        </div>
+      </div>
+      <div className="flex flex-col pt-1 items-center fill-background-50 justify-center px-12">
+        <img
+          src="/images/reset-sitting-legs-up-pose.webp"
+          width={200}
+          alt="Reset position"
+        />
+      </div>
+    </div>
+  );
+}

gui/src/components/onboarding/pages/mounting/sitting-mounting-steps/SittingLegsTogetherReset.tsx

@@ -0,0 +1,69 @@
+import { BodyPart, ResetBodyPose, ResetType } from 'solarxr-protocol';
+import { Button } from '@/components/commons/Button';
+import { Typography } from '@/components/commons/Typography';
+import { ResetButton } from '@/components/home/ResetButton';
+import { useLocalization } from '@fluent/react';
+
+export function SittingLegsTogetherResetStep({
+  nextStep,
+  prevStep,
+  variant,
+}: {
+  nextStep: () => void;
+  prevStep: () => void;
+  variant: 'onboarding' | 'alone';
+}) {
+  const { l10n } = useLocalization();
+
+  return (
+    <div className="flex mobile:flex-col">
+      <div className="flex flex-grow flex-col gap-4 max-w-sm">
+        <Typography variant="main-title" bold>
+          {l10n.getString(
+            'onboarding-automatic_mounting-preparation_feet-title'
+          )}
+        </Typography>
+        <div className="flex flex-col gap-2">
+          <Typography color="secondary">
+            {l10n.getString(
+              'onboarding-automatic_mounting-preparation_feet-step-0'
+            )}
+          </Typography>
+          <Typography color="secondary">
+            {l10n.getString(
+              'onboarding-automatic_mounting-preparation_feet-step-1'
+            )}
+          </Typography>
+        </div>
+        <div className="flex gap-3 mobile:justify-between">
+          <Button
+            variant={variant === 'onboarding' ? 'secondary' : 'tertiary'}
+            onClick={prevStep}
+          >
+            {l10n.getString('onboarding-automatic_mounting-prev_step')}
+          </Button>
+          <ResetButton
+            variant="small"
+            type={ResetType.Full}
+            bodyPose={ResetBodyPose.SITTING_LEANING_BACK}
+            referenceTrackerPosition={BodyPart.CHEST}
+            trackerPositions={[
+              BodyPart.LEFT_LOWER_LEG,
+              BodyPart.RIGHT_LOWER_LEG,
+              BodyPart.LEFT_FOOT,
+              BodyPart.RIGHT_FOOT,
+            ]}
+            onReseted={nextStep}
+          ></ResetButton>
+        </div>
+      </div>
+      <div className="flex flex-col pt-1 items-center fill-background-50 justify-center px-12">
+        <img
+          src="/images/reset-sitting-pose.webp"
+          width={200}
+          alt="Reset position"
+        />
+      </div>
+    </div>
+  );
+}

gui/src/components/onboarding/pages/mounting/sitting-mounting-steps/SittingReset.tsx

@@ -0,0 +1,70 @@
+import { ResetBodyPose, ResetType } from 'solarxr-protocol';
+import { Button } from '@/components/commons/Button';
+import { Typography } from '@/components/commons/Typography';
+import { ResetButton } from '@/components/home/ResetButton';
+import { useLocalization } from '@fluent/react';
+
+export function SittingResetStep({
+  nextStep,
+  prevStep,
+  variant,
+}: {
+  nextStep: () => void;
+  prevStep: () => void;
+  variant: 'onboarding' | 'alone';
+}) {
+  const { l10n } = useLocalization();
+
+  return (
+    <div className="flex mobile:flex-col">
+      <div className="flex flex-grow flex-col gap-4 max-w-sm">
+        <Typography variant="main-title" bold>
+          {l10n.getString('onboarding-automatic_mounting-mounting_reset-title')}
+        </Typography>
+        <div className="flex flex-col gap-2">
+          <Typography color="secondary">
+            {l10n.getString(
+              'onboarding-automatic_mounting-mounting_reset-step-0'
+            )}
+          </Typography>
+          <Typography color="secondary">
+            {l10n.getString(
+              'onboarding-automatic_mounting-mounting_reset-step-1'
+            )}
+          </Typography>
+          <Typography color="secondary">
+            {l10n.getString(
+              'onboarding-automatic_mounting-mounting_reset-step-2'
+            )}
+          </Typography>
+          <Typography color="secondary">
+            {l10n.getString(
+              'onboarding-automatic_mounting-mounting_reset-step-3'
+            )}
+          </Typography>
+        </div>
+        <div className="flex gap-3 mobile:justify-between">
+          <Button
+            variant={variant === 'onboarding' ? 'secondary' : 'tertiary'}
+            onClick={prevStep}
+          >
+            {l10n.getString('onboarding-automatic_mounting-prev_step')}
+          </Button>
+          <ResetButton
+            variant="small"
+            type={ResetType.Mounting}
+            bodyPose={ResetBodyPose.SITTING_LEANING_BACK}
+            onReseted={nextStep}
+          ></ResetButton>
+        </div>
+      </div>
+      <div className="flex flex-col pt-1 items-center fill-background-50 justify-center px-12">
+        <img
+          src="/images/reset-sitting-pose.webp"
+          width={200}
+          alt="Reset position"
+        />
+      </div>
+    </div>
+  );
+}

gui/src/components/onboarding/pages/mounting/sitting-mounting-steps/StandingReset.tsx

@@ -0,0 +1,64 @@
+import { ResetType } from 'solarxr-protocol';
+import { Button } from '@/components/commons/Button';
+import { Typography } from '@/components/commons/Typography';
+import { ResetButton } from '@/components/home/ResetButton';
+import { useLocalization } from '@fluent/react';
+
+export function StandingResetStep({
+  nextStep,
+  prevStep,
+  variant,
+}: {
+  nextStep: () => void;
+  prevStep: () => void;
+  variant: 'onboarding' | 'alone';
+}) {
+  const { l10n } = useLocalization();
+
+  return (
+    <div className="flex mobile:flex-col">
+      <div className="flex flex-grow flex-col gap-4 max-w-sm">
+        <Typography variant="main-title" bold>
+          {l10n.getString('onboarding-automatic_mounting-standing_reset-title')}
+        </Typography>
+        <div className="flex flex-col gap-2">
+          <Typography color="secondary">
+            {l10n.getString(
+              'onboarding-automatic_mounting-standing_reset-step-0'
+            )}
+          </Typography>
+          <Typography color="secondary">
+            {l10n.getString(
+              'onboarding-automatic_mounting-standing_reset-step-1'
+            )}
+          </Typography>
+          <Typography color="secondary">
+            {l10n.getString(
+              'onboarding-automatic_mounting-standing_reset-step-2'
+            )}
+          </Typography>
+        </div>
+        <div className="flex gap-3 mobile:justify-between">
+          <Button
+            variant={variant === 'onboarding' ? 'secondary' : 'tertiary'}
+            onClick={prevStep}
+          >
+            {l10n.getString('onboarding-automatic_mounting-prev_step')}
+          </Button>
+          <ResetButton
+            variant="small"
+            type={ResetType.Full}
+            onReseted={nextStep}
+          ></ResetButton>
+        </div>
+      </div>
+      <div className="flex flex-col pt-1 items-center fill-background-50 justify-center px-12">
+        <img
+          src="/images/reset-standing-pose.webp"
+          width={200}
+          alt="Reset position"
+        />
+      </div>
+    </div>
+  );
+}

gui/src/components/onboarding/pages/stay-aligned/StayAlignedSetup.tsx

@@ -0,0 +1,63 @@
+import { useOnboarding } from '@/hooks/onboarding';
+import { Typography } from '@/components/commons/Typography';
+import { Step, StepperSlider } from '@/components/onboarding/StepperSlider';
+import { DoneStep } from './stay-aligned-steps/Done';
+import { useLocalization } from '@fluent/react';
+import {
+  FlatRelaxedPoseStep,
+  SittingRelaxedPoseStep,
+  StandingRelaxedPoseStep,
+} from './stay-aligned-steps/RelaxedPoseSteps';
+import { EnableStayAlignedRequestT, RpcMessage } from 'solarxr-protocol';
+import { RPCPacketType, useWebsocketAPI } from '@/hooks/websocket-api';
+import { useEffect } from 'react';
+import { sittingMountingSteps } from '@/components/onboarding/pages/mounting/AutomaticMounting';
+
+export function sendEnableStayAligned(
+  enable: boolean,
+  sendRPCPacket: (type: RpcMessage, data: RPCPacketType) => void
+) {
+  const req = new EnableStayAlignedRequestT();
+  req.enable = enable;
+  sendRPCPacket(RpcMessage.EnableStayAlignedRequest, req);
+}
+
+const steps: Step[] = [
+  ...sittingMountingSteps,
+  { type: 'numbered', component: StandingRelaxedPoseStep },
+  { type: 'numbered', component: SittingRelaxedPoseStep },
+  { type: 'numbered', component: FlatRelaxedPoseStep },
+  { type: 'fullsize', component: DoneStep },
+];
+export function StayAlignedSetup() {
+  const { l10n } = useLocalization();
+  const { state } = useOnboarding();
+  const { sendRPCPacket } = useWebsocketAPI();
+
+  useEffect(() => {
+    sendEnableStayAligned(false, sendRPCPacket);
+  }, []);
+
+  return (
+    <>
+      <div className="flex flex-col gap-2 h-full items-center w-full xs:justify-center relative overflow-y-auto overflow-x-hidden px-4 pb-4">
+        <div className="flex flex-col w-full h-full xs:justify-center xs:max-w-3xl gap-5">
+          <div className="flex flex-col xs:max-w-lg gap-3">
+            <Typography variant="main-title">
+              {l10n.getString('onboarding-stay_aligned-title')}
+            </Typography>
+            <Typography color="secondary">
+              {l10n.getString('onboarding-stay_aligned-description')}
+            </Typography>
+          </div>
+          <div className="flex pb-4">
+            <StepperSlider
+              variant={state.alonePage ? 'alone' : 'onboarding'}
+              steps={steps}
+            />
+          </div>
+        </div>
+      </div>
+    </>
+  );
+}

gui/src/components/onboarding/pages/stay-aligned/stay-aligned-steps/Done.tsx

@@ -0,0 +1,47 @@
+import { Button } from '@/components/commons/Button';
+import { Typography } from '@/components/commons/Typography';
+import { SkeletonVisualizerWidget } from '@/components/widgets/SkeletonVisualizerWidget';
+import { useLocalization } from '@fluent/react';
+
+export function DoneStep({
+  resetSteps,
+  variant,
+}: {
+  nextStep: () => void;
+  prevStep: () => void;
+  resetSteps: () => void;
+  variant: 'onboarding' | 'alone';
+}) {
+  const { l10n } = useLocalization();
+
+  return (
+    <div className="flex flex-col items-center w-full justify-center gap-5">
+      <div className="flex gap-1 flex-col justify-center items-center">
+        <Typography variant="section-title">
+          {l10n.getString('onboarding-stay_aligned-done-title')}
+        </Typography>
+        <Typography color="secondary">
+          {l10n.getString('onboarding-stay_aligned-done-description')}
+        </Typography>
+      </div>
+
+      <div className="flex gap-3">
+        <Button
+          variant={variant === 'onboarding' ? 'secondary' : 'tertiary'}
+          onClick={resetSteps}
+        >
+          {l10n.getString('onboarding-stay_aligned-done-restart')}
+        </Button>
+        <Button
+          variant="primary"
+          to="/settings/trackers"
+          state={{ scrollTo: 'stayaligned' }}
+        >
+          {l10n.getString('onboarding-stay_aligned-done-done')}
+        </Button>
+      </div>
+
+      <SkeletonVisualizerWidget />
+    </div>
+  );
+}

gui/src/components/onboarding/pages/stay-aligned/stay-aligned-steps/RelaxedPoseSteps.tsx

@@ -0,0 +1,102 @@
+import { Button } from '@/components/commons/Button';
+import { Typography } from '@/components/commons/Typography';
+import {
+  CurrentRelaxedPose,
+  DetectRelaxedPoseButton,
+} from '@/components/stay-aligned/RelaxedPose';
+import { useWebsocketAPI } from '@/hooks/websocket-api';
+import { useLocalization } from '@fluent/react';
+import { StayAlignedRelaxedPose } from 'solarxr-protocol';
+import { sendEnableStayAligned } from '@/components/onboarding/pages/stay-aligned/StayAlignedSetup';
+
+function makeRelaxedPoseStep(
+  titleKey: string,
+  descriptionKeys: string[],
+  imageUrl: string,
+  relaxedPose: StayAlignedRelaxedPose,
+  enableStayAligned: boolean
+) {
+  return ({
+    nextStep,
+    prevStep,
+    variant,
+  }: {
+    nextStep: () => void;
+    prevStep: () => void;
+    variant: 'onboarding' | 'alone';
+  }) => {
+    const { l10n } = useLocalization();
+    const { sendRPCPacket } = useWebsocketAPI();
+
+    return (
+      <div className="flex mobile:flex-col">
+        <div className="flex flex-grow flex-col gap-4 max-w-sm">
+          <Typography variant="main-title" bold>
+            {l10n.getString(titleKey)}
+          </Typography>
+          <div className="flex flex-col gap-2">
+            {descriptionKeys.map((descriptionKey) => (
+              <Typography color="secondary">
+                {l10n.getString(descriptionKey)}
+              </Typography>
+            ))}
+          </div>
+          <CurrentRelaxedPose />
+          <div className="flex gap-3 mobile:justify-between">
+            <Button
+              variant={variant === 'onboarding' ? 'secondary' : 'tertiary'}
+              onClick={prevStep}
+            >
+              {l10n.getString('onboarding-automatic_mounting-prev_step')}
+            </Button>
+            <DetectRelaxedPoseButton
+              onClick={() => {
+                nextStep();
+                if (enableStayAligned) {
+                  sendEnableStayAligned(true, sendRPCPacket);
+                }
+              }}
+              pose={relaxedPose}
+            />
+          </div>
+        </div>
+        <div className="flex flex-col pt-1 items-center fill-background-50 justify-center px-12">
+          <img src={imageUrl} width={200} alt="Reset position" />
+        </div>
+      </div>
+    );
+  };
+}
+
+export const StandingRelaxedPoseStep = makeRelaxedPoseStep(
+  'onboarding-stay_aligned-relaxed_poses-standing-title',
+  [
+    'onboarding-stay_aligned-relaxed_poses-standing-step-0',
+    'onboarding-stay_aligned-relaxed_poses-standing-step-1',
+  ],
+  '/images/relaxed_pose_standing.webp',
+  StayAlignedRelaxedPose.STANDING,
+  false
+);
+
+export const SittingRelaxedPoseStep = makeRelaxedPoseStep(
+  'onboarding-stay_aligned-relaxed_poses-sitting-title',
+  [
+    'onboarding-stay_aligned-relaxed_poses-sitting-step-0',
+    'onboarding-stay_aligned-relaxed_poses-sitting-step-1',
+  ],
+  '/images/relaxed_pose_sitting.webp',
+  StayAlignedRelaxedPose.SITTING,
+  false
+);
+
+export const FlatRelaxedPoseStep = makeRelaxedPoseStep(
+  'onboarding-stay_aligned-relaxed_poses-flat-title',
+  [
+    'onboarding-stay_aligned-relaxed_poses-flat-step-0',
+    'onboarding-stay_aligned-relaxed_poses-flat-step-1',
+  ],
+  '/images/relaxed_pose_flat.webp',
+  StayAlignedRelaxedPose.FLAT,
+  true
+);

gui/src/components/settings/SettingsSidebar.tsx

@@ -58,6 +58,9 @@ export function SettingsSidebar() {
           <SettingsLink to="/settings/trackers" scrollTo="mechanics">
             {l10n.getString('settings-sidebar-tracker_mechanics')}
           </SettingsLink>
+          <SettingsLink to="/settings/trackers" scrollTo="stayaligned">
+            {l10n.getString('settings-stay_aligned')}
+          </SettingsLink>
           <SettingsLink to="/settings/trackers" scrollTo="fksettings">
             {l10n.getString('settings-sidebar-fk_settings')}
           </SettingsLink>

gui/src/components/settings/pages/GeneralSettings.tsx

@@ -33,8 +33,15 @@ import {
 import { HandsWarningModal } from '@/components/settings/HandsWarningModal';
 import { MagnetometerToggleSetting } from './MagnetometerToggleSetting';
 import { DriftCompensationModal } from '@/components/settings/DriftCompensationModal';
+import {
+  defaultStayAlignedSettings,
+  StayAlignedSettings,
+  StayAlignedSettingsForm,
+  serializeStayAlignedSettings,
+  deserializeStayAlignedSettings,
+} from './components/StayAlignedSettings';
 
-interface SettingsForm {
+export type SettingsForm = {
   trackers: {
     waist: boolean;
     chest: boolean;
@@ -103,7 +110,8 @@ interface SettingsForm {
     saveMountingReset: boolean;
     resetHmdPitch: boolean;
   };
-}
+  stayAligned: StayAlignedSettingsForm;
+};
 
 const defaultValues: SettingsForm = {
   trackers: {
@@ -169,6 +177,7 @@ const defaultValues: SettingsForm = {
     saveMountingReset: false,
     resetHmdPitch: false,
   },
+  stayAligned: defaultStayAlignedSettings,
 };
 
 export function GeneralSettings() {
@@ -297,6 +306,8 @@ export function GeneralSettings() {
     driftCompensation.maxResets = values.driftCompensation.maxResets;
     settings.driftCompensation = driftCompensation;
 
+    settings.stayAligned = serializeStayAlignedSettings(values.stayAligned);
+
     if (values.resetsSettings) {
       const resetsSettings = new ResetsSettingsT();
       resetsSettings.resetMountingFeet =
@@ -420,6 +431,12 @@ export function GeneralSettings() {
       formData.resetsSettings = settings.resetsSettings;
     }
 
+    if (settings.stayAligned) {
+      formData.stayAligned = deserializeStayAlignedSettings(
+        settings.stayAligned
+      );
+    }
+
     reset({ ...getValues(), ...formData });
   });
 
@@ -822,6 +839,7 @@ export function GeneralSettings() {
             />
           </>
         </SettingsPagePaneLayout>
+        <StayAlignedSettings values={getValues()} control={control} />
         <SettingsPagePaneLayout
           icon={<WrenchIcon></WrenchIcon>}
           id="fksettings"

gui/src/components/settings/pages/components/StayAlignedSettings.tsx

@@ -0,0 +1,147 @@
+import { Control } from 'react-hook-form';
+import { StayAlignedSettingsT } from 'solarxr-protocol';
+import { SettingsForm } from '@/components/settings/pages/GeneralSettings';
+import { Button } from '@/components/commons/Button';
+import { CheckBox } from '@/components/commons/Checkbox';
+import { WrenchIcon } from '@/components/commons/icon/WrenchIcons';
+import { Typography } from '@/components/commons/Typography';
+import { SettingsPagePaneLayout } from '@/components/settings/SettingsPageLayout';
+import { useLocalization } from '@fluent/react';
+import { useConfig } from '@/hooks/config';
+import {
+  RelaxedPosesSettings,
+  RelaxedPosesSummary,
+} from '@/components/stay-aligned/RelaxedPose';
+
+export type StayAlignedSettingsForm = {
+  enabled: boolean;
+  extraYawCorrection: boolean;
+  hideYawCorrection: boolean;
+  standingUpperLegAngle: number;
+  standingLowerLegAngle: number;
+  standingFootAngle: number;
+  sittingUpperLegAngle: number;
+  sittingLowerLegAngle: number;
+  sittingFootAngle: number;
+  flatUpperLegAngle: number;
+  flatLowerLegAngle: number;
+  flatFootAngle: number;
+};
+
+export const defaultStayAlignedSettings: StayAlignedSettingsForm = {
+  enabled: false,
+  extraYawCorrection: false,
+  hideYawCorrection: false,
+  standingUpperLegAngle: 0.0,
+  standingLowerLegAngle: 0.0,
+  standingFootAngle: 0.0,
+  sittingUpperLegAngle: 0.0,
+  sittingLowerLegAngle: 0.0,
+  sittingFootAngle: 0.0,
+  flatUpperLegAngle: 0.0,
+  flatLowerLegAngle: 0.0,
+  flatFootAngle: 0.0,
+};
+
+export function serializeStayAlignedSettings(
+  settings: StayAlignedSettingsForm
+): StayAlignedSettingsT {
+  const serialized = new StayAlignedSettingsT();
+  serialized.enabled = settings.enabled;
+  serialized.extraYawCorrection = settings.extraYawCorrection;
+  serialized.hideYawCorrection = settings.hideYawCorrection;
+  serialized.standingUpperLegAngle = settings.standingUpperLegAngle;
+  serialized.standingLowerLegAngle = settings.standingLowerLegAngle;
+  serialized.standingFootAngle = settings.standingFootAngle;
+  serialized.sittingUpperLegAngle = settings.sittingUpperLegAngle;
+  serialized.sittingLowerLegAngle = settings.sittingLowerLegAngle;
+  serialized.sittingFootAngle = settings.sittingFootAngle;
+  serialized.flatUpperLegAngle = settings.flatUpperLegAngle;
+  serialized.flatLowerLegAngle = settings.flatLowerLegAngle;
+  serialized.flatFootAngle = settings.flatFootAngle;
+  return serialized;
+}
+
+export function deserializeStayAlignedSettings(
+  serialized: StayAlignedSettingsT
+): StayAlignedSettingsForm {
+  return serialized;
+}
+
+export function StayAlignedSettings({
+  values,
+  control,
+}: {
+  values: SettingsForm;
+  control: Control<SettingsForm, any>;
+}) {
+  const { l10n } = useLocalization();
+  const { config } = useConfig();
+
+  return (
+    <SettingsPagePaneLayout icon={<WrenchIcon />} id="stayaligned">
+      <Typography variant="main-title">
+        {l10n.getString('settings-stay_aligned')}
+      </Typography>
+      <div className="flex flex-col pt-2 pb-4">
+        {l10n.getString('settings-stay_aligned-description')}
+        {values.stayAligned.enabled && values.driftCompensation.enabled && (
+          <div className="pt-2">
+            {l10n.getString(
+              'settings-stay_aligned-warnings-drift_compensation'
+            )}
+          </div>
+        )}
+      </div>
+      <div className="grid sm:grid-cols-1 gap-3 pb-4">
+        <div className="grid sm:grid-cols-2 gap-3 pb-3">
+          <CheckBox
+            variant="toggle"
+            outlined
+            control={control}
+            name="stayAligned.enabled"
+            label={l10n.getString('settings-stay_aligned-enabled-label')}
+          />
+          <CheckBox
+            variant="toggle"
+            outlined
+            control={control}
+            name="stayAligned.extraYawCorrection"
+            label={l10n.getString(
+              'settings-stay_aligned-extra_yaw_correction-label'
+            )}
+          />
+          <CheckBox
+            variant="toggle"
+            outlined
+            control={control}
+            name="stayAligned.hideYawCorrection"
+            label={l10n.getString(
+              'settings-stay_aligned-hide_yaw_correction-label'
+            )}
+          />
+        </div>
+        <div className="flex flex-col pt-2">
+          <Typography bold>
+            {l10n.getString('settings-stay_aligned-relaxed_poses-label')}
+          </Typography>
+          <Typography color="secondary">
+            {l10n.getString('settings-stay_aligned-relaxed_poses-description')}
+          </Typography>
+        </div>
+        {config?.debug ? (
+          <RelaxedPosesSettings control={control} />
+        ) : (
+          <RelaxedPosesSummary values={values} />
+        )}
+        <Button
+          variant="primary"
+          to="/onboarding/stay-aligned"
+          state={{ alonePage: true }}
+        >
+          {l10n.getString('settings-stay_aligned-setup-label')}
+        </Button>
+      </div>
+    </SettingsPagePaneLayout>
+  );
+}

gui/src/components/settings/pages/components/stay-aligned/DetectRelaxedPoseButton.tsx

@@ -0,0 +1,45 @@
+import { useLocalization } from '@fluent/react';
+import {
+  DetectStayAlignedRelaxedPoseRequestT,
+  RpcMessage,
+  StayAlignedRelaxedPose,
+} from 'solarxr-protocol';
+import { Button } from '@/components/commons/Button';
+import { MouseEventHandler } from 'react';
+import { RPCPacketType, useWebsocketAPI } from '@/hooks/websocket-api';
+
+function detectAngles(
+  sendRPCPacket: (type: RpcMessage, data: RPCPacketType) => void,
+  pose: StayAlignedRelaxedPose
+) {
+  const req = new DetectStayAlignedRelaxedPoseRequestT();
+  req.pose = pose;
+  sendRPCPacket(RpcMessage.DetectStayAlignedRelaxedPoseRequest, req);
+}
+
+export function DetectStayAlignedRelaxedPoseButton({
+  pose,
+  onClick,
+}: {
+  pose: StayAlignedRelaxedPose;
+  onClick?: MouseEventHandler<HTMLButtonElement>;
+}) {
+  const { sendRPCPacket } = useWebsocketAPI();
+  const { l10n } = useLocalization();
+
+  return (
+    <Button
+      variant="primary"
+      onClick={(e) => {
+        detectAngles(sendRPCPacket, pose);
+        if (onClick) {
+          onClick(e);
+        }
+      }}
+    >
+      {l10n.getString(
+        'settings-general-stay_aligned-relaxed_body_angles-auto_detect'
+      )}
+    </Button>
+  );
+}

gui/src/components/settings/pages/components/stay-aligned/ResetRelaxedPoseButton.tsx

@@ -0,0 +1,45 @@
+import { useLocalization } from '@fluent/react';
+import {
+  DetectStayAlignedRelaxedPoseRequestT,
+  RpcMessage,
+  StayAlignedRelaxedPose,
+} from 'solarxr-protocol';
+import { Button } from '@/components/commons/Button';
+import { MouseEventHandler } from 'react';
+import { RPCPacketType, useWebsocketAPI } from '@/hooks/websocket-api';
+
+function detectAngles(
+  sendRPCPacket: (type: RpcMessage, data: RPCPacketType) => void,
+  pose: StayAlignedRelaxedPose
+) {
+  const req = new DetectStayAlignedRelaxedPoseRequestT();
+  req.pose = pose;
+  sendRPCPacket(RpcMessage.ResetStayAlignedRelaxedPoseRequest, req);
+}
+
+export function ResetStayAlignedRelaxedPoseButton({
+  pose,
+  onClick,
+}: {
+  pose: StayAlignedRelaxedPose;
+  onClick?: MouseEventHandler<HTMLButtonElement>;
+}) {
+  const { sendRPCPacket } = useWebsocketAPI();
+  const { l10n } = useLocalization();
+
+  return (
+    <Button
+      variant="primary"
+      onClick={(e) => {
+        detectAngles(sendRPCPacket, pose);
+        if (onClick) {
+          onClick(e);
+        }
+      }}
+    >
+      {l10n.getString(
+        'settings-general-stay_aligned-relaxed_body_angles-reset'
+      )}
+    </Button>
+  );
+}

gui/src/components/stay-aligned/RelaxedPose.tsx

@@ -0,0 +1,427 @@
+import { useLocaleConfig } from '@/i18n/config';
+import { ReactLocalization, useLocalization } from '@fluent/react';
+import {
+  DetectStayAlignedRelaxedPoseRequestT,
+  RpcMessage,
+  StayAlignedDataT,
+  StayAlignedRelaxedPose,
+} from 'solarxr-protocol';
+import { Typography } from '@/components/commons/Typography';
+import { SettingsForm } from '@/components/settings/pages/GeneralSettings';
+import { useAppContext } from '@/hooks/app';
+import { Control } from 'react-hook-form';
+import { NumberSelector } from '@/components/commons/NumberSelector';
+import { useWebsocketAPI } from '@/hooks/websocket-api';
+import { MouseEventHandler } from 'react';
+import { Button } from '@/components/commons/Button';
+
+/**
+ * Creates a pose angle formatter that formats an positive angle as "$angle
+ * outwards" and a negative angle as "$angle inwards". Useful for describing
+ * leg angles in relaxed poses.
+ */
+function PoseAngleFormat(l10n: ReactLocalization, currentLocales: string[]) {
+  const degreeFormat = new Intl.NumberFormat(currentLocales, {
+    style: 'unit',
+    unit: 'degree',
+    maximumFractionDigits: 0,
+  });
+
+  return {
+    format: (angle: number) => {
+      angle = Math.round(angle);
+      const angleStr = degreeFormat.format(Math.abs(angle));
+      if (angle >= 1) {
+        return l10n.getString('settings-stay_aligned-relaxed_poses-outwards', {
+          angle: angleStr,
+        });
+      } else if (angle <= -1) {
+        return l10n.getString('settings-stay_aligned-relaxed_poses-inwards', {
+          angle: angleStr,
+        });
+      } else {
+        return angleStr;
+      }
+    },
+  };
+}
+
+function relaxedPoseKey(pose: StayAlignedRelaxedPose) {
+  switch (pose) {
+    case StayAlignedRelaxedPose.STANDING:
+      return 'settings-stay_aligned-relaxed_poses-standing';
+    case StayAlignedRelaxedPose.SITTING:
+      return 'settings-stay_aligned-relaxed_poses-sitting';
+    case StayAlignedRelaxedPose.FLAT:
+      return 'settings-stay_aligned-relaxed_poses-flat';
+  }
+}
+
+/**
+ * Read-only view of a relaxed pose.
+ */
+export function RelaxedPose({
+  pose,
+  upperLegAngleInDeg,
+  lowerLegAngleInDeg,
+  footAngleInDeg,
+}: {
+  pose: StayAlignedRelaxedPose;
+  upperLegAngleInDeg: number;
+  lowerLegAngleInDeg: number;
+  footAngleInDeg: number;
+}) {
+  const { l10n } = useLocalization();
+  const { currentLocales } = useLocaleConfig();
+  const angleFormat = PoseAngleFormat(l10n, currentLocales);
+
+  return (
+    <div>
+      <Typography color="primary">
+        {l10n.getString(relaxedPoseKey(pose))}
+      </Typography>
+      <Typography color="secondary">
+        {l10n.getString('settings-stay_aligned-relaxed_poses-upper_leg_angle')}:{' '}
+        {angleFormat.format(upperLegAngleInDeg)}
+      </Typography>
+      <Typography color="secondary">
+        {l10n.getString('settings-stay_aligned-relaxed_poses-lower_leg_angle')}:{' '}
+        {angleFormat.format(lowerLegAngleInDeg)}
+      </Typography>
+      <Typography color="secondary">
+        {l10n.getString('settings-stay_aligned-relaxed_poses-foot_angle')}:{' '}
+        {angleFormat.format(footAngleInDeg)}
+      </Typography>
+    </div>
+  );
+}
+
+/**
+ * Read-only view of the current pose's relaxed angles.
+ */
+export function CurrentRelaxedPose() {
+  const { l10n } = useLocalization();
+  const { currentLocales } = useLocaleConfig();
+  const angleFormat = PoseAngleFormat(l10n, currentLocales);
+
+  const { state } = useAppContext();
+  const stayAligned =
+    state.datafeed?.stayAligned || new StayAlignedDataT(0.0, 0.0, 0.0);
+
+  return (
+    <div>
+      <Typography color="primary">
+        {l10n.getString('settings-stay_aligned-relaxed_poses-current_angles')}
+      </Typography>
+      <Typography color="secondary">
+        {l10n.getString('settings-stay_aligned-relaxed_poses-upper_leg_angle')}:{' '}
+        {angleFormat.format(stayAligned.upperLegAngleInDeg)}
+      </Typography>
+      <Typography color="secondary">
+        {l10n.getString('settings-stay_aligned-relaxed_poses-lower_leg_angle')}:{' '}
+        {angleFormat.format(stayAligned.lowerLegAngleInDeg)}
+      </Typography>
+      <Typography color="secondary">
+        {l10n.getString('settings-stay_aligned-relaxed_poses-foot_angle')}:{' '}
+        {angleFormat.format(stayAligned.footAngleInDeg)}
+      </Typography>
+    </div>
+  );
+}
+
+/**
+ * Read-only view of all the relaxed poses, and the current pose's angles.
+ */
+export function RelaxedPosesSummary({ values }: { values: SettingsForm }) {
+  return (
+    <div className="grid sm:grid-cols-4 gap-3 pb-3">
+      <div className="rounded-lg bg-background-60 gap-2 w-full p-3">
+        <RelaxedPose
+          pose={StayAlignedRelaxedPose.STANDING}
+          upperLegAngleInDeg={values.stayAligned.standingUpperLegAngle}
+          lowerLegAngleInDeg={values.stayAligned.standingLowerLegAngle}
+          footAngleInDeg={values.stayAligned.standingFootAngle}
+        />
+      </div>
+      <div className="rounded-lg bg-background-60 gap-2 w-full p-3">
+        <RelaxedPose
+          pose={StayAlignedRelaxedPose.SITTING}
+          upperLegAngleInDeg={values.stayAligned.sittingUpperLegAngle}
+          lowerLegAngleInDeg={values.stayAligned.sittingLowerLegAngle}
+          footAngleInDeg={values.stayAligned.sittingFootAngle}
+        />
+      </div>
+      <div className="rounded-lg bg-background-60 gap-2 w-full p-3">
+        <RelaxedPose
+          pose={StayAlignedRelaxedPose.FLAT}
+          upperLegAngleInDeg={values.stayAligned.flatUpperLegAngle}
+          lowerLegAngleInDeg={values.stayAligned.flatLowerLegAngle}
+          footAngleInDeg={values.stayAligned.flatFootAngle}
+        />
+      </div>
+      <div className="rounded-lg bg-background-60 gap-2 w-full p-3">
+        <CurrentRelaxedPose />
+      </div>
+    </div>
+  );
+}
+
+/**
+ * Tells the server to set a relaxed pose to the current pose's angles.
+ */
+export function DetectRelaxedPoseButton({
+  pose,
+  onClick,
+}: {
+  pose: StayAlignedRelaxedPose;
+  onClick?: MouseEventHandler<HTMLButtonElement>;
+}) {
+  const { sendRPCPacket } = useWebsocketAPI();
+  const { l10n } = useLocalization();
+
+  return (
+    <Button
+      variant="primary"
+      onClick={(e) => {
+        const req = new DetectStayAlignedRelaxedPoseRequestT();
+        req.pose = pose;
+        sendRPCPacket(RpcMessage.DetectStayAlignedRelaxedPoseRequest, req);
+        if (onClick) {
+          onClick(e);
+        }
+      }}
+    >
+      {l10n.getString('settings-stay_aligned-relaxed_poses-auto_detect')}
+    </Button>
+  );
+}
+
+/**
+ * Tells the server to reset the angles in a relaxed pose.
+ */
+export function ResetRelaxedPoseButton({
+  pose,
+  onClick,
+}: {
+  pose: StayAlignedRelaxedPose;
+  onClick?: MouseEventHandler<HTMLButtonElement>;
+}) {
+  const { sendRPCPacket } = useWebsocketAPI();
+  const { l10n } = useLocalization();
+
+  return (
+    <Button
+      variant="primary"
+      onClick={(e) => {
+        const req = new DetectStayAlignedRelaxedPoseRequestT();
+        req.pose = pose;
+        sendRPCPacket(RpcMessage.ResetStayAlignedRelaxedPoseRequest, req);
+        if (onClick) {
+          onClick(e);
+        }
+      }}
+    >
+      {l10n.getString('settings-stay_aligned-relaxed_poses-reset')}
+    </Button>
+  );
+}
+
+/**
+ * Control to edit the angles of a pose.
+ */
+function RelaxedPoseControl({
+  pose,
+  upperLegSettingsKey,
+  lowerLegSettingsKey,
+  footSettingsKey,
+  control,
+}: {
+  pose: StayAlignedRelaxedPose;
+  upperLegSettingsKey: string;
+  lowerLegSettingsKey: string;
+  footSettingsKey: string;
+  control: Control<SettingsForm, any>;
+}) {
+  const { l10n } = useLocalization();
+  const { currentLocales } = useLocaleConfig();
+  const angleFormat = PoseAngleFormat(l10n, currentLocales);
+
+  return (
+    <div className="grid sm:grid-cols-1 gap-3">
+      <Typography color="primary">
+        {l10n.getString(relaxedPoseKey(pose))}
+      </Typography>
+      <NumberSelector
+        control={control}
+        name={upperLegSettingsKey}
+        valueLabelFormat={(value) =>
+          `${l10n.getString(
+            'settings-stay_aligned-relaxed_poses-upper_leg_angle'
+          )}: ${angleFormat.format(value)}`
+        }
+        min={-90.0}
+        max={90.0}
+        step={1.0}
+      />
+      <NumberSelector
+        control={control}
+        name={lowerLegSettingsKey}
+        valueLabelFormat={(value) =>
+          `${l10n.getString(
+            'settings-stay_aligned-relaxed_poses-lower_leg_angle'
+          )}: ${angleFormat.format(value)}`
+        }
+        min={-90.0}
+        max={90.0}
+        step={1.0}
+      />
+      <NumberSelector
+        control={control}
+        name={footSettingsKey}
+        valueLabelFormat={(value) =>
+          `${l10n.getString(
+            'settings-stay_aligned-relaxed_poses-foot_angle'
+          )}: ${angleFormat.format(value)}`
+        }
+        min={-90.0}
+        max={90.0}
+        step={1.0}
+      />
+      <DetectRelaxedPoseButton pose={pose} />
+      <ResetRelaxedPoseButton pose={pose} />
+    </div>
+  );
+}
+
+/**
+ * Control that displays the current pose's relaxed angles, in a similar layout
+ * to <RelaxedPoseControl />.
+ */
+function CurrentRelaxedPoseControl() {
+  const { l10n } = useLocalization();
+  const { currentLocales } = useLocaleConfig();
+  const angleFormat = PoseAngleFormat(l10n, currentLocales);
+
+  const { state } = useAppContext();
+
+  return (
+    <div className="grid sm:grid-cols-1 gap-3">
+      <Typography color="primary">
+        {l10n.getString('settings-stay_aligned-relaxed_poses-current_angles')}
+      </Typography>
+      <div className="flex flex-col gap-1 w-full">
+        <Typography bold></Typography>
+        <div className="flex gap-5 bg-background-60 p-2 rounded-lg">
+          <div className="flex gap-1">
+            <Button variant="tertiary" rounded disabled={true}>
+              -
+            </Button>
+          </div>
+          <div className="flex flex-grow justify-center items-center w-10 text-standard">
+            {l10n.getString(
+              'settings-stay_aligned-relaxed_poses-upper_leg_angle'
+            )}
+            :{' '}
+            {angleFormat.format(
+              state.datafeed?.stayAligned?.upperLegAngleInDeg || 0.0
+            )}
+          </div>
+          <div className="flex gap-1">
+            <Button variant="tertiary" rounded disabled={true}>
+              +
+            </Button>
+          </div>
+        </div>
+      </div>
+      <div className="flex flex-col gap-1 w-full">
+        <Typography bold></Typography>
+        <div className="flex gap-5 bg-background-60 p-2 rounded-lg">
+          <div className="flex gap-1">
+            <Button variant="tertiary" rounded disabled={true}>
+              -
+            </Button>
+          </div>
+          <div className="flex flex-grow justify-center items-center w-10 text-standard">
+            {l10n.getString(
+              'settings-stay_aligned-relaxed_poses-lower_leg_angle'
+            )}
+            :{' '}
+            {angleFormat.format(
+              state.datafeed?.stayAligned?.lowerLegAngleInDeg || 0.0
+            )}
+          </div>
+          <div className="flex gap-1">
+            <Button variant="tertiary" rounded disabled={true}>
+              +
+            </Button>
+          </div>
+        </div>
+      </div>
+      <div className="flex flex-col gap-1 w-full">
+        <Typography bold></Typography>
+        <div className="flex gap-5 bg-background-60 p-2 rounded-lg">
+          <div className="flex gap-1">
+            <Button variant="tertiary" rounded disabled={true}>
+              -
+            </Button>
+          </div>
+          <div className="flex flex-grow justify-center items-center w-10 text-standard">
+            {l10n.getString('settings-stay_aligned-relaxed_poses-foot_angle')}:{' '}
+            {angleFormat.format(
+              state.datafeed?.stayAligned?.footAngleInDeg || 0.0
+            )}
+          </div>
+          <div className="flex gap-1">
+            <Button variant="tertiary" rounded disabled={true}>
+              +
+            </Button>
+          </div>
+        </div>
+      </div>
+    </div>
+  );
+}
+
+/**
+ * Control to edit the angles of all the relaxed poses.
+ */
+export function RelaxedPosesSettings({
+  control,
+}: {
+  control: Control<SettingsForm, any>;
+}) {
+  return (
+    <div className="grid sm:grid-cols-4 gap-3 pb-3">
+      <div className="rounded-lg bg-background-60 gap-2 w-full p-3">
+        <RelaxedPoseControl
+          pose={StayAlignedRelaxedPose.STANDING}
+          upperLegSettingsKey="stayAligned.standingUpperLegAngle"
+          lowerLegSettingsKey="stayAligned.standingLowerLegAngle"
+          footSettingsKey="stayAligned.standingFootAngle"
+          control={control}
+        />
+      </div>
+      <div className="rounded-lg bg-background-60 gap-2 w-full p-3">
+        <RelaxedPoseControl
+          pose={StayAlignedRelaxedPose.SITTING}
+          upperLegSettingsKey="stayAligned.sittingUpperLegAngle"
+          lowerLegSettingsKey="stayAligned.sittingLowerLegAngle"
+          footSettingsKey="stayAligned.sittingFootAngle"
+          control={control}
+        />
+      </div>
+      <div className="rounded-lg bg-background-60 gap-2 w-full p-3">
+        <RelaxedPoseControl
+          pose={StayAlignedRelaxedPose.FLAT}
+          upperLegSettingsKey="stayAligned.flatUpperLegAngle"
+          lowerLegSettingsKey="stayAligned.flatLowerLegAngle"
+          footSettingsKey="stayAligned.flatFootAngle"
+          control={control}
+        />
+      </div>
+      <div className="rounded-lg bg-background-60 gap-2 w-full p-3">
+        <CurrentRelaxedPoseControl />
+      </div>
+    </div>
+  );
+}

gui/src/components/stay-aligned/StayAlignedInfo.tsx

@@ -0,0 +1,53 @@
+import { Typography } from '@/components/commons/Typography';
+import { useLocaleConfig } from '@/i18n/config';
+import { angleIsNearZero } from '@/maths/angle';
+import { TrackerDataT } from 'solarxr-protocol';
+
+export function StayAlignedInfo({
+  color,
+  tracker,
+}: {
+  color: 'primary' | 'secondary';
+  tracker: TrackerDataT;
+}) {
+  const { currentLocales } = useLocaleConfig();
+  const degreeFormat = new Intl.NumberFormat(currentLocales, {
+    style: 'unit',
+    unit: 'degree',
+    minimumFractionDigits: 1,
+    maximumFractionDigits: 1,
+  });
+  const errorFormat = new Intl.NumberFormat(currentLocales, {
+    minimumFractionDigits: 1,
+    maximumFractionDigits: 1,
+  });
+
+  const stayAligned = tracker.stayAligned;
+  if (!stayAligned) {
+    return <></>;
+  }
+
+  const locked = stayAligned.locked ? '🔒' : '';
+
+  const delta = `Δ=${degreeFormat.format(stayAligned.yawCorrectionInDeg)}`;
+
+  const errors = [];
+  const maxErrorToShow = 0.1;
+  if (!angleIsNearZero(stayAligned.lockedErrorInDeg, maxErrorToShow)) {
+    errors.push(`L=${errorFormat.format(stayAligned.lockedErrorInDeg)}`);
+  }
+  if (!angleIsNearZero(stayAligned.centerErrorInDeg, maxErrorToShow)) {
+    errors.push(`C=${errorFormat.format(stayAligned.centerErrorInDeg)}`);
+  }
+  if (!angleIsNearZero(stayAligned.neighborErrorInDeg, maxErrorToShow)) {
+    errors.push(`N=${errorFormat.format(stayAligned.neighborErrorInDeg)}`);
+  }
+
+  const error = errors.length > 0 ? `(${errors.join(', ')})` : '';
+
+  return (
+    <Typography color={color} whitespace="whitespace-nowrap">
+      {locked} {delta} {error}
+    </Typography>
+  );
+}

gui/src/components/tracker/StayAlignedInfo.tsx

@@ -0,0 +1,53 @@
+import { Typography } from '@/components/commons/Typography';
+import { useLocaleConfig } from '@/i18n/config';
+import { angleIsNearZero } from '@/maths/angle';
+import { TrackerDataT } from 'solarxr-protocol';
+
+export function StayAlignedInfo({
+  color,
+  tracker,
+}: {
+  color: 'primary' | 'secondary';
+  tracker: TrackerDataT;
+}) {
+  const { currentLocales } = useLocaleConfig();
+  const degreeFormat = new Intl.NumberFormat(currentLocales, {
+    style: 'unit',
+    unit: 'degree',
+    minimumFractionDigits: 1,
+    maximumFractionDigits: 1,
+  });
+  const errorFormat = new Intl.NumberFormat(currentLocales, {
+    minimumFractionDigits: 1,
+    maximumFractionDigits: 1,
+  });
+
+  const stayAligned = tracker.stayAligned;
+  if (!stayAligned) {
+    return <></>;
+  }
+
+  const locked = stayAligned.locked ? '🔒' : '';
+
+  const delta = `Δ=${degreeFormat.format(stayAligned.yawCorrectionInDeg)}`;
+
+  const errors = [];
+  const maxErrorToShow = 0.1;
+  if (!angleIsNearZero(stayAligned.lockedErrorInDeg, maxErrorToShow)) {
+    errors.push(`L=${errorFormat.format(stayAligned.lockedErrorInDeg)}`);
+  }
+  if (!angleIsNearZero(stayAligned.centerErrorInDeg, maxErrorToShow)) {
+    errors.push(`C=${errorFormat.format(stayAligned.centerErrorInDeg)}`);
+  }
+  if (!angleIsNearZero(stayAligned.neighborErrorInDeg, maxErrorToShow)) {
+    errors.push(`N=${errorFormat.format(stayAligned.neighborErrorInDeg)}`);
+  }
+
+  const error = errors.length > 0 ? `(${errors.join(', ')})` : '';
+
+  return (
+    <Typography color={color} whitespace="whitespace-nowrap">
+      {locked} {delta} {error}
+    </Typography>
+  );
+}

gui/src/components/tracker/TrackersTable.tsx

@@ -17,6 +17,7 @@ import { TrackerBattery } from './TrackerBattery';
 import { TrackerStatus } from './TrackerStatus';
 import { TrackerWifi } from './TrackerWifi';
 import { trackerStatusRelated, useStatusContext } from '@/hooks/status-system';
+import { StayAlignedInfo } from '@/components/stay-aligned/StayAlignedInfo';
 
 enum DisplayColumn {
   NAME,
@@ -28,6 +29,7 @@ enum DisplayColumn {
   TEMPERATURE,
   LINEAR_ACCELERATION,
   POSITION,
+  STAY_ALIGNED,
   URL,
 }
 
@@ -41,6 +43,7 @@ const displayColumns: { [k: string]: boolean } = {
   [DisplayColumn.TEMPERATURE]: true,
   [DisplayColumn.LINEAR_ACCELERATION]: true,
   [DisplayColumn.POSITION]: true,
+  [DisplayColumn.STAY_ALIGNED]: true,
   [DisplayColumn.URL]: true,
 };
 
@@ -196,6 +199,7 @@ export function TrackersTable({
   displayColumns[DisplayColumn.TEMPERATURE] = hasTemperature || false;
   displayColumns[DisplayColumn.POSITION] = moreInfo || false;
   displayColumns[DisplayColumn.LINEAR_ACCELERATION] = moreInfo || false;
+  displayColumns[DisplayColumn.STAY_ALIGNED] = moreInfo || false;
   displayColumns[DisplayColumn.URL] = moreInfo || false;
   const displayColumnsKeys = Object.keys(displayColumns).filter(
     (k) => displayColumns[k]
@@ -362,6 +366,15 @@ export function TrackersTable({
           ),
       })}
 
+      {column({
+        id: DisplayColumn.STAY_ALIGNED,
+        label: l10n.getString('tracker-table-column-stay_aligned'),
+        labelClassName: 'w-36',
+        row: ({ tracker }) => (
+          <StayAlignedInfo color={fontColor} tracker={tracker} />
+        ),
+      })}
+
       {column({
         id: DisplayColumn.URL,
         label: l10n.getString('tracker-table-column-url'),

gui/src/components/widgets/IMUVisualizerWidget.tsx

@@ -12,6 +12,7 @@ import { useLocalization } from '@fluent/react';
 import { Vector3Object } from '@/maths/vector3';
 import { Gltf } from '@react-three/drei';
 import { ErrorBoundary } from 'react-error-boundary';
+import { StayAlignedInfo } from '@/components/stay-aligned/StayAlignedInfo';
 
 const groundColor = '#4444aa';
 
@@ -157,6 +158,15 @@ export function IMUVisualizerWidget({ tracker }: { tracker: TrackerDataT }) {
         </div>
       )}
 
+      {!!tracker.stayAligned && (
+        <div className="flex justify-between">
+          <Typography color="secondary">
+            {l10n.getString('widget-imu_visualizer-stay_aligned')}
+          </Typography>
+          <StayAlignedInfo color="primary" tracker={tracker} />
+        </div>
+      )}
+
       {!enabled && (
         <Button
           variant="secondary"

gui/src/hooks/datafeed-config.ts

@@ -17,6 +17,7 @@ export function useDataFeedConfig() {
   trackerData.rotationReferenceAdjusted = true;
   trackerData.rotationIdentityAdjusted = true;
   trackerData.tps = true;
+  trackerData.stayAligned = true;
 
   const dataMask = new DeviceDataMaskT();
   dataMask.deviceData = true;
@@ -27,6 +28,7 @@ export function useDataFeedConfig() {
   dataFeedConfig.boneMask = true;
   dataFeedConfig.minimumTimeSinceLast = 1000 / feedMaxTps;
   dataFeedConfig.syntheticTrackersMask = trackerData;
+  dataFeedConfig.stayAlignedMask = true;
 
   return {
     dataFeedConfig,

gui/src/maths/angle.ts

@@ -1,3 +1,7 @@
 export const DEG_TO_RAD = Math.PI / 180.0;
 
 export const RAD_TO_DEG = 180.0 / Math.PI;
+
+export function angleIsNearZero(angle: number, maxError = 1e-6): boolean {
+  return Math.abs(angle) < maxError;
+}

gui/src/maths/quaternion.ts

@@ -52,7 +52,8 @@ export function QuaternionToEulerDegrees(q?: QuatObject | null) {
   const angles = { x: 0, y: 0, z: 0 };
   if (!q) return angles;
 
-  const a = new Euler().setFromQuaternion(new Quaternion(q.x, q.y, q.z, q.w));
+  // When comparing tracker rotations, most important angle is yaw, so use YZX order which prioritizes yaw
+  const a = new Euler().setFromQuaternion(new Quaternion(q.x, q.y, q.z, q.w), 'YZX');
   return { x: a.x * RAD_TO_DEG, y: a.y * RAD_TO_DEG, z: a.z * RAD_TO_DEG };
 }
 

server/core/src/main/java/dev/slimevr/VRServer.kt

@@ -290,16 +290,16 @@ class VRServer @JvmOverloads constructor(
 		}
 	}
 
-	fun resetTrackersFull(resetSourceName: String?) {
-		queueTask { humanPoseManager.resetTrackersFull(resetSourceName) }
+	fun resetTrackersFull(params: ResetParams) {
+		queueTask { humanPoseManager.resetTrackersFull(params) }
 	}
 
-	fun resetTrackersYaw(resetSourceName: String?) {
-		queueTask { humanPoseManager.resetTrackersYaw(resetSourceName) }
+	fun resetTrackersYaw(params: ResetParams) {
+		queueTask { humanPoseManager.resetTrackersYaw(params) }
 	}
 
-	fun resetTrackersMounting(resetSourceName: String?) {
-		queueTask { humanPoseManager.resetTrackersMounting(resetSourceName) }
+	fun resetTrackersMounting(params: ResetParams) {
+		queueTask { humanPoseManager.resetTrackersMounting(params) }
 	}
 
 	fun clearTrackersMounting(resetSourceName: String?) {
@@ -334,7 +334,7 @@ class VRServer @JvmOverloads constructor(
 		}
 		timer.schedule(delay) {
 			queueTask {
-				humanPoseManager.resetTrackersFull(resetSourceName)
+				humanPoseManager.resetTrackersFull(ResetParams.makeDefault(resetSourceName))
 				resetHandler.sendFinished(ResetType.Full)
 			}
 		}
@@ -346,7 +346,7 @@ class VRServer @JvmOverloads constructor(
 		}
 		timer.schedule(delay) {
 			queueTask {
-				humanPoseManager.resetTrackersYaw(resetSourceName)
+				humanPoseManager.resetTrackersYaw(ResetParams.makeDefault(resetSourceName))
 				resetHandler.sendFinished(ResetType.Yaw)
 			}
 		}
@@ -358,7 +358,7 @@ class VRServer @JvmOverloads constructor(
 		}
 		timer.schedule(delay) {
 			queueTask {
-				humanPoseManager.resetTrackersMounting(resetSourceName)
+				humanPoseManager.resetTrackersMounting(ResetParams.makeDefault(resetSourceName))
 				resetHandler.sendFinished(ResetType.Mounting)
 			}
 		}

server/core/src/main/java/dev/slimevr/config/StayAlignedConfig.kt

@@ -0,0 +1,28 @@
+package dev.slimevr.config
+
+import com.fasterxml.jackson.annotation.JsonIgnore
+
+class StayAlignedConfig {
+
+	// Apply yaw correction
+	var enabled = false
+
+	// Applies extra yaw correction to support worse IMUs
+	//
+	// We could let players choose a yaw correction amount instead, but this lead to
+	// players agonizing about choosing the "right" yaw correction amount. In practice,
+	// we only need 2 yaw correction amounts - a default one for most IMUs, and an extra
+	// one for terrible IMUs.
+	var extraYawCorrection = false
+
+	// Temporarily hide the yaw correction that Stay Aligned is applying, so that the
+	// player can compare to if Stay Aligned was disabled. Do not serialize to config so
+	// that when the server restarts, it is always false.
+	@JsonIgnore
+	var hideYawCorrection = false
+
+	// Relaxed poses
+	val standingRelaxedPose = StayAlignedRelaxedPoseConfig()
+	val sittingRelaxedPose = StayAlignedRelaxedPoseConfig()
+	val flatRelaxedPose = StayAlignedRelaxedPoseConfig()
+}

server/core/src/main/java/dev/slimevr/config/StayAlignedRelaxedPoseConfig.kt

@@ -0,0 +1,8 @@
+package dev.slimevr.config
+
+class StayAlignedRelaxedPoseConfig {
+
+	var upperLegAngleInDeg = 0.0f
+	var lowerLegAngleInDeg = 0.0f
+	var footAngleInDeg = 0.0f
+}

server/core/src/main/java/dev/slimevr/config/VRConfig.kt

@@ -40,6 +40,8 @@ class VRConfig {
 
 	val resetsConfig: ResetsConfig = ResetsConfig()
 
+	val stayAlignedConfig = StayAlignedConfig()
+
 	@JsonDeserialize(using = TrackerConfigMapDeserializer::class)
 	@JsonSerialize(keyUsing = StdKeySerializers.StringKeySerializer::class)
 	private val trackers: MutableMap<String, TrackerConfig> = HashMap()

server/core/src/main/java/dev/slimevr/math/Angle.kt

@@ -0,0 +1,84 @@
+package dev.slimevr.math
+
+import com.jme3.math.FastMath
+import io.github.axisangles.ktmath.Quaternion
+import io.github.axisangles.ktmath.Vector3
+import kotlin.math.*
+
+/**
+ * An angle between [-PI, PI).
+ */
+@JvmInline
+value class Angle(private val rad: Float) {
+
+	fun toRad() =
+		rad
+
+	fun toDeg() =
+		rad * FastMath.RAD_TO_DEG
+
+	operator fun unaryPlus() =
+		this
+
+	operator fun unaryMinus() =
+		Angle(normalize(-rad))
+
+	operator fun plus(other: Angle) =
+		Angle(normalize(rad + other.rad))
+
+	operator fun minus(other: Angle) =
+		Angle(normalize(rad - other.rad))
+
+	operator fun times(scale: Float) =
+		Angle(normalize(rad * scale))
+
+	operator fun div(scale: Float) =
+		Angle(normalize(rad / scale))
+
+	operator fun compareTo(other: Angle) =
+		rad.compareTo(other.rad)
+
+	override fun toString() =
+		"${toDeg()} deg"
+
+	companion object {
+		val ZERO = Angle(0.0f)
+
+		fun ofRad(rad: Float) =
+			Angle(normalize(rad))
+
+		fun ofDeg(deg: Float) =
+			Angle(normalize(deg * FastMath.DEG_TO_RAD))
+
+		fun abs(angle: Angle) =
+			Angle(normalize(abs(angle.rad)))
+
+		// Angle between two vectors
+		fun absBetween(a: Vector3, b: Vector3) =
+			Angle(normalize(a.angleTo(b)))
+
+		// Angle between two rotations in rotation space
+		fun absBetween(a: Quaternion, b: Quaternion) =
+			Angle(normalize(a.angleToR(b)))
+
+		/**
+		 * Normalizes an angle to [-PI, PI)
+		 */
+		private fun normalize(rad: Float): Float {
+			// Normalize to [0, 2*PI)
+			val r =
+				if (rad < 0.0f || rad >= FastMath.TWO_PI) {
+					rad - floor(rad * FastMath.INV_TWO_PI) * FastMath.TWO_PI
+				} else {
+					rad
+				}
+
+			// Normalize to [-PI, PI)
+			return if (r > FastMath.PI) {
+				r - FastMath.TWO_PI
+			} else {
+				r
+			}
+		}
+	}
+}

server/core/src/main/java/dev/slimevr/math/AngleAverage.kt

@@ -0,0 +1,29 @@
+package dev.slimevr.math
+
+import kotlin.math.*
+
+/**
+ * Averages angles by summing vectors.
+ *
+ * See https://www.themathdoctors.org/averaging-angles/
+ */
+class AngleAverage {
+
+	private var sumX = 0.0f
+	private var sumY = 0.0f
+
+	fun add(angle: Angle, weight: Float = 1.0f) {
+		sumX += cos(angle.toRad()) * weight
+		sumY += sin(angle.toRad()) * weight
+	}
+
+	fun toAngle(): Angle =
+		if (isEmpty()) {
+			Angle.ZERO
+		} else {
+			Angle.ofRad(atan2(sumY, sumX))
+		}
+
+	fun isEmpty() =
+		sumX == 0.0f && sumY == 0.0f
+}

server/core/src/main/java/dev/slimevr/protocol/datafeed/DataFeedBuilder.java

@@ -185,6 +185,12 @@ public class DataFeedBuilder {
 		int trackerInfosOffset = DataFeedBuilder.createTrackerInfos(fbb, mask.getInfo(), tracker);
 		int trackerIdOffset = DataFeedBuilder.createTrackerId(fbb, tracker);
 
+		int stayAlignedOffset = 0;
+		if (mask.getStayAligned()) {
+			stayAlignedOffset = DataFeedBuilderKotlin.INSTANCE
+				.createTrackerStayAlignedTracker(fbb, tracker.getStayAligned());
+		}
+
 		TrackerData.startTrackerData(fbb);
 
 		TrackerData.addTrackerId(fbb, trackerIdOffset);
@@ -233,6 +239,9 @@ public class DataFeedBuilder {
 		if (mask.getTps()) {
 			TrackerData.addTps(fbb, (int) tracker.getTps());
 		}
+		if (mask.getStayAligned()) {
+			TrackerData.addStayAligned(fbb, stayAlignedOffset);
+		}
 
 		return TrackerData.endTrackerData(fbb);
 	}

server/core/src/main/java/dev/slimevr/protocol/datafeed/DataFeedBuilderKotlin.kt

@@ -0,0 +1,41 @@
+package dev.slimevr.protocol.datafeed
+
+import com.google.flatbuffers.FlatBufferBuilder
+import dev.slimevr.tracking.processor.skeleton.HumanSkeleton
+import dev.slimevr.tracking.processor.stayaligned.skeleton.RelaxedPose.Companion.fromTrackers
+import dev.slimevr.tracking.processor.stayaligned.state.StayAlignedTrackerState
+import solarxr_protocol.data_feed.stay_aligned.StayAlignedData
+import solarxr_protocol.data_feed.stay_aligned.StayAlignedTracker
+
+object DataFeedBuilderKotlin {
+
+	fun createStayAlignedData(
+		fbb: FlatBufferBuilder,
+		humanSkeleton: HumanSkeleton,
+	): Int {
+		val relaxedPose = fromTrackers(humanSkeleton)
+
+		StayAlignedData.startStayAlignedData(fbb)
+
+		StayAlignedData.addUpperLegAngleInDeg(fbb, relaxedPose.upperLeg.toDeg())
+		StayAlignedData.addLowerLegAngleInDeg(fbb, relaxedPose.lowerLeg.toDeg())
+		StayAlignedData.addFootAngleInDeg(fbb, relaxedPose.foot.toDeg())
+
+		return StayAlignedData.endStayAlignedData(fbb)
+	}
+
+	fun createTrackerStayAlignedTracker(
+		fbb: FlatBufferBuilder,
+		state: StayAlignedTrackerState,
+	): Int {
+		StayAlignedTracker.startStayAlignedTracker(fbb)
+
+		StayAlignedTracker.addYawCorrectionInDeg(fbb, state.yawCorrection.yaw.toDeg())
+		StayAlignedTracker.addLockedErrorInDeg(fbb, state.yawErrors.lockedError.toDeg())
+		StayAlignedTracker.addCenterErrorInDeg(fbb, state.yawErrors.centerError.toDeg())
+		StayAlignedTracker.addNeighborErrorInDeg(fbb, state.yawErrors.neighborError.toDeg())
+		StayAlignedTracker.addLocked(fbb, state.lockedRotation != null)
+
+		return StayAlignedTracker.endStayAlignedTracker(fbb)
+	}
+}

server/core/src/main/java/dev/slimevr/protocol/datafeed/DataFeedHandler.java

@@ -98,7 +98,20 @@ public class DataFeedHandler extends ProtocolHandler<DataFeedMessageHeader> {
 				h.getAllBones()
 			);
 
-		return DataFeedUpdate.createDataFeedUpdate(fbb, devicesOffset, trackersOffset, bonesOffset);
+		int stayAlignedOffset = 0;
+		if (config.getStayAlignedMask()) {
+			stayAlignedOffset = DataFeedBuilderKotlin.INSTANCE
+				.createStayAlignedData(fbb, this.api.server.humanPoseManager.skeleton);
+		}
+
+		return DataFeedUpdate
+			.createDataFeedUpdate(
+				fbb,
+				devicesOffset,
+				trackersOffset,
+				bonesOffset,
+				stayAlignedOffset
+			);
 	}
 
 	public void sendDataFeedUpdate() {

server/core/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.kt

@@ -11,6 +11,7 @@ import dev.slimevr.protocol.rpc.firmware.RPCFirmwareUpdateHandler
 import dev.slimevr.protocol.rpc.reset.RPCResetHandler
 import dev.slimevr.protocol.rpc.serial.RPCProvisioningHandler
 import dev.slimevr.protocol.rpc.serial.RPCSerialHandler
+import dev.slimevr.protocol.rpc.settings.RPCSettingsBuilder
 import dev.slimevr.protocol.rpc.settings.RPCSettingsHandler
 import dev.slimevr.protocol.rpc.setup.RPCHandshakeHandler
 import dev.slimevr.protocol.rpc.setup.RPCTapSetupHandler
@@ -18,6 +19,9 @@ import dev.slimevr.protocol.rpc.setup.RPCUtil.getLocalIp
 import dev.slimevr.protocol.rpc.status.RPCStatusHandler
 import dev.slimevr.protocol.rpc.trackingpause.RPCTrackingPause
 import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets
+import dev.slimevr.tracking.processor.stayaligned.skeleton.RelaxedPose
+import dev.slimevr.tracking.trackers.ResetBodyPose
+import dev.slimevr.tracking.trackers.ResetParams
 import dev.slimevr.tracking.trackers.TrackerPosition
 import dev.slimevr.tracking.trackers.TrackerPosition.Companion.getByBodyPart
 import dev.slimevr.tracking.trackers.TrackerStatus
@@ -204,6 +208,24 @@ class RPCHandler(private val api: ProtocolAPI) : ProtocolHandler<RpcMessageHeade
 		) { conn: GenericConnection, messageHeader: RpcMessageHeader ->
 			this.onChangeMagToggleRequest(conn, messageHeader)
 		}
+
+		registerPacketListener(
+			RpcMessage.EnableStayAlignedRequest,
+		) { conn: GenericConnection, messageHeader: RpcMessageHeader ->
+			this.onEnableStayAlignedRequest(conn, messageHeader)
+		}
+
+		registerPacketListener(
+			RpcMessage.DetectStayAlignedRelaxedPoseRequest,
+		) { conn: GenericConnection, messageHeader: RpcMessageHeader ->
+			this.onDetectStayAlignedRelaxedPoseRequest(conn, messageHeader)
+		}
+
+		registerPacketListener(
+			RpcMessage.ResetStayAlignedRelaxedPoseRequest,
+		) { conn: GenericConnection, messageHeader: RpcMessageHeader ->
+			this.onResetStayAlignedRelaxedPoseRequest(conn, messageHeader)
+		}
 	}
 
 	private fun onServerInfosRequest(
@@ -324,9 +346,40 @@ class RPCHandler(private val api: ProtocolAPI) : ProtocolHandler<RpcMessageHeade
 	fun onResetRequest(conn: GenericConnection, messageHeader: RpcMessageHeader) {
 		val req = messageHeader.message(ResetRequest()) as? ResetRequest ?: return
 
-		if (req.resetType() == ResetType.Yaw) api.server.resetTrackersYaw(RESET_SOURCE_NAME)
-		if (req.resetType() == ResetType.Full) api.server.resetTrackersFull(RESET_SOURCE_NAME)
-		if (req.resetType() == ResetType.Mounting) api.server.resetTrackersMounting(RESET_SOURCE_NAME)
+		val bodyPose =
+			if (req.hasBodyPose()) {
+				ResetBodyPose.getById(req.bodyPose()) ?: ResetBodyPose.SKIING
+			} else {
+				ResetBodyPose.SKIING
+			}
+
+		val referenceTrackerPosition =
+			if (req.hasReferenceTracker()) {
+				TrackerPosition.getByBodyPart(req.referenceTracker()) ?: TrackerPosition.HEAD
+			} else {
+				TrackerPosition.HEAD
+			}
+
+		val trackerPositions = buildSet {
+			for (i in 0 until req.trackersLength()) {
+				TrackerPosition.getByBodyPart(req.trackers(i))?.let {
+					add(it)
+				}
+			}
+		}
+
+		val params = ResetParams(
+			RESET_SOURCE_NAME,
+			bodyPose,
+			referenceTrackerPosition,
+			trackerPositions,
+		)
+
+		LogManager.info("${params.source} ${params.bodyPose} ${params.referenceTrackerPosition} ${params.trackerPositionsToReset}")
+
+		if (req.resetType() == ResetType.Yaw) api.server.resetTrackersYaw(params)
+		if (req.resetType() == ResetType.Full) api.server.resetTrackersFull(params)
+		if (req.resetType() == ResetType.Mounting) api.server.resetTrackersMounting(params)
 	}
 
 	fun onClearMountingResetRequest(
@@ -590,6 +643,84 @@ class RPCHandler(private val api: ProtocolAPI) : ProtocolHandler<RpcMessageHeade
 		}
 	}
 
+	private fun onEnableStayAlignedRequest(conn: GenericConnection, messageHeader: RpcMessageHeader) {
+		val enableRequest =
+			messageHeader.message(EnableStayAlignedRequest()) as? EnableStayAlignedRequest
+				?: return
+
+		val configManager = api.server.configManager
+
+		val config = configManager.vrConfig.stayAlignedConfig
+		config.enabled = enableRequest.enable()
+
+		configManager.saveConfig()
+
+		sendSettingsChangedResponse(conn)
+	}
+
+	private fun onDetectStayAlignedRelaxedPoseRequest(conn: GenericConnection, messageHeader: RpcMessageHeader) {
+		val resetRequest =
+			messageHeader.message(ResetStayAlignedRelaxedPoseRequest()) as? ResetStayAlignedRelaxedPoseRequest
+				?: return
+
+		setStayAlignedRelaxedPose(
+			resetRequest.pose(),
+			RelaxedPose.fromTrackers(api.server.humanPoseManager.skeleton),
+		)
+
+		sendSettingsChangedResponse(conn)
+	}
+
+	private fun onResetStayAlignedRelaxedPoseRequest(conn: GenericConnection, messageHeader: RpcMessageHeader) {
+		val resetRequest =
+			messageHeader.message(ResetStayAlignedRelaxedPoseRequest()) as? ResetStayAlignedRelaxedPoseRequest
+				?: return
+
+		setStayAlignedRelaxedPose(resetRequest.pose(), RelaxedPose.ZERO)
+
+		sendSettingsChangedResponse(conn)
+	}
+
+	private fun setStayAlignedRelaxedPose(
+		pose: Int,
+		relaxedPose: RelaxedPose,
+	) {
+		val configManager = api.server.configManager
+		val config = configManager.vrConfig.stayAlignedConfig
+
+		when (pose) {
+			StayAlignedRelaxedPose.STANDING -> {
+				config.standingRelaxedPose.upperLegAngleInDeg = relaxedPose.upperLeg.toDeg()
+				config.standingRelaxedPose.lowerLegAngleInDeg = relaxedPose.lowerLeg.toDeg()
+				config.standingRelaxedPose.footAngleInDeg = relaxedPose.foot.toDeg()
+			}
+
+			StayAlignedRelaxedPose.SITTING -> {
+				config.sittingRelaxedPose.upperLegAngleInDeg = relaxedPose.upperLeg.toDeg()
+				config.sittingRelaxedPose.lowerLegAngleInDeg = relaxedPose.lowerLeg.toDeg()
+				config.sittingRelaxedPose.footAngleInDeg = relaxedPose.foot.toDeg()
+			}
+
+			StayAlignedRelaxedPose.FLAT -> {
+				config.flatRelaxedPose.upperLegAngleInDeg = relaxedPose.upperLeg.toDeg()
+				config.flatRelaxedPose.lowerLegAngleInDeg = relaxedPose.lowerLeg.toDeg()
+				config.flatRelaxedPose.footAngleInDeg = relaxedPose.foot.toDeg()
+			}
+		}
+
+		configManager.saveConfig()
+
+		LogManager.info("[setStayAlignedRelaxedPose] pose=$pose $relaxedPose")
+	}
+
+	fun sendSettingsChangedResponse(conn: GenericConnection) {
+		val fbb = FlatBufferBuilder(32)
+		val settings = RPCSettingsBuilder.createSettingsResponse(fbb, api.server)
+		val outbound = createRPCMessage(fbb, RpcMessage.SettingsResponse, settings)
+		fbb.finish(outbound)
+		conn.send(fbb.dataBuffer())
+	}
+
 	companion object {
 		private const val RESET_SOURCE_NAME = "WebSocketAPI"
 	}

server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java

@@ -408,7 +408,11 @@ public class RPCSettingsBuilder {
 						fbb,
 						server.configManager.getVrConfig().getResetsConfig()
 					),
-				0
+				RPCSettingsBuilderKotlin.INSTANCE
+					.createStayAlignedSettings(
+						fbb,
+						server.configManager.getVrConfig().getStayAlignedConfig()
+					)
 			);
 	}
 }

server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilderKotlin.kt

@@ -0,0 +1,29 @@
+package dev.slimevr.protocol.rpc.settings
+
+import com.google.flatbuffers.FlatBufferBuilder
+import dev.slimevr.config.StayAlignedConfig
+import solarxr_protocol.rpc.StayAlignedSettings
+
+object RPCSettingsBuilderKotlin {
+
+	fun createStayAlignedSettings(
+		fbb: FlatBufferBuilder,
+		config: StayAlignedConfig,
+	): Int =
+		StayAlignedSettings
+			.createStayAlignedSettings(
+				fbb,
+				config.enabled,
+				config.extraYawCorrection,
+				config.hideYawCorrection,
+				config.standingRelaxedPose.upperLegAngleInDeg,
+				config.standingRelaxedPose.lowerLegAngleInDeg,
+				config.standingRelaxedPose.footAngleInDeg,
+				config.sittingRelaxedPose.upperLegAngleInDeg,
+				config.sittingRelaxedPose.lowerLegAngleInDeg,
+				config.sittingRelaxedPose.footAngleInDeg,
+				config.flatRelaxedPose.upperLegAngleInDeg,
+				config.flatRelaxedPose.lowerLegAngleInDeg,
+				config.flatRelaxedPose.footAngleInDeg,
+			)
+}

server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt

@@ -39,12 +39,7 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
 	}
 
 	fun onSettingsRequest(conn: GenericConnection, messageHeader: RpcMessageHeader?) {
-		val fbb = FlatBufferBuilder(32)
-
-		val settings = RPCSettingsBuilder.createSettingsResponse(fbb, api.server)
-		val outbound = rpcHandler.createRPCMessage(fbb, RpcMessage.SettingsResponse, settings)
-		fbb.finish(outbound)
-		conn.send(fbb.dataBuffer())
+		rpcHandler.sendSettingsChangedResponse(conn)
 	}
 
 	fun onChangeSettingsRequest(conn: GenericConnection?, messageHeader: RpcMessageHeader) {
@@ -354,6 +349,22 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
 			resetsConfig.updateTrackersResetsSettings()
 		}
 
+		if (req.stayAligned() != null) {
+			val config = api.server.configManager.vrConfig.stayAlignedConfig
+			config.enabled = req.stayAligned().enabled()
+			config.extraYawCorrection = req.stayAligned().extraYawCorrection()
+			config.hideYawCorrection = req.stayAligned().hideYawCorrection()
+			config.standingRelaxedPose.upperLegAngleInDeg = req.stayAligned().standingUpperLegAngle()
+			config.standingRelaxedPose.lowerLegAngleInDeg = req.stayAligned().standingLowerLegAngle()
+			config.standingRelaxedPose.footAngleInDeg = req.stayAligned().standingFootAngle()
+			config.sittingRelaxedPose.upperLegAngleInDeg = req.stayAligned().sittingUpperLegAngle()
+			config.sittingRelaxedPose.lowerLegAngleInDeg = req.stayAligned().sittingLowerLegAngle()
+			config.sittingRelaxedPose.footAngleInDeg = req.stayAligned().sittingFootAngle()
+			config.flatRelaxedPose.upperLegAngleInDeg = req.stayAligned().flatUpperLegAngle()
+			config.flatRelaxedPose.lowerLegAngleInDeg = req.stayAligned().flatLowerLegAngle()
+			config.flatRelaxedPose.footAngleInDeg = req.stayAligned().flatFootAngle()
+		}
+
 		api.server.configManager.saveConfig()
 	}
 

server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt

@@ -4,11 +4,13 @@ import com.jme3.math.FastMath
 import dev.slimevr.VRServer
 import dev.slimevr.VRServer.Companion.getNextLocalTrackerId
 import dev.slimevr.config.ConfigManager
+import dev.slimevr.math.Angle
 import dev.slimevr.tracking.processor.config.SkeletonConfigManager
 import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets
 import dev.slimevr.tracking.processor.config.SkeletonConfigToggles
 import dev.slimevr.tracking.processor.config.SkeletonConfigValues
 import dev.slimevr.tracking.processor.skeleton.HumanSkeleton
+import dev.slimevr.tracking.trackers.ResetParams
 import dev.slimevr.tracking.trackers.Tracker
 import dev.slimevr.tracking.trackers.TrackerPosition
 import dev.slimevr.tracking.trackers.TrackerRole
@@ -481,8 +483,8 @@ class HumanPoseManager(val server: VRServer?) {
 		skeletonConfigManager.computeNodeOffset(node)
 	}
 
-	fun resetTrackersFull(resetSourceName: String?) {
-		skeleton.resetTrackersFull(resetSourceName)
+	fun resetTrackersFull(params: ResetParams) {
+		skeleton.resetTrackersFull(params)
 		if (server != null) {
 			if (skeleton.headTracker == null && skeleton.neckTracker == null) {
 				server.vrcOSCHandler.yawAlign(IDENTITY)
@@ -497,8 +499,8 @@ class HumanPoseManager(val server: VRServer?) {
 		}
 	}
 
-	fun resetTrackersYaw(resetSourceName: String?) {
-		skeleton.resetTrackersYaw(resetSourceName)
+	fun resetTrackersYaw(params: ResetParams) {
+		skeleton.resetTrackersYaw(params)
 		if (server != null) {
 			if (skeleton.headTracker == null && skeleton.neckTracker == null) {
 				server.vrcOSCHandler.yawAlign(IDENTITY)
@@ -559,6 +561,13 @@ class HumanPoseManager(val server: VRServer?) {
 					.append(" deg (")
 					.append(Precision.round(driftPerMin, 4))
 					.append(" deg/min)")
+
+				if (tracker.stayAligned.yawCorrection.yawAtLastReset != Angle.ZERO) {
+					trackersDriftText
+						.append(" (stay aligned yaw correction ")
+						.append(tracker.stayAligned.yawCorrection.yawAtLastReset.toDeg())
+						.append(" deg)")
+				}
 			}
 		}
 
@@ -570,8 +579,8 @@ class HumanPoseManager(val server: VRServer?) {
 		}
 	}
 
-	fun resetTrackersMounting(resetSourceName: String?) {
-		skeleton.resetTrackersMounting(resetSourceName)
+	fun resetTrackersMounting(params: ResetParams) {
+		skeleton.resetTrackersMounting(params)
 	}
 
 	fun clearTrackersMounting(resetSourceName: String?) {

server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt

@@ -2,6 +2,7 @@ package dev.slimevr.tracking.processor.skeleton
 
 import com.jme3.math.FastMath
 import dev.slimevr.VRServer
+import dev.slimevr.config.StayAlignedConfig
 import dev.slimevr.tracking.processor.Bone
 import dev.slimevr.tracking.processor.BoneType
 import dev.slimevr.tracking.processor.Constraint
@@ -9,6 +10,11 @@ import dev.slimevr.tracking.processor.Constraint.Companion.ConstraintType
 import dev.slimevr.tracking.processor.HumanPoseManager
 import dev.slimevr.tracking.processor.config.SkeletonConfigToggles
 import dev.slimevr.tracking.processor.config.SkeletonConfigValues
+import dev.slimevr.tracking.processor.stayaligned.StayAligned
+import dev.slimevr.tracking.processor.stayaligned.skeleton.PlayerPose
+import dev.slimevr.tracking.processor.stayaligned.skeleton.RelaxedPose
+import dev.slimevr.tracking.processor.stayaligned.skeleton.TrackerSkeleton
+import dev.slimevr.tracking.trackers.ResetParams
 import dev.slimevr.tracking.trackers.Tracker
 import dev.slimevr.tracking.trackers.TrackerPosition
 import dev.slimevr.tracking.trackers.TrackerRole
@@ -214,6 +220,10 @@ class HumanSkeleton(
 	var tapDetectionManager = TapDetectionManager(this)
 	var localizer = Localizer(this)
 
+	// Stay Aligned
+	var trackerSkeleton = TrackerSkeleton(this)
+	var stayAlignedConfig = StayAlignedConfig().also { it.enabled = false }
+
 	// Constructors
 	init {
 		assembleSkeleton()
@@ -235,6 +245,7 @@ class HumanSkeleton(
 		)
 		legTweaks.setConfig(server.configManager.vrConfig.legTweaks)
 		localizer.setEnabled(humanPoseManager.getToggle(SkeletonConfigToggles.SELF_LOCALIZATION))
+		stayAlignedConfig = server.configManager.vrConfig.stayAlignedConfig
 	}
 
 	constructor(
@@ -452,6 +463,9 @@ class HumanSkeleton(
 
 		// Update bones tracker field
 		refreshBoneTracker()
+
+		// Update tracker skeleton
+		trackerSkeleton = TrackerSkeleton(this)
 	}
 
 	/**
@@ -508,6 +522,10 @@ class HumanSkeleton(
 	fun updatePose() {
 		tapDetectionManager.update()
 
+		val pose = PlayerPose.ofTrackers(trackerSkeleton)
+		val relaxedPose = RelaxedPose.forPose(pose, stayAlignedConfig)
+		StayAligned.adjustNextTracker(trackerSkeleton, stayAlignedConfig, relaxedPose)
+
 		updateTransforms()
 		updateBones()
 		if (enforceConstraints) {
@@ -1551,17 +1569,25 @@ class HumanSkeleton(
 			rightLittleDistalTracker,
 		)
 
-	fun resetTrackersFull(resetSourceName: String?) {
+	fun resetTrackersFull(params: ResetParams) {
 		var referenceRotation = IDENTITY
-		headTracker?.let {
-			// Always reset the head (ifs in resetsHandler)
-			it.resetsHandler.resetFull(referenceRotation)
-			referenceRotation = it.getRotation()
+		if (params.referenceTrackerPosition == TrackerPosition.HEAD) {
+			headTracker?.let {
+				// Always reset the head (ifs in resetsHandler)
+				it.resetsHandler.resetFull(referenceRotation)
+				referenceRotation = it.getRotation()
+			}
+		} else {
+			val referenceTracker = trackersToReset.find { it?.trackerPosition == params.referenceTrackerPosition }
+			referenceTracker?.let {
+				// Just use the rotation without resetting
+				referenceRotation = it.getRotation()
+			}
 		}
 		// Resets all axes of the trackers with the HMD as reference.
 		for (tracker in trackersToReset) {
 			// Only reset if tracker needsReset
-			if (tracker != null && (tracker.needsReset || tracker.isHmd)) {
+			if (tracker != null && (tracker.needsReset || tracker.isHmd) && params.shouldReset(tracker)) {
 				tracker.resetsHandler.resetFull(referenceRotation)
 			}
 		}
@@ -1573,56 +1599,73 @@ class HumanSkeleton(
 		}
 		legTweaks.resetBuffer()
 		localizer.reset()
-		LogManager.info("[HumanSkeleton] Reset: full ($resetSourceName)")
+		LogManager.info("[HumanSkeleton] Reset: full (${params.source})")
 	}
 
 	@VRServerThread
-	fun resetTrackersYaw(resetSourceName: String?) {
-		// Resets the yaw of the trackers with the head as reference.
+	fun resetTrackersYaw(params: ResetParams) {
 		var referenceRotation = IDENTITY
-		headTracker?.let {
-			// Only reset if head needsReset and isn't computed
-			if (it.needsReset && !it.isComputed) {
-				it.resetsHandler.resetYaw(referenceRotation)
+		if (params.referenceTrackerPosition == TrackerPosition.HEAD) {
+			headTracker?.let {
+				// Only reset if head needsReset and isn't computed
+				if (it.needsReset && !it.isComputed) {
+					it.resetsHandler.resetYaw(referenceRotation)
+				}
+				referenceRotation = it.getRotation()
+			}
+		} else {
+			val referenceTracker = trackersToReset.find { it?.trackerPosition == params.referenceTrackerPosition }
+			referenceTracker?.let {
+				// Just use the rotation without resetting
+				referenceRotation = it.getRotation()
 			}
-			referenceRotation = it.getRotation()
 		}
 		for (tracker in trackersToReset) {
 			// Only reset if tracker needsReset
-			if (tracker != null && tracker.needsReset) {
+			if (tracker != null && tracker.needsReset && params.shouldReset(tracker)) {
 				tracker.resetsHandler.resetYaw(referenceRotation)
 			}
 		}
 		legTweaks.resetBuffer()
-		LogManager.info("[HumanSkeleton] Reset: yaw ($resetSourceName)")
+		LogManager.info("[HumanSkeleton] Reset: yaw (${params.source})")
 	}
 
 	@VRServerThread
-	fun resetTrackersMounting(resetSourceName: String?) {
+	fun resetTrackersMounting(params: ResetParams) {
+		// resetTrackersYaw(params)
+
 		val server = humanPoseManager.server
 		if (server != null && server.statusSystem.hasStatusType(StatusData.StatusTrackerReset)) {
-			LogManager.info("[HumanSkeleton] Reset: mounting ($resetSourceName) failed, reset required")
+			LogManager.info("[HumanSkeleton] Reset: mounting (${params.source}) failed, reset required")
 			return
 		}
 
 		// Resets the mounting orientation of the trackers with the HMD as reference.
 		var referenceRotation = IDENTITY
-		headTracker?.let {
-			// Only reset if head needsMounting or is computed but not HMD
-			if (it.needsMounting || (it.isComputed && !it.isHmd)) {
-				it.resetsHandler.resetMounting(referenceRotation)
+		if (params.referenceTrackerPosition == TrackerPosition.HEAD) {
+			headTracker?.let {
+				// Only reset if head needsMounting or is computed but not HMD
+				if (it.needsMounting || (it.isComputed && !it.isHmd)) {
+					it.resetsHandler.resetMounting(referenceRotation, params.bodyPose)
+				}
+				referenceRotation = it.getRotation()
+			}
+		} else {
+			val referenceTracker = trackersToReset.find { it?.trackerPosition == params.referenceTrackerPosition }
+			referenceTracker?.let {
+				// Just use the rotation without resetting
+				referenceRotation = it.getRotation()
 			}
-			referenceRotation = it.getRotation()
 		}
 		for (tracker in trackersToReset) {
 			// Only reset if tracker needsMounting
-			if (tracker != null && tracker.needsMounting) {
-				tracker.resetsHandler.resetMounting(referenceRotation)
+			if (tracker != null && tracker.needsMounting && params.shouldReset(tracker)) {
+				tracker.resetsHandler.resetMounting(referenceRotation, params.bodyPose)
 			}
 		}
 		legTweaks.resetBuffer()
 		localizer.reset()
-		LogManager.info("[HumanSkeleton] Reset: mounting ($resetSourceName)")
+		LogManager.info("[HumanSkeleton] Reset: mounting (${params.source})")
 	}
 
 	@VRServerThread

server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/TapDetectionManager.java

@@ -6,6 +6,7 @@ import dev.slimevr.config.TapDetectionConfig;
 import dev.slimevr.reset.ResetHandler;
 import dev.slimevr.setup.TapSetupHandler;
 import dev.slimevr.tracking.processor.HumanPoseManager;
+import dev.slimevr.tracking.trackers.ResetParams;
 import dev.slimevr.tracking.trackers.Tracker;
 import solarxr_protocol.rpc.ResetType;
 import solarxr_protocol.rpc.StatusData;
@@ -163,9 +164,10 @@ public class TapDetectionManager {
 			tapped && System.nanoTime() - yawResetDetector.getDetectionTime() > yawResetDelayNs
 		) {
 			if (humanPoseManager != null)
-				humanPoseManager.resetTrackersYaw(resetSourceName);
+				humanPoseManager
+					.resetTrackersYaw(ResetParams.Companion.makeDefault(resetSourceName));
 			else
-				skeleton.resetTrackersYaw(resetSourceName);
+				skeleton.resetTrackersYaw(ResetParams.Companion.makeDefault(resetSourceName));
 
 			yawResetDetector.resetDetector();
 			yawResetAllowPlaySound = true;
@@ -185,9 +187,10 @@ public class TapDetectionManager {
 			tapped && System.nanoTime() - fullResetDetector.getDetectionTime() > fullResetDelayNs
 		) {
 			if (humanPoseManager != null)
-				humanPoseManager.resetTrackersFull(resetSourceName);
+				humanPoseManager
+					.resetTrackersFull(ResetParams.Companion.makeDefault(resetSourceName));
 			else
-				skeleton.resetTrackersFull(resetSourceName);
+				skeleton.resetTrackersFull(ResetParams.Companion.makeDefault(resetSourceName));
 
 			fullResetDetector.resetDetector();
 			fullResetAllowPlaySound = true;
@@ -215,7 +218,7 @@ public class TapDetectionManager {
 				&& System.nanoTime() - mountingResetDetector.getDetectionTime()
 					> mountingResetDelayNs
 		) {
-			skeleton.resetTrackersMounting(resetSourceName);
+			skeleton.resetTrackersMounting(ResetParams.Companion.makeDefault(resetSourceName));
 			mountingResetDetector.resetDetector();
 			mountingResetAllowPlaySound = true;
 			this.resetHandler.sendFinished(ResetType.Mounting);

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/StayAligned.kt

@@ -0,0 +1,58 @@
+package dev.slimevr.tracking.processor.stayaligned
+
+import dev.slimevr.VRServer
+import dev.slimevr.config.StayAlignedConfig
+import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults.EXTRA_YAW_CORRECTION_PER_SEC
+import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults.YAW_CORRECTION_PER_SEC
+import dev.slimevr.tracking.processor.stayaligned.adjust.AdjustTrackerYaw
+import dev.slimevr.tracking.processor.stayaligned.skeleton.RelaxedPose
+import dev.slimevr.tracking.processor.stayaligned.skeleton.TrackerSkeleton
+
+/**
+ * Manager to keep the trackers aligned.
+ */
+object StayAligned {
+
+	private var nextTrackerIndex = 0
+
+	/**
+	 * Adjusts the yaw of the next tracker.
+	 *
+	 * We only adjust one tracker per tick to minimize CPU usage. When the server is
+	 * running at 1000 Hz and there are 20 trackers, each tracker is still updated 50
+	 * times a second.
+	 */
+	fun adjustNextTracker(
+		trackers: TrackerSkeleton,
+		config: StayAlignedConfig,
+		relaxedPose: RelaxedPose?,
+	) {
+		if (!config.enabled) {
+			return
+		}
+
+		val numTrackers = trackers.allTrackers.size
+		if (numTrackers == 0) {
+			return
+		}
+
+		val tracker = trackers.allTrackers[nextTrackerIndex % numTrackers]
+
+		var yawCorrectionPerSec = YAW_CORRECTION_PER_SEC
+		if (config.extraYawCorrection) {
+			yawCorrectionPerSec += EXTRA_YAW_CORRECTION_PER_SEC
+		}
+
+		AdjustTrackerYaw.adjust(
+			tracker,
+			trackers,
+			// Scale yaw correction since we're only updating one tracker per tick
+			yawCorrectionPerSec
+				* VRServer.instance.fpsTimer.timePerFrame
+				* numTrackers.toFloat(),
+			relaxedPose,
+		)
+
+		++nextTrackerIndex
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/StayAlignedDefaults.kt

@@ -0,0 +1,46 @@
+package dev.slimevr.tracking.processor.stayaligned
+
+import dev.slimevr.math.Angle
+import dev.slimevr.tracking.processor.stayaligned.skeleton.RelaxedPose
+import dev.slimevr.tracking.processor.stayaligned.state.RestDetector
+import kotlin.time.Duration.Companion.seconds
+
+/**
+ * All non-user-configurable defaults used by Stay Aligned, so that we can tune the
+ * algorithm from a single place.
+ */
+object StayAlignedDefaults {
+
+	// Maximum yaw correction to apply
+	val YAW_CORRECTION_PER_SEC = Angle.ofDeg(0.20f)
+
+	// Extra yaw correction to apply to terrible IMUs
+	val EXTRA_YAW_CORRECTION_PER_SEC = Angle.ofDeg(0.20f)
+
+	// Rest detector for detecting when trackers are at rest
+	fun restDetector() =
+		RestDetector(
+			maxRotation = Angle.ofDeg(3.0f),
+			minDuration = 2.seconds,
+		)
+
+	// Relaxed pose for kneeling. This isn't that common, so we don't want to ask
+	// players to provide this relaxed pose during setup.
+	val RELAXED_POSE_KNEELING =
+		RelaxedPose(
+			upperLeg = Angle.ofDeg(0.0f),
+			lowerLeg = Angle.ofDeg(0.0f),
+			foot = Angle.ofDeg(0.0f),
+		)
+
+	// Weights to calculate the average yaw of the skeleton
+	const val CENTER_ERROR_HEAD_WEIGHT = 1.0f
+	const val CENTER_ERROR_UPPER_BODY_WEIGHT = 1.0f
+	const val CENTER_ERROR_UPPER_LEG_WEIGHT = 0.4f
+	const val CENTER_ERROR_LOWER_LEG_WEIGHT = 0.3f
+
+	// Weight of each force
+	const val YAW_ERRORS_LOCKED_ERROR_WEIGHT = 10.0f
+	const val YAW_ERRORS_CENTER_ERROR_WEIGHT = 2.0f
+	const val YAW_ERRORS_NEIGHBOR_ERROR_WEIGHT = 1.0f
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/adjust/AdjustTrackerYaw.kt

@@ -0,0 +1,176 @@
+package dev.slimevr.tracking.processor.stayaligned.adjust
+
+import dev.slimevr.math.Angle
+import dev.slimevr.math.Angle.Companion.abs
+import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults
+import dev.slimevr.tracking.processor.stayaligned.skeleton.RelaxedPose
+import dev.slimevr.tracking.processor.stayaligned.skeleton.TrackerSkeleton
+import dev.slimevr.tracking.processor.stayaligned.state.YawErrors
+import dev.slimevr.tracking.trackers.Tracker
+import dev.slimevr.tracking.trackers.udp.MagnetometerStatus
+
+object AdjustTrackerYaw {
+
+	/**
+	 * Adjusts the yaw of a tracker.
+	 *
+	 * Locked Trackers
+	 * ---------------
+	 * After a tracker is at rest for a short time, we lock it and save its initial
+	 * rotation. We assume that locked trackers really are at rest, and that any
+	 * rotation is due to drift. We adjust the tracker's yaw towards its initial
+	 * rotation. If the tracker rotates beyonds a certain angle, we unlock the
+	 * tracker.
+	 *
+	 * This works very well when the player is still and the tracker is supported by
+	 * some surface, e.g. sitting in a chair or lying in a bed. However, it does not
+	 * work well when the player is standing or moving around because the trackers
+	 * will never lock.
+	 *
+	 * Centering Force
+	 * ---------------
+	 * When the player is moving around, we assume that the player will often be in
+	 * a relaxed pose, or will eventually return to a relaxed pose. During setup, we
+	 * collect the player's relaxed posed when standing, sitting and lying on their
+	 * back.
+	 *
+	 * The centering force adjusts the tracker's yaw towards the relaxed pose.
+	 * Upper body trackers are adjusted towards the average yaw of the body. Leg
+	 * trackers are also adjusted towards the average yaw of the body, but with a
+	 * yaw offset corresponding to their relaxed pose.
+	 *
+	 * This works well when the player is moving a lot. However, it doesn't work
+	 * well when some of the trackers are locked, and others are moving. The locked
+	 * trackers will stay in place, while the moving trackers will pull towards the
+	 * relaxed pose, which can result in imbalanced poses.
+	 *
+	 * Neighbor Trackers
+	 * -----------------
+	 * The neighboring force adjusts the tracker's yaw so that it is balanced
+	 * between its neighboring trackers. For example, if the player is standing in a
+	 * very wide stance, the neighboring force will push the upper leg tracker to a
+	 * position that is proportional to their relaxed pose. This keeps the poses
+	 * balanced.
+	 *
+	 * We use gradient descent to find the direction to apply a yaw correction. By
+	 * applying this 50 times a second, the whole body is nudged into a reasonable
+	 * alignment.
+	 */
+	fun adjust(
+		tracker: Tracker,
+		trackers: TrackerSkeleton,
+		yawCorrection: Angle,
+		relaxedPose: RelaxedPose?,
+	) {
+		if (tracker.stayAligned.lockedRotation != null) {
+			adjustLockedTracker(tracker, trackers, yawCorrection)
+		} else if (relaxedPose != null) {
+			adjustMovingTracker(tracker, trackers, yawCorrection, relaxedPose)
+		}
+	}
+
+	/**
+	 * Adjusts a locked tracker.
+	 */
+	private fun adjustLockedTracker(
+		tracker: Tracker,
+		trackers: TrackerSkeleton,
+		yawCorrection: Angle,
+	) {
+		adjustByError(tracker, yawCorrection) {
+			val yawErrors = YawErrors()
+
+			yawErrors.lockedError =
+				trackers.visit(tracker, LockedErrorVisitor())
+					?: Angle.ZERO
+
+			yawErrors
+		}
+	}
+
+	/**
+	 * Adjusts a tracker that is moving.
+	 */
+	private fun adjustMovingTracker(
+		tracker: Tracker,
+		trackers: TrackerSkeleton,
+		yawCorrection: Angle,
+		relaxedPose: RelaxedPose,
+	) {
+		val centerYaw = CenterErrorVisitor.trackersCenterYaw(trackers) ?: return
+
+		adjustByError(tracker, yawCorrection) {
+			val yawErrors = YawErrors()
+
+			yawErrors.centerError =
+				trackers.visit(
+					tracker,
+					CenterErrorVisitor(centerYaw, relaxedPose),
+				) ?: Angle.ZERO
+
+			yawErrors.neighborError =
+				trackers.visit(
+					tracker,
+					NeighborErrorVisitor(relaxedPose),
+				) ?: Angle.ZERO
+
+			yawErrors
+		}
+	}
+
+	/**
+	 * Adjusts the yaw by applying gradient descent.
+	 */
+	private fun adjustByError(
+		tracker: Tracker,
+		yawCorrection: Angle,
+		errorFn: (tracker: Tracker) -> YawErrors,
+	) {
+		// Only IMUs can drift
+		if (!tracker.isImu()) {
+			return
+		}
+
+		// Skip trackers that use magnetometer because it does not drift
+		if (tracker.magStatus == MagnetometerStatus.ENABLED) {
+			return
+		}
+
+		val state = tracker.stayAligned
+
+		val curYaw = state.yawCorrection.yaw
+		val curError = errorFn(tracker)
+
+		val posYaw = curYaw + yawCorrection
+		state.yawCorrection.yaw = posYaw
+		val posError = errorFn(tracker)
+
+		val negYaw = curYaw - yawCorrection
+		state.yawCorrection.yaw = negYaw
+		val negError = errorFn(tracker)
+
+		val posYawDelta = gradient(posError, curError)
+		val negYawDelta = gradient(negError, curError)
+
+		// Pick the yaw correction that minimizes the error
+		if ((posYawDelta < Angle.ZERO) && (posYawDelta < negYawDelta)) {
+			state.yawCorrection.yaw = posYaw
+			state.yawErrors = posError
+		} else if (negYawDelta < Angle.ZERO) {
+			state.yawCorrection.yaw = negYaw
+			state.yawErrors = negError
+		} else {
+			state.yawCorrection.yaw = curYaw
+			state.yawErrors = curError
+		}
+	}
+
+	/**
+	 * Calculates the gradient between two errors. A negative gradient means that there
+	 * is less error in that direction.
+	 */
+	private fun gradient(errors: YawErrors, base: YawErrors) =
+		(abs(errors.lockedError) - abs(base.lockedError)) * StayAlignedDefaults.YAW_ERRORS_LOCKED_ERROR_WEIGHT +
+			(abs(errors.centerError) - abs(base.centerError)) * StayAlignedDefaults.YAW_ERRORS_CENTER_ERROR_WEIGHT +
+			(abs(errors.neighborError) - abs(base.neighborError)) * StayAlignedDefaults.YAW_ERRORS_NEIGHBOR_ERROR_WEIGHT
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/adjust/CenterErrorVisitor.kt

@@ -0,0 +1,148 @@
+package dev.slimevr.tracking.processor.stayaligned.adjust
+
+import dev.slimevr.math.Angle
+import dev.slimevr.math.AngleAverage
+import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults.CENTER_ERROR_HEAD_WEIGHT
+import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults.CENTER_ERROR_LOWER_LEG_WEIGHT
+import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults.CENTER_ERROR_UPPER_BODY_WEIGHT
+import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults.CENTER_ERROR_UPPER_LEG_WEIGHT
+import dev.slimevr.tracking.processor.stayaligned.adjust.TrackerYaw.extraYaw
+import dev.slimevr.tracking.processor.stayaligned.adjust.TrackerYaw.hasTrackerYaw
+import dev.slimevr.tracking.processor.stayaligned.adjust.TrackerYaw.trackerYaw
+import dev.slimevr.tracking.processor.stayaligned.skeleton.RelaxedPose
+import dev.slimevr.tracking.processor.stayaligned.skeleton.Side
+import dev.slimevr.tracking.processor.stayaligned.skeleton.TrackerSkeleton
+import dev.slimevr.tracking.trackers.Tracker
+
+/**
+ * Assumes that the body is centered around an average yaw, and returns the error of the
+ * tracker with respect to that average yaw.
+ */
+class CenterErrorVisitor(
+	private val centerYaw: Angle,
+	private val relaxedPose: RelaxedPose,
+) : TrackerSkeleton.TrackerVisitor<Angle> {
+
+	override fun visitHeadTracker(
+		tracker: Tracker,
+		belowUpperBody: Tracker?,
+	): Angle =
+		centerYaw - trackerYaw(tracker)
+
+	override fun visitUpperBodyTracker(
+		tracker: Tracker,
+		aboveHeadOrUpperBody: Tracker?,
+		belowUpperBody: Tracker?,
+	): Angle =
+		centerYaw - trackerYaw(tracker)
+
+	override fun visitUpperBodyTracker(
+		tracker: Tracker,
+		aboveHeadOrUpperBody: Tracker?,
+		belowLeftUpperLeg: Tracker?,
+		belowRightUpperLeg: Tracker?,
+	): Angle =
+		centerYaw - trackerYaw(tracker)
+
+	override fun visitArmTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperBodyOrArm: Tracker?,
+		belowHandOrArm: Tracker?,
+	): Angle =
+		// Arms can go anywhere
+		Angle.ZERO
+
+	override fun visitHandTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveArm: Tracker?,
+		oppositeHand: Tracker?,
+	): Angle =
+		// Hands can go anywhere
+		Angle.ZERO
+
+	override fun visitUpperLegTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperBody: Tracker?,
+		belowLowerLeg: Tracker?,
+		oppositeUpperLeg: Tracker?,
+	): Angle =
+		(centerYaw + extraYaw(side, relaxedPose.upperLeg)) - trackerYaw(tracker)
+
+	override fun visitLowerLegTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperLeg: Tracker?,
+		belowFoot: Tracker?,
+		oppositeLowerLeg: Tracker?,
+	): Angle =
+		(centerYaw + extraYaw(side, relaxedPose.lowerLeg)) - trackerYaw(tracker)
+
+	override fun visitFootTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveLowerLeg: Tracker?,
+		oppositeFoot: Tracker?,
+	): Angle =
+		(centerYaw + extraYaw(side, relaxedPose.foot)) - trackerYaw(tracker)
+
+	companion object {
+
+		/**
+		 * Finds the center yaw of the skeleton.
+		 */
+		fun trackersCenterYaw(
+			trackers: TrackerSkeleton,
+		): Angle? {
+			val head = trackers.head
+			val upperBody = trackers.upperBody
+			val leftUpperLeg = trackers.leftUpperLeg
+			val rightUpperLeg = trackers.rightUpperLeg
+			val leftLowerLeg = trackers.leftLowerLeg
+			val rightLowerLeg = trackers.rightLowerLeg
+
+			if (
+				// Head is optional
+				upperBody.isEmpty() ||
+				leftUpperLeg == null ||
+				rightUpperLeg == null ||
+				leftLowerLeg == null ||
+				rightLowerLeg == null
+			) {
+				return null
+			}
+
+			// Check whether we can calculate a center yaw
+			val hasCenterYaw =
+				upperBody.all(::hasTrackerYaw) &&
+					hasTrackerYaw(leftUpperLeg) &&
+					hasTrackerYaw(rightUpperLeg) &&
+					hasTrackerYaw(leftLowerLeg) &&
+					hasTrackerYaw(rightLowerLeg)
+			if (!hasCenterYaw) {
+				return null
+			}
+
+			// Calculate average yaw of the body
+			val averageYaw = AngleAverage()
+
+			if (head != null && hasTrackerYaw(head)) {
+				averageYaw.add(trackerYaw(head), CENTER_ERROR_HEAD_WEIGHT)
+			}
+
+			upperBody.forEach {
+				averageYaw.add(trackerYaw(it), CENTER_ERROR_UPPER_BODY_WEIGHT)
+			}
+
+			averageYaw.add(trackerYaw(leftUpperLeg), CENTER_ERROR_UPPER_LEG_WEIGHT)
+			averageYaw.add(trackerYaw(rightUpperLeg), CENTER_ERROR_UPPER_LEG_WEIGHT)
+
+			averageYaw.add(trackerYaw(leftLowerLeg), CENTER_ERROR_LOWER_LEG_WEIGHT)
+			averageYaw.add(trackerYaw(rightLowerLeg), CENTER_ERROR_LOWER_LEG_WEIGHT)
+
+			return averageYaw.toAngle()
+		}
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/adjust/LockedErrorVisitor.kt

@@ -0,0 +1,136 @@
+package dev.slimevr.tracking.processor.stayaligned.adjust
+
+import dev.slimevr.math.Angle
+import dev.slimevr.tracking.processor.stayaligned.skeleton.Side
+import dev.slimevr.tracking.processor.stayaligned.skeleton.TrackerSkeleton
+import dev.slimevr.tracking.trackers.Tracker
+import io.github.axisangles.ktmath.Quaternion
+import io.github.axisangles.ktmath.Vector3
+import kotlin.math.*
+
+/**
+ * Error between a locked tracker's yaw and its yaw when it was initially locked.
+ */
+class LockedErrorVisitor : TrackerSkeleton.TrackerVisitor<Angle> {
+
+	override fun visitHeadTracker(
+		tracker: Tracker,
+		belowUpperBody: Tracker?,
+	): Angle =
+		error(tracker)
+
+	override fun visitUpperBodyTracker(
+		tracker: Tracker,
+		aboveHeadOrUpperBody: Tracker?,
+		belowUpperBody: Tracker?,
+	): Angle =
+		error(tracker)
+
+	override fun visitUpperBodyTracker(
+		tracker: Tracker,
+		aboveHeadOrUpperBody: Tracker?,
+		belowLeftUpperLeg: Tracker?,
+		belowRightUpperLeg: Tracker?,
+	): Angle =
+		error(tracker)
+
+	override fun visitArmTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperBodyOrArm: Tracker?,
+		belowHandOrArm: Tracker?,
+	): Angle =
+		error(tracker)
+
+	override fun visitHandTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveArm: Tracker?,
+		oppositeHand: Tracker?,
+	): Angle =
+		error(tracker)
+
+	override fun visitUpperLegTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperBody: Tracker?,
+		belowLowerLeg: Tracker?,
+		oppositeUpperLeg: Tracker?,
+	): Angle =
+		error(tracker)
+
+	override fun visitLowerLegTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperLeg: Tracker?,
+		belowFoot: Tracker?,
+		oppositeLowerLeg: Tracker?,
+	): Angle =
+		error(tracker)
+
+	override fun visitFootTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveLowerLeg: Tracker?,
+		oppositeFoot: Tracker?,
+	): Angle =
+		error(tracker)
+
+	companion object {
+
+		private fun error(tracker: Tracker): Angle {
+			val lockedRotation = tracker.stayAligned.lockedRotation
+				?: return Angle.ZERO
+
+			return yawDifference(tracker.getRotation(), lockedRotation)
+		}
+
+		/**
+		 * Gets the yaw between two rotations, for small rotations.
+		 *
+		 * A locked tracker can be in any rotation, so we cannot use
+		 * TrackerYaw::trackerYaw, which doesn't work for a tracker that is on its
+		 * side.
+		 *
+		 * WARNING: DO NOT USE for large rotations because the chosen axis might have
+		 * a very small projection on the yaw plane, which yields a low confidence yaw.
+		 *
+		 * TODO: It might be possible to pick a different EulerOrder when we encounter
+		 * 		singularities, but I wasn't able to get this working correctly.
+		 */
+		private fun yawDifference(
+			rotation: Quaternion,
+			targetRotation: Quaternion,
+		): Angle {
+			val targetX = targetRotation.sandwichUnitX()
+			val targetY = targetRotation.sandwichUnitY()
+			val targetZ = targetRotation.sandwichUnitZ()
+
+			// Find the axis that is closest to the yaw plane
+			val axis: Vector3
+			val targetAxis: Vector3
+
+			val targetXScore = abs(targetX.dot(Vector3.POS_Y))
+			val targetYScore = abs(targetY.dot(Vector3.POS_Y))
+			val targetZScore = abs(targetZ.dot(Vector3.POS_Y))
+
+			// The axis that is closest to the yaw plane has the smallest absolute dot
+			// product with the Y axis
+			if ((targetXScore <= targetYScore) && (targetXScore <= targetZScore)) {
+				axis = rotation.sandwichUnitX()
+				targetAxis = targetX
+			} else if ((targetYScore <= targetXScore) && (targetYScore <= targetZScore)) {
+				axis = rotation.sandwichUnitY()
+				targetAxis = targetY
+			} else {
+				axis = rotation.sandwichUnitZ()
+				targetAxis = targetZ
+			}
+
+			val yaw = Angle.ofRad(atan2(axis.z, axis.x))
+			val targetYaw = Angle.ofRad(atan2(targetAxis.z, targetAxis.x))
+
+			return targetYaw - yaw
+		}
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/adjust/NeighborErrorVisitor.kt

@@ -0,0 +1,170 @@
+package dev.slimevr.tracking.processor.stayaligned.adjust
+
+import dev.slimevr.math.Angle
+import dev.slimevr.math.AngleAverage
+import dev.slimevr.tracking.processor.stayaligned.adjust.TrackerYaw.extraYaw
+import dev.slimevr.tracking.processor.stayaligned.adjust.TrackerYaw.trackerYaw
+import dev.slimevr.tracking.processor.stayaligned.skeleton.RelaxedPose
+import dev.slimevr.tracking.processor.stayaligned.skeleton.Side
+import dev.slimevr.tracking.processor.stayaligned.skeleton.TrackerSkeleton
+import dev.slimevr.tracking.trackers.Tracker
+import dev.slimevr.tracking.trackers.TrackerPosition
+
+/**
+ * Error between a tracker's yaw and its neighbors' yaws.
+ */
+class NeighborErrorVisitor(
+	private val relaxedPose: RelaxedPose,
+) : TrackerSkeleton.TrackerVisitor<Angle> {
+
+	override fun visitHeadTracker(
+		tracker: Tracker,
+		belowUpperBody: Tracker?,
+	): Angle {
+		val targetYaw = AngleAverage()
+
+		if (belowUpperBody != null) {
+			targetYaw.add(trackerYaw(belowUpperBody))
+		}
+
+		return error(tracker, targetYaw)
+	}
+
+	override fun visitUpperBodyTracker(
+		tracker: Tracker,
+		aboveHeadOrUpperBody: Tracker?,
+		belowUpperBody: Tracker?,
+	): Angle {
+		val targetYaw = AngleAverage()
+
+		if (
+			aboveHeadOrUpperBody != null &&
+			// Head often drags the upper body trackers off to the side, so ignore it
+			aboveHeadOrUpperBody.trackerPosition != TrackerPosition.HEAD
+		) {
+			targetYaw.add(trackerYaw(aboveHeadOrUpperBody))
+		}
+
+		if (belowUpperBody != null) {
+			targetYaw.add(trackerYaw(belowUpperBody))
+		}
+
+		return error(tracker, targetYaw)
+	}
+
+	override fun visitUpperBodyTracker(
+		tracker: Tracker,
+		aboveHeadOrUpperBody: Tracker?,
+		belowLeftUpperLeg: Tracker?,
+		belowRightUpperLeg: Tracker?,
+	): Angle {
+		val targetYaw = AngleAverage()
+
+		if (
+			aboveHeadOrUpperBody != null &&
+			// Head often drags the upper body trackers off to the side, so ignore it
+			aboveHeadOrUpperBody.trackerPosition != TrackerPosition.HEAD
+		) {
+			targetYaw.add(trackerYaw(aboveHeadOrUpperBody))
+		}
+
+		// Only consider upper leg trackers if both are available, so that the upper
+		// body tracker can be balanced between both
+		if (
+			belowLeftUpperLeg != null &&
+			belowRightUpperLeg != null
+		) {
+			targetYaw.add(trackerYaw(belowLeftUpperLeg) - extraYaw(Side.LEFT, relaxedPose.upperLeg))
+			targetYaw.add(trackerYaw(belowRightUpperLeg) - extraYaw(Side.RIGHT, relaxedPose.upperLeg))
+		}
+
+		return error(tracker, targetYaw)
+	}
+
+	override fun visitArmTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperBodyOrArm: Tracker?,
+		belowHandOrArm: Tracker?,
+	): Angle =
+		// Arms can go anywhere
+		Angle.ZERO
+
+	override fun visitHandTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveArm: Tracker?,
+		oppositeHand: Tracker?,
+	): Angle =
+		// Hands can go anywhere
+		Angle.ZERO
+
+	override fun visitUpperLegTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperBody: Tracker?,
+		belowLowerLeg: Tracker?,
+		oppositeUpperLeg: Tracker?,
+	): Angle {
+		val targetYaw = AngleAverage()
+
+		if (aboveUpperBody != null) {
+			targetYaw.add(
+				trackerYaw(aboveUpperBody) + extraYaw(side, relaxedPose.upperLeg),
+			)
+		}
+
+		if (belowLowerLeg != null) {
+			targetYaw.add(
+				trackerYaw(belowLowerLeg) - extraYaw(side, relaxedPose.lowerLeg - relaxedPose.upperLeg),
+			)
+		}
+
+		return error(tracker, targetYaw)
+	}
+
+	override fun visitLowerLegTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperLeg: Tracker?,
+		belowFoot: Tracker?,
+		oppositeLowerLeg: Tracker?,
+	): Angle {
+		val targetYaw = AngleAverage()
+
+		if (aboveUpperLeg != null) {
+			targetYaw.add(trackerYaw(aboveUpperLeg) + extraYaw(side, relaxedPose.lowerLeg - relaxedPose.upperLeg))
+		}
+
+		if (belowFoot != null) {
+			targetYaw.add(trackerYaw(belowFoot) - extraYaw(side, relaxedPose.foot - relaxedPose.lowerLeg))
+		}
+
+		return error(tracker, targetYaw)
+	}
+
+	override fun visitFootTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveLowerLeg: Tracker?,
+		oppositeFoot: Tracker?,
+	): Angle {
+		val targetYaw = AngleAverage()
+
+		if (aboveLowerLeg != null) {
+			targetYaw.add(trackerYaw(aboveLowerLeg) + extraYaw(side, relaxedPose.foot - relaxedPose.lowerLeg))
+		}
+
+		return error(tracker, targetYaw)
+	}
+
+	companion object {
+
+		fun error(tracker: Tracker, targetYaw: AngleAverage) =
+			if (targetYaw.isEmpty()) {
+				Angle.ZERO
+			} else {
+				targetYaw.toAngle() - trackerYaw(tracker)
+			}
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/adjust/TrackerYaw.kt

@@ -0,0 +1,59 @@
+package dev.slimevr.tracking.processor.stayaligned.adjust
+
+import dev.slimevr.math.Angle
+import dev.slimevr.tracking.processor.stayaligned.skeleton.Side
+import dev.slimevr.tracking.trackers.Tracker
+import io.github.axisangles.ktmath.EulerOrder
+import io.github.axisangles.ktmath.Vector3
+
+/**
+ * Utilities for tracker yaw.
+ *
+ * The SlimeVR coordinate system is x-right, y-up, z-back, which is a right-handed
+ * coordinate system.
+ *
+ * Rotations follow the right-hand rule, for example, a positive rotation around the
+ * y-axis is a counter-clockwise rotation from z to x. From the perspective of a player,
+ * left is positive yaw, right is negative yaw.
+ */
+object TrackerYaw {
+
+	/**
+	 * Whether we can get the yaw of a tracker.
+	 */
+	fun hasTrackerYaw(tracker: Tracker) =
+		Angle.absBetween(
+			tracker.getRotation().sandwichUnitX(),
+			Vector3.POS_Y,
+		) > MIN_ON_SIDE_ANGLE
+
+	/**
+	 * Gets the yaw of the tracker, for trackers that are not on its side.
+	 *
+	 * WARNING: DO NOT USE for a tracker that is on its side. Euler YZX angles have a
+	 * singularity for a tracker that is on its side, and can yield arbitrary yaws.
+	 * For example, the Euler YZX angles (Y=0°, Z=90°, X=30°) and (Y=30°, Z=90°, X=0°)
+	 * are equivalent but yield completely different yaws.
+	 *
+	 * WARNING: It is possible to use another EulerOrder which does not have a
+	 * singularity for this rotation to get "some" yaw, but this yaw will be very
+	 * different from the from YZX. DO NOT ATTEMPT!
+	 */
+	fun trackerYaw(tracker: Tracker) =
+		Angle.ofRad(
+			tracker.getRotation()
+				.toEulerAngles(EulerOrder.YZX)
+				.y,
+		)
+
+	/**
+	 * Applies an extra yaw in the specified direction.
+	 */
+	fun extraYaw(direction: Side, angle: Angle) =
+		when (direction) {
+			Side.LEFT -> angle
+			Side.RIGHT -> -angle
+		}
+
+	private val MIN_ON_SIDE_ANGLE = Angle.ofDeg(30.0f)
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/skeleton/PlayerPose.kt

@@ -0,0 +1,91 @@
+package dev.slimevr.tracking.processor.stayaligned.skeleton
+
+/**
+ * The pose of the player.
+ */
+enum class PlayerPose {
+	UNKNOWN,
+	STANDING,
+	SITTING_IN_CHAIR,
+	SITTING_ON_GROUND,
+	LYING_ON_BACK,
+	KNEELING,
+	;
+
+	companion object {
+
+		fun ofTrackers(trackers: TrackerSkeleton): PlayerPose {
+			val poses = TrackerPoses(
+				trackers.upperBody.map(TrackerPose::ofTracker),
+				trackers.leftUpperLeg?.let(TrackerPose::ofTracker) ?: TrackerPose.NONE,
+				trackers.rightUpperLeg?.let(TrackerPose::ofTracker) ?: TrackerPose.NONE,
+				trackers.leftLowerLeg?.let(TrackerPose::ofTracker) ?: TrackerPose.NONE,
+				trackers.rightLowerLeg?.let(TrackerPose::ofTracker) ?: TrackerPose.NONE,
+			)
+
+			return if (isStanding(poses)) {
+				STANDING
+			} else if (isSittingInChair(poses)) {
+				SITTING_IN_CHAIR
+			} else if (isSittingOnGround(poses)) {
+				SITTING_ON_GROUND
+			} else if (isLyingOnBack(poses)) {
+				LYING_ON_BACK
+			} else if (isKneeling(poses)) {
+				KNEELING
+			} else {
+				UNKNOWN
+			}
+		}
+
+		private class TrackerPoses(
+			val upperBody: List<TrackerPose>,
+			val leftUpperLeg: TrackerPose,
+			val rightUpperLeg: TrackerPose,
+			val leftLowerLeg: TrackerPose,
+			val rightLowerLeg: TrackerPose,
+		)
+
+		private fun isStanding(pose: TrackerPoses) =
+			pose.upperBody.all(::topFacingUp) &&
+				topFacingUp(pose.leftUpperLeg) &&
+				topFacingUp(pose.rightUpperLeg) &&
+				topFacingUp(pose.leftLowerLeg) &&
+				topFacingUp(pose.rightLowerLeg)
+
+		private fun isSittingInChair(pose: TrackerPoses) =
+			pose.upperBody.isNotEmpty() && topFacingUp(pose.upperBody[0]) &&
+				pose.upperBody.all { topFacingUp(it) || frontFacingUp(it) } &&
+				frontFacingUp(pose.leftUpperLeg) &&
+				frontFacingUp(pose.rightUpperLeg) &&
+				topFacingUp(pose.leftLowerLeg) &&
+				topFacingUp(pose.rightLowerLeg)
+
+		private fun isSittingOnGround(pose: TrackerPoses) =
+			pose.upperBody.isNotEmpty() && topFacingUp(pose.upperBody[0]) &&
+				pose.upperBody.all { topFacingUp(it) || frontFacingUp(it) } &&
+				pose.leftUpperLeg.let { frontFacingUp(it) || topFacingDown(it) } &&
+				pose.rightUpperLeg.let { frontFacingUp(it) || topFacingDown(it) } &&
+				pose.leftLowerLeg.let { frontFacingUp(it) || topFacingUp(it) } &&
+				pose.rightLowerLeg.let { frontFacingUp(it) || topFacingUp(it) }
+
+		private fun isLyingOnBack(pose: TrackerPoses) =
+			pose.upperBody.all(::frontFacingUp) &&
+				pose.leftUpperLeg.let { frontFacingUp(it) || topFacingDown(it) } &&
+				pose.rightUpperLeg.let { frontFacingUp(it) || topFacingDown(it) } &&
+				pose.leftLowerLeg.let { frontFacingUp(it) || topFacingUp(it) } &&
+				pose.rightLowerLeg.let { frontFacingUp(it) || topFacingUp(it) }
+
+		private fun isKneeling(pose: TrackerPoses) =
+			pose.leftUpperLeg.let { topFacingUp(it) || frontFacingUp(it) } &&
+				pose.rightUpperLeg.let { topFacingUp(it) || frontFacingUp(it) } &&
+				frontFacingDown(pose.leftLowerLeg) &&
+				frontFacingDown(pose.rightLowerLeg)
+
+		// Helper functions to make checks more readable
+		private fun topFacingUp(pose: TrackerPose) = pose == TrackerPose.TOP_FACING_UP
+		private fun topFacingDown(pose: TrackerPose) = pose == TrackerPose.TOP_FACING_DOWN
+		private fun frontFacingUp(pose: TrackerPose) = pose == TrackerPose.FRONT_FACING_UP
+		private fun frontFacingDown(pose: TrackerPose) = pose == TrackerPose.FRONT_FACING_DOWN
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/skeleton/RelaxedPose.kt

@@ -0,0 +1,93 @@
+package dev.slimevr.tracking.processor.stayaligned.skeleton
+
+import dev.slimevr.config.StayAlignedConfig
+import dev.slimevr.math.Angle
+import dev.slimevr.tracking.processor.skeleton.HumanSkeleton
+import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults
+import dev.slimevr.tracking.processor.stayaligned.adjust.TrackerYaw.trackerYaw
+import dev.slimevr.tracking.trackers.Tracker
+
+class RelaxedPose(
+	val upperLeg: Angle,
+	val lowerLeg: Angle,
+	val foot: Angle,
+) {
+	override fun toString(): String =
+		"upperLeg=$upperLeg lowerLeg=$lowerLeg foot=$foot"
+
+	companion object {
+
+		val ZERO = RelaxedPose(Angle.ZERO, Angle.ZERO, Angle.ZERO)
+
+		/**
+		 * Gets the relaxed angles for a particular pose. May provide defaults if the
+		 * angles aren't configured for the pose.
+		 */
+		fun forPose(
+			playerPose: PlayerPose,
+			config: StayAlignedConfig,
+		) =
+			when (playerPose) {
+				PlayerPose.STANDING ->
+					RelaxedPose(
+						Angle.ofDeg(config.standingRelaxedPose.upperLegAngleInDeg),
+						Angle.ofDeg(config.standingRelaxedPose.lowerLegAngleInDeg),
+						Angle.ofDeg(config.standingRelaxedPose.footAngleInDeg),
+					)
+
+				PlayerPose.SITTING_IN_CHAIR ->
+					RelaxedPose(
+						Angle.ofDeg(config.sittingRelaxedPose.upperLegAngleInDeg),
+						Angle.ofDeg(config.sittingRelaxedPose.lowerLegAngleInDeg),
+						Angle.ofDeg(config.sittingRelaxedPose.footAngleInDeg),
+					)
+
+				PlayerPose.SITTING_ON_GROUND,
+				PlayerPose.LYING_ON_BACK,
+				->
+					RelaxedPose(
+						Angle.ofDeg(config.flatRelaxedPose.upperLegAngleInDeg),
+						Angle.ofDeg(config.flatRelaxedPose.lowerLegAngleInDeg),
+						Angle.ofDeg(config.flatRelaxedPose.footAngleInDeg),
+					)
+
+				PlayerPose.KNEELING ->
+					StayAlignedDefaults.RELAXED_POSE_KNEELING
+
+				else ->
+					null
+			}
+
+		/**
+		 * Gets the relaxed angles from the trackers.
+		 */
+		fun fromTrackers(humanSkeleton: HumanSkeleton): RelaxedPose {
+			val halfAngleBetween = { left: Tracker, right: Tracker ->
+				(trackerYaw(left) - trackerYaw(right)) * 0.5f
+			}
+
+			var upperLegAngle = Angle.ZERO
+			humanSkeleton.leftUpperLegTracker?.let { left ->
+				humanSkeleton.rightUpperLegTracker?.let { right ->
+					upperLegAngle = halfAngleBetween(left, right)
+				}
+			}
+
+			var lowerLegAngle = Angle.ZERO
+			humanSkeleton.leftLowerLegTracker?.let { left ->
+				humanSkeleton.rightLowerLegTracker?.let { right ->
+					lowerLegAngle = halfAngleBetween(left, right)
+				}
+			}
+
+			var footAngle = Angle.ZERO
+			humanSkeleton.leftFootTracker?.let { left ->
+				humanSkeleton.rightFootTracker?.let { right ->
+					footAngle = halfAngleBetween(left, right)
+				}
+			}
+
+			return RelaxedPose(upperLegAngle, lowerLegAngle, footAngle)
+		}
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/skeleton/Side.kt

@@ -0,0 +1,6 @@
+package dev.slimevr.tracking.processor.stayaligned.skeleton
+
+enum class Side {
+	LEFT,
+	RIGHT,
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/skeleton/TrackerPose.kt

@@ -0,0 +1,57 @@
+package dev.slimevr.tracking.processor.stayaligned.skeleton
+
+import dev.slimevr.tracking.trackers.Tracker
+import io.github.axisangles.ktmath.Vector3
+import kotlin.math.*
+
+/**
+ * The pose of a tracker.
+ */
+enum class TrackerPose {
+	NONE,
+	TOP_FACING_UP,
+	TOP_FACING_DOWN,
+	FRONT_FACING_UP,
+	FRONT_FACING_DOWN,
+	ON_SIDE,
+	;
+
+	companion object {
+
+		fun ofTracker(tracker: Tracker): TrackerPose {
+			val rotation = tracker.getRotation()
+
+			val x = rotation.sandwichUnitX()
+			val y = rotation.sandwichUnitY()
+			val z = rotation.sandwichUnitZ()
+
+			val xDot = x.dot(Vector3.POS_Y)
+			val yDot = y.dot(Vector3.POS_Y)
+			val zDot = z.dot(Vector3.POS_Y)
+
+			val xAbsDot = abs(xDot)
+			val yAbsDot = abs(yDot)
+			val zAbsDot = abs(zDot)
+
+			val pose =
+				if ((xAbsDot >= yAbsDot) && (xAbsDot >= zAbsDot)) {
+					ON_SIDE
+				} else if ((yAbsDot >= xAbsDot) && (yAbsDot >= zAbsDot)) {
+					if (yDot >= 0) {
+						TOP_FACING_UP
+					} else {
+						TOP_FACING_DOWN
+					}
+				} else {
+					// Tracker local POS_Z is behind
+					if (zDot >= 0) {
+						FRONT_FACING_DOWN
+					} else {
+						FRONT_FACING_UP
+					}
+				}
+
+			return pose
+		}
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/skeleton/TrackerSkeleton.kt

@@ -0,0 +1,431 @@
+package dev.slimevr.tracking.processor.stayaligned.skeleton
+
+import dev.slimevr.tracking.processor.skeleton.HumanSkeleton
+import dev.slimevr.tracking.trackers.Tracker
+import dev.slimevr.tracking.trackers.TrackerPosition
+
+/**
+ * Represents a skeleton of trackers.
+ *
+ * The skeleton consists of:
+ * - An upper body group
+ * - A head tracker, connected to the top of the upper body group
+ * - Two arm groups, connected to the top of the upper body group
+ * - Two hands connected to the bottom of the corresponding arm group
+ * - Two upper legs, connected to the bottom of the upper body group
+ * - Two lower legs, connected to the bottom of each corresponding upper leg
+ * - Two feet, connected to the bottom of each corresponding lower leg
+ */
+class TrackerSkeleton(skeleton: HumanSkeleton) {
+
+	val allTrackers = with(skeleton) {
+		listOfNotNull(
+			headTracker,
+			// Upper body
+			neckTracker,
+			upperChestTracker,
+			chestTracker,
+			waistTracker,
+			hipTracker,
+			// Left arm
+			leftShoulderTracker,
+			leftUpperArmTracker,
+			leftLowerArmTracker,
+			leftHandTracker,
+			// Right arm
+			rightShoulderTracker,
+			rightUpperArmTracker,
+			rightLowerArmTracker,
+			rightHandTracker,
+			// Left leg
+			leftUpperLegTracker,
+			leftLowerLegTracker,
+			leftFootTracker,
+			// Right leg
+			rightUpperLegTracker,
+			rightLowerLegTracker,
+			rightFootTracker,
+		)
+	}
+
+	// Tracker groups
+	val upperBody = with(skeleton) {
+		listOfNotNull(
+			neckTracker,
+			upperChestTracker,
+			chestTracker,
+			waistTracker,
+			hipTracker,
+		)
+	}
+
+	val leftArm = with(skeleton) {
+		listOfNotNull(
+			leftShoulderTracker,
+			leftUpperArmTracker,
+			leftLowerArmTracker,
+		)
+	}
+
+	val rightArm = with(skeleton) {
+		listOfNotNull(
+			rightShoulderTracker,
+			rightUpperArmTracker,
+			rightLowerArmTracker,
+		)
+	}
+
+	// Individual trackers
+	val head = skeleton.headTracker
+	val leftHand = skeleton.leftHandTracker
+	val rightHand = skeleton.rightHandTracker
+	val leftUpperLeg = skeleton.leftUpperLegTracker
+	val leftLowerLeg = skeleton.leftLowerLegTracker
+	val leftFoot = skeleton.leftFootTracker
+	val rightUpperLeg = skeleton.rightUpperLegTracker
+	val rightLowerLeg = skeleton.rightLowerLegTracker
+	val rightFoot = skeleton.rightFootTracker
+
+	/**
+	 * Visits a tracker within the skeleton.
+	 */
+	fun <V> visit(
+		tracker: Tracker,
+		visitor: TrackerVisitor<V>,
+	): V? =
+		when (tracker.trackerPosition) {
+			TrackerPosition.HEAD ->
+				if (tracker == head) {
+					visitor.visitHeadTracker(tracker, upperBody.firstOrNull())
+				} else {
+					null
+				}
+
+			// Upper body
+			TrackerPosition.NECK,
+			TrackerPosition.UPPER_CHEST,
+			TrackerPosition.CHEST,
+			TrackerPosition.WAIST,
+			TrackerPosition.HIP,
+			->
+				visitUpperBodyTrackers(
+					tracker,
+					visitor,
+					head,
+					upperBody,
+					leftUpperLeg,
+					rightUpperLeg,
+				)
+
+			// Left arm
+			TrackerPosition.LEFT_SHOULDER,
+			TrackerPosition.LEFT_UPPER_ARM,
+			TrackerPosition.LEFT_LOWER_ARM,
+			->
+				visitArmTrackers(
+					tracker,
+					visitor,
+					Side.LEFT,
+					upperBody.firstOrNull(),
+					leftArm,
+					leftHand,
+				)
+
+			// Right arm
+			TrackerPosition.RIGHT_SHOULDER,
+			TrackerPosition.RIGHT_UPPER_ARM,
+			TrackerPosition.RIGHT_LOWER_ARM,
+			->
+				visitArmTrackers(
+					tracker,
+					visitor,
+					Side.RIGHT,
+					upperBody.firstOrNull(),
+					rightArm,
+					rightHand,
+				)
+
+			TrackerPosition.LEFT_HAND ->
+				if (tracker == leftHand) {
+					visitor.visitHandTracker(
+						Side.LEFT,
+						tracker,
+						leftArm.lastOrNull(),
+						rightHand,
+					)
+				} else {
+					null
+				}
+
+			TrackerPosition.RIGHT_HAND ->
+				if (tracker == rightHand) {
+					visitor.visitHandTracker(
+						Side.RIGHT,
+						tracker,
+						rightArm.lastOrNull(),
+						leftHand,
+					)
+				} else {
+					null
+				}
+
+			TrackerPosition.LEFT_UPPER_LEG ->
+				if (tracker == leftUpperLeg) {
+					visitor.visitUpperLegTracker(
+						Side.LEFT,
+						tracker,
+						upperBody.lastOrNull(),
+						leftLowerLeg,
+						rightUpperLeg,
+					)
+				} else {
+					null
+				}
+
+			TrackerPosition.RIGHT_UPPER_LEG ->
+				if (tracker == rightUpperLeg) {
+					visitor.visitUpperLegTracker(
+						Side.RIGHT,
+						tracker,
+						upperBody.lastOrNull(),
+						rightLowerLeg,
+						leftUpperLeg,
+					)
+				} else {
+					null
+				}
+
+			TrackerPosition.LEFT_LOWER_LEG ->
+				if (tracker == leftLowerLeg) {
+					visitor.visitLowerLegTracker(
+						Side.LEFT,
+						tracker,
+						leftUpperLeg,
+						leftFoot,
+						rightLowerLeg,
+					)
+				} else {
+					null
+				}
+
+			TrackerPosition.RIGHT_LOWER_LEG ->
+				if (tracker == rightLowerLeg) {
+					visitor.visitLowerLegTracker(
+						Side.RIGHT,
+						tracker,
+						rightUpperLeg,
+						rightFoot,
+						leftLowerLeg,
+					)
+				} else {
+					null
+				}
+
+			TrackerPosition.LEFT_FOOT ->
+				if (tracker == leftFoot) {
+					visitor.visitFootTracker(
+						Side.LEFT,
+						tracker,
+						leftLowerLeg,
+						rightFoot,
+					)
+				} else {
+					null
+				}
+
+			TrackerPosition.RIGHT_FOOT ->
+				if (tracker == rightFoot) {
+					visitor.visitFootTracker(
+						Side.RIGHT,
+						tracker,
+						rightLowerLeg,
+						leftFoot,
+					)
+				} else {
+					null
+				}
+
+			else ->
+				null
+		}
+
+	private fun <V> visitUpperBodyTrackers(
+		tracker: Tracker,
+		visitor: TrackerVisitor<V>,
+		head: Tracker?,
+		upperBody: List<Tracker>,
+		leftUpperLeg: Tracker?,
+		rightUpperLeg: Tracker?,
+	): V? {
+		val index = upperBody.indexOf(tracker)
+		if (index < 0) {
+			return null
+		}
+
+		return if (index == 0) {
+			if (upperBody.size == 1) {
+				// Only upper body tracker
+				visitor.visitUpperBodyTracker(
+					tracker,
+					head,
+					leftUpperLeg,
+					rightUpperLeg,
+				)
+			} else {
+				// First upper body tracker
+				visitor.visitUpperBodyTracker(
+					tracker,
+					head,
+					upperBody[1],
+				)
+			}
+		} else if (index < upperBody.size - 1) {
+			// Middle upper body tracker
+			visitor.visitUpperBodyTracker(
+				tracker,
+				upperBody[index - 1],
+				upperBody[index + 1],
+			)
+		} else {
+			// Last upper body tracker
+			visitor.visitUpperBodyTracker(
+				tracker,
+				upperBody[index - 1],
+				leftUpperLeg,
+				rightUpperLeg,
+			)
+		}
+	}
+
+	private fun <V> visitArmTrackers(
+		tracker: Tracker,
+		visitor: TrackerVisitor<V>,
+		side: Side,
+		upperBody: Tracker?,
+		arm: List<Tracker>,
+		hand: Tracker?,
+	): V? {
+		val index = arm.indexOf(tracker)
+		if (index < 0) {
+			return null
+		}
+
+		return if (index == 0) {
+			if (arm.size == 1) {
+				// Only arm tracker
+				visitor.visitArmTracker(
+					side,
+					tracker,
+					upperBody,
+					hand,
+				)
+			} else {
+				// First arm tracker
+				visitor.visitArmTracker(
+					side,
+					tracker,
+					upperBody,
+					arm[1],
+				)
+			}
+		} else if (index < arm.size - 1) {
+			// Middle arm tracker
+			visitor.visitArmTracker(
+				side,
+				tracker,
+				arm[index - 1],
+				arm[index + 1],
+			)
+		} else {
+			// Last arm tracker
+			visitor.visitArmTracker(
+				side,
+				tracker,
+				arm[index - 1],
+				hand,
+			)
+		}
+	}
+
+	interface TrackerVisitor<V> {
+
+		/**
+		 * Visits the head tracker.
+		 */
+		fun visitHeadTracker(
+			tracker: Tracker,
+			belowUpperBody: Tracker?,
+		): V
+
+		/**
+		 * Visits an upper body tracker (except for the bottom-most tracker).
+		 */
+		fun visitUpperBodyTracker(
+			tracker: Tracker,
+			aboveHeadOrUpperBody: Tracker?,
+			belowUpperBody: Tracker?,
+		): V
+
+		/**
+		 * Visits the bottom-most upper body tracker.
+		 */
+		fun visitUpperBodyTracker(
+			tracker: Tracker,
+			aboveHeadOrUpperBody: Tracker?,
+			belowLeftUpperLeg: Tracker?,
+			belowRightUpperLeg: Tracker?,
+		): V
+
+		/**
+		 * Visits an arm tracker.
+		 */
+		fun visitArmTracker(
+			side: Side,
+			tracker: Tracker,
+			aboveUpperBodyOrArm: Tracker?,
+			belowHandOrArm: Tracker?,
+		): V
+
+		/**
+		 * Visits a hand tracker.
+		 */
+		fun visitHandTracker(
+			side: Side,
+			tracker: Tracker,
+			aboveArm: Tracker?,
+			oppositeHand: Tracker?,
+		): V
+
+		/**
+		 * Visits an upper leg tracker.
+		 */
+		fun visitUpperLegTracker(
+			side: Side,
+			tracker: Tracker,
+			aboveUpperBody: Tracker?,
+			belowLowerLeg: Tracker?,
+			oppositeUpperLeg: Tracker?,
+		): V
+
+		/**
+		 * Visits a lower leg tracker.
+		 */
+		fun visitLowerLegTracker(
+			side: Side,
+			tracker: Tracker,
+			aboveUpperLeg: Tracker?,
+			belowFoot: Tracker?,
+			oppositeLowerLeg: Tracker?,
+		): V
+
+		/**
+		 * Visits a foot tracker.
+		 */
+		fun visitFootTracker(
+			side: Side,
+			tracker: Tracker,
+			aboveLowerLeg: Tracker?,
+			oppositeFoot: Tracker?,
+		): V
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/state/RestDetector.kt

@@ -0,0 +1,55 @@
+package dev.slimevr.tracking.processor.stayaligned.state
+
+import dev.slimevr.math.Angle
+import io.github.axisangles.ktmath.Quaternion
+import kotlin.time.Duration
+import kotlin.time.TimeSource
+
+/**
+ * Detects whether a tracker is at rest.
+ *
+ * A tracker is at rest when it stays within a certain rotational range for a given
+ * amount of time. If it rotates past that range, it is no longer at rest.
+ *
+ * TODO: In practice this is good enough for Stay Aligned, but we could also consider
+ * 		acceleration if we want to make this a general purpose rest detector.
+ */
+class RestDetector(
+	private val maxRotation: Angle,
+	private val minDuration: Duration,
+) {
+	private var startAt = TimeSource.Monotonic.markNow()
+	private var startRotation = Quaternion.IDENTITY
+	private var atRest = false
+
+	/**
+	 * Resets the detector so that the tracker is no longer at rest
+	 */
+	fun reset(rotation: Quaternion = Quaternion.IDENTITY) {
+		startAt = TimeSource.Monotonic.markNow()
+		startRotation = rotation
+		atRest = false
+	}
+
+	/**
+	 * Provides a new rotation sample to the detector.
+	 *
+	 * @return whether the tracker is at rest
+	 */
+	fun update(rotation: Quaternion): Boolean {
+		if (Angle.absBetween(startRotation, rotation) < maxRotation) {
+			// When we detect the tracker is at rest, use the current rotation as the
+			// new start rotation for continuing to detect the tracker is at rest
+			if (!atRest &&
+				TimeSource.Monotonic.markNow() > startAt.plus(minDuration)
+			) {
+				startRotation = rotation
+				atRest = true
+			}
+		} else {
+			reset(rotation)
+		}
+
+		return atRest
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/state/StayAlignedTrackerState.kt

@@ -0,0 +1,39 @@
+package dev.slimevr.tracking.processor.stayaligned.state
+
+import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults
+import dev.slimevr.tracking.trackers.Tracker
+import io.github.axisangles.ktmath.Quaternion
+
+class StayAlignedTrackerState(
+	val tracker: Tracker,
+) {
+	// Detects whether the tracker is at rest
+	val restDetector = StayAlignedDefaults.restDetector()
+
+	// Rotation of the tracker when it was locked
+	var lockedRotation: Quaternion? = null
+
+	// Yaw correction to apply to tracker rotation
+	val yawCorrection = YawCorrection()
+
+	// Alignment error that yaw correction attempts to minimize
+	var yawErrors = YawErrors()
+
+	fun update() {
+		val atRest = restDetector.update(tracker.getRawRotation())
+		if (atRest) {
+			if (lockedRotation == null) {
+				lockedRotation = tracker.getRotation()
+			}
+		} else {
+			lockedRotation = null
+		}
+	}
+
+	fun reset() {
+		restDetector.reset()
+		lockedRotation = null
+		yawCorrection.reset()
+		yawErrors = YawErrors()
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/state/YawCorrection.kt

@@ -0,0 +1,29 @@
+package dev.slimevr.tracking.processor.stayaligned.state
+
+import dev.slimevr.math.Angle
+import io.github.axisangles.ktmath.Quaternion
+
+/**
+ * Tracks the yaw correction that should be applied to a tracker.
+ */
+class YawCorrection {
+
+	var yaw = Angle.ZERO
+		set(value) {
+			field = value
+			yawRotation = Quaternion.rotationAroundYAxis(value.toRad())
+		}
+
+	var yawRotation = Quaternion.IDENTITY
+		private set
+
+	var yawAtLastReset = Angle.ZERO
+		private set
+
+	fun reset() {
+		yawAtLastReset = yaw
+
+		yaw = Angle.ZERO
+		yawRotation = Quaternion.IDENTITY
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/processor/stayaligned/state/YawErrors.kt

@@ -0,0 +1,9 @@
+package dev.slimevr.tracking.processor.stayaligned.state
+
+import dev.slimevr.math.Angle
+
+class YawErrors {
+	var lockedError = Angle.ZERO
+	var centerError = Angle.ZERO
+	var neighborError = Angle.ZERO
+}

server/core/src/main/java/dev/slimevr/tracking/trackers/ResetBodyPose.kt

@@ -0,0 +1,16 @@
+package dev.slimevr.tracking.trackers
+
+enum class ResetBodyPose(val id: Int) {
+
+	SKIING(solarxr_protocol.rpc.ResetBodyPose.SKIING),
+	SITTING_LEANING_BACK(solarxr_protocol.rpc.ResetBodyPose.SITTING_LEANING_BACK),
+	;
+
+	companion object {
+
+		private val byId = ResetBodyPose.entries.associateBy { it.id }
+
+		@JvmStatic
+		fun getById(id: Int): ResetBodyPose? = byId[id]
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/trackers/ResetParams.kt

@@ -0,0 +1,17 @@
+package dev.slimevr.tracking.trackers
+
+class ResetParams(
+	val source: String,
+	val bodyPose: ResetBodyPose,
+	val referenceTrackerPosition: TrackerPosition,
+	val trackerPositionsToReset: Set<TrackerPosition>,
+) {
+	fun shouldReset(tracker: Tracker) =
+		trackerPositionsToReset.isEmpty() || trackerPositionsToReset.contains(tracker.trackerPosition)
+
+	companion object {
+
+		fun makeDefault(source: String?) =
+			ResetParams(source ?: "UNKNOWN", ResetBodyPose.SKIING, TrackerPosition.HEAD, setOf())
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt

@@ -2,6 +2,7 @@ package dev.slimevr.tracking.trackers
 
 import dev.slimevr.VRServer
 import dev.slimevr.config.TrackerConfig
+import dev.slimevr.tracking.processor.stayaligned.state.StayAlignedTrackerState
 import dev.slimevr.tracking.trackers.TrackerPosition.Companion.getByDesignation
 import dev.slimevr.tracking.trackers.udp.IMUType
 import dev.slimevr.tracking.trackers.udp.MagnetometerStatus
@@ -155,6 +156,8 @@ class Tracker @JvmOverloads constructor(
 	 */
 	val trackerNum: Int = trackerNum ?: id
 
+	val stayAligned = StayAlignedTrackerState(this)
+
 	init {
 		// IMPORTANT: Look here for the required states of inputs
 		require(!needsReset || (hasRotation && needsReset)) {
@@ -316,6 +319,7 @@ class Tracker @JvmOverloads constructor(
 		}
 		filteringHandler.update()
 		resetsHandler.update()
+		stayAligned.update()
 	}
 
 	/**
@@ -359,6 +363,10 @@ class Tracker @JvmOverloads constructor(
 			rot = resetsHandler.getReferenceAdjustedDriftRotationFrom(rot)
 		}
 
+		if (!VRServer.instance.configManager.vrConfig.stayAlignedConfig.hideYawCorrection) {
+			rot = stayAligned.yawCorrection.yawRotation * rot
+		}
+
 		return rot
 	}
 
@@ -385,6 +393,10 @@ class Tracker @JvmOverloads constructor(
 			rot = resetsHandler.getIdentityAdjustedDriftRotationFrom(rot)
 		}
 
+		if (!VRServer.instance.configManager.vrConfig.stayAlignedConfig.hideYawCorrection) {
+			rot = stayAligned.yawCorrection.yawRotation * rot
+		}
+
 		return rot
 	}
 

server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt

@@ -9,6 +9,7 @@ import dev.slimevr.config.ResetsConfig
 import dev.slimevr.filtering.CircularArrayList
 import dev.slimevr.tracking.trackers.udp.TrackerDataType
 import io.eiren.math.FloatMath.animateEase
+import io.eiren.util.logging.LogManager
 import io.github.axisangles.ktmath.EulerAngles
 import io.github.axisangles.ktmath.EulerOrder
 import io.github.axisangles.ktmath.Quaternion
@@ -340,8 +341,13 @@ class TrackerResetsHandler(val tracker: Tracker) {
 			yawResetSmoothTimeRemain = 0.0f
 		}
 
+		// Reset Stay Aligned
+		tracker.stayAligned.reset()
+
 		calculateDrift(oldRot)
 
+		LogManager.info("[resetFull] ${tracker.trackerPosition} reset")
+
 		postProcessResetFull()
 	}
 
@@ -379,6 +385,9 @@ class TrackerResetsHandler(val tracker: Tracker) {
 
 		makeIdentityAdjustmentQuatsYaw()
 
+		// Reset Stay Aligned
+		tracker.stayAligned.reset()
+
 		calculateDrift(oldRot)
 
 		// Start at yaw before reset if smoothing enabled
@@ -396,13 +405,15 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		}
 
 		tracker.resetFilteringQuats()
+
+		LogManager.info("[resetYaw] ${tracker.trackerPosition} reset")
 	}
 
 	/**
 	 * Perform the math to align the tracker to go forward
 	 * and stores it in mountRotFix, and adjusts yawFix
 	 */
-	fun resetMounting(reference: Quaternion) {
+	fun resetMounting(reference: Quaternion, bodyPose: ResetBodyPose) {
 		if (tracker.trackerDataType == TrackerDataType.FLEX_RESISTANCE) {
 			tracker.trackerFlexHandler.resetMax()
 			tracker.resetFilteringQuats()
@@ -421,6 +432,8 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		rotBuf *= attachmentFix
 		rotBuf = yawFix * rotBuf
 
+		// TODO: Fail reset if current rotation is too close to the full reset rotation
+
 		// Adjust buffer to reference
 		rotBuf = reference.project(Vector3.POS_Y).inv().unit() * rotBuf
 
@@ -447,11 +460,32 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		}
 
 		// Adjust for forward/back arms and thighs
-		val isLowerArmBack = armsResetMode == ArmsResetModes.BACK && (isLeftLowerArmTracker() || isRightLowerArmTracker())
-		val isArmForward = armsResetMode == ArmsResetModes.FORWARD && (isLeftArmTracker() || isRightArmTracker())
-		if (!isThighTracker() && !isArmForward && !isLowerArmBack) {
-			// Tracker goes back
-			yawAngle -= FastMath.PI
+		val isArmTracker =
+			isLeftArmTracker() || isLeftLowerArmTracker() || isLeftFingerTracker() ||
+				isRightArmTracker() || isRightLowerArmTracker() || isRightFingerTracker()
+
+		// Fix mounting for trackers that are facing down
+		if (isArmTracker) {
+			// FIXME: This is confusing
+			val isLowerArmBack = armsResetMode == ArmsResetModes.BACK && (isLeftLowerArmTracker() || isRightLowerArmTracker())
+			val isArmForward = armsResetMode == ArmsResetModes.FORWARD && (isLeftArmTracker() || isRightArmTracker())
+			if (!isArmForward && !isLowerArmBack) {
+				// Tracker goes back
+				yawAngle -= FastMath.PI
+			}
+		} else {
+			when (bodyPose) {
+				ResetBodyPose.SKIING -> {
+					// All trackers except thigh trackers are facing down
+					if (!isThighTracker()) {
+						yawAngle -= FastMath.PI
+					}
+				}
+
+				ResetBodyPose.SITTING_LEANING_BACK -> {
+					// All trackers are facing up, so don't adjust any trackers
+				}
+			}
 		}
 
 		// Make an adjustment quaternion from the angle
@@ -461,6 +495,8 @@ class TrackerResetsHandler(val tracker: Tracker) {
 		if (saveMountingReset) tracker.saveMountingResetOrientation(mountRotFix)
 
 		tracker.resetFilteringQuats()
+
+		LogManager.info("[resetMounting] ${tracker.trackerPosition} reset")
 	}
 
 	/**

server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt

@@ -508,13 +508,13 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 					UDPPacket21UserAction.RESET_FULL -> {
 						name = "Full reset"
 						VRServer.instance.resetHandler.sendStarted(ResetType.Full)
-						VRServer.instance.resetTrackersFull(RESET_SOURCE_NAME)
+						VRServer.instance.resetTrackersFull(ResetParams.makeDefault(RESET_SOURCE_NAME))
 					}
 
 					UDPPacket21UserAction.RESET_YAW -> {
 						name = "Yaw reset"
 						VRServer.instance.resetHandler.sendStarted(ResetType.Yaw)
-						VRServer.instance.resetTrackersYaw(RESET_SOURCE_NAME)
+						VRServer.instance.resetTrackersYaw(ResetParams.makeDefault(RESET_SOURCE_NAME))
 					}
 
 					UDPPacket21UserAction.RESET_MOUNTING -> {
@@ -523,7 +523,7 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker
 							.instance
 							.resetHandler
 							.sendStarted(ResetType.Mounting)
-						VRServer.instance.resetTrackersMounting(RESET_SOURCE_NAME)
+						VRServer.instance.resetTrackersMounting(ResetParams.makeDefault(RESET_SOURCE_NAME))
 					}
 
 					UDPPacket21UserAction.PAUSE_TRACKING -> {

server/core/src/main/java/dev/slimevr/websocketapi/WebSocketVRBridge.kt

@@ -6,6 +6,7 @@ import dev.slimevr.VRServer
 import dev.slimevr.VRServer.Companion.getNextLocalTrackerId
 import dev.slimevr.VRServer.Companion.instance
 import dev.slimevr.bridge.Bridge
+import dev.slimevr.tracking.trackers.ResetParams
 import dev.slimevr.tracking.trackers.Tracker
 import dev.slimevr.tracking.trackers.TrackerPosition
 import dev.slimevr.tracking.trackers.TrackerStatus
@@ -203,9 +204,9 @@ class WebSocketVRBridge(
 
 	private fun parseAction(json: ObjectNode, conn: WebSocket) {
 		when (json["name"].asText()) {
-			"calibrate" -> instance.resetTrackersYaw(RESET_SOURCE_NAME)
-			"full_calibrate" -> instance.resetTrackersFull(RESET_SOURCE_NAME)
-			"mounting_calibrate" -> instance.resetTrackersMounting(RESET_SOURCE_NAME)
+			"calibrate" -> instance.resetTrackersYaw(ResetParams.makeDefault(RESET_SOURCE_NAME))
+			"full_calibrate" -> instance.resetTrackersFull(ResetParams.makeDefault(RESET_SOURCE_NAME))
+			"mounting_calibrate" -> instance.resetTrackersMounting(ResetParams.makeDefault(RESET_SOURCE_NAME))
 			"mounting_clear" -> instance.clearTrackersMounting(RESET_SOURCE_NAME)
 			"toggle_pause_tracking" -> instance.togglePauseTracking(RESET_SOURCE_NAME)
 		}

server/core/src/main/java/io/github/axisangles/ktmath/Quaternion.kt

@@ -54,6 +54,33 @@ value class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
 			return (d + d.len()).unit()
 		}
 
+		/**
+		 * Rotation around X-axis
+		 *
+		 * Derived from the axis-angle representation in
+		 * https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Unit_quaternions
+		 */
+		fun rotationAroundXAxis(angle: Float): Quaternion =
+			Quaternion(cos(angle / 2.0f), sin(angle / 2.0f), 0.0f, 0.0f)
+
+		/**
+		 * Rotation around Y-axis
+		 *
+		 * Derived from the axis-angle representation in
+		 * https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Unit_quaternions
+		 */
+		fun rotationAroundYAxis(angle: Float): Quaternion =
+			Quaternion(cos(angle / 2.0f), 0.0f, sin(angle / 2.0f), 0.0f)
+
+		/**
+		 * Rotation around Z-axis
+		 *
+		 * Derived from the axis-angle representation in
+		 * https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Unit_quaternions
+		 */
+		fun rotationAroundZAxis(angle: Float): Quaternion =
+			Quaternion(cos(angle / 2.0f), 0.0f, 0.0f, sin(angle / 2.0f))
+
 		/**
 		 * SlimeVR-specific constants and utils
 		 */
@@ -376,6 +403,45 @@ value class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
 	 **/
 	fun sandwich(that: Vector3): Vector3 = (this * Quaternion(0f, that) / this).xyz
 
+	/**
+	 * Sandwiches the unit X vector
+	 *
+	 * First column of rotation matrix in
+	 * https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Conversion_to_and_from_the_matrix_representation
+	 */
+	fun sandwichUnitX(): Vector3 =
+		Vector3(
+			w * w + x * x - y * y - z * z,
+			2.0f * (x * y + w * z),
+			2.0f * (x * z - w * y),
+		)
+
+	/**
+	 * Sandwiches the unit Y vector
+	 *
+	 * Second column of rotation matrix in
+	 * https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Conversion_to_and_from_the_matrix_representation
+	 */
+	fun sandwichUnitY(): Vector3 =
+		Vector3(
+			2.0f * (x * y - w * z),
+			w * w - x * x + y * y - z * z,
+			2.0f * (y * z + w * x),
+		)
+
+	/**
+	 * Sandwiches the unit Z vector
+	 *
+	 * Third column of rotation matrix in
+	 * https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Conversion_to_and_from_the_matrix_representation
+	 */
+	fun sandwichUnitZ(): Vector3 =
+		Vector3(
+			2.0f * (x * z + w * y),
+			2.0f * (y * z - w * x),
+			w * w - x * x - y * y + z * z,
+		)
+
 	/**
 	 * computes this quaternion's unit length rotation axis
 	 * @return rotation axis

server/core/src/main/java/io/github/axisangles/ktmath/Vector3.kt

@@ -99,6 +99,9 @@ value class Vector3(val x: Float, val y: Float, val z: Float) {
 	 * @return the angle
 	 **/
 	fun angleTo(that: Vector3): Float = atan2(this.cross(that).len(), this.dot(that))
+
+	fun isNear(other: Vector3, maxError: Float = 1e-6f) =
+		abs(x - other.x) <= maxError && abs(y - other.y) <= maxError && abs(z - other.z) <= maxError
 }
 
 operator fun Float.times(that: Vector3): Vector3 = that * this

server/core/src/test/java/dev/slimevr/unit/MountingResetTests.kt

@@ -2,6 +2,7 @@ package dev.slimevr.unit
 
 import com.jme3.math.FastMath
 import dev.slimevr.VRServer.Companion.getNextLocalTrackerId
+import dev.slimevr.tracking.trackers.ResetBodyPose
 import dev.slimevr.tracking.trackers.Tracker
 import dev.slimevr.tracking.trackers.udp.IMUType
 import dev.slimevr.unit.TrackerUtils.assertAnglesApproxEqual
@@ -54,7 +55,7 @@ class MountingResetTests {
 		tracker.setRotation(Quaternion.IDENTITY)
 		tracker.resetsHandler.resetFull(Quaternion.IDENTITY)
 		tracker.setRotation(trackerRot)
-		tracker.resetsHandler.resetMounting(Quaternion.IDENTITY)
+		tracker.resetsHandler.resetMounting(Quaternion.IDENTITY, ResetBodyPose.SKIING)
 
 		val expectedYaw = yaw(expected)
 		val resultYaw = yaw(tracker.resetsHandler.mountRotFix)
@@ -71,7 +72,7 @@ class MountingResetTests {
 		tracker.setRotation(reference * trackerRot)
 		// Since reference is the offset from quat identity (reset) and the rotation,
 		// it needs to be applied twice
-		tracker.resetsHandler.resetMounting(reference * reference)
+		tracker.resetsHandler.resetMounting(reference * reference, ResetBodyPose.SKIING)
 
 		val expectedYaw2 = yaw(expected)
 		val resultYaw2 = yaw(tracker.resetsHandler.mountRotFix)
@@ -86,7 +87,7 @@ class MountingResetTests {
 		tracker.resetsHandler.resetFull(reference)
 		tracker.resetsHandler.resetYaw(Quaternion.IDENTITY)
 		tracker.setRotation(trackerRot)
-		tracker.resetsHandler.resetMounting(Quaternion.IDENTITY)
+		tracker.resetsHandler.resetMounting(Quaternion.IDENTITY, ResetBodyPose.SKIING)
 
 		val expectedYaw3 = yaw(expected)
 		val resultYaw3 = yaw(tracker.resetsHandler.mountRotFix)
@@ -104,7 +105,7 @@ class MountingResetTests {
 		tracker.setRotation(reference * trackerRot)
 		// Since reference is the offset from quat identity (reset) and the rotation,
 		// it needs to be applied twice
-		tracker.resetsHandler.resetMounting(reference * reference)
+		tracker.resetsHandler.resetMounting(reference * reference, ResetBodyPose.SKIING)
 
 		val expectedYaw4 = yaw(expected)
 		val resultYaw4 = yaw(tracker.resetsHandler.mountRotFix)
@@ -139,7 +140,7 @@ class MountingResetTests {
 		tracker.setRotation(Quaternion.IDENTITY)
 		tracker.resetsHandler.resetFull(Quaternion.IDENTITY)
 		tracker.setRotation(trackerRot)
-		tracker.resetsHandler.resetMounting(Quaternion.IDENTITY)
+		tracker.resetsHandler.resetMounting(Quaternion.IDENTITY, ResetBodyPose.SKIING)
 
 		val expectedYaw = yaw(expected)
 		val resultYaw = yaw(tracker.resetsHandler.mountRotFix)

server/core/src/test/java/dev/slimevr/unit/SkeletonResetTests.kt

@@ -3,6 +3,7 @@ package dev.slimevr.unit
 import com.jme3.math.FastMath
 import dev.slimevr.VRServer.Companion.getNextLocalTrackerId
 import dev.slimevr.tracking.processor.HumanPoseManager
+import dev.slimevr.tracking.trackers.ResetParams
 import dev.slimevr.tracking.trackers.Tracker
 import dev.slimevr.tracking.trackers.TrackerPosition
 import dev.slimevr.tracking.trackers.TrackerStatus
@@ -57,7 +58,7 @@ class SkeletonResetTests {
 			tracker.setRotation(init)
 		}
 		hmd.setRotation(headRot1)
-		hpm.resetTrackersFull(resetSource)
+		hpm.resetTrackersFull(ResetParams.makeDefault(resetSource))
 
 		for (tracker in tracks) {
 			val actual = tracker.getRotation()
@@ -78,7 +79,7 @@ class SkeletonResetTests {
 			tracker.setRotation(init)
 		}
 		hmd.setRotation(Quaternion.IDENTITY)
-		hpm.resetTrackersYaw(resetSource)
+		hpm.resetTrackersYaw(ResetParams.makeDefault(resetSource))
 
 		for (tracker in tracks) {
 			val yaw = TrackerUtils.yaw(tracker.getRotation())
@@ -120,7 +121,7 @@ class SkeletonResetTests {
 			tracker.setRotation(mkTrackMount(mountRot))
 		}
 		// Then perform a mounting reset
-		hpm.resetTrackersMounting(resetSource)
+		hpm.resetTrackersMounting(ResetParams.makeDefault(resetSource))
 
 		for ((tracker, mountRot) in expected) {
 			// Some mounting needs to be inverted (when in a specific pose)

server/core/src/test/java/io/github/axisangles/ktmath/QuaternionTest.kt

@@ -180,6 +180,24 @@ class QuaternionTest {
 		assertEquals(v2, v1)
 	}
 
+	@Test
+	fun sandwichUnitX() {
+		val q = Quaternion(0.34f, 0.223f, -0.8f, -0.7f).unit()
+		assertTrue(q.sandwichUnitX().isNear(q.sandwich(Vector3.POS_X)))
+	}
+
+	@Test
+	fun sandwichUnitY() {
+		val q = Quaternion(0.34f, 0.223f, -0.8f, -0.7f).unit()
+		assertTrue(q.sandwichUnitY().isNear(q.sandwich(Vector3.POS_Y)))
+	}
+
+	@Test
+	fun sandwichUnitZ() {
+		val q = Quaternion(0.34f, 0.223f, -0.8f, -0.7f).unit()
+		assertTrue(q.sandwichUnitZ().isNear(q.sandwich(Vector3.POS_Z)))
+	}
+
 	@Test
 	fun axis() {
 		val v1 = Quaternion(0f, Quaternion(1f, 2f, 3f, 4f).axis())

server/desktop/src/main/java/dev/slimevr/desktop/platform/ProtobufBridge.kt

@@ -4,6 +4,7 @@ import dev.slimevr.VRServer.Companion.instance
 import dev.slimevr.bridge.BridgeThread
 import dev.slimevr.bridge.ISteamVRBridge
 import dev.slimevr.desktop.platform.ProtobufMessages.*
+import dev.slimevr.tracking.trackers.ResetParams
 import dev.slimevr.tracking.trackers.Tracker
 import dev.slimevr.tracking.trackers.TrackerStatus
 import dev.slimevr.tracking.trackers.TrackerStatus.Companion.getById
@@ -197,9 +198,9 @@ abstract class ProtobufBridge(@JvmField protected val bridgeName: String) : ISte
 		val resetSourceName = String.format("%s: %s", resetSourceNamePrefix, bridgeName)
 		when (userAction.name) {
 			"reset" -> // TODO : Check pose field
-				instance.resetTrackersFull(resetSourceName)
+				instance.resetTrackersFull(ResetParams.makeDefault(resetSourceName))
 
-			"fast_reset" -> instance.resetTrackersYaw(resetSourceName)
+			"fast_reset" -> instance.resetTrackersYaw(ResetParams.makeDefault(resetSourceName))
 
 			"pause_tracking" ->
 				instance