diff --git a/lib/core/logger.cpp b/lib/core/logger.cpp
index 9159e717..e9550dba 100644
--- a/lib/core/logger.cpp
+++ b/lib/core/logger.cpp
@@ -4,10 +4,10 @@
 
 namespace hyped::core {
 
-Logger::Logger(const char *const label, const LogLevel level, const core::ITimeSource &time_source_)
+Logger::Logger(const char *const label, const LogLevel level, const core::ITimeSource &time_source)
     : label_(label),
       level_(level),
-      time_source_(time_source_)
+      time_source_(time_source)
 {
 }
 
@@ -47,8 +47,8 @@ void Logger::log(const LogLevel level, const char *format, ...)
         file = stderr;
         printHead(file, "FATAL");
         break;
-      default:
-        break;
+      case LogLevel::kNone:
+        return;
     }
     va_list args;
     va_start(args, format);
diff --git a/lib/core/logger.hpp b/lib/core/logger.hpp
index 21a83212..338f5786 100644
--- a/lib/core/logger.hpp
+++ b/lib/core/logger.hpp
@@ -36,7 +36,7 @@ class ILogger {
 
 class Logger : public ILogger {
  public:
-  Logger(const char *const label, const LogLevel level, const core::ITimeSource &timer);
+  Logger(const char *const label, const LogLevel level, const core::ITimeSource &time_source);
 
   void log(const LogLevel level, const char *format, ...);
 
diff --git a/lib/core/time.cpp b/lib/core/time.cpp
new file mode 100644
index 00000000..1f09b2a7
--- /dev/null
+++ b/lib/core/time.cpp
@@ -0,0 +1,25 @@
+#include "time.hpp"
+
+namespace hyped::core {
+
+TimePoint timePointFromSecondsSinceEpoch(const std::uint64_t num_seconds_since_epoch)
+{
+  return std::chrono::system_clock::time_point(std::chrono::seconds(num_seconds_since_epoch));
+}
+
+TimePoint timePointFromNanosSinceEpoch(const std::uint64_t num_seconds_since_epoch)
+{
+#ifdef __linux__
+  return std::chrono::system_clock::time_point(std::chrono::nanoseconds(num_seconds_since_epoch));
+#else
+  return std::chrono::system_clock::time_point(
+    std::chrono::microseconds(num_seconds_since_epoch / 1000));
+#endif
+}
+
+Duration durationFromSeconds(const std::uint64_t num_seconds)
+{
+  return num_seconds * kOneSecond;
+}
+
+}  // namespace hyped::core
diff --git a/lib/core/time.hpp b/lib/core/time.hpp
index f0f07e35..5f3a8d3a 100644
--- a/lib/core/time.hpp
+++ b/lib/core/time.hpp
@@ -8,11 +8,19 @@ namespace hyped::core {
  */
 using TimePoint = std::chrono::system_clock::time_point;
 
+TimePoint timePointFromSecondsSinceEpoch(const std::uint64_t num_seconds_since_epoch);
+
+TimePoint timePointFromNanosSinceEpoch(const std::uint64_t num_seconds_since_epoch);
+
 /**
  * @brief Difference between points in time; nanosecond precision.
  */
 using Duration = std::chrono::system_clock::duration;
 
+static constexpr Duration kOneSecond = static_cast<Duration>(1'000'000'000);
+
+Duration durationFromSeconds(const std::uint64_t num_seconds);
+
 /**
  * @brief Time provider allowing the user to obtain the current time of the
  * system. We abtract this away instead of using `WallClock` directly in order to
@@ -23,4 +31,4 @@ class ITimeSource {
   virtual TimePoint now() const = 0;
 };
 
-}  // namespace hyped::core
\ No newline at end of file
+}  // namespace hyped::core
diff --git a/lib/core/timer.cpp b/lib/core/timer.cpp
index 40b7693e..966c0857 100644
--- a/lib/core/timer.cpp
+++ b/lib/core/timer.cpp
@@ -2,16 +2,22 @@
 
 namespace hyped::core {
 
-Timer::Timer(const ITimeSource &time) : time_(time)
+Timer::Timer(const ITimeSource &time_source)
+    : time_source_(time_source),
+      time_started_(time_source.now())
 {
 }
 
-Duration Timer::measure_execution_time(const std::function<void(void)> task)
+Duration Timer::elapsed() const
 {
-  const auto before = time_.now();
-  task();
-  const auto after = time_.now();
-  return after - before;
+  return time_source_.now() - time_started_;
+}
+
+Duration Timer::reset()
+{
+  const auto previous_time_started = time_started_;
+  time_started_                    = time_source_.now();
+  return time_started_ - previous_time_started;
 }
 
 }  // namespace hyped::core
diff --git a/lib/core/timer.hpp b/lib/core/timer.hpp
index 99fdb114..c0318c50 100644
--- a/lib/core/timer.hpp
+++ b/lib/core/timer.hpp
@@ -8,11 +8,13 @@ namespace hyped::core {
 
 class Timer {
  public:
-  Timer(const ITimeSource &time);
-  Duration measure_execution_time(const std::function<void(void)> task);
+  Timer(const ITimeSource &time_source);
+  Duration elapsed() const;
+  Duration reset();
 
  private:
-  const ITimeSource &time_;
+  const ITimeSource &time_source_;
+  TimePoint time_started_;
 };
 
 }  // namespace hyped::core
diff --git a/lib/core/types.hpp b/lib/core/types.hpp
index ccb3ccc7..16f37673 100644
--- a/lib/core/types.hpp
+++ b/lib/core/types.hpp
@@ -23,7 +23,6 @@ struct CanFrame {
   std::array<std::uint8_t, 8> data;
 };
 
-// current trajectory struct
 struct Trajectory {
   Float displacement;
   Float velocity;
@@ -36,31 +35,25 @@ static constexpr std::uint8_t kNumAxis           = 3;
 static constexpr std::uint8_t kNumEncoders       = 4;
 static constexpr std::uint8_t kNumKeyence        = 2;
 
-// data format for raw sensor data
-using RawAccelerometerData = std::array<std::array<Float, kNumAxis>, kNumAccelerometers>;
-using AccelerometerData    = std::array<Float, kNumAccelerometers>;
-using EncoderData          = std::array<std::uint32_t, kNumEncoders>;
-using KeyenceData          = std::array<std::uint32_t, kNumKeyence>;
-
-// data produced by the accelerometer sensor
-// !! the values are in mg !!
-struct RawAccelerationData {
-  RawAccelerationData(const std::int32_t x,
-                      const std::int32_t y,
-                      const std::int32_t z,
-                      const TimePoint measured_at)
-      : x(x),
-        y(y),
-        z(z),
-        measured_at(measured_at)
+template<typename T>
+struct Measurement {
+  Measurement(const TimePoint &measured_at, const T &value) : measured_at(measured_at), value(value)
   {
   }
 
-  const std::int32_t x;
-  const std::int32_t y;
-  const std::int32_t z;
-  const TimePoint measured_at;
+  TimePoint measured_at;
+  T value;
+};
+
+struct RawAcceleration {
+  std::int32_t x;  // mg
+  std::int32_t y;  // mg
+  std::int32_t z;  // mg
 };
+using RawAccelerometerData         = Measurement<RawAcceleration>;
+using CombinedRawAccelerometerData = Measurement<std::array<RawAcceleration, kNumAccelerometers>>;
+using RawEncoderData               = Measurement<std::array<std::uint32_t, kNumEncoders>>;
+using RawKeyenceData               = Measurement<std::array<std::uint32_t, kNumKeyence>>;
 
 enum class Axis { kX = 0, kY, kZ };
 
diff --git a/lib/debug/repl.cpp b/lib/debug/repl.cpp
index 4928e4cb..ae920d46 100644
--- a/lib/debug/repl.cpp
+++ b/lib/debug/repl.cpp
@@ -228,7 +228,7 @@ void Repl::addCommand(const Command &cmd)
 
 void Repl::addQuitCommand()
 {
-  addCommand(Command{"quit", "Quit the REPL", [this]() { exit(0); }});
+  addCommand(Command{"quit", "Quit the REPL", []() { exit(0); }});
 }
 
 void Repl::addHelpCommand()
@@ -340,7 +340,7 @@ void Repl::addPwmCommands(const std::uint8_t module)
     std::stringstream description;
     description << "Run PWM module: " << static_cast<int>(pwm_module);
     pwm_run_command.description = description.str();
-    pwm_run_command.handler     = [this, pwm, pwm_module]() {
+    pwm_run_command.handler     = [this, pwm]() {
       std::uint32_t period;
       std::cout << "Period: ";
       std::cin >> period;
@@ -383,7 +383,7 @@ void Repl::addPwmCommands(const std::uint8_t module)
     std::stringstream description;
     description << "Stop PWM module: " << static_cast<int>(pwm_module);
     pwm_stop_command.description = description.str();
-    pwm_stop_command.handler     = [this, pwm, pwm_module]() {
+    pwm_stop_command.handler     = [this, pwm]() {
       const core::Result disable_result = pwm->setMode(io::Mode::kStop);
       if (disable_result == core::Result::kFailure) {
         logger_.log(core::LogLevel::kFatal, "Failed to stop PWM module");
@@ -415,7 +415,7 @@ void Repl::addSpiCommands(const std::uint8_t bus)
       std::cout << "Register address: ";
       std::cin >> register_address;
       std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
-      std::uint8_t read_buffer;
+      std::uint8_t read_buffer  = 0;
       const core::Result result = spi->read(register_address, &read_buffer, 1);
       if (result == core::Result::kSuccess) {
         logger_.log(core::LogLevel::kDebug, "SPI value from bus %d: %d", bus, read_buffer);
@@ -466,7 +466,8 @@ void Repl::addAccelerometerCommands(const std::uint8_t bus, const std::uint8_t d
                 "Asking for accelerometer on another address as what is hard coded");
     return;
   }
-  const auto accelerometer = std::make_shared<sensors::Accelerometer>(logger_, *i2c, bus);
+  const auto accelerometer
+    = std::make_shared<sensors::Accelerometer>(logger_, wall_clock_, *i2c, bus);
   accelerometer->configure();
   Command accelerometer_read_command;
   std::stringstream identifier;
@@ -478,16 +479,16 @@ void Repl::addAccelerometerCommands(const std::uint8_t bus, const std::uint8_t d
               << "I2C bus " << static_cast<int>(bus);
   accelerometer_read_command.description = description.str();
   accelerometer_read_command.handler     = [this, accelerometer, bus]() {
-    const auto value = accelerometer->read();
-    if (!value) {
+    const auto acceleration = accelerometer->read();
+    if (!acceleration) {
       logger_.log(core::LogLevel::kFatal, "Failed to read accelerometer from bus %d", bus);
     } else {
-      const core::RawAccelerationData result = value.value();
+      // TODOLater: Print timestamp
       logger_.log(core::LogLevel::kInfo,
                   "Acceleration in mg: \n x %d \n y %d \n z %d",
-                  result.x,
-                  result.y,
-                  result.z);
+                  acceleration->value.x,
+                  acceleration->value.y,
+                  acceleration->value.z);
     }
   };
   addCommand(accelerometer_read_command);
diff --git a/lib/debug/repl.hpp b/lib/debug/repl.hpp
index 49e1bb25..ef49a28d 100644
--- a/lib/debug/repl.hpp
+++ b/lib/debug/repl.hpp
@@ -7,6 +7,7 @@
 #include <vector>
 
 #include <core/logger.hpp>
+#include <core/wall_clock.hpp>
 #include <io/hardware_adc.hpp>
 #include <io/hardware_gpio.hpp>
 #include <io/hardware_i2c.hpp>
@@ -44,7 +45,8 @@ class Repl {
   void addUartCommands(const std::uint8_t bus);
 
   core::ILogger &logger_;
+  core::WallClock wall_clock_;
   std::map<std::string, Command> command_map_;
 };
 
-}  // namespace hyped::debug
\ No newline at end of file
+}  // namespace hyped::debug
diff --git a/lib/navigation/CMakeLists.txt b/lib/navigation/CMakeLists.txt
index dde850b0..44e36fe9 100644
--- a/lib/navigation/CMakeLists.txt
+++ b/lib/navigation/CMakeLists.txt
@@ -2,9 +2,11 @@ set(target hyped_navigation)
 file(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp")
 file(GLOB code "${CMAKE_CURRENT_SOURCE_DIR}/*.cpp")
 add_library(${target} STATIC ${headers} ${code})
+find_package (Eigen3 REQUIRED NO_MODULE)
 include_directories(${target}
     INTERFACE
     "${CMAKE_SOURCE_DIR}/lib"
     "${CMAKE_CURRENT_SOURCE_DIR}"
 )
-target_link_libraries(${target} PUBLIC Eigen3::Eigen)
\ No newline at end of file
+target_link_libraries(${target} PUBLIC Eigen3::Eigen)
+add_subdirectory(benchmark)
diff --git a/lib/navigation/benchmark/CMakeLists.txt b/lib/navigation/benchmark/CMakeLists.txt
new file mode 100644
index 00000000..eac16e13
--- /dev/null
+++ b/lib/navigation/benchmark/CMakeLists.txt
@@ -0,0 +1,10 @@
+set(target hyped_navigation_benchmark)
+file(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp")
+file(GLOB code "${CMAKE_CURRENT_SOURCE_DIR}/*.cpp")
+add_library(${target} STATIC ${headers} ${code})
+include_directories(${target}
+    INTERFACE
+    "${CMAKE_SOURCE_DIR}/lib"
+    "${CMAKE_CURRENT_SOURCE_DIR}"
+)
+target_link_libraries(${target} hyped_navigation)
diff --git a/lib/navigation/benchmark/benchmark.cpp b/lib/navigation/benchmark/benchmark.cpp
new file mode 100644
index 00000000..43e01403
--- /dev/null
+++ b/lib/navigation/benchmark/benchmark.cpp
@@ -0,0 +1,66 @@
+#include "benchmark.hpp"
+
+#include <core/timer.hpp>
+
+namespace hyped::navigation::benchmark {
+
+TrajectoryError::TrajectoryError(const core::Trajectory &expected, const core::Trajectory &actual)
+    : displacement(std::abs(expected.displacement - actual.displacement)),
+      velocity(std::abs(expected.velocity - actual.velocity)),
+      acceleration(std::abs(expected.acceleration - actual.acceleration))
+{
+}
+
+Benchmark::Benchmark(const core::ITimeSource &time_source, const Data &data)
+    : time_source_(time_source),
+      data_(data),
+      relevant_times_(data.getRelevantTimes())
+{
+}
+
+// TODOLater: Allow this to operate using constructors rather than already initialised navigators.
+// This would have two benefits:
+// 1. The constraints and relations of the input arguments would be more clear.
+// 2. We could measure and compare the initialisation time taken by each of the navigators.
+std::optional<Result> Benchmark::run(utils::ManualTime &manual_time, INavigator &navigator)
+{
+  if (manual_time.now() != std::chrono::system_clock::from_time_t(0)) { return std::nullopt; }
+  const core::Timer full_benchmark_timer(time_source_);
+  Result result;
+  for (const auto &time_point : relevant_times_) {
+    const auto encoder_data = data_.getEncoderDataAt(time_point);
+    if (encoder_data) {
+      const core::Timer encoder_data_timer(time_source_);
+      navigator.encoderUpdate(*encoder_data);
+      result.time_taken_for_encoder_data.push_back(encoder_data_timer.elapsed());
+    }
+    const auto acceleration_data = data_.getAccelerationDataAt(time_point);
+    if (acceleration_data) {
+      const core::Timer acceleration_data_timer(time_source_);
+      navigator.accelerometerUpdate(*acceleration_data);
+      result.time_taken_for_acceleration_data.push_back(acceleration_data_timer.elapsed());
+    }
+    const auto keyence_data = data_.getKeyenceDataAt(time_point);
+    if (keyence_data) {
+      const core::Timer keyence_data_timer(time_source_);
+      navigator.keyenceUpdate(*keyence_data);
+      result.time_taken_for_keyence_data.push_back(keyence_data_timer.elapsed());
+    }
+    const auto expected_trajectory = data_.getTrajectoryDataAt(time_point);
+    if (expected_trajectory) {
+      const core::Timer trajectory_timer(time_source_);
+      const auto calculated_trajectory = navigator.currentTrajectory();
+      if (calculated_trajectory) {
+        result.trajectory_errors.emplace_back(
+          TrajectoryError(*expected_trajectory, *calculated_trajectory));
+      } else {
+        result.trajectory_errors.push_back(std::nullopt);
+      }
+      result.time_taken_for_trajectory.push_back(trajectory_timer.elapsed());
+    }
+  }
+  result.total_time_taken = full_benchmark_timer.elapsed();
+  return result;
+}
+
+}  // namespace hyped::navigation::benchmark
diff --git a/lib/navigation/benchmark/benchmark.hpp b/lib/navigation/benchmark/benchmark.hpp
new file mode 100644
index 00000000..d1986137
--- /dev/null
+++ b/lib/navigation/benchmark/benchmark.hpp
@@ -0,0 +1,45 @@
+#pragma once
+
+#include "data.hpp"
+
+#include <map>
+#include <vector>
+
+#include <core/time.hpp>
+#include <navigation/navigator.hpp>
+#include <utils/manual_time.hpp>
+
+namespace hyped::navigation::benchmark {
+
+struct TrajectoryError {
+  const core::Float displacement;
+  const core::Float velocity;
+  const core::Float acceleration;
+
+  TrajectoryError(const core::Trajectory &expected, const core::Trajectory &actual);
+};
+
+struct Result {
+  core::Duration total_time_taken;
+  std::vector<core::Duration> time_taken_for_encoder_data;
+  std::vector<core::Duration> time_taken_for_acceleration_data;
+  std::vector<core::Duration> time_taken_for_keyence_data;
+  std::vector<core::Duration> time_taken_for_trajectory;
+  std::vector<std::optional<TrajectoryError>> trajectory_errors;
+};
+
+class Benchmark {
+ public:
+  // TODOLater: Use std::input_iterator and concepts to do this to avoid having to construct
+  // these ugly maps.
+  Benchmark(const core::ITimeSource &time_source, const Data &data);
+
+  std::optional<Result> run(utils::ManualTime &manual_time, INavigator &navigator);
+
+ private:
+  const core::ITimeSource &time_source_;
+  const Data data_;
+  const std::vector<core::TimePoint> relevant_times_;
+};
+
+}  // namespace hyped::navigation::benchmark
diff --git a/lib/navigation/benchmark/benchmarker.hpp b/lib/navigation/benchmark/benchmarker.hpp
new file mode 100644
index 00000000..e751c40e
--- /dev/null
+++ b/lib/navigation/benchmark/benchmarker.hpp
@@ -0,0 +1,20 @@
+#pragma once
+
+#include "benchmark.hpp"
+
+#include <core/time.hpp>
+#include <navigation/navigator.hpp>
+#include <utils/manual_time.hpp>
+
+namespace hyped::navigation::benchmark {
+
+class Benchmarker {
+ public:
+  Benchmarker(const core::ITimeSource &time_source_);
+
+ private:
+  const core::ITimeSource &time_source_;
+  utils::ManualTime maunal_time_;
+};
+
+}  // namespace hyped::navigation::benchmark
diff --git a/lib/navigation/benchmark/data.cpp b/lib/navigation/benchmark/data.cpp
new file mode 100644
index 00000000..1f220178
--- /dev/null
+++ b/lib/navigation/benchmark/data.cpp
@@ -0,0 +1,134 @@
+#include "data.hpp"
+
+#include <algorithm>
+
+#include <core/types.hpp>
+
+namespace hyped::navigation::benchmark {
+
+std::vector<core::TimePoint> Data::getRelevantTimes() const
+{
+  std::vector<core::TimePoint> times(encoder_data_by_time.size() + acceleration_data_by_time.size()
+                                     + keyence_data_by_time.size()
+                                     + trajectory_data_by_time.size());
+  for (const auto &[time_point, _] : encoder_data_by_time) {
+    times.push_back(time_point);
+  }
+  for (const auto &[time_point, _] : acceleration_data_by_time) {
+    times.push_back(time_point);
+  }
+  for (const auto &[time_point, _] : keyence_data_by_time) {
+    times.push_back(time_point);
+  }
+  for (const auto &[time_point, _] : trajectory_data_by_time) {
+    times.push_back(time_point);
+  }
+  std::sort(times.begin(), times.end());
+  return times;
+}
+
+std::optional<core::RawEncoderData> Data::getEncoderDataAt(const core::TimePoint time_point) const
+{
+  const auto it = encoder_data_by_time.find(time_point);
+  if (it == encoder_data_by_time.end()) { return std::nullopt; }
+  return it->second;
+}
+
+std::optional<core::CombinedRawAccelerometerData> Data::getAccelerationDataAt(
+  const core::TimePoint time_point) const
+{
+  const auto it = acceleration_data_by_time.find(time_point);
+  if (it == acceleration_data_by_time.end()) { return std::nullopt; }
+  return it->second;
+}
+
+std::optional<core::RawKeyenceData> Data::getKeyenceDataAt(const core::TimePoint time_point) const
+{
+  const auto it = keyence_data_by_time.find(time_point);
+  if (it == keyence_data_by_time.end()) { return std::nullopt; }
+  return it->second;
+}
+
+std::optional<core::Trajectory> Data::getTrajectoryDataAt(const core::TimePoint time_point) const
+{
+  const auto it = trajectory_data_by_time.find(time_point);
+  if (it == trajectory_data_by_time.end()) { return std::nullopt; }
+  return it->second;
+}
+
+DataBuilder::DataBuilder()
+{
+}
+
+Data DataBuilder::build()
+{
+  return data_;
+}
+
+core::Result DataBuilder::addEncoderData(const core::RawEncoderData &encoder_data)
+{
+  const auto it = data_.encoder_data_by_time.emplace(encoder_data.measured_at, encoder_data);
+  if (!(it.second)) { return core::Result::kFailure; }
+  return core::Result::kSuccess;
+}
+
+core::Result DataBuilder::addUniformEncoderData(const std::uint64_t nanos_since_epoch,
+                                                const std::uint32_t total_num_revolutions)
+{
+  const auto measured_at
+    = std::chrono::system_clock::time_point(std::chrono::seconds(nanos_since_epoch));
+  std::array<std::uint32_t, core::kNumEncoders> num_revolutions_per_wheel;
+  for (std::size_t i = 0; i < core::kNumEncoders; ++i) {
+    num_revolutions_per_wheel.at(i) = total_num_revolutions;
+  }
+  return addEncoderData(core::Measurement(measured_at, num_revolutions_per_wheel));
+}
+
+core::Result DataBuilder::addAccelerationData(
+  const core::CombinedRawAccelerometerData &acceleration_data)
+{
+  const auto it
+    = data_.acceleration_data_by_time.emplace(acceleration_data.measured_at, acceleration_data);
+  if (!(it.second)) { return core::Result::kFailure; }
+  return core::Result::kSuccess;
+}
+
+core::Result DataBuilder::addUniformAccelerationData(const std::uint64_t nanos_since_epoch,
+                                                     const core::RawAcceleration &raw_acceleration)
+{
+  const auto measured_at = core::timePointFromNanosSinceEpoch(nanos_since_epoch);
+  std::array<core::RawAcceleration, core::kNumAccelerometers> raw_accelerometer_data;
+  for (std::size_t i = 0; i < core::kNumAccelerometers; ++i) {
+    raw_accelerometer_data.at(i) = raw_acceleration;
+  }
+  return addAccelerationData(
+    core::CombinedRawAccelerometerData(measured_at, raw_accelerometer_data));
+}
+
+core::Result DataBuilder::addKeyenceData(const core::RawKeyenceData &keyence_data)
+{
+  const auto it = data_.keyence_data_by_time.emplace(keyence_data.measured_at, keyence_data);
+  if (!(it.second)) { return core::Result::kFailure; }
+  return core::Result::kSuccess;
+}
+
+core::Result DataBuilder::addTrajectoryData(const core::TimePoint timestamp,
+                                            const core::Trajectory &trajectory)
+{
+  const auto was_insertion_successful
+    = data_.trajectory_data_by_time.emplace(timestamp, trajectory).second;
+  if (was_insertion_successful) {
+    return core::Result::kSuccess;
+  } else {
+    return core::Result::kFailure;
+  }
+}
+
+core::Result DataBuilder::addTrajectoryData(const std::uint64_t nanos_since_epoch,
+                                            const core::Trajectory &trajectory)
+{
+  const auto timestamp = core::timePointFromNanosSinceEpoch(nanos_since_epoch);
+  return addTrajectoryData(timestamp, trajectory);
+}
+
+}  // namespace hyped::navigation::benchmark
diff --git a/lib/navigation/benchmark/data.hpp b/lib/navigation/benchmark/data.hpp
new file mode 100644
index 00000000..d214bfe2
--- /dev/null
+++ b/lib/navigation/benchmark/data.hpp
@@ -0,0 +1,47 @@
+#pragma once
+
+#include <map>
+#include <optional>
+#include <vector>
+
+#include <core/time.hpp>
+#include <core/types.hpp>
+
+namespace hyped::navigation::benchmark {
+
+struct Data {
+  std::map<core::TimePoint, core::RawEncoderData> encoder_data_by_time;
+  std::map<core::TimePoint, core::CombinedRawAccelerometerData> acceleration_data_by_time;
+  std::map<core::TimePoint, core::RawKeyenceData> keyence_data_by_time;
+  std::map<core::TimePoint, core::Trajectory> trajectory_data_by_time;
+
+  std::vector<core::TimePoint> getRelevantTimes() const;
+  std::optional<core::RawEncoderData> getEncoderDataAt(const core::TimePoint time_point) const;
+  std::optional<core::CombinedRawAccelerometerData> getAccelerationDataAt(
+    const core::TimePoint time_point) const;
+  std::optional<core::RawKeyenceData> getKeyenceDataAt(const core::TimePoint time_point) const;
+  std::optional<core::Trajectory> getTrajectoryDataAt(const core::TimePoint time_point) const;
+};
+
+class DataBuilder {
+ public:
+  DataBuilder();
+  Data build();
+
+  core::Result addEncoderData(const core::RawEncoderData &encoder_data);
+  core::Result addUniformEncoderData(const std::uint64_t nanos_since_epoch,
+                                     const std::uint32_t total_num_revolutions);
+  core::Result addAccelerationData(const core::CombinedRawAccelerometerData &acceleration_data);
+  core::Result addUniformAccelerationData(const std::uint64_t nanos_since_epoch,
+                                          const core::RawAcceleration &raw_acceleration);
+  core::Result addKeyenceData(const core::RawKeyenceData &keyence_data);
+  core::Result addTrajectoryData(const core::TimePoint timestamp,
+                                 const core::Trajectory &trajectory);
+  core::Result addTrajectoryData(const std::uint64_t nanos_since_epoch,
+                                 const core::Trajectory &trajectory);
+
+ private:
+  Data data_;
+};
+
+}  // namespace hyped::navigation::benchmark
diff --git a/lib/navigation/consts.hpp b/lib/navigation/consts.hpp
index c666e978..b06e38c4 100644
--- a/lib/navigation/consts.hpp
+++ b/lib/navigation/consts.hpp
@@ -4,7 +4,7 @@
 #include <cstdint>
 #include <optional>
 
-#include "core/types.hpp"
+#include <core/types.hpp>
 
 namespace hyped::navigation {
 
@@ -14,22 +14,8 @@ static constexpr core::Float kPi                = 3.14159265359;
 static constexpr core::Float kWheelCicumference = kPi * 0.1;  // m TODOLater: check!
 static constexpr core::Float kStripeDistance    = 10.0;       // m TODOLater:check!
 
-// define sensor checks return type
-enum class SensorChecks { kUnacceptable = 0, kAcceptable };
-
-struct Quartiles {
-  core::Float q1;
-  core::Float median;
-  core::Float q3;
-};
+static constexpr core::Trajectory kZeroTrajectory = {0, 0, 0};
 
 inline core::Trajectory zero_trajectory = {0, 0, 0};
 
-class INavigator {
- public:
-  virtual std::optional<core::Trajectory> currentTrajectory()                            = 0;
-  virtual void keyenceUpdate(const core::KeyenceData &keyence_data)                      = 0;
-  virtual void encoderUpdate(const core::EncoderData &encoder_data)                      = 0;
-  virtual void accelerometerUpdate(const core::RawAccelerometerData &accelerometer_data) = 0;
-};
-}  // namespace hyped::navigation
\ No newline at end of file
+}  // namespace hyped::navigation
diff --git a/lib/navigation/crosscheck.cpp b/lib/navigation/crosscheck.cpp
index 928ffac9..36d61482 100644
--- a/lib/navigation/crosscheck.cpp
+++ b/lib/navigation/crosscheck.cpp
@@ -1,4 +1,5 @@
 #include "crosscheck.hpp"
+#include "types.hpp"
 
 namespace hyped::navigation {
 
@@ -7,9 +8,9 @@ Crosscheck::Crosscheck()
   // TODOLater: implement
 }
 
-SensorChecks Crosscheck::checkTrajectoryAgreement(const core::AccelerometerData accelerometer_data,
-                                                  const core::EncoderData encoders_data,
-                                                  const core::KeyenceData keyence_data)
+SensorDisagreement Crosscheck::checkTrajectoryAgreement(const AccelerometerData accelerometer_data,
+                                                        const EncoderData encoders_data,
+                                                        const KeyenceData keyence_data)
 {
   /*
   TODOLater: implement
@@ -21,11 +22,11 @@ SensorChecks Crosscheck::checkTrajectoryAgreement(const core::AccelerometerData
     Also need to figure out how data flow is going to work with the historic data and what we use.
     The basic infrastrucutre is there for now so will be a problem for another day.
   */
-  return SensorChecks::kAcceptable;
+  return SensorDisagreement::kAcceptable;
 }
 
-SensorChecks checkEncoderAccelerometer(const core::AccelerometerData accelerometer_data,
-                                       const core::EncoderData encoders_data)
+SensorDisagreement Crosscheck::checkEncoderAccelerometer(const AccelerometerData accelerometer_data,
+                                                         const EncoderData encoders_data)
 {
   /*
   TODOLater: implement.
@@ -35,11 +36,11 @@ SensorChecks checkEncoderAccelerometer(const core::AccelerometerData acceleromet
   accelerometer displacement too high, fail state and return false
   - otherwise all good, return true
   */
-  return SensorChecks::kAcceptable;
+  return SensorDisagreement::kAcceptable;
 }
 
-SensorChecks checkEncoderKeyence(const core::EncoderData encoder_data,
-                                 const core::KeyenceData keyence_data)
+SensorDisagreement Crosscheck::checkEncoderKeyence(const EncoderData encoder_data,
+                                                   const KeyenceData keyence_data)
 {
   /*
   TODOLater: implement.
@@ -48,6 +49,6 @@ SensorChecks checkEncoderKeyence(const core::EncoderData encoder_data,
   keyence displacement too high, fail state and return false
   - otherwise all good, return true
   */
-  return SensorChecks::kAcceptable;
+  return SensorDisagreement::kAcceptable;
 }
-}  // namespace hyped::navigation
\ No newline at end of file
+}  // namespace hyped::navigation
diff --git a/lib/navigation/crosscheck.hpp b/lib/navigation/crosscheck.hpp
index aaf876c8..1b3ed8d3 100644
--- a/lib/navigation/crosscheck.hpp
+++ b/lib/navigation/crosscheck.hpp
@@ -1,10 +1,10 @@
 #pragma once
 
-#include "consts.hpp"
+#include "types.hpp"
 
 #include <array>
 
-#include "core/types.hpp"
+#include <core/types.hpp>
 
 namespace hyped::navigation {
 
@@ -20,9 +20,9 @@ class Crosscheck {
    * @return true signifies trajectory agreement
    * @return false signifies trajectory disagreement. We enter fail state
    */
-  SensorChecks checkTrajectoryAgreement(const core::AccelerometerData accelerometer_data,
-                                        const core::EncoderData encoders_data,
-                                        const core::KeyenceData keyence_data);
+  SensorDisagreement checkTrajectoryAgreement(const AccelerometerData accelerometer_data,
+                                              const EncoderData encoders_data,
+                                              const KeyenceData keyence_data);
 
  private:
   /**
@@ -33,8 +33,8 @@ class Crosscheck {
    * @return true accelerometer and wheel encoders agree
    * @return false accelerometers and wheel encoders disagree
    */
-  SensorChecks checkEncoderAccelerometer(const core::AccelerometerData accelerometer_data,
-                                         const core::EncoderData encoders_data);
+  SensorDisagreement checkEncoderAccelerometer(const AccelerometerData accelerometer_data,
+                                               const EncoderData encoders_data);
 
   /**
    * @brief Checks the keyence value of displacement against the
@@ -44,7 +44,7 @@ class Crosscheck {
    * @return true Keyence and wheel encoders agree
    * @return false Keyence and wheel encoders disagree
    */
-  SensorChecks checkEncooderKeyence(const core::EncoderData encoder_data,
-                                    const core::KeyenceData keyence_data);
+  SensorDisagreement checkEncoderKeyence(const EncoderData encoder_data,
+                                         const KeyenceData keyence_data);
 };
-}  // namespace hyped::navigation
\ No newline at end of file
+}  // namespace hyped::navigation
diff --git a/lib/navigation/navigator.cpp b/lib/navigation/navigator.cpp
index f2069219..e03b1ef3 100644
--- a/lib/navigation/navigator.cpp
+++ b/lib/navigation/navigator.cpp
@@ -2,9 +2,13 @@
 
 #include <cstdint>
 
