Adding new orientation filtering framework

* Updating magnetometer calibration to record the magnetic field direction for orientation alignment
* Can switch between MadgwickIMU, MadgwickMARG, and new ComplimentaryMARG filters
 - MadgwickIMU and MadgwickMARG are reimplementations of Madgwick's filters using vector primitives and using the magnetometer calibration
 - ComplimentaryMARG is a new algorithm that uses the gradient descent approach outlined by Madgwick, but all in one time step.
* Made the transforms applied to sensor data and identity-pose for orientation computation more explicit.

examples/CMakeLists.txt

@@ -51,11 +51,16 @@ endif()
 
 # C test programs
 if(PSMOVE_BUILD_TESTS)
-    foreach(TESTNAME led_update read_performance calibration responsiveness extension led_pwm_frequency)
+    foreach(TESTNAME led_update read_performance gyroscope_error responsiveness extension led_pwm_frequency)
         add_executable(test_${TESTNAME} ${CMAKE_CURRENT_LIST_DIR}/c/test_${TESTNAME}.c ${MSVC_SRCS})
         target_link_libraries(test_${TESTNAME} psmoveapi ${MSVC_LIBS})
     endforeach(TESTNAME)
 
+    foreach(TESTNAME calibration)
+        add_executable(test_${TESTNAME} ${CMAKE_CURRENT_LIST_DIR}/c/test_${TESTNAME}.cpp ${MSVC_SRCS})
+        target_link_libraries(test_${TESTNAME} psmoveapi ${MSVC_LIBS})
+    endforeach(TESTNAME)
+
     if(PSMOVE_BUILD_TRACKER)
         foreach(TESTNAME tracker capture_performance record_video roi_size end2end_latency fusion)
             add_executable(test_${TESTNAME} ${CMAKE_CURRENT_LIST_DIR}/c/test_${TESTNAME}.c ${MSVC_SRCS})

examples/c/test_calibration.cpp

@@ -0,0 +1,152 @@
+
+ /**
+ * PS Move API - An interface for the PS Move Motion Controller
+ * Copyright (c) 2012 Thomas Perl <[email protected]>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *    1. Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *
+ *    2. Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ **/
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <assert.h>
+
+#include "psmove.h"
+#include "../src/psmove_orientation.h"
+#include "math/psmove_vector.h"
+#include "math/psmove_quaternion.hpp"
+#include "math/psmove_alignment.hpp"
+
+//#define PSMOVE_NATIVE_ORIENTATION
+
+int
+main(int argc, char* argv[])
+{
+    PSMove *move;
+
+	if (!psmove_init(PSMOVE_CURRENT_VERSION)) {
+		fprintf(stderr, "PS Move API init failed (wrong version?)\n");
+		exit(1);
+	}
+
+    move = psmove_connect();
+
+    if (move == NULL) {
+        fprintf(stderr, "Could not connect to controller.\n");
+        return EXIT_FAILURE;
+    }
+
+    if (psmove_has_calibration(move))
+	{
+		if (psmove_connection_type(move) == Conn_Bluetooth) 
+		{
+			Eigen::Quaternionf mg_orientation = Eigen::Quaternionf::Identity();
+
+			PSMoveOrientation *orientation_state= psmove_get_orientation_state(move);
+
+			#ifdef PSMOVE_NATIVE_ORIENTATION
+			psmove_orientation_set_calibration_transform(orientation_state, k_psmove_identity_pose_upright);
+			psmove_orientation_set_sensor_data_transform(orientation_state, k_psmove_sensor_transform_identity);
+			#else 
+			// This is the historical default
+			psmove_orientation_set_calibration_transform(orientation_state, k_psmove_identity_pose_laying_flat);
+			psmove_orientation_set_sensor_data_transform(orientation_state, k_psmove_sensor_transform_opengl);
+			#endif
+
+			PSMove_3AxisVector calibration_a= psmove_orientation_get_gravity_calibration_direction(orientation_state);
+			Eigen::Vector3f gravity_calibration_direction = Eigen::Vector3f(calibration_a.x, calibration_a.y, calibration_a.z);
+
+			PSMove_3AxisVector calibration_m= psmove_orientation_get_magnetometer_calibration_direction(orientation_state);
+			Eigen::Vector3f magnetometer_calibration_direction = Eigen::Vector3f(calibration_m.x, calibration_m.y, calibration_m.z);
+
+			while ((psmove_get_buttons(move) & Btn_PS) == 0) 
+			{
+				int res = psmove_poll(move);
+
+				if (res) 
+				{
+					// Get the sensor measurements in Right-Handed Cartesian Coordinates (a.k.a. OpenGL Coordinates)					
+					PSMove_3AxisVector w= psmove_orientation_get_gyroscope_vector(orientation_state, Frame_SecondHalf);
+					Eigen::Vector3f omega = Eigen::Vector3f(w.x, w.y, w.z);
+
+					PSMove_3AxisVector a= psmove_orientation_get_accelerometer_vector(orientation_state, Frame_SecondHalf);
+					Eigen::Vector3f acceleration = Eigen::Vector3f(a.x, a.y, a.z);
+					Eigen::Vector3f gravity_direction = acceleration;
+					gravity_direction.normalize();
+
+					PSMove_3AxisVector m= psmove_orientation_get_magnetometer_normalized_vector(orientation_state);
+					Eigen::Vector3f magnetometer_direction = Eigen::Vector3f(m.x, m.y, m.z);
+
+					// Attempt to compute a quaternion that would align the calibration gravity and magnetometer directions
+					// with the currently read gravity and magnetometer directions
+					const Eigen::Vector3f* mg_from[2] = { &gravity_calibration_direction, &magnetometer_calibration_direction };
+					const Eigen::Vector3f* mg_to[2] = { &gravity_direction, &magnetometer_direction };
+					Eigen::Quaternionf new_mg_orientation;
+					bool mg_align_success =
+						psmove_alignment_quaternion_between_vector_frames(
+							mg_from, mg_to, 0.15f, mg_orientation, new_mg_orientation);
+
+					if (mg_align_success)
+					{
+						float yaw = 0.f, pitch = 0.f, roll = 0.f;
+
+						mg_orientation = new_mg_orientation;
+						psmove_quaternion_get_yaw_pitch_roll(mg_orientation, &yaw, &pitch, &roll);
+
+						yaw = radians_to_degrees(yaw);
+						pitch = radians_to_degrees(pitch);
+						roll = radians_to_degrees(roll);
+
+						printf("A:<%5.2f %5.2f %5.2f> M:<%6.2f %6.2f %6.2f> W:<%6.2f %6.2f %6.2f> Q:<%6.2f %6.2f %6.2f>\n",
+							acceleration.x(), acceleration.y(), acceleration.z(),
+							magnetometer_direction.x(), magnetometer_direction.y(), magnetometer_direction.z(),
+							omega.x(), omega.y(), omega.z(),
+							pitch, yaw, roll);
+					}
+					else
+					{
+						printf("A:<%5.2f %5.2f %5.2f> M:<%6.2f %6.2f %6.2f> W:<%6.2f %6.2f %6.2f> Q:<- - ->\n",
+							acceleration.x(), acceleration.y(), acceleration.z(),
+							magnetometer_direction.x(), magnetometer_direction.y(), magnetometer_direction.z(),
+							omega.x(), omega.y(), omega.z());
+					}
+				}
+			}
+		}
+		else
+		{
+			fprintf(stderr, "Controller isn't connected via bluetooth!\n");
+		}
+	}
+	else
+	{
+		fprintf(stderr, "Controller doesn't have valid calibration data!\n");
+	}
+
+    psmove_disconnect(move);
+	psmove_shutdown();
+
+    return EXIT_SUCCESS;
+}
+

