diff --git a/src/controller.h b/src/controller.h
index 346b59d1..aaa31985 100644
--- a/src/controller.h
+++ b/src/controller.h
@@ -1924,7 +1924,7 @@ Controller<Renderer>::_calibrate_client()
 #if defined(ENABLE_INTERSENSE) || defined(ENABLE_POLHEMUS) || defined(ENABLE_VRPN) || defined(ENABLE_RAZOR)
   if (_tracker)
   {
-    _tracker->calibrate();
+    _tracker->reset();
   }
   else
   {
diff --git a/src/geometry.h b/src/geometry.h
index 0a60f391..d0e71cbf 100644
--- a/src/geometry.h
+++ b/src/geometry.h
@@ -66,6 +66,14 @@ struct quat : gml::quat
   }
 };
 
+// Converting from yaw (around Z), pitch (around Y), roll (around X) to quaternions.
+// This a lot worse than the other way around and should be avoided.
+template <typename T>
+ssr::quat ypr2quaternion(T yaw, T pitch, T roll)
+{
+  return gml::qrotate(gml::vec3{roll, pitch, yaw});
+}
+
 /// Build a unit quaternion representing the rotation
 /// from u to v. The input vectors need not be normalised.
 /// From http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final
diff --git a/src/gui/qopenglplotter.cpp b/src/gui/qopenglplotter.cpp
index ad8d84e4..76b75a35 100644
--- a/src/gui/qopenglplotter.cpp
+++ b/src/gui/qopenglplotter.cpp
@@ -382,8 +382,10 @@ void ssr::QOpenGLPlotter::_draw_reference()
 
     glTranslatef(0.03f, -0.03f, 0.0f);
 
-    // rotate according to reference position
-    glRotatef(_scene.get_reference().orientation.azimuth, 0.0f, 0.0f, 1.0f);
+    // rotate according to reference offset position
+    glRotatef(_scene.get_reference().orientation.azimuth +
+              _scene.get_reference_offset().orientation.azimuth,
+              0.0f, 0.0f, 1.0f);
 
     glBindTexture(GL_TEXTURE_2D, _listener_shadow_texture);
 
@@ -396,8 +398,10 @@ void ssr::QOpenGLPlotter::_draw_reference()
 
     glPopMatrix();
 
-    // rotate according to reference position
-    glRotatef(_scene.get_reference().orientation.azimuth, 0.0f, 0.0f, 1.0f);
+    // rotate according to reference offset position
+    glRotatef(_scene.get_reference().orientation.azimuth +
+              _scene.get_reference_offset().orientation.azimuth,
+              0.0f, 0.0f, 1.0f);
 
     glBindTexture(GL_TEXTURE_2D, _listener_texture);
 
diff --git a/src/gui/quserinterface.cpp b/src/gui/quserinterface.cpp
index 43b1155e..a5f7c881 100644
--- a/src/gui/quserinterface.cpp
+++ b/src/gui/quserinterface.cpp
@@ -980,8 +980,8 @@ void ssr::QUserInterface::mouseMoveEvent(QMouseEvent *event)
     // absolut mouse position in OpenGL coordinates
     Position mouse_pos(static_cast<float>(pos_x), static_cast<float>(pos_y));
 
-    // position relative to reference position
-    mouse_pos -= _scene.get_reference().position;
+    // position relative to reference position offset
+    mouse_pos -= _scene.get_reference_offset().position;
 
     _get_openGL_pos(_previous_mouse_event.x(),
                     _previous_mouse_event.y(),
@@ -990,11 +990,12 @@ void ssr::QUserInterface::mouseMoveEvent(QMouseEvent *event)
     // previous absolut position in OpenGL coordinates
     Position prev_mouse_pos(static_cast<float>(pos_x), static_cast<float>(pos_y));
 
-    // previous position relative to relative position
-    prev_mouse_pos -= _scene.get_reference().position;
+    // previous position relative to relative position offset
+    prev_mouse_pos -= _scene.get_reference_offset().position;
 
-    _controller.take_control()->reference_rotation(_scene.get_reference().orientation +
-                                          (mouse_pos.orientation() - prev_mouse_pos.orientation()));
+    _controller.take_control()->reference_rotation_offset(_scene.get_reference().orientation +
+                                                          _scene.get_reference_offset().orientation +
+                                                          (mouse_pos.orientation() - prev_mouse_pos.orientation()));
 
   } // else if
 
