Add "Setup Stay Aligned" flow

Keeps your trackers aligned by slowly adjusting the yaw of your trackers to
correct for drift.

Every tick, Stay Aligned adjusts the yaw of one tracker. It looks at the
player's current pose, and adjusts the yaw of the tracker by a tiny amount, in
the direction which is closest to the relaxed pose that the player set up. Over
time, this has the effect of keeping all the trackers in a human-like pose.

gui/public/i18n/en/translation.ftl

@@ -293,6 +293,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
@@ -317,6 +318,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
@@ -447,6 +449,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
@@ -539,6 +542,29 @@ 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 = Stay Aligned reduces drift by gradually adjusting your trackers to match your most common poses.
+settings-stay_aligned-setup-label = Setup Stay Aligned
+settings-stay_aligned-setup-description = You must complete "Setup Stay Aligned" to enable Stay Aligned.
+settings-stay_aligned-warnings-drift_compensation = ⚠ Please turn off Drift Compensation! Drift Compensation will conflict with Stay Aligned.
+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-general-label = General
+settings-stay_aligned-relaxed_poses-label = Relaxed Poses
+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-detect_pose = Detect pose
+settings-stay_aligned-relaxed_poses-reset_pose = Reset pose
+settings-stay_aligned-relaxed_poses-outwards = {$angle} outwards
+settings-stay_aligned-relaxed_poses-inwards = {$angle} inwards
+settings-stay_aligned-relaxed_poses-disabled = Not enabled
+
 ## FK/Tracking settings
 settings-general-fk_settings = Tracking settings
 
@@ -1223,6 +1249,34 @@ 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-verify_mounting-title = Check your Mounting
+onboarding-stay_aligned-verify_mounting-step-0 = Stay Aligned requires good mounting. Otherwise, you won't get a good experience with Stay Aligned.
+onboarding-stay_aligned-verify_mounting-step-1 = 1. Move around while standing.
+onboarding-stay_aligned-verify_mounting-step-2 = 2. Sit down and move your legs and feet.
+onboarding-stay_aligned-verify_mounting-step-3 = 3. If your trackers aren't in the right place, restart the process.
+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-standing-step-2 = 3. Press the "Detect pose" button.
+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-sitting-step-2 = 3. Press the "Detect pose" button.
+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.
+onboarding-stay_aligned-relaxed_poses-flat-step-2 = 3. Press the "Detect pose" button.
+onboarding-stay_aligned-relaxed_poses-skip_step = Skip
+onboarding-stay_aligned-done-title = Stay Aligned enabled!
+onboarding-stay_aligned-done-description = Your Stay Aligned setup is complete!
+onboarding-stay_aligned-previous_step = Previous
+onboarding-stay_aligned-next_step = Next
+onboarding-stay_aligned-restart = Restart
+onboarding-stay_aligned-done = Done
+
 ## Home
 home-no_trackers = No trackers detected or assigned
 

gui/src/App.tsx

@@ -60,6 +60,7 @@ import { AdvancedSettings } from './components/settings/pages/AdvancedSettings';
 import { FirmwareUpdate } from './components/firmware-update/FirmwareUpdate';
 import { ConnectionLost } from './components/onboarding/pages/ConnectionLost';
 import { VRCWarningsPage } from './components/vrc/VRCWarningsPage';
+import { StayAlignedSetup } from './components/onboarding/pages/stay-aligned/StayAlignedSetup';
 
 export const GH_REPO = 'SlimeVR/SlimeVR-Server';
 export const VersionContext = createContext('');
@@ -173,6 +174,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/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/onboarding/pages/mounting/AutomaticMounting.tsx

@@ -7,12 +7,18 @@ import { PreparationStep } from './mounting-steps/Preparation';
 import { PutTrackersOnStep } from './mounting-steps/PutTrackersOn';
 import { useLocalization } from '@fluent/react';
 
-const steps: Step[] = [
+// Auto mounting steps that can be included within other flows
+export const autoMountingSteps: Step[] = [
   { type: 'numbered', component: PutTrackersOnStep },
   { type: 'numbered', component: PreparationStep },
   { type: 'numbered', component: MountingResetStep },
+];
+
+const steps: Step[] = [
+  ...autoMountingSteps,
   { type: 'fullsize', component: DoneStep },
 ];
+
 export function AutomaticMountingPage() {
   const { l10n } = useLocalization();
   const { applyProgress, state } = useOnboarding();

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

@@ -0,0 +1,65 @@
+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 { autoMountingSteps } from '@/components/onboarding/pages/mounting/AutomaticMounting';
+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 { VerifyMountingStep } from './stay-aligned-steps/VerifyMounting';
+
+export function enableStayAligned(
+  enable: boolean,
+  sendRPCPacket: (type: RpcMessage, data: RPCPacketType) => void
+) {
+  const req = new EnableStayAlignedRequestT();
+  req.enable = enable;
+  sendRPCPacket(RpcMessage.EnableStayAlignedRequest, req);
+}
+
+const steps: Step[] = [
+  ...autoMountingSteps,
+  { type: 'numbered', component: VerifyMountingStep },
+  { 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(() => {
+    // Disable Stay Aligned as soon as we enter the setup flow so that we don't
+    // adjust the trackers while trying to set up the feature
+    enableStayAligned(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,45 @@
+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,
+}: {
+  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-restart')}
+        </Button>
+        <Button
+          variant="primary"
+          to="/settings/trackers"
+          state={{ scrollTo: 'stayaligned' }}
+        >
+          {l10n.getString('onboarding-stay_aligned-done')}
+        </Button>
+      </div>
+
+      <SkeletonVisualizerWidget />
+    </div>
+  );
+}

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

@@ -0,0 +1,119 @@
+import { Button } from '@/components/commons/Button';
+import { Typography } from '@/components/commons/Typography';
+import {
+  CurrentRelaxedPose,
+  DetectRelaxedPoseButton,
+  ResetRelaxedPoseButton,
+} from '@/components/stay-aligned/RelaxedPose';
+import { useLocalization } from '@fluent/react';
+import { StayAlignedRelaxedPose } from 'solarxr-protocol';
+import { enableStayAligned } from '@/components/onboarding/pages/stay-aligned/StayAlignedSetup';
+import { useWebsocketAPI } from '@/hooks/websocket-api';
+
+function makeRelaxedPoseStep(
+  titleKey: string,
+  descriptionKeys: string[],
+  imageUrl: string,
+  relaxedPose: StayAlignedRelaxedPose,
+  lastStep: 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-stay_aligned-previous_step')}
+            </Button>
+            <DetectRelaxedPoseButton
+              onClick={() => {
+                if (lastStep) {
+                  enableStayAligned(true, sendRPCPacket);
+                }
+                nextStep();
+              }}
+              pose={relaxedPose}
+            />
+            <ResetRelaxedPoseButton
+              onClick={() => {
+                if (lastStep) {
+                  enableStayAligned(true, sendRPCPacket);
+                }
+                nextStep();
+              }}
+              pose={relaxedPose}
+            >
+              {l10n.getString(
+                'onboarding-stay_aligned-relaxed_poses-skip_step'
+              )}
+            </ResetRelaxedPoseButton>
+          </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',
+    'onboarding-stay_aligned-relaxed_poses-standing-step-2',
+  ],
+  '/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',
+    'onboarding-stay_aligned-relaxed_poses-sitting-step-2',
+  ],
+  '/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',
+    'onboarding-stay_aligned-relaxed_poses-flat-step-2',
+  ],
+  '/images/relaxed_pose_flat.webp',
+  StayAlignedRelaxedPose.FLAT,
+  true
+);

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

@@ -0,0 +1,50 @@
+import { Button } from '@/components/commons/Button';
+import { Typography } from '@/components/commons/Typography';
+import { useLocalization } from '@fluent/react';
+
+export function VerifyMountingStep({
+  nextStep,
+  resetSteps,
+  variant,
+}: {
+  nextStep: () => void;
+  resetSteps: () => 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-stay_aligned-verify_mounting-title')}
+        </Typography>
+        <div className="flex flex-col gap-2">
+          <Typography color="secondary">
+            {l10n.getString('onboarding-stay_aligned-verify_mounting-step-0')}
+          </Typography>
+          <Typography color="secondary">
+            {l10n.getString('onboarding-stay_aligned-verify_mounting-step-1')}
+          </Typography>
+          <Typography color="secondary">
+            {l10n.getString('onboarding-stay_aligned-verify_mounting-step-2')}
+          </Typography>
+          <Typography color="secondary">
+            {l10n.getString('onboarding-stay_aligned-verify_mounting-step-3')}
+          </Typography>
+        </div>
+        <div className="flex gap-3 mobile:justify-between">
+          <Button
+            variant={variant === 'onboarding' ? 'secondary' : 'tertiary'}
+            onClick={resetSteps}
+          >
+            {l10n.getString('onboarding-stay_aligned-restart')}
+          </Button>
+          <Button variant="primary" onClick={nextStep}>
+            {l10n.getString('onboarding-stay_aligned-next_step')}
+          </Button>
+        </div>
+      </div>
+    </div>
+  );
+}

gui/src/components/settings/SettingsSidebar.tsx

@@ -57,6 +57,9 @@ export function SettingsSidebar() {
           <SettingsLink to="/settings/trackers" scrollTo="steamvr">
             SteamVR
           </SettingsLink>
+          <SettingsLink to="/settings/trackers" scrollTo="stayaligned">
+            {l10n.getString('settings-sidebar-stay_aligned')}
+          </SettingsLink>
           <SettingsLink to="/settings/trackers" scrollTo="mechanics">
             {l10n.getString('settings-sidebar-tracker_mechanics')}
           </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 });
   });
 
