diff --git a/src/xrt/auxiliary/CMakeLists.txt b/src/xrt/auxiliary/CMakeLists.txt
index b68aedf0a748fbfdfed5350cacd7bf1489eaff6e..82b6e80bef9ed8a7d10cb940632336dbdb8654a0 100644
--- a/src/xrt/auxiliary/CMakeLists.txt
+++ b/src/xrt/auxiliary/CMakeLists.txt
@@ -8,6 +8,7 @@ set(MATH_SOURCE_FILES
 	math/m_hash.cpp
 	math/m_optics.c
 	math/m_quatexpmap.cpp
+	math/m_track.c
 	)
 
 set(OS_SOURCE_FILES
diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h
index 36d8ba0173c79243f0a684284e721e28d1e33ace..79be0eb5c45d51f5283e9aa24198f78df9a850e7 100644
--- a/src/xrt/auxiliary/math/m_api.h
+++ b/src/xrt/auxiliary/math/m_api.h
@@ -307,6 +307,16 @@ math_compute_fovs(double w_total,
                   double vertfov_total,
                   struct xrt_fov *fov);
 
+void
+math_euler_to_quat(struct xrt_vec3 euler, struct xrt_quat* q);
+
+int
+math_min(int a, int b);
+
+int
+math_max(int a, int b);
+
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/xrt/auxiliary/math/m_track.c b/src/xrt/auxiliary/math/m_track.c
index 0a9ecc485edc2bc05934a4f204e9030fdcd145b2..008482edb478ca7045509600e0f398734a984dc4 100644
--- a/src/xrt/auxiliary/math/m_track.c
+++ b/src/xrt/auxiliary/math/m_track.c
@@ -25,3 +25,19 @@ math_euler_to_quat(struct xrt_vec3 euler, struct xrt_quat* q)
 	q->w=1.0f;
 
 }
+
+int
+math_min(int a, int b) {
+    if (a > b) {
+        return b;
+    }
+    return a;
+}
+
+int
+math_max(int a, int b) {
+    if (a > b) {
+        return a;
+    }
+    return b;
+}
diff --git a/src/xrt/drivers/montrack/CMakeLists.txt b/src/xrt/drivers/montrack/CMakeLists.txt
index 82db94c597786f13dc34367b6b7b0ed79cc60ec3..28310fe5db41a043159a7bf2ed15919c61a567e8 100644
--- a/src/xrt/drivers/montrack/CMakeLists.txt
+++ b/src/xrt/drivers/montrack/CMakeLists.txt
@@ -6,7 +6,7 @@ set (MONTRACK_SOURCE_FILES
 	mt_events.h
 	mt_framequeue.h
 	mt_framequeue.c
-	)
+)
 
 
 add_subdirectory(frameservers)