+#include "core/time.hpp"
+
 namespace hyped::navigation {
 
 Navigator::Navigator()
+    : previous_encoder_reading_(core::timePointFromSecondsSinceEpoch(0), {0, 0, 0}),
+      previous_keyence_reading_(core::timePointFromSecondsSinceEpoch(0), {0, 0})
 {
   // TODOLater: impement
 }
@@ -21,7 +25,7 @@ std::optional<core::Trajectory> Navigator::currentTrajectory()
   return trajectory_;
 }
 
-void Navigator::keyenceUpdate(const core::KeyenceData &keyence_data)
+void Navigator::keyenceUpdate(const core::RawKeyenceData &keyence_data)
 {
   /*
   TODOLater:
@@ -30,7 +34,7 @@ void Navigator::keyenceUpdate(const core::KeyenceData &keyence_data)
   */
 }
 
-void Navigator::encoderUpdate(const core::EncoderData &encoder_data)
+void Navigator::encoderUpdate(const core::RawEncoderData &encoder_data)
 {
   /*
   TODOLater:
diff --git a/lib/navigation/navigator.hpp b/lib/navigation/navigator.hpp
index 2b701d07..916ea0df 100644
--- a/lib/navigation/navigator.hpp
+++ b/lib/navigation/navigator.hpp
@@ -1,10 +1,10 @@
 #pragma once
 
-#include "consts.hpp"
+#include "types.hpp"
 
 #include <optional>
 
-#include "core/types.hpp"
+#include <core/types.hpp>
 
 namespace hyped::navigation {
 
@@ -15,34 +15,36 @@ class Navigator : public INavigator {
   /**
    *@brief runs cross checking and returns trajectory
    */
-  std::optional<core::Trajectory> currentTrajectory();
+  virtual std::optional<core::Trajectory> currentTrajectory();
 
   /**
    * @brief preprocesses keyence data and updates trajectory
    *
    * @param keyence_data
    */
-  void keyenceUpdate(const core::KeyenceData &keyence_data);
+  virtual void keyenceUpdate(const core::RawKeyenceData &keyence_data);
+
   /**
    * @brief preprocesses encoder data and updates trajectory
    *
    * @param encoder_data
    */
-  void encoderUpdate(const core::EncoderData &encoder_data);
+  virtual void encoderUpdate(const core::RawEncoderData &encoder_data);
+
   /**
    * @brief preprocesses accelerometer data and updates trajectory
    *
    * @param accelerometer_data
    */
-  void accelerometerUpdate(const core::RawAccelerometerData &accelerometer_data);
+  virtual void accelerometerUpdate(const core::RawAccelerometerData &accelerometer_data);
 
  private:
   // previous readings
-  core::EncoderData previous_encoder_reading_;
-  core::KeyenceData previous_keyence_reading_;
+  core::RawEncoderData previous_encoder_reading_;
+  core::RawKeyenceData previous_keyence_reading_;
 
   // current navigation trajectory
   core::Trajectory trajectory_;
 };
 