@@ -612,6 +629,7 @@ export function GeneralSettings() {
             </div>
           </>
         </SettingsPagePaneLayout>
+        <StayAlignedSettings values={getValues()} control={control} />
         <SettingsPagePaneLayout icon={<WrenchIcon></WrenchIcon>} id="mechanics">
           <>
             <Typography variant="main-title">

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

@@ -0,0 +1,165 @@
+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;
+  standingEnabled: boolean;
+  standingUpperLegAngle: number;
+  standingLowerLegAngle: number;
+  standingFootAngle: number;
+  sittingEnabled: boolean;
+  sittingUpperLegAngle: number;
+  sittingLowerLegAngle: number;
+  sittingFootAngle: number;
+  flatEnabled: boolean;
+  flatUpperLegAngle: number;
+  flatLowerLegAngle: number;
+  flatFootAngle: number;
+};
+
+export const defaultStayAlignedSettings: StayAlignedSettingsForm = {
+  enabled: false,
+  extraYawCorrection: false,
+  hideYawCorrection: false,
+  standingEnabled: false,
+  standingUpperLegAngle: 0.0,
+  standingLowerLegAngle: 0.0,
+  standingFootAngle: 0.0,
+  sittingEnabled: false,
+  sittingUpperLegAngle: 0.0,
+  sittingLowerLegAngle: 0.0,
+  sittingFootAngle: 0.0,
+  flatEnabled: false,
+  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.standingEnabled = settings.standingEnabled;
+  serialized.standingUpperLegAngle = settings.standingUpperLegAngle;
+  serialized.standingLowerLegAngle = settings.standingLowerLegAngle;
+  serialized.standingFootAngle = settings.standingFootAngle;
+  serialized.sittingEnabled = settings.sittingEnabled;
+  serialized.sittingUpperLegAngle = settings.sittingUpperLegAngle;
+  serialized.sittingLowerLegAngle = settings.sittingLowerLegAngle;
+  serialized.sittingFootAngle = settings.sittingFootAngle;
+  serialized.flatEnabled = settings.flatEnabled;
+  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="mt-2">
+        <Typography color="secondary">
+          {l10n.getString('settings-stay_aligned-description')}
+        </Typography>
+        <Typography color="secondary">
+          {l10n.getString('settings-stay_aligned-setup-description')}
+        </Typography>
+        <div className="flex mt-2">
+          <Button
+            variant="primary"
+            to="/onboarding/stay-aligned"
+            state={{ alonePage: true }}
+          >
+            {l10n.getString('settings-stay_aligned-setup-label')}
+          </Button>
+        </div>
+      </div>
+      <div className="mt-4">
+        <Typography bold>
+          {l10n.getString('settings-stay_aligned-general-label')}
+        </Typography>
+        {values.stayAligned.enabled && values.driftCompensation.enabled && (
+          <div className="mt-2">
+            {l10n.getString(
+              'settings-stay_aligned-warnings-drift_compensation'
+            )}
+          </div>
+        )}
+        <div className="grid sm:grid-cols-2 gap-3 mt-2">
+          <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>
+      <div className="mt-4">
+        <Typography bold>
+          {l10n.getString('settings-stay_aligned-relaxed_poses-label')}
+        </Typography>
+        <div className="grid sm:grid-cols-1 gap-3 mt-2">
+          {config?.debug ? (
+            <RelaxedPosesSettings control={control} />
+          ) : (
+            <RelaxedPosesSummary values={values} />
+          )}
+        </div>
+      </div>
+    </SettingsPagePaneLayout>
+  );
+}

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

@@ -0,0 +1,439 @@
+import { useLocaleConfig } from '@/i18n/config';
+import { ReactLocalization, useLocalization } from '@fluent/react';
+import {
+  DetectStayAlignedRelaxedPoseRequestT,
+  RpcMessage,
+  StayAlignedPoseT,
+  StayAlignedRelaxedPose,
+} from 'solarxr-protocol';
+import { Typography } from '@/components/commons/Typography';
+import { SettingsForm } from '@/components/settings/pages/GeneralSettings';
+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';
+import { CheckBox } from '@/components/commons/Checkbox';
+import { useAtomValue } from 'jotai';
+import { stayAlignedPoseAtom } from '@/store/app-store';
+
+/**
+ * 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,
+  enabled,
+  upperLegAngleInDeg,
+  lowerLegAngleInDeg,
+  footAngleInDeg,
+}: {
+  pose: StayAlignedRelaxedPose;
+  enabled: boolean;
+  upperLegAngleInDeg: number;
+  lowerLegAngleInDeg: number;
+  footAngleInDeg: number;
+}) {
+  const { l10n } = useLocalization();
+  const { currentLocales } = useLocaleConfig();
+  const angleFormat = PoseAngleFormat(l10n, currentLocales);
+
+  if (enabled) {
+    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>
+    );
+  } else {
+    return (
+      <div>
+        <Typography color="primary">
+          {l10n.getString(relaxedPoseKey(pose))}
+        </Typography>
+        <Typography color="secondary">
+          {l10n.getString('settings-stay_aligned-relaxed_poses-disabled')}
+        </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 stayAlignedPose =
+    useAtomValue(stayAlignedPoseAtom) || new StayAlignedPoseT(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(stayAlignedPose.upperLegAngleInDeg)}
+      </Typography>
+      <Typography color="secondary">
+        {l10n.getString('settings-stay_aligned-relaxed_poses-lower_leg_angle')}:{' '}
+        {angleFormat.format(stayAlignedPose.lowerLegAngleInDeg)}
+      </Typography>
+      <Typography color="secondary">
+        {l10n.getString('settings-stay_aligned-relaxed_poses-foot_angle')}:{' '}
+        {angleFormat.format(stayAlignedPose.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}
+          enabled={values.stayAligned.standingEnabled}
+          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}
+          enabled={values.stayAligned.sittingEnabled}
+          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}
+          enabled={values.stayAligned.flatEnabled}
+          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-detect_pose')}
+    </Button>
+  );
+}
+
+/**
+ * Tells the server to reset the angles in a relaxed pose.
+ */
+export function ResetRelaxedPoseButton({
+  pose,
+  onClick,
+  children,
+}: {
+  pose: StayAlignedRelaxedPose;
+  onClick?: MouseEventHandler<HTMLButtonElement>;
+} & React.PropsWithChildren) {
+  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);
+        }
+      }}
+    >
+      {children ||
+        l10n.getString('settings-stay_aligned-relaxed_poses-reset_pose')}
+    </Button>
+  );
+}
+
+/**
+ * Control to edit the angles of a pose.
+ */
+function RelaxedPoseControl({
+  pose,
+  enabledKey,
+  upperLegSettingsKey,
+  lowerLegSettingsKey,
+  footSettingsKey,
+  control,
+}: {
+  pose: StayAlignedRelaxedPose;
+  enabledKey: string;
+  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}
+      />
+      <CheckBox
+        variant="toggle"
+        outlined
+        control={control}
+        name={enabledKey}
+        label={l10n.getString('settings-stay_aligned-enabled-label')}
+      />
+      <DetectRelaxedPoseButton pose={pose} />
+      <ResetRelaxedPoseButton pose={pose} />
+    </div>
+  );
+}
+
+/**
+ * Control that displays the current pose's relaxed angles, in a similar layout
+ * to <RelaxedPoseControl />.
+ */
+function CurrentRelaxedPoseControl({
+  control,
+}: {
+  control: Control<SettingsForm, any>;
+}) {
+  const { l10n } = useLocalization();
+  const { currentLocales } = useLocaleConfig();
+  const angleFormat = PoseAngleFormat(l10n, currentLocales);
+
+  const stayAlignedPose =
+    useAtomValue(stayAlignedPoseAtom) || new StayAlignedPoseT(0.0, 0.0, 0.0);
+
+  return (
+    <div className="grid sm:grid-cols-1 gap-3">
+      <Typography color="primary">
+        {l10n.getString('settings-stay_aligned-relaxed_poses-current_angles')}
+      </Typography>
+      <NumberSelector
+        control={control}
+        name=""
+        disabled
+        valueLabelFormat={() =>
+          `${l10n.getString(
+            'settings-stay_aligned-relaxed_poses-upper_leg_angle'
+          )}: ${angleFormat.format(stayAlignedPose.upperLegAngleInDeg || 0.0)}`
+        }
+        min={-90.0}
+        max={90.0}
+        step={1.0}
+      />
+      <NumberSelector
+        control={control}
+        name=""
+        disabled
+        valueLabelFormat={() =>
+          `${l10n.getString(
+            'settings-stay_aligned-relaxed_poses-lower_leg_angle'
+          )}: ${angleFormat.format(stayAlignedPose.lowerLegAngleInDeg || 0.0)}`
+        }
+        min={-90.0}
+        max={90.0}
+        step={1.0}
+      />
+      <NumberSelector
+        control={control}
+        name=""
+        disabled
+        valueLabelFormat={() =>
+          `${l10n.getString(
+            'settings-stay_aligned-relaxed_poses-foot_angle'
+          )}: ${angleFormat.format(stayAlignedPose.footAngleInDeg || 0.0)}`
+        }
+        min={-90.0}
+        max={90.0}
+        step={1.0}
+      />
+    </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}
+          enabledKey="stayAligned.standingEnabled"
+          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}
+          enabledKey="stayAligned.sittingEnabled"
+          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}
+          enabledKey="stayAligned.flatEnabled"
+          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 control={control} />
+      </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/TrackersTable.tsx