diff --git a/src/razor-ahrs/RazorAHRS.h b/src/razor-ahrs/RazorAHRS.h
index 5a59d021..6764dd33 100644
--- a/src/razor-ahrs/RazorAHRS.h
+++ b/src/razor-ahrs/RazorAHRS.h
@@ -131,11 +131,9 @@ class RazorAHRS
 
     // thread related stuff
     std::thread _tracker_thread;
-
     std::atomic<bool> _stop_thread; // thread stop flag
     void _start(); ///< start the tracking thread
     void _stop();  ///< stop the tracking thread
-
     void _thread();  // thread main function
 };
 
diff --git a/src/tracker.h b/src/tracker.h
index 51748fa2..1177da91 100644
--- a/src/tracker.h
+++ b/src/tracker.h
@@ -30,13 +30,46 @@
 #ifndef SSR_TRACKER_H
 #define SSR_TRACKER_H
 
+#include <atomic>
+#include <thread>
+
+#include "geometry.h"
+
+
+namespace ssr
+{
+
 /// Class definition
-struct Tracker
+class Tracker
 {
-  virtual ~Tracker() = default;  ///< destructor
+  public:
+    Tracker(api::Publisher& controller) : _controller(controller){};  ///< constructor
 
-  /// calibrate tracker; set the instantaneous position to be the reference
-  virtual void calibrate() = 0;
+    virtual ~Tracker() = default;  ///< destructor
+
+    /// reset tracker; set the instantaneous position to be the reference
+    virtual void reset()
+    {
+      SSR_VERBOSE2("Tracker reset.");
+      this->_correction_rot = this->_current_rot;
+    }
+
+    // Update SSR
+    virtual void update()
+    {
+      ssr::quat r = inverse(_correction_rot) * inverse(_current_rot);
+      _controller.take_control()->reference_rotation_offset(r);
+    }
+
+    protected:
+      // Current tracker data (should be atomic)
+      ssr::quat _current_rot;
+      ssr::quat _correction_rot;
+
+    private:
+      api::Publisher& _controller;
 };
 
+}  // namespace ssr
+
 #endif
diff --git a/src/trackerpolhemus.cpp b/src/trackerpolhemus.cpp
index c2bb060d..4c232c49 100644
--- a/src/trackerpolhemus.cpp
+++ b/src/trackerpolhemus.cpp
@@ -38,16 +38,13 @@
 #include "api.h"  // for Publisher
 #include "legacy_orientation.h"  // for Orientation
 #include "trackerpolhemus.h"
-#include "ssr_global.h"
 #include "apf/stringtools.h"
 
 using apf::str::A2S;
 
 ssr::TrackerPolhemus::TrackerPolhemus(api::Publisher& controller
     , const std::string& type, const std::string& ports)
-  : Tracker()
-  , _controller(controller)
-  , _az_corr(0.0f)
+  : Tracker(controller)
   , _stop_thread(false)
 {
   if (ports == "")
@@ -141,7 +138,7 @@ ssr::TrackerPolhemus::TrackerPolhemus(api::Publisher& controller
   // wait until tracker has started
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  this->calibrate();
+  Tracker::reset();
 }
 
 ssr::TrackerPolhemus::~TrackerPolhemus()
@@ -189,12 +186,6 @@ ssr::TrackerPolhemus::_open_serial_port(const char *portname)
   return -1;
 }
 
-void
-ssr::TrackerPolhemus::calibrate()
-{
-  _az_corr = _current_data.azimuth;
-}
-
 void
 ssr::TrackerPolhemus::_start()
 {
@@ -225,6 +216,17 @@ ssr::TrackerPolhemus::_thread()
   fds.fd = _tracker_port;
   fds.events = POLLRDNORM;
 
+  struct
+    {
+      float header{};
+      float x{};
+      float y{};
+      float z{};
+      float azimuth{};
+      float elevation{};
+      float roll{};
+    } _current_data{} ;
+
   while (!_stop_thread)
   {
     c = 0;
@@ -291,7 +293,12 @@ ssr::TrackerPolhemus::_thread()
               >> _current_data.elevation
               >> _current_data.roll;
 
-    _controller.take_control()->reference_rotation_offset(
-        Orientation(-_current_data.azimuth + _az_corr));
+    // Write back to tracker_data
+    Tracker::_current_rot = ypr2quaternion(_current_data.azimuth,
+                                           _current_data.elevation,
+                                           _current_data.roll);
+
+    // Push updates to SSR
+    Tracker::update();
   };
 }
