diff --git a/src/auto/mod.rs b/src/auto/mod.rs
index 2989128..e0c093f 100644
--- a/src/auto/mod.rs
+++ b/src/auto/mod.rs
@@ -2,6 +2,10 @@ mod path;
 
 use crate::auto::path::drive;
 use nalgebra::Vector2;
+use uom::si::{
+    f64::{Length},
+    length::meter,
+};
 use serde::{Deserialize, Serialize};
 use std::ops::Deref;
 use std::time::Duration;
@@ -60,7 +64,7 @@ pub async fn blue_triangle(robot: Ferris) -> Result<(), Box<dyn std::error::Erro
 
     drivetrain
         .odometry
-        .set_abs(Vector2::new(8.075126647949219, 2.0993127822875977));
+        .set_abs(Vector2::new(Length::new::<meter>(8.075126647949219), Length::new::<meter>(2.0993127822875977)));
 
     drive("BlueTriangle", &mut drivetrain, 1).await?;
     println!("BlueTriangle.1 done");
diff --git a/src/auto/path.rs b/src/auto/path.rs
index 3557829..bde182a 100644
--- a/src/auto/path.rs
+++ b/src/auto/path.rs
@@ -85,7 +85,7 @@ pub async fn follow_path_segment(
         let angle = -setpoint.heading;
         let position = Vector2::new(setpoint.x.get::<meter>(), setpoint.y.get::<meter>());
 
-        let mut error_position = position - drivetrain.odometry.position;
+        let mut error_position = position - drivetrain.odometry.robot_pose_estimate.get_position_meters();
         let mut error_angle = (angle - drivetrain.get_angle()).get::<radian>();
 
         if error_position.abs().max() < SWERVE_DRIVE_IE {
diff --git a/src/constants.rs b/src/constants.rs
index ceb8d9d..05afbef 100644
--- a/src/constants.rs
+++ b/src/constants.rs
@@ -76,6 +76,12 @@ pub mod drivetrain {
     pub const YAW_ACCEPTABLE_ERROR: f64 = 0.02;
 
 }
+pub mod pose_estimation {
+    pub const ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS: f64 = 0.000001;
+    pub const MIN_FOM: f64 = 0.00001;
+    pub const START_POSITION_FOM: f64 = 0.01; //meters
+    pub const DRIFT_RATIO: f64 = 0.02; // Robot odometry pose estimate drifts [ratio] meters for every meter driven
+}
 pub mod elevator {
     pub const BOTTOM: f64 = 0.0; // unit is rotations
     pub const L2: f64 = 1.; // unit is rotations
diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs
index f478e1f..948b078 100644
--- a/src/subsystems/drivetrain.rs
+++ b/src/subsystems/drivetrain.rs
@@ -141,7 +141,7 @@ impl Drivetrain {
         self.limelight_upper
             .update(self.get_offset().get::<degree>() + 180.)
             .await;
-
+        /*
         let pose = self.limelight_lower.get_botpose();
 
         // Calculate offset from robot center to limelight
@@ -168,21 +168,23 @@ impl Drivetrain {
 
         // Will return 0, 0 if no tag found
         if pose.x.get::<meter>() != 0.0 {
-            self.update_odo(pose + robot_to_limelight);
+            self.update_odo_absolute(pose + robot_to_limelight);
         }
+
+         */
         //println!("lower ll tx: {}", self.limelight_lower.get_tx().get::<degree>());
         //println!("upper ll tx: {}", self.limelight_upper.get_tx().get::<degree>());
     }
 
     pub async fn post_odo(&self) {
-        Telemetry::put_number("odo_x", self.odometry.position.x).await;
-        Telemetry::put_number("odo_y", self.odometry.position.y).await;
+        Telemetry::put_number("odo_x", self.odometry.robot_pose_estimate.get_position().x.get::<meter>()).await;
+        Telemetry::put_number("odo_y", self.odometry.robot_pose_estimate.get_position().y.get::<meter>()).await;
         Telemetry::put_number("angle", self.get_offset().get::<radian>()).await;
     }
 
-    pub fn update_odo(&mut self, pose: Vector2<Length>) {
+    pub fn update_odo_absolute(&mut self, pose: Vector2<Length>) {
         self.odometry
-            .set_abs(Vector2::new(pose.x.value, pose.y.value));
+            .set_abs(pose);
     }
 
     pub fn stop(&self) {
@@ -240,8 +242,10 @@ impl Drivetrain {
 
     pub fn set_speeds(&mut self, fwd: f64, str: f64, rot: f64, style: SwerveControlStyle) {
         /*println!(
-            "ODO XY: {}, {}",
-            self.odometry.position.x, self.odometry.position.y
+            "ODO XY: {}, {} ODO FOM: {}",
+            self.odometry.robot_pose_estimate.get_position_meters().x,
+            self.odometry.robot_pose_estimate.get_position_meters().y,
+            self.odometry.robot_pose_estimate.figure_of_merit.get::<meter>()
         );*/
         let mut transform = Vector2::new(-str, fwd);
         match style {
@@ -259,7 +263,12 @@ impl Drivetrain {
 
         let angle = self.get_offset();
 
-        self.odometry.calculate(positions, angle);
+        let mut sensor_measurements = Vec::new();
+        if let Some(odo_estimate) = self.odometry.calculate_arcs(positions, (angle + Angle::new::<degree>(180.))) {
+            //println!("new odo pose estimate: x {} y {} fom {}",odo_estimate.get_position_meters().x, odo_estimate.get_position_meters().y, odo_estimate.figure_of_merit.get::<meter>());
+            sensor_measurements.push(odo_estimate);
+        }
+        if sensor_measurements.len() != 0 {self.odometry.fuse_sensors_fom(sensor_measurements);}
 
         let wheel_speeds: Vec<ModuleState> = wheel_speeds
             .into_iter()
@@ -353,7 +362,7 @@ impl Drivetrain {
         let mut i = Vector2::zeros();
 
         if let Some(target) = self.calculate_target_lineup_position(side) {
-            let mut error_position = target.position - self.odometry.position;
+            let mut error_position = target.position - self.odometry.robot_pose_estimate.get_position_meters();
             let mut error_angle = (target.angle - self.get_offset()).get::<radian>();
 
             if error_position.abs().max() < SWERVE_DRIVE_IE {
@@ -578,6 +587,7 @@ mod tests {
     use uom::si::length::meter;
 
     #[test]
+    #[ignore]
     fn calculate_target_lineup_position() {
         let side = LineupSide::Right;
 
diff --git a/src/swerve/odometry.rs b/src/swerve/odometry.rs
index dbba49d..ef23ea7 100644
--- a/src/swerve/odometry.rs
+++ b/src/swerve/odometry.rs
@@ -4,12 +4,11 @@ use frcrs::alliance_station;
 use frcrs::networktables::SmartDashboard;
 use nalgebra::{Rotation2, Vector2};
 use uom::si::{
-    angle::radian,
+    angle::{radian, degree},
     f64::{Angle, Length},
     length::meter,
 };
-
-use crate::constants::HALF_FIELD_WIDTH_METERS;
+use crate::constants::pose_estimation::{ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS, DRIFT_RATIO, MIN_FOM, START_POSITION_FOM};
 
 #[derive(Default, Clone)]
 pub struct ModuleReturn {
@@ -36,10 +35,29 @@ impl Sub for ModuleReturn {
         }
     }
 }
+#[derive(Clone)]
+pub struct PoseEstimate {
+    position: Vector2<Length>,
+    pub figure_of_merit: Length
+}
+impl PoseEstimate {
+    pub fn get_position_meters(&self) -> Vector2<f64> {
+        Vector2::new(
+            self.position.x.get::<meter>(),
+            self.position.y.get::<meter>(),
+        )
+    }
+    pub fn get_position(&self) -> Vector2<Length> {
+        self.position
+    }
+    pub fn set_absolute(&mut self, position: Vector2<Length>) {
+        self.position = position;
+    }
+}
 
 pub struct Odometry {
     last_modules: Vec<ModuleReturn>,
-    pub position: Vector2<f64>,
+    pub robot_pose_estimate: PoseEstimate,
 }
 
 impl Default for Odometry {
@@ -51,31 +69,22 @@ impl Default for Odometry {
 impl Odometry {
     pub fn new() -> Self {
         let last_modules = Vec::new();
-        let position = Vector2::new(0., 0.);
+        let position:Vector2<Length> = Vector2::new(Length::new::<meter>(0.), Length::new::<meter>(0.));
 
         Self {
             last_modules,
-            position,
+            robot_pose_estimate: PoseEstimate {position, figure_of_merit: Length::new::<meter>(START_POSITION_FOM) },
         }
     }
 
-    pub fn set(&mut self, position: Vector2<f64>) {
-        if alliance_station().red() {
-            self.position.x = position.x;
-            self.position.y = HALF_FIELD_WIDTH_METERS - position.y;
-        } else {
-            self.position = position;
-        }
+    pub fn set_abs(&mut self, position: Vector2<Length>) {
+        self.robot_pose_estimate.set_absolute(position);
     }
 
-    pub fn set_abs(&mut self, position: Vector2<f64>) {
-        self.position = position;
-    }
-
-    pub fn calculate(&mut self, positions: Vec<ModuleReturn>, angle: Angle) {
+    pub fn calculate(&mut self, positions: Vec<ModuleReturn>, angle: Angle) -> Option<PoseEstimate> {
         if positions.len() != self.last_modules.len() {
             self.last_modules = positions;
-            return;
+            return None
         }
 
         let mut deltas: Vec<ModuleReturn> = positions
@@ -89,12 +98,565 @@ impl Odometry {
         }
 
         let mut delta: Vector2<f64> = deltas.into_iter().map(Into::<Vector2<f64>>::into).sum();
-
         delta /= positions.len() as f64;
 
-        self.position += -delta;
+        let delta_length: Vector2<Length> = Vector2::new(
+            Length::new::<meter>(delta.x),
+            Length::new::<meter>(delta.y)
+        );
+        println!("delta: x {} y {}", delta.x, delta.y);
         self.last_modules = positions;
+        let fom = self.robot_pose_estimate.figure_of_merit + Length::new::<meter>(delta.magnitude() * DRIFT_RATIO);
+
+        Some(PoseEstimate {
+            position: self.robot_pose_estimate.get_position() + delta_length,
+            figure_of_merit: self.robot_pose_estimate.figure_of_merit + Length::new::<meter>(delta.magnitude() * DRIFT_RATIO)
+        })
+
+    }
+    pub fn calculate_arcs(&mut self, positions: Vec<ModuleReturn>, drivetrain_angle: Angle) -> Option<PoseEstimate>{
+        if positions.len() != self.last_modules.len() {
+            self.last_modules = positions;
+            return None
+        }
+        //println!("drivetrain angle for calculate_arcs: {}", drivetrain_angle.get::<radian>());
+
+        let mut theta_deltas : Vec<Angle> = positions
+            .iter()
+            .zip(self.last_modules.iter())
+            .map(|(current_modules, last_modules)| {
+                //println!("theta delta: {}", (current_modules.to_owned().angle - last_modules.to_owned().angle).get::<radian>());
+                current_modules.to_owned().angle - last_modules.to_owned().angle
+            }).collect();
+        let mut arc_lengths: Vec<Length> = positions
+            .iter()
+            .zip(self.last_modules.iter())
+            .map(|(current_modules, last_modules)|{
+                //println!("arc length: {}", ((current_modules.to_owned().distance - last_modules.to_owned().distance)).get::<meter>());
+                (current_modules.to_owned().distance - last_modules.to_owned().distance)
+            })
+            .collect();
+        let mut arc_radii: Vec<Length> = arc_lengths.clone()
+            .iter()
+            .zip(theta_deltas.clone())
+            .map(|(arc_length, theta_delta)|{
+                //println!("arc radius: {}", (arc_length.get::<meter>() / (2. * std::f64::consts::PI) * ((2. * std::f64::consts::PI) / theta_delta.get::<radian>().abs())));
+                Length::new::<meter>((arc_length.get::<meter>() / (2. * std::f64::consts::PI) * ((2. * std::f64::consts::PI) / theta_delta.get::<radian>().abs())))
+            }).collect();
+        let mut arc_centers: Vec<Vector2<Length>> = self.last_modules.clone()
+            .iter()
+            .zip(arc_radii.clone())
+            .zip(theta_deltas.clone())
+            .map(|((last_module, arc_radius), theta_delta)| {
+                let mut angle_to_center = last_module.angle;
+                if theta_delta.get::<radian>() < 0. {
+                    angle_to_center -= Angle::new::<degree>(90.);
+                } else {
+                    angle_to_center += Angle::new::<degree>(90.);
+                }
+                //println!("angle_to_center degrees: {}", angle_to_center.get::<degree>());
+                //println!("center x: {}", arc_radius.get::<meter>() * (-angle_to_center.get::<radian>()).cos());
+                //println!("center y: {}", arc_radius.get::<meter>() * (-angle_to_center.get::<radian>()).sin());
+                Vector2::new(
+                    Length::new::<meter>(arc_radius.get::<meter>() * (-angle_to_center.get::<radian>()).cos()),
+                    Length::new::<meter>(arc_radius.get::<meter>() * (-angle_to_center.get::<radian>()).sin()),
+                )
+            }).collect();
+        let mut delta_positions: Vec<Vector2<Length>> = arc_centers.clone()
+            .iter()
+            .zip(arc_radii.clone())
+            .zip(positions.clone())
+            .zip(theta_deltas.clone())
+            .zip(self.last_modules.clone())
+            .map(|((((arc_center, arc_radius), current_module), theta_delta), last_module)|{
+                let mut endpoint_angle_to_center = current_module.angle;
+                if theta_delta.get::<radian>() < 0. {
+                    endpoint_angle_to_center -= Angle::new::<degree>(90.);
+                } else {
+                    endpoint_angle_to_center += Angle::new::<degree>(90.);
+                }
+                //println!("endpoint angle to center degrees: {}", endpoint_angle_to_center.get::<degree>());
+                let endpoint_to_center = Vector2::new(
+                    Length::new::<meter>(arc_radius.get::<meter>() * (-endpoint_angle_to_center.get::<radian>()).cos()),
+                    Length::new::<meter>(arc_radius.get::<meter>() * (-endpoint_angle_to_center.get::<radian>()).sin()),
+                );
+                //println!("endpoint to center x: {}", arc_radius.get::<meter>() * (-endpoint_angle_to_center.get::<radian>()).cos());
+                //println!("endpoint to center y: {}",arc_radius.get::<meter>() * (-endpoint_angle_to_center.get::<radian>()).sin());
+                if theta_delta.get::<radian>().abs() < ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS || theta_delta.get::<radian>().is_nan() {
+                    //println!("delta theta too small");
+                    Vector2::new(
+                        Length::new::<meter>( Vector2::from((current_module.clone() - last_module.clone())).x),
+                        Length::new::<meter>( Vector2::from((current_module.clone() - last_module.clone())).y),
+                    )
+                } else {
+                    //println!("delta x: {}", (arc_center - endpoint_to_center).x.get::<meter>());
+                    //println!("delta y: {}", (arc_center - endpoint_to_center).y.get::<meter>());
+                    arc_center - endpoint_to_center
+                }
+            }).collect();
+        let drivetrain_angle_rotation = Rotation2::new(-drivetrain_angle.get::<radian>());
+
+        // Rotate all by the drivetrain angle to get into field coordinates
+        delta_positions = delta_positions.clone()
+            .iter()
+            .map(|delta_position| {
+                let mut delta_position_meters = Vector2::new(
+                    delta_position.x.get::<meter>(),
+                    delta_position.y.get::<meter>()
+                );
+                delta_position_meters = drivetrain_angle_rotation.clone() * delta_position_meters;
+                Vector2::new(
+                    Length::new::<meter>(delta_position_meters.x),
+                    Length::new::<meter>(delta_position_meters.y),
+                )
+            }).collect();
+
+        // Average the module deltas to get robot delta
+        let mut delta: Vector2<Length> = delta_positions.iter().sum();
+        let mut delta_meters: Vector2<f64> = Vector2::new(delta.x.get::<meter>(), delta.y.get::<meter>());
+        delta_meters /= delta_positions.len() as f64;
+        delta = Vector2::new(
+            Length::new::<meter>(delta_meters.x),
+            Length::new::<meter>(delta_meters.y),
+        );
+        self.last_modules = positions;
+
+        let mut average_arc_length_meters: f64 = 0.;
+        arc_lengths.iter().for_each(|arc_length| {average_arc_length_meters += arc_length.get::<meter>().abs();});
+        average_arc_length_meters /= arc_lengths.len() as f64;
+
+        Some(PoseEstimate {
+            position: self.robot_pose_estimate.get_position() + delta,
+            figure_of_merit: self.robot_pose_estimate.figure_of_merit + Length::new::<meter>(average_arc_length_meters * DRIFT_RATIO),
+        })
+    }
+    pub fn fuse_sensors_fom(&mut self, pose_estimates: Vec<PoseEstimate>) {
+        let pose_estimates: Vec<PoseEstimate> = pose_estimates.iter().map(|pose_estimate| {
+            if pose_estimate.figure_of_merit.get::<meter>() < MIN_FOM {
+                PoseEstimate {
+                    position: pose_estimate.position.clone(),
+                    figure_of_merit: Length::new::<meter>(MIN_FOM),
+                }
+            } else {
+                pose_estimate.clone()
+            }
+        }).collect();
+        let fom_inverse_squared_values: Vec<f64> = pose_estimates
+            .iter()
+            .map(|pose_estimate| (1. / (pose_estimate.figure_of_merit.get::<meter>() * pose_estimate.figure_of_merit.get::<meter>())))
+            .collect();
+        let poses_times_fom_inverse_squared_values: Vec<Vector2<f64>> = pose_estimates
+            .iter()
+            .zip(fom_inverse_squared_values.clone())
+            .map(|(pose_estimate, inverse_squared_value)| {
+                Vector2::new(
+                    pose_estimate.position.x.get::<meter>(),
+                    pose_estimate.position.y.get::<meter>(),
+                ) * inverse_squared_value
+            }).collect();
+        let mut robot_pose_estimate_meters_x: f64 = 0.;
+        let mut robot_pose_estimate_meters_y: f64 = 0.;
+        let mut inverse_squared_sum: f64 = 0.;
+        poses_times_fom_inverse_squared_values.iter().zip(fom_inverse_squared_values.clone()).for_each(|(pose_estimate, fom_inverse_squared_value)| {
+            robot_pose_estimate_meters_x += pose_estimate.x;
+            robot_pose_estimate_meters_y += pose_estimate.y;
+            inverse_squared_sum += fom_inverse_squared_value;
+        });
+        robot_pose_estimate_meters_x /= inverse_squared_sum;
+        robot_pose_estimate_meters_y /= inverse_squared_sum;
+
+        let mut min_sensor_fom: f64 = 1000.;
+        pose_estimates.iter().for_each(|pose_estimate| {
+            if pose_estimate.figure_of_merit.get::<meter>() < min_sensor_fom {
+                min_sensor_fom = pose_estimate.figure_of_merit.get::<meter>();
+            }
+        });
+
+        self.robot_pose_estimate = PoseEstimate {
+            position: Vector2::new(
+                Length::new::<meter>(robot_pose_estimate_meters_x),
+                Length::new::<meter>(robot_pose_estimate_meters_y),
+            ),
+            figure_of_merit: Length::new::<meter>(min_sensor_fom),
+        }
+    }
+}
+#[cfg(test)]
+mod tests {
+    use super::*;
+    #[test]
+    fn calculate_arcs_forward_forward_forward() { //last modules forward, new modules forward, drivetrain forward
+        let mut odometry = Odometry::new();
+
+        let mut last_modules = Vec::new();
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+
+        odometry.last_modules = last_modules;
+
+        let mut new_modules = Vec::new();
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+
+        let drivetrain_angle = Angle::new::<degree>(0.);
+
+        let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap();
+        let correct_pose = PoseEstimate {
+            position: Vector2::new(Length::new::<meter>(1.), Length::new::<meter>(0.)),
+            figure_of_merit: Length::new::<meter>(0.05 + START_POSITION_FOM),
+        };
+
+        println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x);
+        println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,);
+        println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::<meter>(), correct_pose.figure_of_merit.get::<meter>());
+
+        assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001);
+        assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001);
+        assert!((estimated_pose.figure_of_merit.get::<meter>() - correct_pose.figure_of_merit.get::<meter>()).abs() <= 0.0001);
+    }
+
+    #[test]
+    fn calculate_arcs_forward_forward_left() {
+        let mut odometry = Odometry::new();
+
+        let mut last_modules = Vec::new();
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+
+        odometry.last_modules = last_modules;
+
+        let mut new_modules = Vec::new();
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+
+        let drivetrain_angle = Angle::new::<degree>(-90.);
+
+        let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap();
+        let correct_pose = PoseEstimate {
+            position: Vector2::new(Length::new::<meter>(0.), Length::new::<meter>(1.)),
+            figure_of_merit: Length::new::<meter>(0.05 + START_POSITION_FOM),
+        };
+
+        println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x);
+        println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,);
+        println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::<meter>(), correct_pose.figure_of_merit.get::<meter>());
+
+        assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001);
+        assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001);
+        assert!((estimated_pose.figure_of_merit.get::<meter>() - correct_pose.figure_of_merit.get::<meter>()).abs() <= 0.0001);
+    }
+
+    #[test]
+    fn calculate_arcs_forward_forward_backward() {
+        let mut odometry = Odometry::new();
+
+        let mut last_modules = Vec::new();
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+
+        odometry.last_modules = last_modules;
+
+        let mut new_modules = Vec::new();
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+
+        let drivetrain_angle = Angle::new::<degree>(180.);
+
+        let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap();
+        let correct_pose = PoseEstimate {
+            position: Vector2::new(Length::new::<meter>(-1.), Length::new::<meter>(0.)),
+            figure_of_merit: Length::new::<meter>(0.05 + START_POSITION_FOM),
+        };
+
+        println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x);
+        println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,);
+        println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::<meter>(), correct_pose.figure_of_merit.get::<meter>());
+
+        assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001);
+        assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001);
+        assert!((estimated_pose.figure_of_merit.get::<meter>() - correct_pose.figure_of_merit.get::<meter>()).abs() <= 0.0001);
+    }
+    #[test]
+    fn calculate_arcs_forward_forward_right() {
+        let mut odometry = Odometry::new();
+
+        let mut last_modules = Vec::new();
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+
+        odometry.last_modules = last_modules;
+
+        let mut new_modules = Vec::new();
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(0.) });
+
+        let drivetrain_angle = Angle::new::<degree>(90.);
+
+        let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap();
+        let correct_pose = PoseEstimate {
+            position: Vector2::new(Length::new::<meter>(0.), Length::new::<meter>(-1.)),
+            figure_of_merit: Length::new::<meter>(0.05 + START_POSITION_FOM),
+        };
+
+        println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x);
+        println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,);
+        println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::<meter>(), correct_pose.figure_of_merit.get::<meter>());
+
+        assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001);
+        assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001);
+        assert!((estimated_pose.figure_of_merit.get::<meter>() - correct_pose.figure_of_merit.get::<meter>()).abs() <= 0.0001);
+    }
+    #[test]
+    fn calculate_arcs_backward_backward_forward() {
+        let mut odometry = Odometry::new();
+
+        let mut last_modules = Vec::new();
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(-180.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(-180.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(-180.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(-180.) });
+
+        odometry.last_modules = last_modules;
+
+        let mut new_modules = Vec::new();
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-180.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-180.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-180.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-180.) });
+
+        let drivetrain_angle = Angle::new::<degree>(0.);
+
+        let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap();
+        let correct_pose = PoseEstimate {
+            position: Vector2::new(Length::new::<meter>(-1.), Length::new::<meter>(0.)),
+            figure_of_merit: Length::new::<meter>(0.05 + START_POSITION_FOM),
+        };
+
+        println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x);
+        println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,);
+        println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::<meter>(), correct_pose.figure_of_merit.get::<meter>());
+
+        assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001);
+        assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001);
+        assert!((estimated_pose.figure_of_merit.get::<meter>() - correct_pose.figure_of_merit.get::<meter>()).abs() <= 0.0001);
+    }
+
+    #[test]
+    fn calculate_arcs_backward_backward_backward() {
+        let mut odometry = Odometry::new();
+
+        let mut last_modules = Vec::new();
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(-180.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(-180.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(-180.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(-180.) });
+
+        odometry.last_modules = last_modules;
+
+        let mut new_modules = Vec::new();
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-180.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-180.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-180.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-180.) });
+
+        let drivetrain_angle = Angle::new::<degree>(-180.);
+
+        let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap();
+        let correct_pose = PoseEstimate {
+            position: Vector2::new(Length::new::<meter>(1.), Length::new::<meter>(0.)),
+            figure_of_merit: Length::new::<meter>(0.05 + START_POSITION_FOM),
+        };
+
+        println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x);
+        println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,);
+        println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::<meter>(), correct_pose.figure_of_merit.get::<meter>());
+
+        assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001);
+        assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001);
+        assert!((estimated_pose.figure_of_merit.get::<meter>() - correct_pose.figure_of_merit.get::<meter>()).abs() <= 0.0001);
+    }
+    #[test]
+    fn calculate_arcs_forward_backward_forward() {
+        let mut odometry = Odometry::new();
+
+        let mut last_modules = Vec::new();
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+
+        odometry.last_modules = last_modules;
+
+        let mut new_modules = Vec::new();
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-180.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-180.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-180.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-180.) });
+
+        let drivetrain_angle = Angle::new::<degree>(0.);
+
+        let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap();
+        let correct_pose = PoseEstimate {
+            position: Vector2::new(Length::new::<meter>(0.), Length::new::<meter>(0.)),
+            figure_of_merit: Length::new::<meter>(0.05 + START_POSITION_FOM),
+        };
+
+        println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x);
+        println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,);
+        println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::<meter>(), correct_pose.figure_of_merit.get::<meter>());
+
+        // This one just needs to not crash - it should never happen in real life
+    }
+    #[test]
+    fn calculate_arcs_left_left_forward() {
+        let mut odometry = Odometry::new();
+
+        let mut last_modules = Vec::new();
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(-90.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(-90.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(-90.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(-90.) });
+
+        odometry.last_modules = last_modules;
+
+        let mut new_modules = Vec::new();
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-90.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-90.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-90.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-90.) });
+
+        let drivetrain_angle = Angle::new::<degree>(0.);
+
+        let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap();
+        let correct_pose = PoseEstimate {
+            position: Vector2::new(Length::new::<meter>(0.), Length::new::<meter>(1.)),
+            figure_of_merit: Length::new::<meter>(0.05 + START_POSITION_FOM),
+        };
+
+        println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x);
+        println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,);
+        println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::<meter>(), correct_pose.figure_of_merit.get::<meter>());
+
+        assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001);
+        assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001);
+        assert!((estimated_pose.figure_of_merit.get::<meter>() - correct_pose.figure_of_merit.get::<meter>()).abs() <= 0.0001);
+    }
+    #[test]
+    fn calculate_arcs_forward_left_forward() {
+        let mut odometry = Odometry::new();
+
+        let mut last_modules = Vec::new();
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+
+        odometry.last_modules = last_modules;
+
+        let mut new_modules = Vec::new();
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-90.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-90.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-90.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-90.) });
+
+        let drivetrain_angle = Angle::new::<degree>(0.);
+
+        let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap();
+        let correct_pose = PoseEstimate {
+            position: Vector2::new(Length::new::<meter>(std::f64::consts::FRAC_2_PI), Length::new::<meter>(std::f64::consts::FRAC_2_PI)),
+            figure_of_merit: Length::new::<meter>(0.05 + START_POSITION_FOM),
+        };
+
+        println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x);
+        println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,);
+        println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::<meter>(), correct_pose.figure_of_merit.get::<meter>());
+
+        assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001);
+        assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001);
+        assert!((estimated_pose.figure_of_merit.get::<meter>() - correct_pose.figure_of_merit.get::<meter>()).abs() <= 0.0001);
+    }
+    #[test]
+    fn calculate_arcs_forward_left_backward() {
+        let mut odometry = Odometry::new();
+
+        let mut last_modules = Vec::new();
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+
+        odometry.last_modules = last_modules;
+
+        let mut new_modules = Vec::new();
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-90.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-90.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-90.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(-90.) });
+
+        let drivetrain_angle = Angle::new::<degree>(-180.);
+
+        let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap();
+        let correct_pose = PoseEstimate {
+            position: Vector2::new(Length::new::<meter>(-std::f64::consts::FRAC_2_PI), Length::new::<meter>(-std::f64::consts::FRAC_2_PI)),
+            figure_of_merit: Length::new::<meter>(0.05 + START_POSITION_FOM),
+        };
+
+        println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x);
+        println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,);
+        println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::<meter>(), correct_pose.figure_of_merit.get::<meter>());
+
+        assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001);
+        assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001);
+        assert!((estimated_pose.figure_of_merit.get::<meter>() - correct_pose.figure_of_merit.get::<meter>()).abs() <= 0.0001);
+    }
+    #[test]
+    fn calculate_arcs_forward_right_forward() {
+        let mut odometry = Odometry::new();
+
+        let mut last_modules = Vec::new();
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+        last_modules.push(ModuleReturn { distance: Length::new::<meter>(0.), angle: Angle::new::<degree>(0.) });
+
+        odometry.last_modules = last_modules;
+
+        let mut new_modules = Vec::new();
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(90.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(90.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(90.) });
+        new_modules.push(ModuleReturn { distance: Length::new::<meter>(1.), angle: Angle::new::<degree>(90.) });
+
+        let drivetrain_angle = Angle::new::<degree>(0.);
+
+        let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap();
+        let correct_pose = PoseEstimate {
+            position: Vector2::new(Length::new::<meter>(std::f64::consts::FRAC_2_PI), Length::new::<meter>(-std::f64::consts::FRAC_2_PI)),
+            figure_of_merit: Length::new::<meter>(0.05 + START_POSITION_FOM),
+        };
+
+        println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x);
+        println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,);
+        println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::<meter>(), correct_pose.figure_of_merit.get::<meter>());
 
-        SmartDashboard::set_position(self.position, angle);
+        assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001);
+        assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001);
+        assert!((estimated_pose.figure_of_merit.get::<meter>() - correct_pose.figure_of_merit.get::<meter>()).abs() <= 0.0001);
     }
 }