@@ -17,6 +17,7 @@ import { TrackerStatus } from './TrackerStatus';
 import { TrackerWifi } from './TrackerWifi';
 import { trackerStatusRelated, useStatusContext } from '@/hooks/status-system';
 import { FlatDeviceTracker } from '@/store/app-store';
+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, Vector3FromVec3fT } 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';
 
@@ -168,6 +169,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

@@ -18,6 +18,7 @@ export function useDataFeedConfig() {
   trackerData.rotationIdentityAdjusted = true;
   trackerData.tps = true;
   trackerData.rawMagneticVector = true;
+  trackerData.stayAligned = true;
 
   const dataMask = new DeviceDataMaskT();
   dataMask.deviceData = true;
@@ -28,6 +29,7 @@ export function useDataFeedConfig() {
   dataFeedConfig.boneMask = true;
   dataFeedConfig.minimumTimeSinceLast = 1000 / feedMaxTps;
   dataFeedConfig.syntheticTrackersMask = trackerData;
+  dataFeedConfig.stayAlignedPoseMask = 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 };
 }
 

gui/src/store/app-store.ts

@@ -72,6 +72,12 @@ export const hasHMDTrackerAtom = atom((get) => {
   );
 });
 
+export const stayAlignedPoseAtom = selectAtom(
+  datafeedAtom,
+  (datafeed) => datafeed.stayAlignedPose,
+  isEqual
+);
+
 export const trackerFromIdAtom = ({
   trackerNum,
   deviceId,

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

@@ -0,0 +1,49 @@
+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 from Stay Aligned.
+	 *
+	 * Players can enable this to compare to when Stay Aligned is not enabled. Useful to
+	 * verify if Stay Aligned improved the situation. Also useful to prevent players
+	 * from saying "Stay Aligned screwed up my trackers!!" when it's actually a tracker
+	 * that is drifting extremely badly.
+	 *
+	 * Do not serialize to config so that when the server restarts, it is always false.
+	 */
+	@JsonIgnore
+	var hideYawCorrection = false
+
+	/**
+	 * Standing relaxed pose
+	 */
+	val standingRelaxedPose = StayAlignedRelaxedPoseConfig()
+
+	/**
+	 * Sitting relaxed pose
+	 */
+	val sittingRelaxedPose = StayAlignedRelaxedPoseConfig()
+
+	/**
+	 * Flat relaxed pose
+	 */
+	val flatRelaxedPose = StayAlignedRelaxedPoseConfig()
+}

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

@@ -0,0 +1,25 @@
+package dev.slimevr.config
+
+class StayAlignedRelaxedPoseConfig {
+
+	/**
+	 * Whether Stay Aligned should adjust the tracker yaws when the player is in this
+	 * pose.
+	 */
+	var enabled = false
+
+	/**
+	 * Angle between the upper leg yaw and the center yaw.
+	 */
+	var upperLegAngleInDeg = 0.0f
+
+	/**
+	 * Angle between the lower leg yaw and the center yaw.
+	 */
+	var lowerLegAngleInDeg = 0.0f
+
+	/**
+	 * Angle between the foot and the center yaw.
+	 */
+	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,81 @@
+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))
+
+		// 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,38 @@
+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
+
+	/**
+	 * Adds another angle to the average.
+	 */
+	fun add(angle: Angle, weight: Float = 1.0f) {
+		sumX += cos(angle.toRad()) * weight
+		sumY += sin(angle.toRad()) * weight
+	}
+
+	/**
+	 * Gets the average angle.
+	 */
+	fun toAngle(): Angle =
+		if (isEmpty()) {
+			Angle.ZERO
+		} else {
+			Angle.ofRad(atan2(sumY, sumX))
+		}
+
+	/**
+	 * Whether there are any angles to average.
+	 */
+	fun isEmpty() =
+		sumX == 0.0f && sumY == 0.0f
+}

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

@@ -0,0 +1,15 @@
+package dev.slimevr.math
+
+import kotlin.math.*
+
+class AngleErrors {
+
+	private var sumSqrErrors = 0.0f
+
+	fun add(error: Angle) {
+		sumSqrErrors += error.toRad() * error.toRad()
+	}
+
+	fun toL2Norm() =
+		Angle.ofRad(sqrt(sumSqrErrors))
+}

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