diff --git a/src/trackerpolhemus.h b/src/trackerpolhemus.h
index 507b6853..28b19162 100644
--- a/src/trackerpolhemus.h
+++ b/src/trackerpolhemus.h
@@ -36,7 +36,9 @@
 #include <memory>
 #include <stdexcept> // for std::runtime_error
 
-#include "tracker.h"
+#include "ssr_global.h"  // for ERROR, VERBOSE
+#include "tracker.h"  // base class
+
 
 namespace ssr
 {
@@ -55,48 +57,21 @@ class TrackerPolhemus : public Tracker
     static ptr_t create(api::Publisher& controller, const std::string& type
         , const std::string& ports);
 
-    virtual void calibrate();
-
   private:
     /// constructor
     TrackerPolhemus(api::Publisher& controller, const std::string& type
         , const std::string& ports);
 
-    struct tracker_data_t
-    {
-      float header;
-      float x;
-      float y;
-      float z;
-      float azimuth;
-      float elevation;
-      float roll;
-
-      // contructor
-      tracker_data_t()
-        : header(0.0f), x(0.0f), y(0.0f), z(0.0f)
-        , azimuth(0.0f), elevation(0.0f), roll(0.0f)
-      {}
-    };
-
-    api::Publisher& _controller;
-
-    tracker_data_t _current_data;
-
     int _tracker_port;
-    int _open_serial_port(const char *portname);
-
-    float _az_corr; ///< correction of the azimuth due to calibration
 
+    int _open_serial_port(const char *portname);
     std::string::size_type _line_size;
 
     // thread related stuff
     std::thread _tracker_thread;
-
     std::atomic<bool> _stop_thread; // thread stop flag
     void _start(); ///< start the tracking thread
     void _stop();  ///< stop the tracking thread
-
     void _thread();  // thread main function
 };
 
diff --git a/src/trackerrazor.cpp b/src/trackerrazor.cpp
index 96b024de..fce55654 100644
--- a/src/trackerrazor.cpp
+++ b/src/trackerrazor.cpp
@@ -32,11 +32,7 @@
 
 ssr::TrackerRazor::TrackerRazor(api::Publisher& controller
     , const std::string& ports)
-  : Tracker()
-  , _controller(controller)
-  , _current_azimuth(0.0f)
-  , _az_corr(90.0f)
-  , _init_az_corr(true)
+  : Tracker(controller)
   , _tracker(nullptr)
 {
   if (ports == "")
@@ -72,6 +68,11 @@ ssr::TrackerRazor::TrackerRazor(api::Publisher& controller
   {
     throw std::runtime_error("Could not open serial port!");
   }
+
+  // wait until tracker has started
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+  Tracker::reset();
 }
 
 ssr::TrackerRazor::ptr_t
diff --git a/src/trackerrazor.h b/src/trackerrazor.h
index b1f40a14..fba2e3ab 100644
--- a/src/trackerrazor.h
+++ b/src/trackerrazor.h
@@ -32,8 +32,9 @@
 #define SSR_TRACKERRAZOR_H
 
 #include "legacy_orientation.h"  // for Orientation
-#include "ssr_global.h"  // for ERROR
+#include "ssr_global.h"  // for ERROR, VERBOSE
 #include "tracker.h"  // base class
+#include "apf/math.h"  // for deg2rad()
 
 #include "razor-ahrs/RazorAHRS.h"
 
@@ -57,33 +58,23 @@ class TrackerRazor : public Tracker
       if (_tracker != nullptr) delete _tracker;
     }
 
-  virtual void calibrate() { _az_corr = _current_azimuth; }
-
   private:
     /// constructor
     TrackerRazor(api::Publisher& controller, const std::string& ports);
 