diff --git a/src/xrt/drivers/montrack/filters/CMakeLists.txt b/src/xrt/drivers/montrack/filters/CMakeLists.txt
index cd5e2e43a85b1656d347a8f092221faac542cbba..f808dd492ec6b656b8bad2d29bbb47672f15d5f4 100644
--- a/src/xrt/drivers/montrack/filters/CMakeLists.txt
+++ b/src/xrt/drivers/montrack/filters/CMakeLists.txt
@@ -10,8 +10,13 @@ include_directories(
 set(FILTER_SOURCE_FILES
 	common/filter.h
 	common/filter.c
+	common/measurementqueue.h
+	common/measurementqueue.c
 	filter_opencv_kalman.cpp
 	filter_opencv_kalman.h
+	filter_complementary.c
+	filter_complementary.h
+
 	)
 
 # Use OBJECT to not create a archive, since it just gets in the way.
diff --git a/src/xrt/drivers/montrack/filters/common/filter.c b/src/xrt/drivers/montrack/filters/common/filter.c
index 003dc92bc3a634908c91e0ce16af00f8f3fe9b94..ea79b6afb68b09e2c654be846ed279e45832914f 100644
--- a/src/xrt/drivers/montrack/filters/common/filter.c
+++ b/src/xrt/drivers/montrack/filters/common/filter.c
@@ -1,4 +1,5 @@
 #include "filter.h"
+#include "filter_complementary.h"
 #include "filter_opencv_kalman.h"
 #include <string.h>
 
@@ -18,9 +19,21 @@ filter_create(filter_type_t t)
 			    filter_opencv_kalman_predict_state;
 			i->filter_set_state = filter_opencv_kalman_set_state;
 			i->filter_queue = filter_opencv_kalman_queue;
-			i->internal_instance = (filter_internal_instance_ptr)
-			    filter_opencv_kalman_create(i);
+			i->internal_instance = filter_opencv_kalman_create(i);
+			i->measurement_queue = measurement_queue_create();
 			break;
+        case FILTER_TYPE_COMPLEMENTARY:
+            i->tracker_type = t;
+            i->filter_configure = filter_complementary_configure;
+            i->filter_get_state = filter_complementary_get_state;
+            i->filter_predict_state =
+                filter_complementary_predict_state;
+            i->filter_set_state = filter_complementary_set_state;
+            i->filter_queue = filter_complementary_queue;
+            i->internal_instance = filter_complementary_create(i);
+            i->measurement_queue = measurement_queue_create();
+
+            break;
 		case FILTER_TYPE_NONE:
 		default:
 			free(i);
diff --git a/src/xrt/drivers/montrack/filters/common/filter.h b/src/xrt/drivers/montrack/filters/common/filter.h
index 34ae5e6ee0ebf777fd515938d45163abaf5cd6f5..a953f5df072ebe5ef4e7c740c8cacd363188bae7 100644
--- a/src/xrt/drivers/montrack/filters/common/filter.h
+++ b/src/xrt/drivers/montrack/filters/common/filter.h
@@ -2,7 +2,8 @@
 #define FILTER_H
 #include <xrt/xrt_defines.h>
 #include <../auxiliary/util/u_time.h>
-#include <../optical_tracking/common/tracker.h>
+#include <optical_tracking/common/tracker.h>
+#include "measurementqueue.h"
 
 typedef void* filter_instance_ptr;
 typedef void* filter_internal_instance_ptr;
@@ -14,18 +15,20 @@ typedef struct filter_state
 	struct xrt_pose pose;
 	bool has_position;
 	bool has_rotation;
+	struct xrt_vec3 rotation_euler;
 	struct xrt_vec3 velocity;
 	struct xrt_vec3 acceleration;
 	struct xrt_quat angular_velocity;
 	struct xrt_quat angular_accel;
-	timepoint_ns timestamp;
+    int64_t timestamp;
 } filter_state_t;
 
 
 typedef enum filter_type
 {
 	FILTER_TYPE_NONE,
-	FILTER_TYPE_OPENCV_KALMAN
+	FILTER_TYPE_OPENCV_KALMAN,
+	FILTER_TYPE_COMPLEMENTARY
 } filter_type_t;
 
 typedef struct _filter_instance
@@ -43,6 +46,7 @@ typedef struct _filter_instance
 	bool (*filter_configure)(filter_instance_ptr inst,
 	                         filter_configuration_ptr config);
 	filter_internal_instance_ptr internal_instance;
+	measurement_queue_t* measurement_queue;
 } filter_instance_t;
 
 filter_instance_t*
diff --git a/src/xrt/drivers/montrack/filters/common/measurementqueue.c b/src/xrt/drivers/montrack/filters/common/measurementqueue.c
index b2a2508d98bda62051ec886d312d02d142b24a3f..a37062272f741cdfeff25e186a98393eac769088 100644
--- a/src/xrt/drivers/montrack/filters/common/measurementqueue.c
+++ b/src/xrt/drivers/montrack/filters/common/measurementqueue.c
@@ -95,33 +95,33 @@ uint32_t measurement_queue_get_since_last_frame(measurement_queue_t* mq,uint32_t
 	return count;
 }
 