@@ -186,6 +186,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);
@@ -237,6 +243,9 @@ public class DataFeedBuilder {
 		if (mask.getRawMagneticVector() && tracker.getMagStatus() == MagnetometerStatus.ENABLED) {
 			TrackerData.addRawMagneticVector(fbb, createTrackerMagneticVector(fbb, tracker));
 		}
+		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,42 @@
+package dev.slimevr.protocol.datafeed
+
+import com.google.flatbuffers.FlatBufferBuilder
+import dev.slimevr.tracking.processor.skeleton.HumanSkeleton
+import dev.slimevr.tracking.processor.stayaligned.poses.RelaxedPose
+import dev.slimevr.tracking.processor.stayaligned.trackers.RestDetector
+import dev.slimevr.tracking.processor.stayaligned.trackers.StayAlignedTrackerState
+import solarxr_protocol.data_feed.stay_aligned.StayAlignedPose
+import solarxr_protocol.data_feed.stay_aligned.StayAlignedTracker
+
+object DataFeedBuilderKotlin {
+
+	fun createStayAlignedPose(
+		fbb: FlatBufferBuilder,
+		humanSkeleton: HumanSkeleton,
+	): Int {
+		val relaxedPose = RelaxedPose.fromTrackers(humanSkeleton)
+
+		StayAlignedPose.startStayAlignedPose(fbb)
+
+		StayAlignedPose.addUpperLegAngleInDeg(fbb, relaxedPose.upperLeg.toDeg())
+		StayAlignedPose.addLowerLegAngleInDeg(fbb, relaxedPose.lowerLeg.toDeg())
+		StayAlignedPose.addFootAngleInDeg(fbb, relaxedPose.foot.toDeg())
+
+		return StayAlignedPose.endStayAlignedPose(fbb)
+	}
+
+	fun createTrackerStayAlignedTracker(
+		fbb: FlatBufferBuilder,
+		state: StayAlignedTrackerState,
+	): Int {
+		StayAlignedTracker.startStayAlignedTracker(fbb)
+
+		StayAlignedTracker.addYawCorrectionInDeg(fbb, state.yawCorrection.toDeg())
+		StayAlignedTracker.addLockedErrorInDeg(fbb, state.yawErrors.lockedError.toL2Norm().toDeg())
+		StayAlignedTracker.addCenterErrorInDeg(fbb, state.yawErrors.centerError.toL2Norm().toDeg())
+		StayAlignedTracker.addNeighborErrorInDeg(fbb, state.yawErrors.neighborError.toL2Norm().toDeg())
+		StayAlignedTracker.addLocked(fbb, state.restDetector.state == RestDetector.State.AT_REST)
+
+		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 stayAlignedPoseOffset = 0;
+		if (config.getStayAlignedPoseMask()) {
+			stayAlignedPoseOffset = DataFeedBuilderKotlin.INSTANCE
+				.createStayAlignedPose(fbb, this.api.server.humanPoseManager.skeleton);
+		}
+
+		return DataFeedUpdate
+			.createDataFeedUpdate(
+				fbb,
+				devicesOffset,
+				trackersOffset,
+				bonesOffset,
+				stayAlignedPoseOffset
+			);
 	}
 
 	public void sendDataFeedUpdate() {

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

@@ -12,6 +12,7 @@ import dev.slimevr.protocol.rpc.games.vrchat.RPCVRChatHandler
 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
@@ -19,6 +20,7 @@ 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.poses.RelaxedPose
 import dev.slimevr.tracking.trackers.TrackerPosition
 import dev.slimevr.tracking.trackers.TrackerPosition.Companion.getByBodyPart
 import dev.slimevr.tracking.trackers.TrackerStatus
@@ -206,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(
@@ -592,6 +612,89 @@ class RPCHandler(private val api: ProtocolAPI) : ProtocolHandler<RpcMessageHeade
 		}
 	}
 
+	private fun onEnableStayAlignedRequest(conn: GenericConnection, messageHeader: RpcMessageHeader) {
+		val request =
+			messageHeader.message(EnableStayAlignedRequest()) as? EnableStayAlignedRequest
+				?: return
+
+		val configManager = api.server.configManager
+
+		val config = configManager.vrConfig.stayAlignedConfig
+		config.enabled = request.enable()
+
+		configManager.saveConfig()
+
+		sendSettingsChangedResponse(conn)
+	}
+
+	private fun onDetectStayAlignedRelaxedPoseRequest(conn: GenericConnection, messageHeader: RpcMessageHeader) {
+		val request =
+			messageHeader.message(ResetStayAlignedRelaxedPoseRequest()) as? ResetStayAlignedRelaxedPoseRequest
+				?: return
+
+		val configManager = api.server.configManager
+		val config = configManager.vrConfig.stayAlignedConfig
+
+		val pose = request.pose()
+
+		val poseConfig =
+			when (pose) {
+				StayAlignedRelaxedPose.STANDING -> config.standingRelaxedPose
+				StayAlignedRelaxedPose.SITTING -> config.sittingRelaxedPose
+				StayAlignedRelaxedPose.FLAT -> config.flatRelaxedPose
+				else -> return
+			}
+
+		val relaxedPose = RelaxedPose.fromTrackers(api.server.humanPoseManager.skeleton)
+
+		poseConfig.enabled = true
+		poseConfig.upperLegAngleInDeg = relaxedPose.upperLeg.toDeg()
+		poseConfig.lowerLegAngleInDeg = relaxedPose.lowerLeg.toDeg()
+		poseConfig.footAngleInDeg = relaxedPose.foot.toDeg()
+
+		configManager.saveConfig()
+
+		LogManager.info("[detectStayAlignedRelaxedPose] pose=$pose $relaxedPose")
+
+		sendSettingsChangedResponse(conn)
+	}
+
+	private fun onResetStayAlignedRelaxedPoseRequest(conn: GenericConnection, messageHeader: RpcMessageHeader) {
+		val request =
+			messageHeader.message(ResetStayAlignedRelaxedPoseRequest()) as? ResetStayAlignedRelaxedPoseRequest
+				?: return
+
+		val configManager = api.server.configManager
+		val config = configManager.vrConfig.stayAlignedConfig
+
+		val pose = request.pose()
+
+		val poseConfig =
+			when (pose) {
+				StayAlignedRelaxedPose.STANDING -> config.standingRelaxedPose
+				StayAlignedRelaxedPose.SITTING -> config.sittingRelaxedPose
+				StayAlignedRelaxedPose.FLAT -> config.flatRelaxedPose
+				else -> return
+			}
+
+		poseConfig.enabled = false
+		poseConfig.upperLegAngleInDeg = 0.0f
+		poseConfig.lowerLegAngleInDeg = 0.0f
+		poseConfig.footAngleInDeg = 0.0f
+
+		LogManager.info("[resetStayAlignedRelaxedPose] pose=$pose")
+
+		sendSettingsChangedResponse(conn)
+	}
+
+	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,32 @@
+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.enabled,
+				config.standingRelaxedPose.upperLegAngleInDeg,
+				config.standingRelaxedPose.lowerLegAngleInDeg,
+				config.standingRelaxedPose.footAngleInDeg,
+				config.sittingRelaxedPose.enabled,
+				config.sittingRelaxedPose.upperLegAngleInDeg,
+				config.sittingRelaxedPose.lowerLegAngleInDeg,
+				config.sittingRelaxedPose.footAngleInDeg,
+				config.flatRelaxedPose.enabled,
+				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,26 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
 			resetsConfig.updateTrackersResetsSettings()
 		}
 
+		if (req.stayAligned() != null) {
+			val config = api.server.configManager.vrConfig.stayAlignedConfig
+			val requestConfig = req.stayAligned()
+			config.enabled = requestConfig.enabled()
+			config.extraYawCorrection = requestConfig.extraYawCorrection()
+			config.hideYawCorrection = requestConfig.hideYawCorrection()
+			config.standingRelaxedPose.enabled = requestConfig.standingEnabled()
+			config.standingRelaxedPose.upperLegAngleInDeg = requestConfig.standingUpperLegAngle()
+			config.standingRelaxedPose.lowerLegAngleInDeg = requestConfig.standingLowerLegAngle()
+			config.standingRelaxedPose.footAngleInDeg = requestConfig.standingFootAngle()
+			config.sittingRelaxedPose.enabled = requestConfig.sittingEnabled()
+			config.sittingRelaxedPose.upperLegAngleInDeg = requestConfig.sittingUpperLegAngle()
+			config.sittingRelaxedPose.lowerLegAngleInDeg = requestConfig.sittingLowerLegAngle()
+			config.sittingRelaxedPose.footAngleInDeg = requestConfig.sittingFootAngle()
+			config.flatRelaxedPose.enabled = requestConfig.flatEnabled()
+			config.flatRelaxedPose.upperLegAngleInDeg = requestConfig.flatUpperLegAngle()
+			config.flatRelaxedPose.lowerLegAngleInDeg = requestConfig.flatLowerLegAngle()
+			config.flatRelaxedPose.footAngleInDeg = requestConfig.flatFootAngle()
+		}
+
 		api.server.configManager.saveConfig()
 	}
 

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

@@ -1,6 +1,7 @@
 package dev.slimevr.tracking.processor.skeleton
 
 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
@@ -8,6 +9,8 @@ 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.trackers.TrackerSkeleton
 import dev.slimevr.tracking.trackers.Tracker
 import dev.slimevr.tracking.trackers.TrackerPosition
 import dev.slimevr.tracking.trackers.TrackerRole