-}  // namespace hyped::navigation
\ No newline at end of file
+}  // namespace hyped::navigation
diff --git a/lib/navigation/preprocess_accelerometer.cpp b/lib/navigation/preprocess_accelerometer.cpp
index 4d82bc7e..2a113399 100644
--- a/lib/navigation/preprocess_accelerometer.cpp
+++ b/lib/navigation/preprocess_accelerometer.cpp
@@ -1,5 +1,7 @@
 #include "preprocess_accelerometer.hpp"
 
+#include <core/types.hpp>
+
 namespace hyped::navigation {
 
 AccelerometerPreprocessor::AccelerometerPreprocessor(core::ILogger &logger)
@@ -10,44 +12,36 @@ AccelerometerPreprocessor::AccelerometerPreprocessor(core::ILogger &logger)
 {
 }
 
-std::optional<core::AccelerometerData> AccelerometerPreprocessor::processData(
-  const core::RawAccelerometerData raw_accelerometer_data)
+std::optional<AccelerometerData> AccelerometerPreprocessor::processData(
+  const core::CombinedRawAccelerometerData &raw_accelerometer_data)
 {
-  core::AccelerometerData accelerometer_data;
+  std::array<core::Float, core::kNumAccelerometers> accelerometer_data;
   // TODOLater : check on direction of travel
   for (std::size_t i = 0; i < core::kNumAccelerometers; ++i) {
-    core::Float magnitude = 0;
-    for (std::size_t j = 0; j < core::kNumAxis; ++j) {
-      magnitude += std::pow(raw_accelerometer_data.at(i).at(j), 2);
-    }
+    const auto raw_acceleration = raw_accelerometer_data.value.at(i);
+    std::uint64_t magnitude     = 0;
+    magnitude += raw_acceleration.x * raw_acceleration.x;
+    magnitude += raw_acceleration.y * raw_acceleration.y;
+    magnitude += raw_acceleration.z * raw_acceleration.z;
     accelerometer_data.at(i) = std::sqrt(magnitude);
   }
-  const core::AccelerometerData accel_data = detectOutliers(accelerometer_data);
-  SensorChecks sensorcheck                 = checkReliable();
-
-  if (sensorcheck == SensorChecks::kUnacceptable) {
-    return std::nullopt;
-  } else {
-    return accelerometer_data;
-  }
+  const auto sanitised_accelerometer_data
+    = detectOutliers(AccelerometerData(raw_accelerometer_data.measured_at, accelerometer_data));
+  if (checkReliable() == SensorDisagreement::kUnacceptable) { return std::nullopt; }
+  return sanitised_accelerometer_data;
 }
 
-core::AccelerometerData AccelerometerPreprocessor::detectOutliers(
-  core::AccelerometerData accelerometer_data)
+AccelerometerData AccelerometerPreprocessor::detectOutliers(AccelerometerData accelerometer_data)
 {
-  // core::AccelerometerData accelerometer_data;
-  // for (std::size_t i; i < core::kNumAccelerometers; ++i) {
-  //   accelerometer_data.at(i) = accel_data.at(i);
-  // }
   Quartiles quartiles;
   if (num_reliable_accelerometers_ == core::kNumAccelerometers) {
-    quartiles = getQuartiles(accelerometer_data);
+    quartiles = getQuartiles(accelerometer_data.value);
   } else if (num_reliable_accelerometers_ == core::kNumAccelerometers - 1) {
     std::array<core::Float, core::kNumAccelerometers - 1> filtered_data;
     std::size_t j = 0;
     for (std::size_t i = 0; i < core::kNumAccelerometers; ++i) {
       if (are_accelerometers_reliable_.at(i)) {
-        filtered_data.at(j) = accelerometer_data.at(i);
+        filtered_data.at(j) = accelerometer_data.value.at(i);
         ++j;
       }
     }
@@ -71,9 +65,10 @@ core::AccelerometerData AccelerometerPreprocessor::detectOutliers(
     // converts outliers or unreliables to medians, updates number of consecutive outliers for each
     // sensor
     if (are_accelerometers_reliable_.at(i) == false) {
-      accelerometer_data.at(i) = quartiles.median;
-    } else if (accelerometer_data.at(i) < lower_bound || accelerometer_data.at(i) > upper_bound) {
-      accelerometer_data.at(i) = quartiles.median;
+      accelerometer_data.value.at(i) = quartiles.median;
+    } else if (accelerometer_data.value.at(i) < lower_bound
+               || accelerometer_data.value.at(i) > upper_bound) {
+      accelerometer_data.value.at(i) = quartiles.median;
       ++num_outliers_per_accelerometer_.at(i);
     } else {
       num_outliers_per_accelerometer_.at(i) = 0;
@@ -82,7 +77,7 @@ core::AccelerometerData AccelerometerPreprocessor::detectOutliers(
   return accelerometer_data;
 }
 
-SensorChecks AccelerometerPreprocessor::checkReliable()
+SensorDisagreement AccelerometerPreprocessor::checkReliable()
 {  // changes reliable sensor to false if max consecutive outliers are reached
   for (std::size_t i = 0; i < core::kNumAccelerometers; ++i) {
     if (are_accelerometers_reliable_.at(i) == true
@@ -93,9 +88,9 @@ SensorChecks AccelerometerPreprocessor::checkReliable()
   }
   if (num_reliable_accelerometers_ < core::kNumAccelerometers - 1) {
     logger_.log(core::LogLevel::kFatal, "Maximum number of unreliable accelerometers exceeded");
-    return SensorChecks::kUnacceptable;
+    return SensorDisagreement::kUnacceptable;
   }
-  return SensorChecks::kAcceptable;
+  return SensorDisagreement::kAcceptable;
 }
 
-}  // namespace hyped::navigation
\ No newline at end of file
+}  // namespace hyped::navigation
diff --git a/lib/navigation/preprocess_accelerometer.hpp b/lib/navigation/preprocess_accelerometer.hpp
index 36a4035e..9f706a38 100644
--- a/lib/navigation/preprocess_accelerometer.hpp
+++ b/lib/navigation/preprocess_accelerometer.hpp
@@ -1,6 +1,7 @@
 #pragma once
 
 #include "consts.hpp"
+#include "types.hpp"
 
 #include <cmath>
 
@@ -9,8 +10,8 @@
 #include <cstdint>
 #include <optional>
 
-#include "core/logger.hpp"
-#include "core/types.hpp"
+#include <core/logger.hpp>
+#include <core/types.hpp>
 
 namespace hyped::navigation {
 class AccelerometerPreprocessor {
@@ -23,8 +24,8 @@ class AccelerometerPreprocessor {
    * @param raw_accelerometer_data
    * @return clean accelerometer data or optionally fail
    */
-  std::optional<core::AccelerometerData> processData(
-    const core::RawAccelerometerData raw_accelerometer_data);
+  std::optional<AccelerometerData> processData(
+    const core::CombinedRawAccelerometerData &raw_accelerometer_data);
 
  private:
   core::ILogger &logger_;
@@ -41,14 +42,14 @@ class AccelerometerPreprocessor {
    * @param accelerometer_data
    * @return filtered accelerometer data
    */
-  core::AccelerometerData detectOutliers(core::AccelerometerData accelerometer_data);
+  AccelerometerData detectOutliers(AccelerometerData accelerometer_data);
 
   /**
    * @brief check the reliability of all accelerometer's
    *
-   * @return SensorChecks with value kAcceptable or kUnacceptable
+   * @return SensorDisagreement with value kAcceptable or kUnacceptable
    */
-  SensorChecks checkReliable();
+  SensorDisagreement checkReliable();
 
   // TODOLater: Optimize this further
   /**
diff --git a/lib/navigation/preprocess_encoders.cpp b/lib/navigation/preprocess_encoders.cpp
index 58c7af4c..ba9737ec 100644
--- a/lib/navigation/preprocess_encoders.cpp
+++ b/lib/navigation/preprocess_encoders.cpp
@@ -1,5 +1,7 @@
 #include "preprocess_encoders.hpp"
 
+#include <core/time.hpp>
+
 namespace hyped::navigation {
 
 EncodersPreprocessor::EncodersPreprocessor()
@@ -7,7 +9,8 @@ EncodersPreprocessor::EncodersPreprocessor()
   // TODOLater: implement
 }
 
-core::EncoderData EncodersPreprocessor::processData(const core::EncoderData)
+std::optional<EncoderData> EncodersPreprocessor::processData(
+  const core::RawEncoderData &raw_encoder_data)
 {
   /*
   TODOLater: implement
@@ -16,10 +19,10 @@ core::EncoderData EncodersPreprocessor::processData(const core::EncoderData)
   - call checkRelaible
 
   */
-  return {0, 0, 0, 0};
+  return std::nullopt;
 }
 
-core::EncoderData EncodersPreprocessor::detectOutliers(const core::EncoderData encoder_data)
+EncoderData EncodersPreprocessor::detectOutliers(const core::RawEncoderData &raw_encoder_data)
 {
   /*
   TODOLater: implement
@@ -34,10 +37,10 @@ core::EncoderData EncodersPreprocessor::detectOutliers(const core::EncoderData e
   -also figure out return type/ what we update and update
   documentation as appropriate
   */
-  return {0, 0, 0, 0};
+  return EncoderData(core::timePointFromSecondsSinceEpoch(0), {0, 0, 0, 0});
 }
 
-void EncodersPreprocessor::checkReliable(const core::EncoderData &encoder_data)
+void EncodersPreprocessor::checkReliable(const core::RawEncoderData &raw_encoder_data)
 {
   /*
   TODOLater: implement
@@ -53,4 +56,4 @@ void EncodersPreprocessor::checkReliable(const core::EncoderData &encoder_data)
   */
 }
 
-}  // namespace hyped::navigation
\ No newline at end of file
+}  // namespace hyped::navigation
diff --git a/lib/navigation/preprocess_encoders.hpp b/lib/navigation/preprocess_encoders.hpp
index 142fc70d..384f5eee 100644
--- a/lib/navigation/preprocess_encoders.hpp
+++ b/lib/navigation/preprocess_encoders.hpp
@@ -1,21 +1,24 @@
 #pragma once
 
+#include "types.hpp"
+
 #include <array>
 #include <cstdint>
 
-#include "core/types.hpp"
+#include <core/types.hpp>
 
 namespace hyped::navigation {
+
 class EncodersPreprocessor {
  public:
   EncodersPreprocessor();
 
-  core::EncoderData processData(const core::EncoderData encoder_data);
+  std::optional<EncoderData> processData(const core::RawEncoderData &encoder_data);
 
  private:
-  core::EncoderData detectOutliers(const core::EncoderData encoder_data);
+  EncoderData detectOutliers(const core::RawEncoderData &encoder_data);
 
-  void checkReliable(const core::EncoderData &encoder_data);
+  void checkReliable(const core::RawEncoderData &raw_encoder_data);
 
   // initialised as {0, 0, 0, 0}, count of consecutive outliers
   std::array<std::uint16_t, core::kNumEncoders> num_outliers_per_encoder_;
diff --git a/lib/navigation/preprocess_keyence.cpp b/lib/navigation/preprocess_keyence.cpp
index cd2a4a81..f05ff502 100644
--- a/lib/navigation/preprocess_keyence.cpp
+++ b/lib/navigation/preprocess_keyence.cpp
@@ -9,23 +9,23 @@ KeyencePreprocessor::KeyencePreprocessor(core::ILogger &logger)
       previous_data_status_(KeyenceDataStatus::kAgreed)
 {
 }
-SensorChecks KeyencePreprocessor::checkKeyenceAgrees(const core::KeyenceData &keyence_data)
+
+SensorDisagreement KeyencePreprocessor::checkKeyenceAgrees(const KeyenceData &keyence_data)
 {
-  KeyenceDataStatus current_data_status = KeyenceDataStatus::kDisagreed;
-  for (std::size_t i = 0; i < keyence_data.size() - 1; ++i) {
-    if (keyence_data.at(i) != keyence_data.at(i + 1)) {
+  KeyenceDataStatus current_data_status = KeyenceDataStatus::kAgreed;
+  for (std::size_t i = 1; i < keyence_data.value.size(); ++i) {
+    if (keyence_data.value.at(0) != keyence_data.value.at(i)) {
       current_data_status = KeyenceDataStatus::kDisagreed;
+      break;
     }
   }
-
   if (current_data_status == KeyenceDataStatus::kDisagreed
       && previous_data_status_ == KeyenceDataStatus::kDisagreed) {
     log_.log(core::LogLevel::kFatal, "Keyence disagreement for two consecutive readings.");
-    return SensorChecks::kUnacceptable;
+    return SensorDisagreement::kUnacceptable;
   }
   previous_data_status_ = current_data_status;
-
-  return SensorChecks::kAcceptable;
+  return SensorDisagreement::kAcceptable;
 }
 
-}  // namespace hyped::navigation
\ No newline at end of file
+}  // namespace hyped::navigation
diff --git a/lib/navigation/preprocess_keyence.hpp b/lib/navigation/preprocess_keyence.hpp
index 8dba0cf4..8a6c8cc2 100644
--- a/lib/navigation/preprocess_keyence.hpp
+++ b/lib/navigation/preprocess_keyence.hpp
@@ -1,9 +1,10 @@
 #pragma once
 
 #include "consts.hpp"
+#include "types.hpp"
 
-#include "core/logger.hpp"
-#include "core/types.hpp"
+#include <core/logger.hpp>
+#include <core/types.hpp>
 
 namespace hyped::navigation {
 
@@ -22,15 +23,13 @@ class KeyencePreprocessor {
    * @brief Checks that keyences have not disagreed twice in a row.
    *
    * @param keyence_data: inputs from Keyence Sensors
-   * @return SensorChecks: enum indicating if Keyence Sensors have failed
-   */
-  SensorChecks checkKeyenceAgrees(const core::KeyenceData &keyence_data);
-  /**
-   *
+   * @return SensorDisagreement: enum indicating if Keyence Sensors have failed
    */
+  SensorDisagreement checkKeyenceAgrees(const KeyenceData &keyence_data);
+
  private:
   core::ILogger &log_;
   KeyenceDataStatus previous_data_status_;
 };
 
-}  // namespace hyped::navigation
\ No newline at end of file
+}  // namespace hyped::navigation
diff --git a/lib/navigation/types.hpp b/lib/navigation/types.hpp
new file mode 100644
index 00000000..94b98213
--- /dev/null
+++ b/lib/navigation/types.hpp
@@ -0,0 +1,28 @@
+#pragma once
+
+#include <optional>
+
+#include <core/types.hpp>
+namespace hyped::navigation {
+
+enum class SensorDisagreement { kUnacceptable = 0, kAcceptable };
+
+using EncoderData       = core::Measurement<std::array<std::uint64_t, core::kNumEncoders>>;
+using AccelerometerData = core::Measurement<std::array<core::Float, core::kNumAccelerometers>>;
+using KeyenceData       = core::Measurement<std::array<std::uint32_t, core::kNumKeyence>>;
+
+struct Quartiles {
+  core::Float q1;
+  core::Float median;
+  core::Float q3;
+};
+
+class INavigator {
+ public:
+  virtual std::optional<core::Trajectory> currentTrajectory()                           = 0;
+  virtual void keyenceUpdate(const core::RawKeyenceData &keyence_data)                  = 0;
+  virtual void encoderUpdate(const core::RawEncoderData &encoder_data)                  = 0;
+  virtual void accelerometerUpdate(const core::CombinedRawAccelerometerData &acceleration_data) = 0;
+};
+
+}  // namespace hyped::navigation
diff --git a/lib/sensors/accelerometer.cpp b/lib/sensors/accelerometer.cpp
index 1d187d93..a1707e12 100644
--- a/lib/sensors/accelerometer.cpp
+++ b/lib/sensors/accelerometer.cpp
@@ -1,9 +1,15 @@
 #include "accelerometer.hpp"
 
+#include <core/types.hpp>
+
 namespace hyped::sensors {
 
-Accelerometer::Accelerometer(core::ILogger &logger, io::II2c &i2c, const std::uint8_t channel)
+Accelerometer::Accelerometer(core::ILogger &logger,
+                             core::ITimeSource &time_source,
+                             io::II2c &i2c,
+                             const std::uint8_t channel)
     : logger_(logger),
+      time_source_(time_source),
       i2c_(i2c),
       channel_(channel)
 {
@@ -64,7 +70,7 @@ std::int32_t Accelerometer::getAccelerationFromRawValue(const std::int16_t rawAc
 
 // TODOlater: current settings of the accelerometer make it read in +-16g but with high noise. Check
 // whether other configuration could be better
-std::optional<core::RawAccelerationData> Accelerometer::read()
+std::optional<core::Measurement<core::RawAcceleration>> Accelerometer::read()
 {
   // check to see if the values are ready to be read
   const auto data_ready = i2c_.readByte(kDefaultAccelerometerAddress, kDataReady);
@@ -79,21 +85,15 @@ std::optional<core::RawAccelerationData> Accelerometer::read()
   }
   const auto result_x = getRawAcceleration(core::Axis::kX);
   if (!result_x) { return std::nullopt; }
-  const std::int32_t x_acceleration = getAccelerationFromRawValue(*result_x);
-  const auto result_y               = getRawAcceleration(core::Axis::kY);
+  const auto result_y = getRawAcceleration(core::Axis::kY);
   if (!result_y) { return std::nullopt; }
-  const std::int32_t y_acceleration = getAccelerationFromRawValue(*result_y);
-  const auto result_z               = getRawAcceleration(core::Axis::kZ);
+  const auto result_z = getRawAcceleration(core::Axis::kZ);
   if (!result_z) { return std::nullopt; }
-  const std::int32_t z_acceleration = getAccelerationFromRawValue(*result_z);
-  const std::optional<core::RawAccelerationData> acceleration_3_axis{
-    std::in_place,
-    x_acceleration,
-    y_acceleration,
-    z_acceleration,
-    std::chrono::system_clock::now()};
+  const core::RawAcceleration raw_acceleration = {.x = getAccelerationFromRawValue(*result_x),
+                                                  .y = getAccelerationFromRawValue(*result_y),
+                                                  .z = getAccelerationFromRawValue(*result_z)};
   logger_.log(core::LogLevel::kDebug, "Successfully read accelerometer data");
-  return acceleration_3_axis;
+  return core::Measurement<core::RawAcceleration>(time_source_.now(), raw_acceleration);
 }
 
 core::Result Accelerometer::configure()
@@ -120,4 +120,4 @@ core::Result Accelerometer::configure()
   return core::Result::kSuccess;
 }
 
-}  // namespace hyped::sensors
\ No newline at end of file
+}  // namespace hyped::sensors
diff --git a/lib/sensors/accelerometer.hpp b/lib/sensors/accelerometer.hpp
index ad1796f9..9699b8ac 100644
--- a/lib/sensors/accelerometer.hpp
+++ b/lib/sensors/accelerometer.hpp
@@ -7,7 +7,9 @@
 #include <cstdint>
 #include <optional>
 
+#include "core/time.hpp"
 #include <core/logger.hpp>
+#include <core/time.hpp>
 #include <core/types.hpp>
 #include <io/i2c.hpp>
 
@@ -48,14 +50,17 @@ constexpr std::uint8_t kDataReady = 0x27;
 constexpr std::uint8_t kDeviceIdAddress       = 0x0F;
 constexpr std::uint8_t kExpectedDeviceIdValue = 0x44;
 
-class Accelerometer : public II2cMuxSensor<core::RawAccelerationData> {
+class Accelerometer : public II2cMuxSensor<core::RawAcceleration> {
  public:
-  Accelerometer(core::ILogger &logger, io::II2c &i2c, const std::uint8_t channel);
+  Accelerometer(core::ILogger &logger,
+                core::ITimeSource &time_source,
+                io::II2c &i2c,
+                const std::uint8_t channel);
   ~Accelerometer();
 
-  core::Result configure();
-  std::optional<core::RawAccelerationData> read();
-  std::uint8_t getChannel() const;
+  virtual core::Result configure();
+  virtual std::optional<core::Measurement<core::RawAcceleration>> read();
+  virtual std::uint8_t getChannel() const;
 
  private:
   std::optional<std::int16_t> getRawAcceleration(const core::Axis axis);
@@ -64,10 +69,11 @@ class Accelerometer : public II2cMuxSensor<core::RawAccelerationData> {
 
  private:
   core::ILogger &logger_;
+  core::ITimeSource &time_source_;
   io::II2c &i2c_;
   const std::uint8_t channel_;
   std::uint8_t low_byte_address_;
   std::uint8_t high_byte_address_;
 };
 
-}  // namespace hyped::sensors
\ No newline at end of file
+}  // namespace hyped::sensors
diff --git a/lib/sensors/i2c_sensors.hpp b/lib/sensors/i2c_sensors.hpp
index a4f9dba0..2013ead9 100644
--- a/lib/sensors/i2c_sensors.hpp
+++ b/lib/sensors/i2c_sensors.hpp
@@ -17,9 +17,9 @@ class II2cMuxSensor {
    * This function carries out the initilization steps for a particular sensor.
    */
   virtual core::Result configure()        = 0;
-  virtual std::optional<T> read()         = 0;
+  virtual std::optional<core::Measurement<T>> read()         = 0;
   virtual std::uint8_t getChannel() const = 0;
   virtual ~II2cMuxSensor() {}
 };
 
-}  // namespace hyped::sensors
\ No newline at end of file
+}  // namespace hyped::sensors
diff --git a/lib/sensors/temperature.cpp b/lib/sensors/temperature.cpp
index e752976e..176673fc 100644
--- a/lib/sensors/temperature.cpp
+++ b/lib/sensors/temperature.cpp
@@ -3,9 +3,11 @@
 namespace hyped::sensors {
 
 Temperature::Temperature(core::ILogger &logger,
+                         core::ITimeSource &time_source,
                          std::shared_ptr<io::II2c> i2c,
                          const std::uint8_t channel)
     : logger_(logger),
+      time_source_(time_source),
       i2c_(i2c),
       channel_(channel)
 {
@@ -15,7 +17,7 @@ Temperature::~Temperature()
 {
 }
 
-std::optional<std::int16_t> Temperature::read()
+std::optional<core::Measurement<std::int16_t>> Temperature::read()
 {
   const auto status_check_result = i2c_->readByte(kTemperatureDefaultAddress, kStatus);
   if (!status_check_result) {
@@ -60,7 +62,8 @@ std::optional<std::int16_t> Temperature::read()
   logger_.log(
     core::LogLevel::kDebug, "Successfully read from temperature sensor at channel %d", channel_);
   // Scaling temperature as per the datasheet
-  return static_cast<std::int16_t>(temperature * kTemperatureScaleFactor);
+  return core::Measurement(time_source_.now(),
+                           static_cast<std::int16_t>(temperature * kTemperatureScaleFactor));
 }
 
 core::Result Temperature::configure()
@@ -82,4 +85,4 @@ std::uint8_t Temperature::getChannel() const
   return channel_;
 }
 
-}  // namespace hyped::sensors
\ No newline at end of file
+}  // namespace hyped::sensors
diff --git a/lib/sensors/temperature.hpp b/lib/sensors/temperature.hpp
index a2afff29..fee0438a 100644
--- a/lib/sensors/temperature.hpp
+++ b/lib/sensors/temperature.hpp
@@ -7,9 +7,11 @@
 #include <optional>
 
 #include <core/logger.hpp>
+#include <core/time.hpp>
 #include <io/i2c.hpp>
 
 namespace hyped::sensors {
+
 // Values and register names from data sheet
 static constexpr std::uint8_t kTemperatureDefaultAddress = 0x38;
 static constexpr std::uint8_t kCtrl                      = 0x04;
@@ -26,17 +28,21 @@ static constexpr core::Float kTemperatureScaleFactor = 0.01;
 
 class Temperature : public II2cMuxSensor<std::int16_t> {
  public:
-  Temperature(core::ILogger &logger, std::shared_ptr<io::II2c> i2c, const std::uint8_t channel);
+  Temperature(core::ILogger &logger,
+              core::ITimeSource &time_source,
+              std::shared_ptr<io::II2c> i2c,
+              const std::uint8_t channel);
   ~Temperature();
 
-  core::Result configure();
-  std::optional<std::int16_t> read();
-  std::uint8_t getChannel() const;
+  virtual core::Result configure();
+  virtual std::optional<core::Measurement<std::int16_t>> read();
+  virtual std::uint8_t getChannel() const;
 
  private:
   core::ILogger &logger_;
+  core::ITimeSource &time_source_;
   std::shared_ptr<io::II2c> i2c_;
   const std::uint8_t channel_;
 };
 
-}  // namespace hyped::sensors
\ No newline at end of file
+}  // namespace hyped::sensors
diff --git a/lib/utils/dummy_i2c_sensor.cpp b/lib/utils/dummy_i2c_sensor.cpp
index 42934ab8..702c7cf9 100644
--- a/lib/utils/dummy_i2c_sensor.cpp
+++ b/lib/utils/dummy_i2c_sensor.cpp
@@ -2,7 +2,7 @@
 
 namespace hyped::utils {
 
-DummyI2cSensor::DummyI2cSensor()
+DummyI2cSensor::DummyI2cSensor(core::ITimeSource &time_source) : time_source_(time_source)
 {
 }
 
@@ -11,9 +11,9 @@ core::Result DummyI2cSensor::configure()
   return core::Result::kSuccess;
 }
 
-std::optional<std::uint8_t> DummyI2cSensor::read()
+std::optional<core::Measurement<std::uint8_t>> DummyI2cSensor::read()
 {
-  return 0;
+  return core::Measurement<std::uint8_t>(time_source_.now(), 0);
 }
 
 std::uint8_t DummyI2cSensor::getChannel() const
diff --git a/lib/utils/dummy_i2c_sensor.hpp b/lib/utils/dummy_i2c_sensor.hpp
index 2fd6e13b..2d49442c 100644
--- a/lib/utils/dummy_i2c_sensor.hpp
+++ b/lib/utils/dummy_i2c_sensor.hpp
@@ -1,15 +1,19 @@
 #pragma once
 
+#include <core/time.hpp>
 #include <sensors/i2c_sensors.hpp>
 
 namespace hyped::utils {
 
 class DummyI2cSensor : public sensors::II2cMuxSensor<std::uint8_t> {
  public:
-  DummyI2cSensor();
+  DummyI2cSensor(core::ITimeSource &time_source);
   virtual core::Result configure();
-  virtual std::optional<std::uint8_t> read();
+  virtual std::optional<core::Measurement<std::uint8_t>> read();
   virtual std::uint8_t getChannel() const;
+
+ private:
+  core::ITimeSource &time_source_;
 };
 
 }  // namespace hyped::utils
diff --git a/lib/utils/manual_time.cpp b/lib/utils/manual_time.cpp
index 50464bdf..5e89df57 100644
--- a/lib/utils/manual_time.cpp
+++ b/lib/utils/manual_time.cpp
@@ -16,14 +16,24 @@ core::TimePoint ManualTime::now() const
   return current_time_;
 }
 
-void ManualTime::set_time(const core::TimePoint time_point)
+void ManualTime::setTime(const core::TimePoint time_point)
 {
   current_time_ = time_point;
 }
 
-void ManualTime::set_seconds_since_epoch(const std::uint64_t seconds_since_epoch)
+void ManualTime::setSecondsSinceEpoch(const std::uint64_t seconds_since_epoch)
 {
   current_time_ = std::chrono::system_clock::time_point(std::chrono::seconds(seconds_since_epoch));
 }
 
+void ManualTime::addTime(const core::Duration duration)
+{
+  current_time_ += duration;
+}
+
+void ManualTime::addSeconds(const std::uint64_t num_seconds)
+{
+  current_time_ += core::durationFromSeconds(1);
+}
+
 }  // namespace hyped::utils