+    RazorAHRS* _tracker;
+
     /// Razor AHRS callback functions
     void on_data(const float ypr[])
     {
-      // TODO: get 3D rotation
-      _current_azimuth = ypr[0];
-      if (_init_az_corr)
-      {
-        calibrate();
-        _init_az_corr = false;
-      }
-      _controller.take_control()->reference_rotation_offset(
-          Orientation(-_current_azimuth + _az_corr));
+      Tracker::_current_rot = ypr2quaternion(apf::math::deg2rad(ypr[0]),
+                                             apf::math::deg2rad(ypr[1]),
+                                             apf::math::deg2rad(ypr[2]));
+      // Push updates to SSR
+      Tracker::update();
     }
     void on_error(const std::string &msg) { SSR_ERROR("Razor AHRS: " << msg); }
 
-    api::Publisher& _controller;
-    volatile float _current_azimuth;
-    volatile float _az_corr;
-    volatile bool _init_az_corr;
-
-    RazorAHRS* _tracker;
 };
 
 }  // namespace ssr
diff --git a/src/trackervrpn.cpp b/src/trackervrpn.cpp
index ff982b26..8030759b 100644
--- a/src/trackervrpn.cpp
+++ b/src/trackervrpn.cpp
@@ -32,15 +32,17 @@
 #include <stdexcept>  // for runtime_error
 #include <cmath>  // for std::atan2()
 
+#include "ssr_global.h"
 #include "api.h"  // for Publisher
 #include "legacy_orientation.h"  // for Orientation
-#include "ssr_global.h"
+#include "apf/math.h"  // for rad2deg()
+
 
 ssr::TrackerVrpn::TrackerVrpn(api::Publisher& controller
     , const std::string& address)
   : vrpn_Tracker_Remote(address.c_str())
   , _controller(controller)