@@ -210,6 +213,10 @@ class HumanSkeleton(
 	var tapDetectionManager = TapDetectionManager(this)
 	var localizer = Localizer(this)
 
+	// Stay Aligned
+	var trackerSkeleton = TrackerSkeleton(this)
+	var stayAlignedConfig = StayAlignedConfig()
+
 	// Constructors
 	init {
 		assembleSkeleton()
@@ -231,6 +238,7 @@ class HumanSkeleton(
 		)
 		legTweaks.setConfig(server.configManager.vrConfig.legTweaks)
 		localizer.setEnabled(humanPoseManager.getToggle(SkeletonConfigToggles.SELF_LOCALIZATION))
+		stayAlignedConfig = server.configManager.vrConfig.stayAlignedConfig
 	}
 
 	constructor(
@@ -448,6 +456,9 @@ class HumanSkeleton(
 
 		// Update bones tracker field
 		refreshBoneTracker()
+
+		// Update tracker skeleton
+		trackerSkeleton = TrackerSkeleton(this)
 	}
 
 	/**
@@ -504,6 +515,8 @@ class HumanSkeleton(
 	fun updatePose() {
 		tapDetectionManager.update()
 
+		StayAligned.adjustNextTracker(trackerSkeleton, stayAlignedConfig)
+
 		updateTransforms()
 		updateBones()
 		if (enforceConstraints) {

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.trackers.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) {
+		if (!config.enabled) {
+			return
+		}
+
+		val numTrackers = trackers.allTrackers.size
+		if (numTrackers == 0) {
+			return
+		}
+
+		val trackerToAdjust = trackers.allTrackers[nextTrackerIndex % numTrackers]
+		++nextTrackerIndex
+
+		// Update hide correction since the config could have changed
+		trackerToAdjust.stayAligned.hideCorrection = config.hideYawCorrection
+
+		var yawCorrectionPerSec = YAW_CORRECTION_PER_SEC
+		if (config.extraYawCorrection) {
+			yawCorrectionPerSec += EXTRA_YAW_CORRECTION_PER_SEC
+		}
+
+		// Scale yaw correction since we're only updating one tracker per tick
+		val yawCorrection =
+			yawCorrectionPerSec *
+				VRServer.instance.fpsTimer.timePerFrame *
+				numTrackers.toFloat()
+
+		AdjustTrackerYaw.adjust(
+			trackerToAdjust,
+			trackers,
+			yawCorrection,
+			config,
+		)
+	}
+}

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

@@ -0,0 +1,47 @@
+package dev.slimevr.tracking.processor.stayaligned
+
+import dev.slimevr.math.Angle
+import dev.slimevr.tracking.processor.stayaligned.poses.RelaxedPose
+import dev.slimevr.tracking.processor.stayaligned.trackers.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 makeRestDetector() =
+		RestDetector(
+			maxRotation = Angle.ofDeg(2.0f),
+			enterRestTime = 1.seconds,
+			enterMovingTime = 3.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 = 0.5f
+	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,183 @@
+package dev.slimevr.tracking.processor.stayaligned.adjust
+
+import dev.slimevr.config.StayAlignedConfig
+import dev.slimevr.math.Angle
+import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults.YAW_ERRORS_CENTER_ERROR_WEIGHT
+import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults.YAW_ERRORS_LOCKED_ERROR_WEIGHT
+import dev.slimevr.tracking.processor.stayaligned.StayAlignedDefaults.YAW_ERRORS_NEIGHBOR_ERROR_WEIGHT
+import dev.slimevr.tracking.processor.stayaligned.poses.PlayerPose
+import dev.slimevr.tracking.processor.stayaligned.poses.RelaxedPose
+import dev.slimevr.tracking.processor.stayaligned.trackers.RestDetector
+import dev.slimevr.tracking.processor.stayaligned.trackers.TrackerSkeleton
+import dev.slimevr.tracking.processor.stayaligned.trackers.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,
+		config: StayAlignedConfig,
+	) {
+		// Only IMUs can drift
+		if (!tracker.isImu()) {
+			return
+		}
+
+		// Skip trackers that use magnetometer, because the magnetometer should know the
+		// absolute yaw of the tracker
+		if (tracker.magStatus == MagnetometerStatus.ENABLED) {
+			return
+		}
+
+		// Clear errors, in case we don't adjust the tracker
+		val state = tracker.stayAligned
+		state.yawErrors = YawErrors()
+
+		val restDetector = state.restDetector
+		when (restDetector.state) {
+			RestDetector.State.MOVING ->
+				adjustMovingTracker(tracker, trackers, yawCorrection, config)
+
+			RestDetector.State.AT_REST ->
+				adjustLockedTracker(tracker, trackers, yawCorrection)
+
+			RestDetector.State.RECENTLY_AT_REST -> {
+				// Do not adjust trackers that were recently at rest, to support play
+				// styles that are primarily at rest
+			}
+		}
+	}
+
+	/**
+	 * Adjusts a locked tracker.
+	 */
+	private fun adjustLockedTracker(
+		tracker: Tracker,
+		trackers: TrackerSkeleton,
+		yawCorrection: Angle,
+	) {
+		val lockedRotation = tracker.stayAligned.lockedRotation ?: return
+
+		adjustByError(tracker, yawCorrection) {
+			YawErrors().also {
+				trackers.visit(tracker, LockedErrorVisitor(lockedRotation, it.lockedError))
+			}
+		}
+	}
+
+	/**
+	 * Adjusts a tracker that is moving.
+	 */
+	private fun adjustMovingTracker(
+		tracker: Tracker,
+		trackers: TrackerSkeleton,
+		yawCorrection: Angle,
+		config: StayAlignedConfig,
+	) {
+		val centerYaw = CenterYaw.ofSkeleton(trackers) ?: return
+
+		val pose = PlayerPose.ofTrackers(trackers)
+		val relaxedPose = RelaxedPose.forPose(pose, config) ?: return
+
+		adjustByError(tracker, yawCorrection) {
+			YawErrors().also {
+				trackers.visit(tracker, CenterErrorVisitor(centerYaw, relaxedPose, it.centerError))
+				trackers.visit(tracker, NeighborErrorVisitor(relaxedPose, it.neighborError))
+			}
+		}
+	}
+
+	/**
+	 * Adjusts the yaw by applying gradient descent.
+	 */
+	private fun adjustByError(
+		tracker: Tracker,
+		yawCorrection: Angle,
+		errorFn: (tracker: Tracker) -> YawErrors,
+	) {
+		val state = tracker.stayAligned
+
+		val curYaw = state.yawCorrection
+		val curError = errorFn(tracker)
+
+		val posYaw = curYaw + yawCorrection
+		state.yawCorrection = posYaw
+		val posError = errorFn(tracker)
+
+		val negYaw = curYaw - yawCorrection
+		state.yawCorrection = 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 = posYaw
+			state.yawErrors = posError
+		} else if (negYawDelta < Angle.ZERO) {
+			state.yawCorrection = negYaw
+			state.yawErrors = negError
+		} else {
+			state.yawCorrection = 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) =
+		(errors.lockedError.toL2Norm() - base.lockedError.toL2Norm()) * YAW_ERRORS_LOCKED_ERROR_WEIGHT +
+			(errors.centerError.toL2Norm() - base.centerError.toL2Norm()) * YAW_ERRORS_CENTER_ERROR_WEIGHT +
+			(errors.neighborError.toL2Norm() - base.neighborError.toL2Norm()) * YAW_ERRORS_NEIGHBOR_ERROR_WEIGHT
+}

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

@@ -0,0 +1,92 @@
+package dev.slimevr.tracking.processor.stayaligned.adjust
+
+import dev.slimevr.math.Angle
+import dev.slimevr.math.AngleErrors
+import dev.slimevr.tracking.processor.stayaligned.adjust.TrackerYaw.extraYaw
+import dev.slimevr.tracking.processor.stayaligned.adjust.TrackerYaw.trackerYaw
+import dev.slimevr.tracking.processor.stayaligned.poses.RelaxedPose
+import dev.slimevr.tracking.processor.stayaligned.trackers.Side
+import dev.slimevr.tracking.processor.stayaligned.trackers.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(
+	val centerYaw: Angle,
+	val relaxedPose: RelaxedPose,
+	val errors: AngleErrors,
+) : TrackerSkeleton.TrackerVisitor {
+
+	override fun visitHeadTracker(
+		tracker: Tracker,
+		belowUpperBody: Tracker?,
+	) {
+		errors.add(centerYaw - trackerYaw(tracker))
+	}
+
+	override fun visitUpperBodyTracker(
+		tracker: Tracker,
+		aboveHeadOrUpperBody: Tracker?,
+		belowUpperBody: Tracker?,
+	) {
+		errors.add(centerYaw - trackerYaw(tracker))
+	}
+
+	override fun visitUpperBodyTracker(
+		tracker: Tracker,
+		aboveHeadOrUpperBody: Tracker?,
+		belowLeftUpperLeg: Tracker?,
+		belowRightUpperLeg: Tracker?,
+	) {
+		errors.add(centerYaw - trackerYaw(tracker))
+	}
+
+	override fun visitArmTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperBodyOrArm: Tracker?,
+		belowHandOrArm: Tracker?,
+	) {
+		// No error because arms can go anywhere
+	}
+
+	override fun visitHandTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveArm: Tracker?,
+		oppositeHand: Tracker?,
+	) {
+		// No error because hands can go anywhere
+	}
+
+	override fun visitUpperLegTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperBody: Tracker?,
+		belowLowerLeg: Tracker?,
+		oppositeUpperLeg: Tracker?,
+	) {
+		errors.add(centerYaw + extraYaw(side, relaxedPose.upperLeg) - trackerYaw(tracker))
+	}
+
+	override fun visitLowerLegTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperLeg: Tracker?,
+		belowFoot: Tracker?,
+		oppositeLowerLeg: Tracker?,
+	) {
+		errors.add(centerYaw + extraYaw(side, relaxedPose.lowerLeg) - trackerYaw(tracker))
+	}
+
+	override fun visitFootTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveLowerLeg: Tracker?,
+		oppositeFoot: Tracker?,
+	) {
+		errors.add(centerYaw + extraYaw(side, relaxedPose.foot) - trackerYaw(tracker))
+	}
+}

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