examples/c/test_gyroscope_error.c

@@ -0,0 +1,183 @@
+
+ /**
+ * PS Move API - An interface for the PS Move Motion Controller
+ * Copyright (c) 2012 Thomas Perl <[email protected]>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *    1. Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *
+ *    2. Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ **/
+
+
+#include <stdio.h>
+#include <conio.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <math.h>
+
+#include "psmove.h"
+
+#define DESIRED_NOISE_SAMPLE_COUNT 1000
+#define DESIRED_SAMPLING_TIME 30 // seconds
+#define RADIANS_TO_DEGREES (180.0f / (float)M_PI)
+
+int
+main(int argc, char* argv[])
+{
+    PSMove *move;
+
+	if (!psmove_init(PSMOVE_CURRENT_VERSION)) 
+	{
+		fprintf(stderr, "PS Move API init failed (wrong version?)\n");
+		return EXIT_FAILURE;
+	}
+
+    move = psmove_connect();
+    if (move == NULL) 
+	{
+        fprintf(stderr, "Could not connect to controller.\n");
+        return EXIT_FAILURE;
+    }
+
+	if (!psmove_has_calibration(move))
+	{
+		fprintf(stderr, "Controller missing sensor calibration.\n");
+		return EXIT_FAILURE;
+	}
+
+    if (psmove_connection_type(move) == Conn_Bluetooth) 
+	{
+		float omega_x_samples[DESIRED_NOISE_SAMPLE_COUNT];
+		float omega_y_samples[DESIRED_NOISE_SAMPLE_COUNT];
+		float omega_z_samples[DESIRED_NOISE_SAMPLE_COUNT];
+
+		float omega_x, omega_y, omega_z;
+		double theta_x = 0.f, theta_y = 0.f, theta_z = 0.f;
+		double mean_omega_x = 0.f, mean_omega_y = 0.f, mean_omega_z = 0.f;
+		double var_omega_x = 0.f, var_omega_y = 0.f, var_omega_z = 0.f;
+		double stddev_omega_x = 0.f, stddev_omega_y = 0.f, stddev_omega_z = 0.f;
+		double elapsed_time = 0.f;
+		int noise_sample_count = 0;
+		int total_sample_count = 0;
+
+		fprintf(stdout, "Press any keyboard key once the controller is stable, pointing upright.\n");
+		_getch();
+		fprintf(stdout, "Sampling gyro for %d seconds...\n", DESIRED_SAMPLING_TIME);
+
+		PSMove_timestamp last_update_time = psmove_timestamp();
+
+		while (noise_sample_count < DESIRED_NOISE_SAMPLE_COUNT || elapsed_time < DESIRED_SAMPLING_TIME)
+		{
+            int res = psmove_poll(move);
+
+            if (res) 
+			{
+				PSMove_timestamp update_time = psmove_timestamp();
+				double dt= psmove_timestamp_value(psmove_timestamp_diff(update_time, last_update_time));
+
+				psmove_get_gyroscope_frame(move, Frame_SecondHalf, &omega_x, &omega_y, &omega_z);
+
+				omega_x *= RADIANS_TO_DEGREES;
+				omega_y *= RADIANS_TO_DEGREES;
+				omega_z *= RADIANS_TO_DEGREES;
+
+				// Integrate the gyros over time to compute
+				theta_x += (double)omega_x*dt;
+				theta_y += (double)omega_y*dt;
+				theta_z += (double)omega_z*dt;
+				elapsed_time += dt;
+
+				if (noise_sample_count < DESIRED_NOISE_SAMPLE_COUNT)
+				{
+					omega_x_samples[noise_sample_count] = omega_x;
+					omega_y_samples[noise_sample_count] = omega_y;
+					omega_z_samples[noise_sample_count] = omega_z;
+					noise_sample_count++;
+				}
+				total_sample_count++;
+
+				last_update_time = update_time;
+            }
+        }
+
+		double N = (double)total_sample_count;
+
+		// Compute the mean of the samples
+		for (int i = 0; i < DESIRED_NOISE_SAMPLE_COUNT; i++)
+		{
+			mean_omega_x += (double)omega_x_samples[i];
+			mean_omega_y += (double)omega_y_samples[i];
+			mean_omega_z += (double)omega_z_samples[i];
+		}
+		mean_omega_x /= N;
+		mean_omega_y /= N;
+		mean_omega_z /= N;
+
+		// Compute the standard deviation of the samples
+		for (int i = 0; i < DESIRED_NOISE_SAMPLE_COUNT; i++)
+		{
+			var_omega_x += ((double)omega_x_samples[i] - mean_omega_x)*((double)omega_x_samples[i] - mean_omega_x);
+			var_omega_y += ((double)omega_y_samples[i] - mean_omega_y)*((double)omega_y_samples[i] - mean_omega_y);
+			var_omega_z += ((double)omega_z_samples[i] - mean_omega_z)*((double)omega_z_samples[i] - mean_omega_z);
+		}
+		var_omega_x = var_omega_x / (N - 1);
+		var_omega_y = var_omega_y / (N - 1);
+		var_omega_z = var_omega_z / (N - 1);
+		stddev_omega_x = sqrt(var_omega_x);
+		stddev_omega_y = sqrt(var_omega_y);
+		stddev_omega_z = sqrt(var_omega_z);
+
+		fprintf(stdout, "[Gyroscope Statistics]\n");
+		fprintf(stdout, "Total samples: %d\n", 
+			total_sample_count);
+		fprintf(stdout, "Total sampling time: %fs\n", 
+			elapsed_time);
+		fprintf(stdout, "Total angular drift (deg): : <%f, %f, %f>\n",
+			theta_x, theta_y, theta_z);
+		fprintf(stdout, "Angular drift rate (deg/s): : <%f, %f, %f>\n",
+			fabs(theta_x / elapsed_time), fabs(theta_y / elapsed_time), fabs(theta_z / elapsed_time));
+		fprintf(stdout, "Mean time delta: %fs\n", 
+			elapsed_time / (float)total_sample_count);
+		fprintf(stdout, "Mean angular velocity (deg/s): <%f, %f, %f>\n", 
+			mean_omega_x, mean_omega_y, mean_omega_z);
+		fprintf(stdout, "Std. dev angular velocity (deg/s): <%f, %f, %f>\n", 
+			stddev_omega_x, stddev_omega_y, stddev_omega_z);
+		fprintf(stdout, "Angular velocity variance (deg/s/s): <%f, %f, %f>\n",
+			var_omega_x, var_omega_y, var_omega_z);
+		fprintf(stdout, "[Madgwick Parameters]\n");
+		fprintf(stdout, "[Beta] Max angular velocity variance (deg/s/s): %f\n",
+			max(max(var_omega_x, var_omega_y), var_omega_z));
+		fprintf(stdout, "[Zeta] Max drift rate (deg/s): %f\n",
+			max(max(fabs(theta_x / elapsed_time), fabs(theta_y / elapsed_time)), fabs(theta_z / elapsed_time)));
+		_getch();
+    }
+	else
+	{
+		fprintf(stderr, "Controller must be connected over bluetooth.\n");
+	}
+
+    psmove_disconnect(move);
+	psmove_shutdown();
+
+    return EXIT_SUCCESS;
+}
+