-  , _az_corr(0.0f)
+  , _current_azimuth(0.0)
   , _stop_thread(false)
 {
   SSR_VERBOSE("Starting VRPN tracker \"" << address << "\"");
@@ -48,8 +50,10 @@ ssr::TrackerVrpn::TrackerVrpn(api::Publisher& controller
   // TODO: what exactly is this supposed to do?
   //this->set_update_rate(120);
 
-  this->register_change_handler(this, _vrpn_change_handler);
+  // register vrpn callback
+  this->vrpn_Tracker_Remote::register_change_handler(&Tracker::current_data, this->handle_tracker);
 
+  // start thread
   _start();
 
   // wait until tracker has started
@@ -60,6 +64,8 @@ ssr::TrackerVrpn::TrackerVrpn(api::Publisher& controller
 
 ssr::TrackerVrpn::~TrackerVrpn()
 {
+  // Probably not absolutely necessary
+  this->vrpn_Tracker_Remote::unregister_change_handler(&Tracker::current_data, this->handle_tracker);
   // stop thread
   _stop();
   // Release any ports?
@@ -79,37 +85,42 @@ ssr::TrackerVrpn::create(api::Publisher& controller, const std::string& ports)
   }
   return temp;
 }
-void VRPN_CALLBACK
-ssr::TrackerVrpn::_vrpn_change_handler(void* arg, const vrpn_TRACKERCB t)
-{
-  return static_cast<TrackerVrpn*>(arg)->vrpn_change_handler(t);
-}
 
-void
-ssr::TrackerVrpn::vrpn_change_handler(const vrpn_TRACKERCB t)
+void VRPN_CALLBACK
+ssr::TrackerVrpn::handle_tracker(void *userdata, const vrpn_TRACKERCB t)
 {
-  // TODO: check t.sensor for sensor number!
-
-  // get quaternions information
-  double w = t.quat[0];
-  double x = t.quat[1];
-  double y = t.quat[2];
-  double z = t.quat[3];
+  //this function gets called when the tracker's POSITION xform is updated
 
-  // TODO: store _az_corr as quaternion and directly set 3D rotation
+  ssr::Tracker::Tracker_data *_data = reinterpret_cast<ssr::Tracker::Tracker_data*>(userdata);
 
-  // calculate yaw (azimuth) (in radians) from quaternions
-  double azi = std::atan2(2*(w*x+y*z),1-2*(x*x+y*y));
+  // https://github.com/vrpn/vrpn/wiki/Client-side-VRPN-Devices#type-definitions
+  double x = t.quat[0];
+  double y = t.quat[1];
+  double z = t.quat[2];
+  double w = t.quat[3];
 
-  _current_azimuth = azi;
-  _controller.take_control()->reference_rotation_offset(
-      Orientation(-azi + _az_corr));
+  // write back to tracker_data
+  _data->x = x;
+  _data->y = y;
+  _data->z = z;
+  _data->w = w;
 }
 
 void
-ssr::TrackerVrpn::calibrate()
+ssr::TrackerVrpn::update(const Tracker::Tracker_data& _data)
 {
-  _az_corr = _current_azimuth;
+  double x = _data.x;
+  double y = _data.y;
+  double z = _data.z;
+  double w = _data.w;
+
+  // yaw (z-axis rotation), from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+  double yaw = std::atan2(2.0f * (w * z + x * y),
+                          1.0f - 2.0f * (y * y + z * z));
+  _current_azimuth = apf::math::rad2deg(yaw);
+
+  _controller.take_control()->reference_rotation_offset(
+      Orientation(-_current_azimuth + Tracker::azi_correction));
 }
 
 void
@@ -136,8 +147,10 @@ ssr::TrackerVrpn::_thread()
 {
   while (!_stop_thread)
   {
-    this->mainloop();
-
+    // This calls the callback
+    this->vrpn_Tracker_Remote::mainloop();
+    // Push updates to SSR
+    this->update(*Tracker::get_tracker_data());
     // TODO: make this configurable:
     vrpn_SleepMsecs(10);
   };
diff --git a/src/trackervrpn.h b/src/trackervrpn.h
index 8fa977ce..17401baf 100644
--- a/src/trackervrpn.h
+++ b/src/trackervrpn.h
@@ -37,7 +37,9 @@
 
 #include <vrpn_Tracker.h>
 
-#include "tracker.h"
+#include "ssr_global.h"  // for ERROR, VERBOSE
+#include "tracker.h"  // base class
+
 
 namespace ssr
 {
@@ -45,7 +47,7 @@ namespace ssr
 namespace api { struct Publisher; }
 
 /// VRPN tracker
-class TrackerVrpn : public vrpn_Tracker_Remote, public Tracker
+class TrackerVrpn : public Tracker, public vrpn_Tracker_Remote
 {
   public:
     using ptr_t = std::unique_ptr<TrackerVrpn>;
@@ -55,33 +57,32 @@ class TrackerVrpn : public vrpn_Tracker_Remote, public Tracker
     /// "named constructor"
     static ptr_t create(api::Publisher& controller, const std::string& ports);
 
-    virtual void calibrate();
-    void set_value(double azi);
+    virtual void calibrate() override
+    {
+        SSR_VERBOSE2("Calibrate.");
+        Tracker::azi_correction = _current_azimuth + 90;
+    }
 
   private:
+    /// constructor
     TrackerVrpn(api::Publisher& controller, const std::string& ports);
 
     api::Publisher& _controller;
-
-    std::string _address;
-
     double _current_azimuth;
 
-    float _az_corr; ///< correction of the azimuth due to calibration
+    std::string _address;
 
-    static void VRPN_CALLBACK _vrpn_change_handler(void* arg
-        , const vrpn_TRACKERCB t);
+    /// VRPN callback function
+    static void VRPN_CALLBACK handle_tracker(void *userdata, const vrpn_TRACKERCB t);
 
-    void vrpn_change_handler(const vrpn_TRACKERCB t);
+    void update(const Tracker::Tracker_data &_data) override;
 
     // thread related stuff
     std::thread _tracker_thread;
-
     std::atomic<bool> _stop_thread; // thread stop flag
-    void _start(); ///< start the tracking thread
-    void _stop();  ///< stop the tracking thread
-
-    void _thread();  // thread main function
+    void _start() override; ///< start the tracking thread
+    void _stop() override;  ///< stop the tracking thread
+    void _thread() override;  // thread main function
 };
 
 }  // namespace ssr