@@ -0,0 +1,67 @@
+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.hasTrackerYaw
+import dev.slimevr.tracking.processor.stayaligned.adjust.TrackerYaw.trackerYaw
+import dev.slimevr.tracking.processor.stayaligned.trackers.TrackerSkeleton
+
+object CenterYaw {
+
+	fun ofSkeleton(
+		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 optional, because some mocap scenarios don't use one
+			upperBody.isEmpty() ||
+			leftUpperLeg == null ||
+			rightUpperLeg == null ||
+			leftLowerLeg == null ||
+			rightLowerLeg == null
+		) {
+			return null
+		}
+
+		// Need a minimum set of trackers, and the trackers need to be oriented in a
+		// way where we can actually calculate its 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,144 @@
+package dev.slimevr.tracking.processor.stayaligned.adjust
+
+import dev.slimevr.math.Angle
+import dev.slimevr.math.AngleErrors
+import dev.slimevr.tracking.processor.stayaligned.trackers.Side
+import dev.slimevr.tracking.processor.stayaligned.trackers.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(
+	val lockedRotation: Quaternion,
+	val errors: AngleErrors,
+) : TrackerSkeleton.TrackerVisitor {
+
+	override fun visitHeadTracker(
+		tracker: Tracker,
+		belowUpperBody: Tracker?,
+	) {
+		errors.add(error(tracker))
+	}
+
+	override fun visitUpperBodyTracker(
+		tracker: Tracker,
+		aboveHeadOrUpperBody: Tracker?,
+		belowUpperBody: Tracker?,
+	) {
+		errors.add(error(tracker))
+	}
+
+	override fun visitUpperBodyTracker(
+		tracker: Tracker,
+		aboveHeadOrUpperBody: Tracker?,
+		belowLeftUpperLeg: Tracker?,
+		belowRightUpperLeg: Tracker?,
+	) {
+		errors.add(error(tracker))
+	}
+
+	override fun visitArmTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperBodyOrArm: Tracker?,
+		belowHandOrArm: Tracker?,
+	) {
+		errors.add(error(tracker))
+	}
+
+	override fun visitHandTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveArm: Tracker?,
+		oppositeHand: Tracker?,
+	) {
+		errors.add(error(tracker))
+	}
+
+	override fun visitUpperLegTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperBody: Tracker?,
+		belowLowerLeg: Tracker?,
+		oppositeUpperLeg: Tracker?,
+	) {
+		errors.add(error(tracker))
+	}
+
+	override fun visitLowerLegTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperLeg: Tracker?,
+		belowFoot: Tracker?,
+		oppositeLowerLeg: Tracker?,
+	) {
+		errors.add(error(tracker))
+	}
+
+	override fun visitFootTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveLowerLeg: Tracker?,
+		oppositeFoot: Tracker?,
+	) {
+		errors.add(error(tracker))
+	}
+
+	private fun error(tracker: Tracker): Angle =
+		yawDifference(tracker.getAdjustedRotationForceStayAligned(), lockedRotation)
+
+	companion object {
+
+		/**
+		 * 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,164 @@
+package dev.slimevr.tracking.processor.stayaligned.adjust
+
+import dev.slimevr.math.AngleErrors
+import dev.slimevr.tracking.processor.stayaligned.adjust.TrackerYaw.extraYaw
+import dev.slimevr.tracking.processor.stayaligned.adjust.TrackerYaw.trackerYaw
+import dev.slimevr.tracking.processor.stayaligned.poses.RelaxedPose
+import dev.slimevr.tracking.processor.stayaligned.trackers.Side
+import dev.slimevr.tracking.processor.stayaligned.trackers.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(
+	val relaxedPose: RelaxedPose,
+	val errors: AngleErrors,
+) : TrackerSkeleton.TrackerVisitor {
+
+	override fun visitHeadTracker(
+		tracker: Tracker,
+		belowUpperBody: Tracker?,
+	) {
+		if (belowUpperBody != null) {
+			errors.add(trackerYaw(belowUpperBody) - trackerYaw(tracker))
+		}
+	}
+
+	override fun visitUpperBodyTracker(
+		tracker: Tracker,
+		aboveHeadOrUpperBody: Tracker?,
+		belowUpperBody: Tracker?,
+	) {
+		if (
+			aboveHeadOrUpperBody != null &&
+			// Head often drags the upper body trackers off to the side, so ignore it
+			aboveHeadOrUpperBody.trackerPosition != TrackerPosition.HEAD
+		) {
+			errors.add(trackerYaw(aboveHeadOrUpperBody) - trackerYaw(tracker))
+		}
+
+		if (belowUpperBody != null) {
+			errors.add(trackerYaw(belowUpperBody) - trackerYaw(tracker))
+		}
+	}
+
+	override fun visitUpperBodyTracker(
+		tracker: Tracker,
+		aboveHeadOrUpperBody: Tracker?,
+		belowLeftUpperLeg: Tracker?,
+		belowRightUpperLeg: Tracker?,
+	) {
+		if (
+			aboveHeadOrUpperBody != null &&
+			// Head often drags the upper body trackers off to the side, so ignore it
+			aboveHeadOrUpperBody.trackerPosition != TrackerPosition.HEAD
+		) {
+			errors.add(trackerYaw(aboveHeadOrUpperBody) - trackerYaw(tracker))
+		}
+
+		// 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
+		) {
+			errors.add(
+				trackerYaw(belowLeftUpperLeg) -
+					extraYaw(Side.LEFT, relaxedPose.upperLeg) -
+					trackerYaw(tracker),
+			)
+			errors.add(
+				trackerYaw(belowRightUpperLeg) -
+					extraYaw(Side.RIGHT, relaxedPose.upperLeg) -
+					trackerYaw(tracker),
+			)
+		}
+	}
+
+	override fun visitArmTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperBodyOrArm: Tracker?,
+		belowHandOrArm: Tracker?,
+	) {
+		// No error because arms can go anywhere
+	}
+
+	override fun visitHandTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveArm: Tracker?,
+		oppositeHand: Tracker?,
+	) {
+		// No error because hands can go anywhere
+	}
+
+	override fun visitUpperLegTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperBody: Tracker?,
+		belowLowerLeg: Tracker?,
+		oppositeUpperLeg: Tracker?,
+	) {
+		if (aboveUpperBody != null) {
+			errors.add(
+				trackerYaw(aboveUpperBody) +
+					extraYaw(side, relaxedPose.upperLeg) -
+					trackerYaw(tracker),
+			)
+		}
+
+		if (belowLowerLeg != null) {
+			errors.add(
+				trackerYaw(belowLowerLeg) -
+					extraYaw(side, relaxedPose.lowerLeg) +
+					extraYaw(side, relaxedPose.upperLeg) -
+					trackerYaw(tracker),
+			)
+		}
+	}
+
+	override fun visitLowerLegTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveUpperLeg: Tracker?,
+		belowFoot: Tracker?,
+		oppositeLowerLeg: Tracker?,
+	) {
+		if (aboveUpperLeg != null) {
+			errors.add(
+				trackerYaw(aboveUpperLeg) -
+					extraYaw(side, relaxedPose.upperLeg) +
+					extraYaw(side, relaxedPose.lowerLeg) -
+					trackerYaw(tracker),
+			)
+		}
+
+		if (belowFoot != null) {
+			errors.add(
+				trackerYaw(belowFoot) -
+					extraYaw(side, relaxedPose.foot) +
+					extraYaw(side, relaxedPose.lowerLeg) -
+					trackerYaw(tracker),
+			)
+		}
+	}
+
+	override fun visitFootTracker(
+		side: Side,
+		tracker: Tracker,
+		aboveLowerLeg: Tracker?,
+		oppositeFoot: Tracker?,
+	) {
+		if (aboveLowerLeg != null) {
+			errors.add(
+				trackerYaw(aboveLowerLeg) -
+					extraYaw(side, relaxedPose.lowerLeg) +
+					extraYaw(side, relaxedPose.foot) -
+					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.trackers.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.getAdjustedRotationForceStayAligned().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.getAdjustedRotationForceStayAligned()
+				.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/poses/PlayerPose.kt

@@ -0,0 +1,92 @@
+package dev.slimevr.tracking.processor.stayaligned.poses
+
+import dev.slimevr.tracking.processor.stayaligned.trackers.TrackerSkeleton
+
+/**
+ * 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.Companion::ofTracker),
+					TrackerPose.ofTracker(trackers.leftUpperLeg),
+					TrackerPose.ofTracker(trackers.rightUpperLeg),
+					TrackerPose.ofTracker(trackers.leftLowerLeg),
+					TrackerPose.ofTracker(trackers.rightLowerLeg),
+				)
+
+			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 { it == TrackerPose.TOP_FACING_UP } &&
+				pose.leftUpperLeg == TrackerPose.TOP_FACING_UP &&
+				pose.rightUpperLeg == TrackerPose.TOP_FACING_UP &&
+				pose.leftLowerLeg == TrackerPose.TOP_FACING_UP &&
+				pose.rightLowerLeg == TrackerPose.TOP_FACING_UP
+
+		private fun isSittingInChair(pose: TrackerPoses) =
+			pose.upperBody.isNotEmpty() && pose.upperBody[0] == TrackerPose.TOP_FACING_UP &&
+				pose.upperBody.all { it == TrackerPose.TOP_FACING_UP || it == TrackerPose.FRONT_FACING_UP } &&
+				pose.leftUpperLeg == TrackerPose.FRONT_FACING_UP &&
+				pose.rightUpperLeg == TrackerPose.FRONT_FACING_UP &&
+				pose.leftLowerLeg == TrackerPose.TOP_FACING_UP &&
+				pose.rightLowerLeg == TrackerPose.TOP_FACING_UP
+
+		private fun isSittingOnGround(pose: TrackerPoses) =
+			pose.upperBody.isNotEmpty() && pose.upperBody[0] == TrackerPose.TOP_FACING_UP &&
+				pose.upperBody.all { it == TrackerPose.TOP_FACING_UP || it == TrackerPose.FRONT_FACING_UP } &&
+				// Allow legs to be flat on ground, or knees-up
+				pose.leftUpperLeg.let { it == TrackerPose.FRONT_FACING_UP || it == TrackerPose.TOP_FACING_DOWN } &&
+				pose.rightUpperLeg.let { it == TrackerPose.FRONT_FACING_UP || it == TrackerPose.TOP_FACING_DOWN } &&
+				pose.leftLowerLeg.let { it == TrackerPose.FRONT_FACING_UP || it == TrackerPose.TOP_FACING_UP } &&
+				pose.rightLowerLeg.let { it == TrackerPose.FRONT_FACING_UP || it == TrackerPose.TOP_FACING_UP }
+
+		private fun isLyingOnBack(pose: TrackerPoses) =
+			pose.upperBody.all { it == TrackerPose.FRONT_FACING_UP } &&
+				// Allow legs to be flat on ground, or knees-up
+				pose.leftUpperLeg.let { it == TrackerPose.FRONT_FACING_UP || it == TrackerPose.TOP_FACING_DOWN } &&
+				pose.rightUpperLeg.let { it == TrackerPose.FRONT_FACING_UP || it == TrackerPose.TOP_FACING_DOWN } &&
+				pose.leftLowerLeg.let { it == TrackerPose.FRONT_FACING_UP || it == TrackerPose.TOP_FACING_UP } &&
+				pose.rightLowerLeg.let { it == TrackerPose.FRONT_FACING_UP || it == TrackerPose.TOP_FACING_UP }
+
+		private fun isKneeling(pose: TrackerPoses) =
+			pose.leftUpperLeg.let { it == TrackerPose.TOP_FACING_UP || it == TrackerPose.FRONT_FACING_UP } &&
+				pose.rightUpperLeg.let { it == TrackerPose.TOP_FACING_UP || it == TrackerPose.FRONT_FACING_UP } &&
+				pose.leftLowerLeg == TrackerPose.FRONT_FACING_DOWN &&
+				pose.rightLowerLeg == TrackerPose.FRONT_FACING_DOWN
+	}
+}

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

@@ -0,0 +1,111 @@
+package dev.slimevr.tracking.processor.stayaligned.poses
+
+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 -> {
+					val poseConfig = config.standingRelaxedPose
+					if (poseConfig.enabled) {
+						RelaxedPose(
+							Angle.ofDeg(poseConfig.upperLegAngleInDeg),
+							Angle.ofDeg(poseConfig.lowerLegAngleInDeg),
+							Angle.ofDeg(poseConfig.footAngleInDeg),
+						)
+					} else {
+						null
+					}
+				}
+
+				PlayerPose.SITTING_IN_CHAIR -> {
+					val poseConfig = config.sittingRelaxedPose
+					if (poseConfig.enabled) {
+						RelaxedPose(
+							Angle.ofDeg(poseConfig.upperLegAngleInDeg),
+							Angle.ofDeg(poseConfig.lowerLegAngleInDeg),
+							Angle.ofDeg(poseConfig.footAngleInDeg),
+						)
+					} else {
+						null
+					}
+				}
+
+				PlayerPose.SITTING_ON_GROUND,
+				PlayerPose.LYING_ON_BACK,
+				-> {
+					val poseConfig = config.flatRelaxedPose
+					if (poseConfig.enabled) {
+						RelaxedPose(
+							Angle.ofDeg(poseConfig.upperLegAngleInDeg),
+							Angle.ofDeg(poseConfig.lowerLegAngleInDeg),
+							Angle.ofDeg(poseConfig.footAngleInDeg),
+						)
+					} else {
+						null
+					}
+				}
+
+				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/poses/TrackerPose.kt

@@ -0,0 +1,61 @@
+package dev.slimevr.tracking.processor.stayaligned.poses
+
+import dev.slimevr.tracking.trackers.Tracker
+import io.github.axisangles.ktmath.Vector3
+import kotlin.math.*
+
+/**
+ * The orientation 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 {
+			if (tracker == null) {
+				return NONE
+			}
+
+			val rotation = tracker.getAdjustedRotationForceStayAligned()
+
+			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/trackers/RestDetector.kt

@@ -0,0 +1,95 @@
+package dev.slimevr.tracking.processor.stayaligned.trackers
+
+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 enterRestTime: Duration,
+	private val enterMovingTime: Duration,
+) {
+	enum class State {
+		MOVING,
+		AT_REST,
+		RECENTLY_AT_REST,
+	}
+
+	var state = State.MOVING
+		private set
+
+	// Instant that we entered the current state
+	private var startTime = TimeSource.Monotonic.markNow()
+
+	// Rotation which could
+	private var lastRotation = Quaternion.IDENTITY
+	private var lastRotationTime = TimeSource.Monotonic.markNow()
+
+	/**
+	 * Resets the detector
+	 */
+	fun reset() {
+		val now = TimeSource.Monotonic.markNow()
+
+		state = State.MOVING
+		startTime = now
+		lastRotation = Quaternion.IDENTITY
+		lastRotationTime = now
+	}
+
+	/**
+	 * Provides a new rotation sample to the detector.
+	 *
+	 * @return whether the tracker is at rest
+	 */
+	fun update(rotation: Quaternion) {
+		val now = TimeSource.Monotonic.markNow()
+
+		if (
+			state == State.RECENTLY_AT_REST &&
+			now > startTime.plus(enterMovingTime)
+		) {
+			state = State.MOVING
+			startTime = now
+			lastRotation = rotation
+			lastRotationTime = now
+		}
+
+		when (state) {
+			State.MOVING,
+			State.RECENTLY_AT_REST,
+			->
+				if (Angle.absBetween(lastRotation, rotation) > maxRotation) {
+					lastRotation = rotation
+					lastRotationTime = now
+				} else {
+					// 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 (now > lastRotationTime.plus(enterRestTime)) {
+						state = State.AT_REST
+						startTime = now
+						lastRotation = rotation
+						lastRotationTime = now
+					}
+				}
+
+			State.AT_REST ->
+				if (Angle.absBetween(lastRotation, rotation) > maxRotation) {
+					state = State.RECENTLY_AT_REST
+					startTime = now
+					lastRotation = rotation
+					lastRotationTime = now
+				}
+		}
+	}
+}

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

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

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

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

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