-uint32_t measurement_queue_get_since_timestamp(measurement_queue_t* mq,uint32_t source_id,uint64_t timestamp_ns,tracker_measurement_t** cm) {
+uint32_t measurement_queue_get_since_timestamp(measurement_queue_t* mq,uint32_t source_id,int64_t timestamp_us,tracker_measurement_t** cm) {
     pthread_mutex_lock(&mq->queue_lock);
     //iterate over our measurements, count the number of records we will copy
     //our starting timestamp is the one on our last frame
     uint32_t count=0;
     for (uint32_t i=mq->measurements.head_index;i<mq->measurements.size;i++) {
-        if (mq->measurements.items[i].source_timestamp > timestamp_ns) {
-           printf("H %lld\n",mq->measurements.items[i].source_timestamp);
+        if (mq->measurements.items[i].source_timestamp > timestamp_us) {
+           //printf("H %lld\n",mq->measurements.items[i].source_timestamp);
             count++;
         }
     }
     for (uint32_t i=0;i<mq->measurements.tail_index;i++) {
-        if (mq->measurements.items[i].source_timestamp > timestamp_ns) {
-            printf("0 %lld\n",mq->measurements.items[i].source_timestamp);
+        if (mq->measurements.items[i].source_timestamp > timestamp_us) {
+            //printf("0 %lld\n",mq->measurements.items[i].source_timestamp);
             count++;
         }
     }
     *cm = U_TYPED_ARRAY_CALLOC(tracker_measurement_t,count);
     count =0;
     for (uint32_t i=mq->measurements.head_index;i<mq->measurements.size;i++) {
-        if (mq->measurements.items[i].source_timestamp > timestamp_ns) {
+        if (mq->measurements.items[i].source_timestamp > timestamp_us) {
             memcpy(*cm+count,&mq->measurements.items[i],sizeof(tracker_measurement_t));
             count++;
         }
     }
     for (uint32_t i=0;i<mq->measurements.tail_index;i++) {
-        if (mq->measurements.items[i].source_timestamp > timestamp_ns) {
+        if (mq->measurements.items[i].source_timestamp > timestamp_us) {
             memcpy(*cm+count,&mq->measurements.items[i],sizeof(tracker_measurement_t));
             count++;
         }
diff --git a/src/xrt/drivers/montrack/filters/common/measurementqueue.h b/src/xrt/drivers/montrack/filters/common/measurementqueue.h
index d75c335885bd93cb66aa61191b88500781263db9..639deebcdb5d34b82fbc367f548a97e550905060 100644
--- a/src/xrt/drivers/montrack/filters/common/measurementqueue.h
+++ b/src/xrt/drivers/montrack/filters/common/measurementqueue.h
@@ -29,7 +29,7 @@ void measurement_queue_destroy(measurement_queue_t* mq);
 
 //bool measurement_queue_get_latest_n(measurement_queue_t* mq,uint32_t source_id, uint32_t* count,tracker_measurement_t* cm); //used by consumers
 uint32_t measurement_queue_get_since_last_frame(measurement_queue_t* mq,uint32_t source_id,tracker_measurement_t** cm);
-uint32_t measurement_queue_get_since_timestamp(measurement_queue_t* mq,uint32_t source_id,uint64_t timestamp_ns,tracker_measurement_t** cm);
+uint32_t measurement_queue_get_since_timestamp(measurement_queue_t* mq,uint32_t source_id,int64_t timestamp_us,tracker_measurement_t** cm);
 void measurement_queue_add(measurement_queue_t* mq, tracker_measurement_t* m); //used by producers
 uint64_t measurement_queue_uniq_source_id(measurement_queue_t* mq); //used by producers
 
diff --git a/src/xrt/drivers/montrack/filters/filter_complementary.c b/src/xrt/drivers/montrack/filters/filter_complementary.c
index 40382b8c64953adf3fd26f9cdc6d68dbb72b3866..3b7d64b3cfdf5904f8bf56590020d1815a6c2c3b 100644
--- a/src/xrt/drivers/montrack/filters/filter_complementary.c
+++ b/src/xrt/drivers/montrack/filters/filter_complementary.c
@@ -2,12 +2,15 @@
 #include "filter_complementary.h"
 #include "util/u_misc.h"
 
+
 struct filter_complementary_instance_t
 {
 	bool configured;
 	filter_complementary_configuration_t configuration;
 	filter_state_t last_state;
 	filter_state_t state;
+    float gyro_yaw_correction;
+    uint8_t avg_count;
 	float alpha;
 	bool running;
 };
@@ -44,39 +47,53 @@ filter_complementary_get_state(filter_instance_t* inst, filter_state_t* state)
 {
 	filter_complementary_instance_t* internal =
 	    filter_complementary_instance(inst->internal_instance);
-	// printf("getting filtered pose\n");
-	if (!internal->running) {
+    if (!internal->running) {
 		return false;
 	}
 	tracker_measurement_t* measurement_array;
     uint64_t last_timestamp = internal->last_state.timestamp;
-    if (internal->configuration.max_timestamp > 0)
-    {
-        if (last_timestamp >= internal->configuration.max_timestamp){
-            //wrap timestamp
-            last_timestamp = internal->configuration.max_timestamp - last_timestamp;
-        }
-    }
     uint32_t count = measurement_queue_get_since_timestamp(inst->measurement_queue,0,last_timestamp,&measurement_array);
 	float one_minus_bias = 1.0f - internal->configuration.bias;
 	for (uint32_t i=0;i<count;i++)
 	{
 		tracker_measurement_t* m = &measurement_array[i];
-        float dt = (m->source_timestamp - internal->last_state.timestamp) * 0.0000001;
-        float normAccel = sqrt(m->accel.x * m->accel.x + m->accel.y * m->accel.y + m->accel.z+m->accel.z);
+        float dt = (m->source_timestamp - internal->last_state.timestamp) *0.00000001;
+        if (dt > 1.0f) {
+            internal->last_state.timestamp = m->source_timestamp;
+            return false; //this is our first frame, or something has gone wrong... big dt will blow up calculations.
+        }
+
+        float magAccel = sqrt(m->accel.x * m->accel.x + m->accel.y * m->accel.y + m->accel.z+m->accel.z);
+
+        //assume that, if acceleration is only gravity, then any change in the gyro is drift and update compensation
+        int avg_max = 32;
+        if (internal->avg_count < avg_max) {
+            internal->avg_count++;
+        }
+
+        printf("%f %f %f %f %f %f %f\n",m->accel.x,m->accel.y,m->accel.z,m->gyro.x,m->gyro.y,m->gyro.z,dt);
+
+        float accelDiff =1.0f;
+        if (internal->gyro_yaw_correction > 0.0f)
+        {
+            accelDiff = (magAccel - internal->gyro_yaw_correction) / internal->gyro_yaw_correction;
+        }
+
+        internal->gyro_yaw_correction = magAccel + (magAccel - internal->gyro_yaw_correction) / math_min(internal->avg_count,avg_max);
+        //printf("accelDiff %f magAccel: %f gyro.z %f yaw corr %f\n",accelDiff,magAccel,m->gyro.z, internal->gyro_yaw_correction);
 
 		//calculate filtered euler angles
-		internal->state.rotation_euler.z = internal->last_state.rotation_euler.z + m->gyro.z * dt;
-		internal->state.rotation_euler.y = internal->configuration.bias * (internal->last_state.rotation_euler.y + m->gyro.y * dt) + one_minus_bias * (m->accel.y * internal->configuration.scale/normAccel);
-		internal->state.rotation_euler.x = internal->configuration.bias * (internal->last_state.rotation_euler.x + m->gyro.x * dt) + one_minus_bias * (m->accel.x * internal->configuration.scale/normAccel);
+        internal->state.rotation_euler.z = internal->configuration.bias * (internal->last_state.rotation_euler.z + m->gyro.z * dt) + one_minus_bias * (m->accel.z * internal->configuration.scale/magAccel);
+        internal->state.rotation_euler.y = internal->configuration.bias * (internal->last_state.rotation_euler.y + m->gyro.y * dt) + one_minus_bias * (m->accel.y * internal->configuration.scale/magAccel);
+        internal->state.rotation_euler.x = internal->configuration.bias * (internal->last_state.rotation_euler.x + m->gyro.x * dt) + one_minus_bias * (m->accel.x * internal->configuration.scale/magAccel);
         internal->state.timestamp = m->source_timestamp;
         internal->last_state = internal->state;
-        printf("source tstamp: %lld\n",m->source_timestamp);
+        //printf("source tstamp: %lld\n",m->source_timestamp);
 	    }
 	//TODO: come up with a way to avoid alloc/free - use max length buffer?
 	free(measurement_array);
 	//convert to a quat for consumption as pose
-    printf(" integrated %d measurements after %lld X %f Y %f Z %f\n",count,last_timestamp,internal->state.rotation_euler.x,internal->state.rotation_euler.y,internal->state.rotation_euler.z);
+    //printf(" integrated %d measurements after %lld X %f Y %f Z %f\n",count,last_timestamp,internal->state.rotation_euler.x,internal->state.rotation_euler.y,internal->state.rotation_euler.z);
 	math_euler_to_quat(internal->state.rotation_euler,&internal->state.pose.orientation);
 	*state = internal->state;
 	return true;
@@ -108,6 +125,7 @@ filter_complementary_configure(filter_instance_t* inst,
 	filter_complementary_configuration_t* config =
 	    (filter_complementary_configuration_t*)config_generic;
 	internal->configuration = *config;
+    internal->gyro_yaw_correction=0.0f;
 	internal->configured = true;
 	return true;
 }
@@ -126,7 +144,11 @@ filter_complementary_create(filter_instance_t* inst)
 		// we a are a rotational-only filter
 		i->state.has_rotation = true;
 		i->state.has_position = false;
-		i->state.timestamp = 0;
+        i->state.timestamp = 0;
+        i->last_state.rotation_euler.x=0.0f;
+        i->last_state.rotation_euler.y=0.0f;
+        i->last_state.rotation_euler.z=0.0f;
+
 		i->last_state = i->state;
 		return i;
 	}
diff --git a/src/xrt/drivers/montrack/filters/filter_complementary.h b/src/xrt/drivers/montrack/filters/filter_complementary.h
index 23ee85eedf1cc9ef470a01dfd87a748d6dc12627..7cec7e9e84541a52dd8d5096700416bb4d43b6f4 100644
--- a/src/xrt/drivers/montrack/filters/filter_complementary.h
+++ b/src/xrt/drivers/montrack/filters/filter_complementary.h
@@ -4,6 +4,7 @@
 #include <xrt/xrt_defines.h>
 #include "common/filter.h"
 #include "common/measurementqueue.h"
+#include <stdlib.h>
 
 typedef struct filter_complementary_configuration
 {
diff --git a/src/xrt/drivers/montrack/filters/filter_opencv_kalman.cpp b/src/xrt/drivers/montrack/filters/filter_opencv_kalman.cpp
index 4e4e3f66952734b4da9e8d06f6788e033b9856c5..22b34f536562b3044af6d01dcde1bd615bb7ced0 100644
--- a/src/xrt/drivers/montrack/filters/filter_opencv_kalman.cpp
+++ b/src/xrt/drivers/montrack/filters/filter_opencv_kalman.cpp
@@ -40,10 +40,11 @@ filter_opencv_kalman_queue(filter_instance_t* inst,
 	filter_opencv_kalman_instance_t* internal =
 	    filter_opencv_kalman_instance(inst->internal_instance);
 	printf("queueing measurement in filter\n");
-	internal->observation.at<float>(0, 0) = measurement->pose.position.x;
-	internal->observation.at<float>(1, 0) = measurement->pose.position.y;
-	internal->observation.at<float>(2, 0) = measurement->pose.position.z;
-	internal->kalman_filter.correct(internal->observation);
+	measurement_queue_add(inst->measurement_queue,measurement);
+	//internal->observation.at<float>(0, 0) = measurement->pose.position.x;
+	//internal->observation.at<float>(1, 0) = measurement->pose.position.y;
+	//internal->observation.at<float>(2, 0) = measurement->pose.position.z;
+	//internal->kalman_filter.correct(internal->observation);
 	internal->running = true;
 	return false;
 }
@@ -68,6 +69,11 @@ filter_opencv_kalman_predict_state(filter_instance_t* inst,
 	if (!internal->running) {
 		return false;
 	}
+	//get all our measurements including the last optical frame,
+	//and run our filter on them to make a prediction
+
+	tracker_measurement_t* measurement_array;
+
 	internal->prediction = internal->kalman_filter.predict();
 	state->has_position = true;
 	state->pose.position.x = internal->prediction.at<float>(0, 0);
diff --git a/src/xrt/drivers/montrack/filters/filter_opencv_kalman.h b/src/xrt/drivers/montrack/filters/filter_opencv_kalman.h
index d4fa11c95a150edb251a170489c31b0f76298ac8..082503ea9c88bff75cdb2cf6cf52614664bad9c4 100644
--- a/src/xrt/drivers/montrack/filters/filter_opencv_kalman.h
+++ b/src/xrt/drivers/montrack/filters/filter_opencv_kalman.h
@@ -3,6 +3,7 @@
 
 #include <xrt/xrt_defines.h>
 #include "common/filter.h"
+#include "common/measurementqueue.h"
 
 typedef struct opencv_filter_configuration
 {
@@ -10,11 +11,6 @@ typedef struct opencv_filter_configuration
 	float process_noise_cov;
 } opencv_filter_configuration_t;
 
-typedef struct opencv_kalman_filter_state
-{
-	struct xrt_pose pose;
-} opencv_kalman_filter_state_t;
-
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -26,11 +22,11 @@ typedef struct filter_opencv_kalman_instance_t filter_opencv_kalman_instance_t;
 filter_opencv_kalman_instance_t*
 filter_opencv_kalman_create(filter_instance_t* inst);
 bool
-filter_opencv_kalman__destroy(filter_instance_t* inst);
-
+filter_opencv_kalman_destroy(filter_instance_t* inst);
 bool
 filter_opencv_kalman_queue(filter_instance_t* inst,
                            tracker_measurement_t* measurement);
+
 bool
 filter_opencv_kalman_get_state(filter_instance_t* inst, filter_state_t* state);
 bool
@@ -41,7 +37,7 @@ filter_opencv_kalman_predict_state(filter_instance_t* inst,
                                    timepoint_ns time);
 bool
 filter_opencv_kalman_configure(filter_instance_t* inst,
-                               filter_configuration_ptr config_generic);
+                               filter_configuration_ptr config);
 
 #ifdef __cplusplus
 } // extern "C"
diff --git a/src/xrt/drivers/montrack/mt_device.c b/src/xrt/drivers/montrack/mt_device.c
index d1bfbb8c62109c9592e26c9c8d6424a3a0fe5d19..803c8d2943d858cb732271d601af202cab6825b8 100644
--- a/src/xrt/drivers/montrack/mt_device.c
+++ b/src/xrt/drivers/montrack/mt_device.c
@@ -31,7 +31,7 @@
 //#IFDEF have_v4l2
 #include "frameservers/v4l2/v4l2_frameserver.h"
 
-#include "mt_framequeue.h"
+#include <mt_framequeue.h>
 
 
 static void
diff --git a/src/xrt/drivers/montrack/mt_framequeue.h b/src/xrt/drivers/montrack/mt_framequeue.h
index 4146c88e2039a6bdd89515e0976193c9450d6830..0d19029d9f9b06497edca5e2f23acdb05557a908 100644
--- a/src/xrt/drivers/montrack/mt_framequeue.h
+++ b/src/xrt/drivers/montrack/mt_framequeue.h
@@ -1,5 +1,5 @@
-#ifndef VECTOR_H
-#define VECTOR_H
+#ifndef MT_FRAMEQUEUE_H
+#define MT_FRAMEQUEUE_H
 
 #include <pthread.h>
 
diff --git a/src/xrt/drivers/montrack/optical_tracking/common/tracked_object.h b/src/xrt/drivers/montrack/optical_tracking/common/tracked_object.h
index 3ed7423a7fccc26ce21d3b6a39f83ff57d3c8f17..7c4f31519fc381f251d736c283d90fb717ff63f4 100644
--- a/src/xrt/drivers/montrack/optical_tracking/common/tracked_object.h
+++ b/src/xrt/drivers/montrack/optical_tracking/common/tracked_object.h
@@ -8,6 +8,10 @@ typedef struct tracked_blob
 	struct xrt_vec2 center;
 	float diameter;
 	struct xrt_vec2 velocity;
+	uint32_t tracking_tag; // a tracker may assign an opaque tag to denote
+	                       // object type
+	uint32_t tracking_id; // a tracker may assign an opaque id to facilitate
+	                      // interframe correlation
 } tracked_blob_t;
 
 typedef struct tracked_object
diff --git a/src/xrt/drivers/montrack/optical_tracking/common/tracker.h b/src/xrt/drivers/montrack/optical_tracking/common/tracker.h
index 19fdb5143924fd7e9f0a0c7bd08b078921bb1eff..2989268adad752415466ab3e317c07003bfd529e 100644
--- a/src/xrt/drivers/montrack/optical_tracking/common/tracker.h
+++ b/src/xrt/drivers/montrack/optical_tracking/common/tracker.h
@@ -12,12 +12,25 @@
 extern "C" {
 #endif
 
+typedef enum tracker_measurement_flags {MEASUREMENT_NONE=0,
+	                                    MEASUREMENT_POSITION=1,
+	                                    MEASUREMENT_ROTATION=2,
+	                                    MEASUREMENT_RAW_ACCEL=4,
+	                                    MEASUREMENT_RAW_GYRO=8,
+	                                    MEASUREMENT_RAW_MAG=16,
+	                                    MEASUREMENT_OPTICAL=32,
+	                                    MEASUREMENT_IMU=64,
+                                       } tracker_measurement_flags_t;
 typedef struct tracker_measurement
 {
 	struct xrt_pose pose;
-	bool has_position;
-	bool has_rotation;
-	timepoint_ns timestamp;
+	struct xrt_vec3 accel;
+	struct xrt_vec3 gyro;
+	struct xrt_vec3 mag;
+	tracker_measurement_flags_t flags;
+    int64_t source_timestamp;
+    uint8_t source_id;
+    uint64_t source_sequence;
 } tracker_measurement_t;
 
 typedef enum tracker_type
diff --git a/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_mono.cpp b/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_mono.cpp
index e1516a308c4431b3d30e5798276e4804dd005524..8d6794a536c14994391ee260f875b9e29dfd1b52 100644
--- a/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_mono.cpp
+++ b/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_mono.cpp
@@ -224,8 +224,8 @@ tracker3D_sphere_mono_track(tracker_instance_t* inst)
 
 		// printf("%f %f %f\n",x,y,z);
 
-		m.has_position = true;
-		m.timestamp = 0;
+		m.flags = (tracker_measurement_flags_t)(MEASUREMENT_POSITION | MEASUREMENT_OPTICAL);
+		m.source_timestamp=0;
 		m.pose.position.x = x;
 		m.pose.position.y = y;
 		m.pose.position.z = z;
diff --git a/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_stereo.cpp b/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_stereo.cpp
index 793f2553347b769ad2b5ab7992c430cfa7568321..f7294ab16b420f0ce1a934982649951880aeb167 100644
--- a/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_stereo.cpp
+++ b/src/xrt/drivers/montrack/optical_tracking/tracker3D_sphere_stereo.cpp
@@ -477,8 +477,7 @@ tracker3D_sphere_stereo_track(tracker_instance_t* inst)
 		cv::Point3f world_point = world_points[tracked_index];
 
 		// create our measurement for the filter
-		m.has_position = true;
-		m.has_rotation = false;
+		m.flags = (tracker_measurement_flags_t)(MEASUREMENT_POSITION | MEASUREMENT_OPTICAL);
 		m.pose.position.x = world_point.x;
 		m.pose.position.y = world_point.y;
 		m.pose.position.z = world_point.z;
diff --git a/src/xrt/drivers/psmv/psmv_driver.c b/src/xrt/drivers/psmv/psmv_driver.c
index ab89ccfb1d74260cbd6e33e05ba3c20939fac22a..b3ef63910016b9d80f5f63da95aeb3f974b4843d 100644
--- a/src/xrt/drivers/psmv/psmv_driver.c
+++ b/src/xrt/drivers/psmv/psmv_driver.c
@@ -24,8 +24,12 @@
 #include <unistd.h>
 
 #include <optical_tracking/common/tracker.h>
+#include <filters/filter_complementary.h>
+
+
 #include <mt_framequeue.h>
 #include <pthread.h>
+#include <sys/time.h>
 
 
 /*
@@ -198,8 +202,13 @@ struct psmv_device
 	tracker_instance_t* tracker;
 	uint32_t tracked_objects;
 	tracked_object_t* tracked_object_array;
-    uint64_t last_frame_seq;
+
+	filter_instance_t* filter;
+
+	uint64_t last_frame_seq;
     pthread_t* tracking_thread;
+    int64_t start_us;
+
 
 	bool print_spew;
 	bool print_debug;
@@ -252,8 +261,8 @@ psmv_read_hid(struct psmv_device *psmv)
 		input.buttons |= data.input.buttons[1] << 16;
 		input.buttons |= data.input.buttons[2] << 8;
 		input.buttons |= data.input.buttons[3] & 0xf0;
-		input.timestamp |= data.input.timestamp_low;
-		input.timestamp |= data.input.timestamp_high << 8;
+        input.timestamp = data.input.timestamp_low;
+        input.timestamp |= data.input.timestamp_high << 8;
 
 		input.frame[0].trigger = data.input.trigger_f1;
 		psmv_vec3_from_16_be_to_32(&input.frame[0].accel,
@@ -266,7 +275,8 @@ psmv_read_hid(struct psmv_device *psmv)
 		psmv_vec3_from_16_be_to_32(&input.frame[1].gyro,
 		                           &data.input.gyro_f2);
 
-		int32_t diff = input.timestamp - psmv->last.timestamp;
+        int16_t diff = input.timestamp - psmv->last.timestamp;
+
 		bool missed = input.seq_no != ((psmv->last.seqno + 1) & 0x0f);
 
 		psmv->last.trigger = input.frame[1].trigger;
@@ -274,6 +284,26 @@ psmv_read_hid(struct psmv_device *psmv)
 		psmv->last.seqno = input.seq_no;
 		psmv->last.buttons = input.buttons;
 
+		if (psmv->filter) {
+			tracker_measurement_t m = {0};
+			m.flags = (tracker_measurement_flags_t)(MEASUREMENT_IMU | MEASUREMENT_RAW_ACCEL | MEASUREMENT_RAW_GYRO);
+            m.accel.x = input.frame[1].accel.x;
+            m.accel.y = input.frame[1].accel.y;
+            m.accel.z = input.frame[1].accel.z;
+			m.gyro.x = input.frame[1].gyro.x;
+			m.gyro.y = input.frame[1].gyro.y;
+			m.gyro.z = input.frame[1].gyro.z;
+            m.source_id = 0;
+			m.source_sequence = input.seq_no;
+
+            //m.source_timestamp = input.timestamp;
+
+
+            psmv->start_us += (diff*10000);
+            m.source_timestamp = psmv->start_us;
+            //printf("queueing timestamp %lld %d %d %d\n",m.source_timestamp,input.timestamp,diff,input.seq_no);
+			psmv->filter->filter_queue(psmv->filter,&m);
+		}
 
 		PSMV_SPEW(psmv,
 		          "\n\t"
@@ -299,6 +329,8 @@ psmv_read_hid(struct psmv_device *psmv)
 		          input.timestamp, diff, input.seq_no);
 	} while (true);
 
+
+
 	return 0;
 }
 
@@ -417,8 +449,9 @@ psmv_device_get_tracked_pose(struct xrt_device *xdev,
 	struct psmv_device *psmv = psmv_device(xdev);
 
 	psmv_read_hid(psmv);
-
-    //TODO: get the latest pose from the tracker and send it down the pipe
+	filter_state_t fs;
+	psmv->filter->filter_get_state(psmv->filter,&fs);
+	out_relation->pose = fs.pose;
 }
 
 static void
@@ -525,6 +558,17 @@ psmv_found(struct xrt_prober *xp,
 
 
     psmv->tracker = tracker_create(TRACKER_TYPE_SPHERE_STEREO);
+	psmv->filter = filter_create(FILTER_TYPE_COMPLEMENTARY);
+
+    filter_complementary_configuration_t config;
+    config.bias = 0.1f;
+    config.scale = 3.14159 / 2.0f;
+    config.max_timestamp=65535; //ps move timestamps are 16 bit
+
+    psmv->filter->filter_configure(psmv->filter,&config);
+
+	//send our optical measurements to the filter
+	psmv->tracker->tracker_register_measurement_callback(psmv->tracker,psmv->filter,psmv->filter->filter_queue);
 
     // if we want to generate a calibration, we can use this calibration tracker,
     // instead of the above. this will move to a standalone application.
@@ -536,6 +580,9 @@ psmv_found(struct xrt_prober *xp,
     // initialise some info about how many objects are tracked, and the initial frame seq.
     psmv->tracker->tracker_get_poses(psmv->tracker,NULL,&psmv->tracked_objects);
     psmv->last_frame_seq = 0;
+    struct timeval tp;
+    gettimeofday(&tp,NULL);
+    psmv->start_us = tp.tv_sec * 1000000 + tp.tv_usec;
     pthread_create(&psmv->tracking_thread,NULL,psmv_tracking_run,psmv);
 
 	// And finally done
@@ -549,6 +596,7 @@ void* psmv_tracking_run(void* arg) {
     frame_t f;
     //TODO: orderly shutdown
     while(1) {
+
         if (frame_queue_ref_latest(fq,0,&f)) {
 
             if (! psmv->tracker->configured)
diff --git a/src/xrt/targets/openxr/make_manifest.cmake b/src/xrt/targets/openxr/make_manifest.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..c5a4e6e21b7d871e5e97f184680bebf20958d0b3
--- /dev/null
+++ b/src/xrt/targets/openxr/make_manifest.cmake
@@ -0,0 +1,31 @@
+# SPDX-License-Identifier: BSL-1.0
+
+# Get input from main CMake script
+set(MANIFEST_INPUT @MANIFEST_INPUT@)
+set(MANIFEST_RELATIVE_DIR @MANIFEST_RELATIVE_DIR@)
+set(RUNTIME_RELATIVE_DIR @RUNTIME_RELATIVE_DIR@)
+set(RUNTIME_FILENAME @RUNTIME_FILENAME@)
+set(OPENXR_INSTALL_ABSOLUTE_RUNTIME_PATH @OPENXR_INSTALL_ABSOLUTE_RUNTIME_PATH@)
+
+# Remove trailing slash
+string(REGEX REPLACE "/$" "" MANIFEST_RELATIVE_DIR "${MANIFEST_RELATIVE_DIR}")
+
+if(OPENXR_INSTALL_ABSOLUTE_RUNTIME_PATH)
+    # Absolute path to runtime
+    set(RUNTIME_PATH ${CMAKE_INSTALL_PREFIX}/${RUNTIME_RELATIVE_DIR}/${RUNTIME_FILENAME})
+else()
+    # Relative path to runtime: requires it exist on the system shared library search path.
+    set(RUNTIME_PATH ${RUNTIME_FILENAME})
+endif()
+
+if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT)
+
+    # Create manifest
+    configure_file(${MANIFEST_INPUT} ${CMAKE_CURRENT_LIST_DIR}/@RUNTIME_TARGET@.json)
+
+    # Install it
+    file(INSTALL
+        DESTINATION "${CMAKE_INSTALL_PREFIX}/${MANIFEST_RELATIVE_DIR}"
+        TYPE FILE
+        FILES "${CMAKE_CURRENT_LIST_DIR}/@RUNTIME_TARGET@.json")
+endif()
diff --git a/src/xrt/targets/openxr/openxr_monado.in.json b/src/xrt/targets/openxr/openxr_monado.in.json
new file mode 100644
index 0000000000000000000000000000000000000000..c1a469e098e3b6bfb5fdd755248f13bdee0711f0
--- /dev/null
+++ b/src/xrt/targets/openxr/openxr_monado.in.json
@@ -0,0 +1,6 @@
+{
+    "file_format_version": "1.0.0",
+    "runtime": {
+        "library_path": "${RUNTIME_PATH}"
+    }
+}