diff --git a/lib/utils/manual_time.hpp b/lib/utils/manual_time.hpp
index 0687c97a..a1153139 100644
--- a/lib/utils/manual_time.hpp
+++ b/lib/utils/manual_time.hpp
@@ -9,8 +9,10 @@ class ManualTime : public core::ITimeSource {
   ManualTime();
   virtual core::TimePoint now() const;
 
-  void set_time(const core::TimePoint time_point);
-  void set_seconds_since_epoch(const std::uint64_t seconds_since_epoch);
+  void setTime(const core::TimePoint time_point);
+  void setSecondsSinceEpoch(const std::uint64_t seconds_since_epoch);
+  void addTime(const core::Duration duration);
+  void addSeconds(const std::uint64_t num_seconds);
 
  private:
   core::TimePoint current_time_;
diff --git a/lib/utils/naive_navigator.cpp b/lib/utils/naive_navigator.cpp
index 6b3c4d19..f4506911 100644
--- a/lib/utils/naive_navigator.cpp
+++ b/lib/utils/naive_navigator.cpp
@@ -1,10 +1,16 @@
 #include "naive_navigator.hpp"
 
+#include <cmath>
+
 #include <numeric>
 
+#include "core/time.hpp"
+
 namespace hyped::utils {
 
-NaiveNavigator::NaiveNavigator() : current_trajectory_{}
+NaiveNavigator::NaiveNavigator()
+    : current_trajectory_{0, 0, 0},
+      last_encoder_update_(core::timePointFromNanosSinceEpoch(0))
 {
 }
 
@@ -13,27 +19,34 @@ std::optional<core::Trajectory> NaiveNavigator::currentTrajectory()
   return current_trajectory_;
 }
 
-void NaiveNavigator::keyenceUpdate(const core::KeyenceData &keyence_data)
+void NaiveNavigator::keyenceUpdate(const core::RawKeyenceData &keyence_data)
 {
   // Do nothing. Keyence has no direct influence on trajectory
 }
 
-void NaiveNavigator::encoderUpdate(const core::EncoderData &encoder_data)
+void NaiveNavigator::encoderUpdate(const core::RawEncoderData &encoder_data)
 {
-  core::Float sum                  = std::accumulate(encoder_data.begin(), encoder_data.end(), 0.0);
-  core::Float encoder_average      = static_cast<core::Float>(sum / core::kNumEncoders);
+  core::Float sum = std::accumulate(encoder_data.value.begin(), encoder_data.value.end(), 0.0);
+  core::Float encoder_average = static_cast<core::Float>(sum / core::kNumEncoders);
+  // we assume that one revolution equals one metre
   current_trajectory_.displacement = encoder_average;
+  current_trajectory_.velocity
+    = encoder_average / ((last_encoder_update_ - encoder_data.measured_at) / core::kOneSecond);
 }
 
-void NaiveNavigator::accelerometerUpdate(const core::RawAccelerometerData &accelerometer_data)
+void NaiveNavigator::accelerometerUpdate(
+  const core::CombinedRawAccelerometerData &acceleration_data)
 {
   core::Float sum = 0.0;
   for (std::size_t i = 0; i < core::kNumAccelerometers; ++i) {
-    for (std::size_t j = 0; j < core::kNumAxis; ++j) {
-      sum += accelerometer_data.at(i).at(j);
-    }
+    const auto raw_acceleration = acceleration_data.value.at(i);
+    std::uint64_t magnitude     = 0;
+    magnitude += raw_acceleration.x * raw_acceleration.x;
+    magnitude += raw_acceleration.y * raw_acceleration.y;
+    magnitude += raw_acceleration.z * raw_acceleration.z;
+    sum += std::sqrt(magnitude);
   }
-  core::Float accelerometer_average = static_cast<core::Float>(sum / core::kNumAccelerometers);
+  core::Float accelerometer_average = sum / static_cast<core::Float>(core::kNumAccelerometers);
   current_trajectory_.acceleration  = accelerometer_average;
   // TODOLater: improve this...
   current_trajectory_.velocity = 0;
diff --git a/lib/utils/naive_navigator.hpp b/lib/utils/naive_navigator.hpp
index 94cb0806..6d510a16 100644
--- a/lib/utils/naive_navigator.hpp
+++ b/lib/utils/naive_navigator.hpp
@@ -1,7 +1,9 @@
 #pragma once
 
-#include "core/types.hpp"
-#include <navigation/navigator.hpp>
+#include <core/time.hpp>
+#include <core/timer.hpp>
+#include <core/types.hpp>
+#include <navigation/types.hpp>
 
 namespace hyped::utils {
 
@@ -9,12 +11,13 @@ class NaiveNavigator : public navigation::INavigator {
  public:
   NaiveNavigator();
   virtual std::optional<core::Trajectory> currentTrajectory();
-  virtual void keyenceUpdate(const core::KeyenceData &keyence_data);
-  virtual void encoderUpdate(const core::EncoderData &encoder_data);
-  virtual void accelerometerUpdate(const core::RawAccelerometerData &accelerometer_data);
+  virtual void keyenceUpdate(const core::RawKeyenceData &keyence_data);
+  virtual void encoderUpdate(const core::RawEncoderData &encoder_data);
+  virtual void accelerometerUpdate(const core::CombinedRawAccelerometerData &accelerometer_data);
 
  private:
   core::Trajectory current_trajectory_;
+  core::TimePoint last_encoder_update_;
 };
 
 }  // namespace hyped::utils
diff --git a/scripts/format b/scripts/format
new file mode 100755
index 00000000..62b97a71
--- /dev/null
+++ b/scripts/format
@@ -0,0 +1,3 @@
+#!/bin/sh
+clang-format -i $(find lib src test -type f | grep ".[ch]pp$")
+
diff --git a/scripts/hooks/pre-commit b/scripts/hooks/pre-commit
index f868ca8b..73ae227d 100755
--- a/scripts/hooks/pre-commit
+++ b/scripts/hooks/pre-commit
@@ -1,8 +1,8 @@
 #!/bin/sh
 
 FILES=$(git diff --cached --name-only | grep "*.[ch]pp$")
-clang-format -Werror --dry-run $FILES
+clang-format -Werror --dry-run $(find lib src test -type f | grep ".[ch]pp$")
 
 cmake -S . -B build
 cmake --build build -j
-ctest --test-dir build -j
\ No newline at end of file
+ctest --test-dir build -j
diff --git a/src/pod/main.cpp b/src/pod/main.cpp
index 45854a1c..78996783 100644
--- a/src/pod/main.cpp
+++ b/src/pod/main.cpp
@@ -10,20 +10,18 @@ int main(int argc, char **argv)
 {
   hyped::core::WallClock time;
   hyped::core::Timer timer(time);
-  const auto execution_time = timer.measure_execution_time([time]() {
-    hyped::core::Logger logger("GPIO", hyped::core::LogLevel::kDebug, time);
-    hyped::io::HardwareGpio gpio(logger);
-    auto gpio_reader_opt = gpio.getReader(0);
-    if (!gpio_reader_opt) {
-      logger.log(hyped::core::LogLevel::kFatal, "Error");
-      return;
-    }
+  hyped::core::Logger logger("GPIO", hyped::core::LogLevel::kDebug, time);
+  hyped::io::HardwareGpio gpio(logger);
+  auto gpio_reader_opt = gpio.getReader(0);
+  if (!gpio_reader_opt) {
+    logger.log(hyped::core::LogLevel::kFatal, "Error");
+  } else {
     auto gpio_reader = *gpio_reader_opt;
     if (gpio_reader->read() == hyped::core::DigitalSignal::kHigh) {
       logger.log(hyped::core::LogLevel::kInfo, "High");
     } else {
       logger.log(hyped::core::LogLevel::kInfo, "Low");
     };
-  });
-  std::cout << "Ran for " << execution_time.count() << " ns" << std::endl;
+  }
+  std::cout << "Ran for " << timer.elapsed().count() << " ns" << std::endl;
 }
diff --git a/test/core/logger.cpp b/test/core/logger.cpp
index 044ea28d..fe923fd3 100644
--- a/test/core/logger.cpp
+++ b/test/core/logger.cpp
@@ -48,13 +48,13 @@ TEST(Logger, Stderr)
 TEST(Logger, VaryingTimes)
 {
   utils::ManualTime manual_time;
-  manual_time.set_time(std::chrono::system_clock::from_time_t(3600));
+  manual_time.setTime(std::chrono::system_clock::from_time_t(3600));
   testStdoutLog(core::LogLevel::kDebug, manual_time, "02:00:00.000 DEBUG[test] test\n");
-  manual_time.set_time(std::chrono::system_clock::from_time_t(24 * 3600));
+  manual_time.setTime(std::chrono::system_clock::from_time_t(24 * 3600));
   testStdoutLog(core::LogLevel::kDebug, manual_time, "01:00:00.000 DEBUG[test] test\n");
-  manual_time.set_time(std::chrono::system_clock::from_time_t(60));
+  manual_time.setTime(std::chrono::system_clock::from_time_t(60));
   testStdoutLog(core::LogLevel::kDebug, manual_time, "01:01:00.000 DEBUG[test] test\n");
-  manual_time.set_time(std::chrono::system_clock::from_time_t(1));
+  manual_time.setTime(std::chrono::system_clock::from_time_t(1));
   testStdoutLog(core::LogLevel::kDebug, manual_time, "01:00:01.000 DEBUG[test] test\n");
 }
 
@@ -82,7 +82,7 @@ TEST(Logger, logEveryNth)
   core::Logger logger("test", core::LogLevel::kDebug, manual_time);
   for (std::size_t i = 0; i < 15; ++i) {
     logEveryNth(logger, 5, core::LogLevel::kDebug, "test");
-    manual_time.set_time(std::chrono::system_clock::from_time_t(3600 * i));
+    manual_time.setTime(std::chrono::system_clock::from_time_t(3600 * i));
   }
   ASSERT_EQ(testing::internal::GetCapturedStdout(),
             "01:00:00.000 DEBUG[test] test\n05:00:00.000 DEBUG[test] test\n10:00:00.000 "
diff --git a/test/navigation/CMakeLists.txt b/test/navigation/CMakeLists.txt
index a22c0ba2..744623d7 100644
--- a/test/navigation/CMakeLists.txt
+++ b/test/navigation/CMakeLists.txt
@@ -13,3 +13,5 @@ target_link_libraries(${target}
     hyped_utils
 )
 gtest_discover_tests(${target})
+
+add_subdirectory(benchmark)
diff --git a/test/navigation/accelerometer_preprocessing.cpp b/test/navigation/accelerometer_preprocessing.cpp
index aa83c7c5..cafc472a 100644
--- a/test/navigation/accelerometer_preprocessing.cpp
+++ b/test/navigation/accelerometer_preprocessing.cpp
@@ -2,6 +2,7 @@
 
 #include <gtest/gtest.h>
 
+#include "navigation/types.hpp"
 #include <core/logger.hpp>
 #include <core/types.hpp>
 #include <navigation/preprocess_accelerometer.hpp>
@@ -10,8 +11,8 @@
 namespace hyped::test {
 
 core::Float epsilon = 1e-5;
-bool checkArrayEquality(const core::AccelerometerData &data_a,
-                        const core::AccelerometerData &data_b)
+bool checkArrayEquality(const std::array<core::Float, core::kNumAccelerometers> &data_a,
+                        const std::array<core::Float, core::kNumAccelerometers> &data_b)
 {
   if (data_a.size() != data_b.size()) { return false; }
   for (std::size_t i; i < data_a.size(); ++i) {
@@ -25,10 +26,15 @@ TEST(Accelerometer, equal_data)
   utils::ManualTime manual_time;
   core::Logger logger("test", core::LogLevel::kFatal, manual_time);
   navigation::AccelerometerPreprocessor accelerometer_processer(logger);
-  const core::RawAccelerometerData data = {{1, 1, 1}};
-  const core::AccelerometerData answer  = {static_cast<core::Float>(std::sqrt(3.0))};
-  const auto final_data                 = accelerometer_processer.processData(data);
-  ASSERT_TRUE(checkArrayEquality(*final_data, answer));
+  const core::CombinedRawAccelerometerData data(manual_time.now(),
+                                                {{{.x = 1, .y = 0, .z = 0},
+                                                  {.x = 1, .y = 0, .z = 0},
+                                                  {.x = 1, .y = 0, .z = 0},
+                                                  {.x = 1, .y = 0, .z = 0}}});
+  const navigation::AccelerometerData expected(manual_time.now(), {1.0, 1.0, 1.0, 1.0});
+  const auto calculated = accelerometer_processer.processData(data);
+  ASSERT_EQ(calculated->measured_at, expected.measured_at);
+  ASSERT_TRUE(checkArrayEquality(calculated->value, expected.value));
 }
 
 TEST(Accelerometer, not_equal_data)
@@ -36,10 +42,15 @@ TEST(Accelerometer, not_equal_data)
   utils::ManualTime manual_time;
   core::Logger logger("test", core::LogLevel::kFatal, manual_time);
   navigation::AccelerometerPreprocessor accelerometer_processer(logger);
-  const core::RawAccelerometerData data = {{{3, 5, 6}, {1, 1, 1}, {1, 1, 1}, {1, 1, 1}}};
-  const core::AccelerometerData answer  = {static_cast<core::Float>(std::sqrt(3.0))};
-  const auto final_data                 = accelerometer_processer.processData(data);
-  ASSERT_TRUE(checkArrayEquality(*final_data, answer));
+  const core::CombinedRawAccelerometerData data(manual_time.now(),
+                                                {{{.x = 3, .y = 5, .z = 6},
+                                                  {.x = 1, .y = 0, .z = 0},
+                                                  {.x = 1, .y = 0, .z = 0},
+                                                  {.x = 1, .y = 0, .z = 0}}});
+  const navigation::AccelerometerData expected(manual_time.now(), {1.0, 1.0, 1.0, 1.0});
+  const auto calculated = accelerometer_processer.processData(data);
+  ASSERT_EQ(calculated->measured_at, expected.measured_at);
+  ASSERT_TRUE(checkArrayEquality(calculated->value, expected.value));
 }
 
 TEST(Accelerometer, one_unreliable_sensor)
@@ -47,12 +58,19 @@ TEST(Accelerometer, one_unreliable_sensor)
   utils::ManualTime manual_time;
   core::Logger logger("test", core::LogLevel::kFatal, manual_time);
   navigation::AccelerometerPreprocessor accelerometer_processer(logger);
-  const core::RawAccelerometerData data = {{{3, 5, 6}, {1, 1, 1}, {1, 1, 1}, {1, 1, 1}}};
-  for (std::size_t i; i < 22; ++i) {
-    accelerometer_processer.processData(data);
+  const navigation::AccelerometerData expected(manual_time.now(), {1.0, 1.0, 1.0, 1.0});
+  const std::array<core::RawAcceleration, core::kNumAccelerometers> data
+    = {{{.x = 3, .y = 5, .z = 6},
+        {.x = 1, .y = 0, .z = 0},
+        {.x = 1, .y = 0, .z = 0},
+        {.x = 1, .y = 0, .z = 0}}};
+  for (std::size_t i = 0; i < 100; ++i) {
+    manual_time.addSeconds(1);
+    const auto calculated = accelerometer_processer.processData(
+      core::CombinedRawAccelerometerData(manual_time.now(), data));
+    ASSERT_EQ(calculated->measured_at, expected.measured_at);
+    ASSERT_TRUE(checkArrayEquality(calculated->value, expected.value));
   }
-  const core::AccelerometerData answer = {static_cast<core::Float>(std::sqrt(3.0))};
-  const auto final_data                = accelerometer_processer.processData(data);
-  ASSERT_TRUE(checkArrayEquality(*final_data, answer));
 }
-}  // namespace hyped::test
\ No newline at end of file
+
+}  // namespace hyped::test
diff --git a/test/navigation/benchmark/CMakeLists.txt b/test/navigation/benchmark/CMakeLists.txt
new file mode 100644
index 00000000..319b1681
--- /dev/null
+++ b/test/navigation/benchmark/CMakeLists.txt
@@ -0,0 +1,16 @@
+set(target hyped_test_navgation_benchmark)
+file(GLOB code "${CMAKE_CURRENT_SOURCE_DIR}/*.cpp")
+add_executable(${target} ${code})
+target_include_directories(${target}
+    PRIVATE
+    "${CMAKE_SOURCE_DIR}/lib"
+    "${CMAKE_CURRENT_SOURCE_DIR}"
+)
+target_link_libraries(${target}
+    GTest::gtest_main
+    hyped_navigation_benchmark
+    hyped_navigation
+    hyped_utils
+    hyped_core
+)
+gtest_discover_tests(${target})
diff --git a/test/navigation/benchmark/benchmark.cpp b/test/navigation/benchmark/benchmark.cpp
new file mode 100644
index 00000000..a2fe2d32
--- /dev/null
+++ b/test/navigation/benchmark/benchmark.cpp
@@ -0,0 +1,52 @@
+#include <gtest/gtest.h>
+
+#include <core/wall_clock.hpp>
+#include <navigation/benchmark/benchmark.hpp>
+#include <navigation/benchmark/data.hpp>
+#include <utils/naive_navigator.hpp>
+
+namespace hyped::test {
+
+constexpr std::uint64_t kNanosPerSecond = 1'000'000'000;
+
+navigation::benchmark::Data makeEmptyData()
+{
+  navigation::benchmark::DataBuilder builder;
+  return builder.build();
+}
+
+TEST(Benchmark, construction)
+{
+  core::WallClock wall_clock;
+  navigation::benchmark::Benchmark benchmark(wall_clock, makeEmptyData());
+  utils::ManualTime manual_time;
+  utils::NaiveNavigator naive_navigator;
+  benchmark.run(manual_time, naive_navigator);
+}
+
+navigation::benchmark::Data makeDataFromConstantAcceleration(const core::Float acceleration,
+                                                             const std::size_t num_steps)
+{
+  navigation::benchmark::DataBuilder builder;
+  for (std::size_t t = 1; t <= num_steps; ++t) {
+    const core::Trajectory trajectory
+      = {0.5f * acceleration * acceleration * t * t, acceleration * t, acceleration};
+    builder.addUniformAccelerationData(t * kNanosPerSecond, {.x = 1, .y = 0, .z = 0});
+    builder.addUniformEncoderData(t * kNanosPerSecond,
+                                  static_cast<std::uint32_t>(trajectory.displacement));
+    builder.addTrajectoryData(t * kNanosPerSecond, trajectory);
+  }
+  return builder.build();
+}
+
+TEST(Benchmark, naiveNavigation)
+{
+  core::WallClock wall_clock;
+  const auto data = makeDataFromConstantAcceleration(1.0, 10);
+  navigation::benchmark::Benchmark benchmark(wall_clock, data);
+  utils::ManualTime manual_time;
+  utils::NaiveNavigator naive_navigator;
+  const auto result = benchmark.run(manual_time, naive_navigator);
+}
+
+}  // namespace hyped::test
diff --git a/test/sensors/mux.cpp b/test/sensors/mux.cpp
index f4c159ad..8b9fba09 100644
--- a/test/sensors/mux.cpp
+++ b/test/sensors/mux.cpp
@@ -1,6 +1,7 @@
 #include <gtest/gtest.h>
 
 #include <core/logger.hpp>
+#include <core/wall_clock.hpp>
 #include <sensors/mux.hpp>
 #include <utils/dummy_i2c.hpp>
 #include <utils/dummy_i2c_sensor.hpp>
@@ -11,9 +12,10 @@ namespace hyped::test {
 TEST(Mux, construction)
 {
   static constexpr std::uint8_t kSize = 8;
+  core::WallClock wall_clock;
   std::array<std::unique_ptr<sensors::II2cMuxSensor<std::uint8_t>>, kSize> sensors;
   for (auto &sensor : sensors) {
-    sensor = std::make_unique<utils::DummyI2cSensor>();
+    sensor = std::make_unique<utils::DummyI2cSensor>(wall_clock);
   }
   utils::DummyI2c i2c;
   utils::DummyLogger logger;
diff --git a/test/utils/CMakeLists.txt b/test/utils/CMakeLists.txt
index e882fab8..cf75a13c 100644
--- a/test/utils/CMakeLists.txt
+++ b/test/utils/CMakeLists.txt
@@ -9,5 +9,6 @@ target_include_directories(${target}
 target_link_libraries(${target}
     GTest::gtest_main
     hyped_utils
+    hyped_core
 )
-gtest_discover_tests(${target})
\ No newline at end of file
+gtest_discover_tests(${target})
diff --git a/test/utils/dummy_i2c_sensor.cpp b/test/utils/dummy_i2c_sensor.cpp
index 0b4346d2..f334638e 100644
--- a/test/utils/dummy_i2c_sensor.cpp
+++ b/test/utils/dummy_i2c_sensor.cpp
@@ -2,13 +2,15 @@
 
 #include <gtest/gtest.h>
 
+#include <utils/manual_time.hpp>
 #include <utils/dummy_i2c_sensor.hpp>
 
 namespace hyped::test {
 
 TEST(DummyI2cSensor, construction)
 {
-  utils::DummyI2cSensor dummy_i2c_sensor;
+  utils::ManualTime manual_time;
+  utils::DummyI2cSensor dummy_i2c_sensor(manual_time);
 }
 
 }  // namespace hyped::test
diff --git a/test/utils/manual_time.cpp b/test/utils/manual_time.cpp
index 47aae9f2..7e404cd2 100644
--- a/test/utils/manual_time.cpp
+++ b/test/utils/manual_time.cpp
@@ -4,26 +4,26 @@
 
 namespace hyped::test {
 
-void test_set_time(utils::ManualTime &manual_time, const std::time_t time)
+void testSetTime(utils::ManualTime &manual_time, const std::time_t time)
 {
   const auto time_point = std::chrono::system_clock::from_time_t(time);
-  manual_time.set_time(time_point);
+  manual_time.setTime(time_point);
   ASSERT_EQ(manual_time.now(), time_point);
 }
 
-TEST(ManualTime, set_time)
+TEST(ManualTime, setTime)
 {
   utils::ManualTime manual_time;
-  test_set_time(manual_time, 0);
-  test_set_time(manual_time, 1000);
-  test_set_time(manual_time, 100);
-  test_set_time(manual_time, 2000);
+  testSetTime(manual_time, 0);
+  testSetTime(manual_time, 1000);
+  testSetTime(manual_time, 100);
+  testSetTime(manual_time, 2000);
 }
 
-void test_set_seconds_since_epoch(utils::ManualTime &manual_time,
-                                  const std::uint64_t seconds_since_epoch)
+void testSetSecondsSinceEpoch(utils::ManualTime &manual_time,
+                              const std::uint64_t seconds_since_epoch)
 {
-  manual_time.set_seconds_since_epoch(seconds_since_epoch);
+  manual_time.setSecondsSinceEpoch(seconds_since_epoch);
   ASSERT_EQ(
     std::chrono::duration_cast<std::chrono::seconds>(manual_time.now().time_since_epoch()).count(),
     seconds_since_epoch);
@@ -32,10 +32,10 @@ void test_set_seconds_since_epoch(utils::ManualTime &manual_time,
 TEST(ManualTime, basic)
 {
   utils::ManualTime manual_time;
-  test_set_seconds_since_epoch(manual_time, 0);
-  test_set_seconds_since_epoch(manual_time, 1000);
-  test_set_seconds_since_epoch(manual_time, 100);
-  test_set_seconds_since_epoch(manual_time, 2000);
+  testSetSecondsSinceEpoch(manual_time, 0);
+  testSetSecondsSinceEpoch(manual_time, 1000);
+  testSetSecondsSinceEpoch(manual_time, 100);
+  testSetSecondsSinceEpoch(manual_time, 2000);
 }
 
 }  // namespace hyped::test
diff --git a/test/utils/naive_navigator.cpp b/test/utils/naive_navigator.cpp
index 9c9bd3c8..6077ae67 100644
--- a/test/utils/naive_navigator.cpp
+++ b/test/utils/naive_navigator.cpp
@@ -3,52 +3,56 @@
 #include <gtest/gtest.h>
 
 #include <core/types.hpp>
+#include <utils/manual_time.hpp>
 #include <utils/naive_navigator.hpp>
 
 namespace hyped::test {
 
 // TODOLater: improve testing method here!
-void testWithTrajectory(utils::NaiveNavigator &naive_navigator,
-                        const core::RawAccelerometerData accelerometer_data,
-                        const core::EncoderData encoder_data,
-                        const core::KeyenceData keyence_data)
+void testWithTrajectory(
+  utils::NaiveNavigator &naive_navigator,
+  core::ITimeSource &time_source,
+  const std::array<core::RawAcceleration, core::kNumAccelerometers> &acceleration_data,
+  const std::array<std::uint32_t, core::kNumEncoders> &encoder_data,
+  const std::array<std::uint32_t, core::kNumKeyence> &keyence_data,
+  const core::Trajectory &expected_trajectory)
 {
-  naive_navigator.accelerometerUpdate(accelerometer_data);
-  naive_navigator.encoderUpdate(encoder_data);
-  naive_navigator.keyenceUpdate(keyence_data);
-
-  core::Float sum_accelerometer = 0;
-  for (std::size_t i = 0; i < core::kNumAccelerometers; ++i) {
-    for (std::size_t j = 0; j < core::kNumAxis; ++j) {
-      sum_accelerometer += accelerometer_data.at(i).at(j);
-    }
-  }
-  core::Float new_acceleration = sum_accelerometer / core::kNumAccelerometers;
-  core::Float new_velocity     = 0;
-  core::Float new_displacement
-    = std::accumulate(encoder_data.begin(), encoder_data.end(), 0.0) / core::kNumEncoders;
-
+  naive_navigator.accelerometerUpdate(
+    core::CombinedRawAccelerometerData(time_source.now(), acceleration_data));
+  naive_navigator.encoderUpdate(core::RawEncoderData(time_source.now(), encoder_data));
+  naive_navigator.keyenceUpdate(core::RawKeyenceData(time_source.now(), keyence_data));
   const auto &current_trajectory = naive_navigator.currentTrajectory();
-  if (current_trajectory.has_value()) {
-    ASSERT_FLOAT_EQ(current_trajectory.value().acceleration, new_acceleration);
-    ASSERT_FLOAT_EQ(current_trajectory.value().velocity, new_velocity);
-    ASSERT_FLOAT_EQ(current_trajectory.value().displacement, new_displacement);
+  if (current_trajectory) {
+    ASSERT_FLOAT_EQ(current_trajectory->displacement, expected_trajectory.displacement);
+    ASSERT_FLOAT_EQ(current_trajectory->velocity, expected_trajectory.velocity);
+    ASSERT_FLOAT_EQ(current_trajectory->acceleration, expected_trajectory.acceleration);
   }
 }
 
 TEST(NaiveNavigator, basic)
 {
+  utils::ManualTime manual_time;
   utils::NaiveNavigator naive_navigator;
+  manual_time.addSeconds(1);
   testWithTrajectory(naive_navigator,
-                     {{{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}}},
+                     manual_time,
+                     {{{.x = 0, .y = 0, .z = 0},
+                       {.x = 0, .y = 0, .z = 0},
+                       {.x = 0, .y = 0, .z = 0},
+                       {.x = 0, .y = 0, .z = 0}}},
                      {0, 0, 0, 0},
-                     {0, 0});
-  testWithTrajectory(
-    naive_navigator, {{{1, 2, 3}, {4, 5, 6}, {7, 8, 9}, {10, 11, 12}}}, {1, 2, 3, 4}, {1, 2});
-  testWithTrajectory(
-    naive_navigator, {{{10, 5, 1}, {0, 100, 3}, {2, 3, 2}, {50, 1, 200}}}, {20, 30, 1, 5}, {30, 2});
-  testWithTrajectory(
-    naive_navigator, {{{1, 1, 1}, {1, 1, 1}, {1, 1, 1}, {1, 1, 1}}}, {1, 1, 1, 1}, {1, 1});
+                     {0, 0},
+                     {0, 0, 0});
+  manual_time.addSeconds(1);
+  testWithTrajectory(naive_navigator,
+                     manual_time,
+                     {{{.x = 1, .y = 0, .z = 0},
+                       {.x = 1, .y = 0, .z = 0},
+                       {.x = 1, .y = 0, .z = 0},
+                       {.x = 1, .y = 0, .z = 0}}},
+                     {1, 1, 1, 1},
+                     {0, 0},
+                     {1, 1, 1});
 }
 
 }  // namespace hyped::test