@@ -0,0 +1,415 @@
+package dev.slimevr.tracking.processor.stayaligned.trackers
+
+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 visit(
+		tracker: Tracker,
+		visitor: TrackerVisitor,
+	) {
+		when (tracker.trackerPosition) {
+			TrackerPosition.HEAD ->
+				if (tracker == head) {
+					visitor.visitHeadTracker(tracker, upperBody.firstOrNull())
+				}
+
+			// 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,
+					)
+				}
+
+			TrackerPosition.RIGHT_HAND ->
+				if (tracker == rightHand) {
+					visitor.visitHandTracker(
+						Side.RIGHT,
+						tracker,
+						rightArm.lastOrNull(),
+						leftHand,
+					)
+				}
+
+			TrackerPosition.LEFT_UPPER_LEG ->
+				if (tracker == leftUpperLeg) {
+					visitor.visitUpperLegTracker(
+						Side.LEFT,
+						tracker,
+						upperBody.lastOrNull(),
+						leftLowerLeg,
+						rightUpperLeg,
+					)
+				}
+
+			TrackerPosition.RIGHT_UPPER_LEG ->
+				if (tracker == rightUpperLeg) {
+					visitor.visitUpperLegTracker(
+						Side.RIGHT,
+						tracker,
+						upperBody.lastOrNull(),
+						rightLowerLeg,
+						leftUpperLeg,
+					)
+				}
+
+			TrackerPosition.LEFT_LOWER_LEG ->
+				if (tracker == leftLowerLeg) {
+					visitor.visitLowerLegTracker(
+						Side.LEFT,
+						tracker,
+						leftUpperLeg,
+						leftFoot,
+						rightLowerLeg,
+					)
+				}
+
+			TrackerPosition.RIGHT_LOWER_LEG ->
+				if (tracker == rightLowerLeg) {
+					visitor.visitLowerLegTracker(
+						Side.RIGHT,
+						tracker,
+						rightUpperLeg,
+						rightFoot,
+						leftLowerLeg,
+					)
+				}
+
+			TrackerPosition.LEFT_FOOT ->
+				if (tracker == leftFoot) {
+					visitor.visitFootTracker(
+						Side.LEFT,
+						tracker,
+						leftLowerLeg,
+						rightFoot,
+					)
+				}
+
+			TrackerPosition.RIGHT_FOOT ->
+				if (tracker == rightFoot) {
+					visitor.visitFootTracker(
+						Side.RIGHT,
+						tracker,
+						rightLowerLeg,
+						leftFoot,
+					)
+				}
+
+			else -> {
+				// No tracker to visit
+			}
+		}
+	}
+
+	private fun visitUpperBodyTrackers(
+		tracker: Tracker,
+		visitor: TrackerVisitor,
+		head: Tracker?,
+		upperBody: List<Tracker>,
+		leftUpperLeg: Tracker?,
+		rightUpperLeg: Tracker?,
+	) {
+		val index = upperBody.indexOf(tracker)
+		if (index < 0) {
+			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 visitArmTrackers(
+		tracker: Tracker,
+		visitor: TrackerVisitor,
+		side: Side,
+		upperBody: Tracker?,
+		arm: List<Tracker>,
+		hand: Tracker?,
+	) {
+		val index = arm.indexOf(tracker)
+		if (index < 0) {
+			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 {
+
+		/**
+		 * Visits the head tracker.
+		 */
+		fun visitHeadTracker(
+			tracker: Tracker,
+			belowUpperBody: Tracker?,
+		)
+
+		/**
+		 * Visits an upper body tracker (except for the bottom-most tracker).
+		 */
+		fun visitUpperBodyTracker(
+			tracker: Tracker,
+			aboveHeadOrUpperBody: Tracker?,
+			belowUpperBody: Tracker?,
+		)
+
+		/**
+		 * Visits the bottom-most upper body tracker.
+		 */
+		fun visitUpperBodyTracker(
+			tracker: Tracker,
+			aboveHeadOrUpperBody: Tracker?,
+			belowLeftUpperLeg: Tracker?,
+			belowRightUpperLeg: Tracker?,
+		)
+
+		/**
+		 * Visits an arm tracker.
+		 */
+		fun visitArmTracker(
+			side: Side,
+			tracker: Tracker,
+			aboveUpperBodyOrArm: Tracker?,
+			belowHandOrArm: Tracker?,
+		)
+
+		/**
+		 * Visits a hand tracker.
+		 */
+		fun visitHandTracker(
+			side: Side,
+			tracker: Tracker,
+			aboveArm: Tracker?,
+			oppositeHand: Tracker?,
+		)
+
+		/**
+		 * Visits an upper leg tracker.
+		 */
+		fun visitUpperLegTracker(
+			side: Side,
+			tracker: Tracker,
+			aboveUpperBody: Tracker?,
+			belowLowerLeg: Tracker?,
+			oppositeUpperLeg: Tracker?,
+		)
+
+		/**
+		 * Visits a lower leg tracker.
+		 */
+		fun visitLowerLegTracker(
+			side: Side,
+			tracker: Tracker,
+			aboveUpperLeg: Tracker?,
+			belowFoot: Tracker?,
+			oppositeLowerLeg: Tracker?,
+		)
+
+		/**
+		 * Visits a foot tracker.
+		 */
+		fun visitFootTracker(
+			side: Side,
+			tracker: Tracker,
+			aboveLowerLeg: Tracker?,
+			oppositeFoot: Tracker?,
+		)
+	}
+}

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

@@ -0,0 +1,12 @@
+package dev.slimevr.tracking.processor.stayaligned.trackers
+
+import dev.slimevr.math.AngleErrors
+
+/**
+ * Aggregates the yaw errors from multiple forces.
+ */
+class YawErrors {
+	var lockedError = AngleErrors()
+	var centerError = AngleErrors()
+	var neighborError = AngleErrors()
+}

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.trackers.StayAlignedTrackerState
 import dev.slimevr.tracking.trackers.TrackerPosition.Companion.getByDesignation
 import dev.slimevr.tracking.trackers.udp.IMUType
 import dev.slimevr.tracking.trackers.udp.MagnetometerStatus
@@ -156,6 +157,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)) {
@@ -317,6 +320,7 @@ class Tracker @JvmOverloads constructor(
 		}
 		filteringHandler.update()
 		resetsHandler.update()
+		stayAligned.update()
 	}
 
 	/**
@@ -345,12 +349,39 @@ class Tracker @JvmOverloads constructor(
 	 * it too much should be avoided for performance reasons.
 	 */
 	private fun getAdjustedRotation(): Quaternion {
+		var rot = _rotation
+
+		if (!stayAligned.hideCorrection) {
+			// Yaw drift happens in the raw rotation space
+			rot = Quaternion.rotationAroundYAxis(stayAligned.yawCorrection.toRad()) * rot
+		}
+
 		// Reset if needed and is not computed and internal
 		return if (needsReset && !(isComputed && isInternal) && trackerDataType == TrackerDataType.ROTATION) {
 			// Adjust to reset, mounting and drift compensation
-			resetsHandler.getReferenceAdjustedDriftRotationFrom(_rotation)
+			resetsHandler.getReferenceAdjustedDriftRotationFrom(rot)
 		} else {
-			_rotation
+			rot
+		}
+	}
+
+	/**
+	 * Same as getAdjustedRotation except that Stay Aligned correction is always
+	 * applied. This allows Stay Aligned to do gradient descent with the tracker's
+	 * rotation.
+	 */
+	fun getAdjustedRotationForceStayAligned(): Quaternion {
+		var rot = _rotation
+
+		// Yaw drift happens in the raw rotation space
+		rot = Quaternion.rotationAroundYAxis(stayAligned.yawCorrection.toRad()) * rot
+
+		// Reset if needed and is not computed and internal
+		return if (needsReset && !(isComputed && isInternal) && trackerDataType == TrackerDataType.ROTATION) {
+			// Adjust to reset, mounting and drift compensation
+			resetsHandler.getReferenceAdjustedDriftRotationFrom(rot)
+		} else {
+			rot
 		}
 	}
 
@@ -360,12 +391,19 @@ class Tracker @JvmOverloads constructor(
 	 * This is used for debugging/visualizing tracker data
 	 */
 	fun getIdentityAdjustedRotation(): Quaternion {
+		var rot = _rotation
+
+		if (!stayAligned.hideCorrection) {
+			// Yaw drift happens in the raw rotation space
+			rot = Quaternion.rotationAroundYAxis(stayAligned.yawCorrection.toRad()) * rot
+		}
+
 		// Reset if needed or is a computed tracker besides head
 		return if (needsReset && !(isComputed && trackerPosition != TrackerPosition.HEAD) && trackerDataType == TrackerDataType.ROTATION) {
 			// Adjust to reset and mounting
-			resetsHandler.getIdentityAdjustedDriftRotationFrom(_rotation)
+			resetsHandler.getIdentityAdjustedDriftRotationFrom(rot)
 		} else {
-			_rotation
+			rot
 		}
 	}
 

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

@@ -340,6 +340,9 @@ class TrackerResetsHandler(val tracker: Tracker) {
 			yawResetSmoothTimeRemain = 0.0f
 		}
 
+		// Reset Stay Aligned
+		tracker.stayAligned.reset()
+
 		calculateDrift(oldRot)
 
 		postProcessResetFull(reference)
@@ -382,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

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

@@ -59,6 +59,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
 		 */
@@ -407,6 +434,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/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())