WIP: save cal if disarmed, C -> C++, cleanup

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -214,6 +214,7 @@ battery_simulator start
 tone_alarm start
 rc_update start
 sensors start
+mag_calibrator start
 commander start
 navigator start
 

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -139,3 +139,5 @@ fi
 ###############################################################################
 
 sensors start
+
+mag_calibrator start

boards/bitcraze/crazyflie/default.cmake

@@ -15,7 +15,7 @@ px4_add_board(
 		optical_flow/pmw3901
 		pwm_out
 	MODULES
-		attitude_estimator_q
+		#attitude_estimator_q
 		#camera_feedback
 		commander
 		dataman
@@ -24,7 +24,7 @@ px4_add_board(
 		land_detector
 		landing_target_estimator
 		load_mon
-		local_position_estimator
+		#local_position_estimator
 		logger
 		mavlink
 		mc_att_control
@@ -41,7 +41,7 @@ px4_add_board(
 		dumpfile
 		esc_calib
 		hardfault_log
-		i2cdetect
+		#i2cdetect
 		led_control
 		mixer
 		motor_ramp

boards/px4/fmu-v4/default.cmake

@@ -73,6 +73,7 @@ px4_add_board(
 		load_mon
 		local_position_estimator
 		logger
+		mag_calibrator
 		mavlink
 		mc_att_control
 		mc_hover_thrust_estimator

boards/px4/fmu-v5/default.cmake

@@ -78,6 +78,7 @@ px4_add_board(
 		load_mon
 		local_position_estimator
 		logger
+		mag_calibrator
 		mavlink
 		mc_att_control
 		mc_hover_thrust_estimator

boards/px4/sitl/default.cmake

@@ -39,6 +39,7 @@ px4_add_board(
 		#load_mon
 		local_position_estimator
 		logger
+		mag_calibrator
 		mavlink
 		mc_att_control
 		mc_hover_thrust_estimator

boards/px4/sitl/test.cmake

@@ -38,6 +38,7 @@ px4_add_board(
 		#load_mon
 		local_position_estimator
 		logger
+		mag_calibrator
 		mavlink
 		mc_att_control
 		mc_hover_thrust_estimator

msg/CMakeLists.txt

@@ -55,6 +55,7 @@ set(msg_files
 	ekf_gps_drift.msg
 	esc_report.msg
 	esc_status.msg
+	estimator_mag_cal.msg
 	estimator_innovations.msg
 	estimator_sensor_bias.msg
 	estimator_states.msg

msg/estimator_mag_cal.msg

@@ -0,0 +1,29 @@
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 instance
+
+uint32 device_id                # unique device ID for the selected magnetometer
+
+float32[3] sample		# magnetic field in the NED board axis in Gauss
+
+float32[3] expected_field
+
+float32[3] calibrated_sample
+
+float32 calibrated_sample_magnitude
+
+float32 error
+
+float32 declination
+float32 inclination
+float32 mag_strength
+
+float32[3] bias_estimate
+float32[9] scale_estimate
+float32[3] bias_estimate_variance
+float32[9] scale_estimate_variance
+
+
+float32[3] diagonal_scale
+float32[3] offdiagonal_scale
+float32[3] offsets

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp

@@ -83,7 +83,7 @@ static constexpr wq_config_t UART7{"wq:UART7", 1400, -24};
 static constexpr wq_config_t UART8{"wq:UART8", 1400, -25};
 static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -26};
 
-static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50};
+static constexpr wq_config_t lp_default{"wq:lp_default", 2700, -50};
 
 static constexpr wq_config_t test1{"wq:test1", 2000, 0};
 static constexpr wq_config_t test2{"wq:test2", 2000, 0};

src/lib/sensor_calibration/Magnetometer.cpp

@@ -269,8 +269,8 @@ void Magnetometer::PrintStatus()
 			 (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2));
 	}
 
-#if defined(DEBUG_BUILD)
-	_scale.print()
+#if defined(DEBUG_BUILD) || true
+	_scale.print();
 #endif // DEBUG_BUILD
 }
 

src/lib/sensor_calibration/Magnetometer.hpp

@@ -69,6 +69,7 @@ public:
 	void set_rotation(Rotation rotation);
 
 	uint8_t calibration_count() const { return _calibration_count; }
+	int8_t calibration_index() const { return _calibration_index; }
 	uint32_t device_id() const { return _device_id; }
 	bool enabled() const { return (_priority > 0); }
 	bool external() const { return _external; }

src/modules/logger/logged_topics.cpp

@@ -130,6 +130,8 @@ void LoggedTopics::add_default_topics()
 	add_topic_multi("vehicle_imu", 500, 4);
 	add_topic_multi("vehicle_imu_status", 1000, 4);
 
+	add_topic_multi("estimator_mag_cal");
+
 #ifdef CONFIG_ARCH_BOARD_PX4_SITL
 	add_topic("actuator_controls_virtual_fw");
 	add_topic("actuator_controls_virtual_mc");

src/modules/mag_calibrator/CMakeLists.txt

@@ -0,0 +1,55 @@
+############################################################################
+#
+#   Copyright (c) 2020 PX4 Development Team. 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.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# 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 OWNER 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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE modules__mag_calibrator
+	MAIN mag_calibrator
+	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
+		#-O0
+		#-DDEBUG_BUILD
+	SRCS
+		MagCalibrator.cpp
+		MagCalibrator.hpp
+
+		# TRICAL
+		filter.cpp
+		filter.h
+		TRICAL.cpp
+		TRICAL.h
+	DEPENDS
+		ecl_geo_lookup
+		mathlib
+		px4_work_queue
+	#UNITY_BUILD
+	)

src/modules/mag_calibrator/MagCalibrator.cpp

@@ -0,0 +1,300 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. 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.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * 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 OWNER 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 "MagCalibrator.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <circuit_breaker/circuit_breaker.h>
+#include <mathlib/math/Limits.hpp>
+#include <mathlib/math/Functions.hpp>
+
+#include <lib/ecl/geo_lookup/geo_mag_declination.h>
+
+using namespace matrix;
+using namespace time_literals;
+using math::radians;
+
+MagCalibrator::MagCalibrator() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
+{
+	for (int mag_instance = 0; mag_instance < MAX_SENSOR_COUNT; mag_instance++) {
+		TRICAL_init(&_trical_instance[mag_instance]);
+
+		TRICAL_norm_set(&_trical_instance[mag_instance], 0.4f);
+		TRICAL_noise_set(&_trical_instance[mag_instance], 0.01f);
+	}
+}
+
+MagCalibrator::~MagCalibrator()
+{
+	perf_free(_loop_perf);
+}
+
+bool MagCalibrator::init()
+{
+	ScheduleOnInterval(10_ms); // 100 Hz
+
+	return true;
+}
+
+void MagCalibrator::Run()
+{
+	if (should_exit()) {
+		//_vehicle_angular_velocity_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	// Check if parameters have changed
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		updateParams();
+		//parameters_updated();
+	}
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
+		}
+	}
+
+	if (_vehicle_gps_position_sub.updated()) {
+		vehicle_gps_position_s gps;
+
+		if (_vehicle_gps_position_sub.copy(&gps)) {
+			if ((gps.fix_type >= 2) && (gps.eph < 1000)) {
+
+				const double lat = gps.lat / 1.e7;
+				const double lon = gps.lon / 1.e7;
+
+				// set the magnetic field data returned by the geo library using the current GPS position
+				_mag_declination_gps = get_mag_declination_radians(lat, lon);
+				_mag_inclination_gps = get_mag_inclination_radians(lat, lon);
+
+				const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
+
+				_mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
+
+				//  || (fabsf(mag_strength_gps - _mag_strength_gps) > 0.01f)
+
+				if (!_mag_earth_available) {
+					for (int mag_instance = 0; mag_instance < MAX_SENSOR_COUNT; mag_instance++) {
+						TRICAL_norm_set(&_trical_instance[mag_instance], mag_strength_gps);
+					}
+				}
+
+				_mag_strength_gps = mag_strength_gps;
+
+				_mag_earth_available = true;
+			}
+		}
+	}
+
+	if (_mag_earth_available) {
+		vehicle_attitude_s attitude;
+
+		for (int mag_instance = 0; mag_instance < MAX_SENSOR_COUNT; mag_instance++) {
+			sensor_mag_s mag;
+
+			while (_sensor_mag_subs[mag_instance].update(&mag)) {
+
+				_vehicle_attitude_sub.update(&attitude);
+
+				if (labs(mag.timestamp - attitude.timestamp) < 20_ms) {
+
+					_initialized[mag_instance] = true;
+
+					// expected earth field
+					const Vector3f expected_field_v = Dcmf(Quatf{attitude.q}).transpose() * _mag_earth_pred;
+
+					float sensor_reading[3] {mag.x, mag.y, mag.z};
+					float expected_field[3];
+					expected_field_v.copyTo(expected_field);
+
+					TRICAL_estimate_update(&_trical_instance[mag_instance], sensor_reading, expected_field);
+
+					// Calibrates `measurement` based on the current calibration estimates, and copies the result to `calibrated_measurement`.
+					float calibrated_reading[3];
+					TRICAL_measurement_calibrate(&_trical_instance[mag_instance], sensor_reading, calibrated_reading);
+
+
+					estimator_mag_cal_s mag_cal{};
+
+					mag_cal.instance = mag_instance;
+					mag_cal.device_id = mag.device_id;
+
+					mag_cal.sample[0] = mag.x;
+					mag_cal.sample[1] = mag.y;
+					mag_cal.sample[2] = mag.z;
+
+					expected_field_v.copyTo(mag_cal.expected_field);
+
+					mag_cal.calibrated_sample[0] = calibrated_reading[0];
+					mag_cal.calibrated_sample[1] = calibrated_reading[1];
+					mag_cal.calibrated_sample[2] = calibrated_reading[2];
+
+					mag_cal.calibrated_sample_magnitude = Vector3f(calibrated_reading).norm();
+
+					mag_cal.declination = _mag_declination_gps;
+					mag_cal.inclination = _mag_inclination_gps;
+					mag_cal.mag_strength = _mag_strength_gps;
+
+					// simple error metric
+					mag_cal.error = Vector3f(expected_field_v - Vector3f{calibrated_reading}).norm();
+
+					TRICAL_estimate_get_ext(&_trical_instance[mag_instance], mag_cal.bias_estimate, mag_cal.scale_estimate,
+								mag_cal.bias_estimate_variance, mag_cal.scale_estimate_variance);
+
+					//  0  1  2
+					//  3  4  5
+					//  6  7  8
+					SquareMatrix3f I3;
+					I3.setIdentity();
+					SquareMatrix3f D = SquareMatrix3f{I3 + SquareMatrix3f{mag_cal.scale_estimate}};
+					D.makeBlockSymmetric<3>(0);
+					mag_cal.diagonal_scale[0] = D(0, 0);
+					mag_cal.diagonal_scale[1] = D(1, 1);
+					mag_cal.diagonal_scale[2] = D(2, 2);
+
+					mag_cal.offdiagonal_scale[0] = (D(0, 1) + D(1, 0)) / 2;
+					mag_cal.offdiagonal_scale[1] = (D(0, 2) + D(2, 0)) / 2;
+					mag_cal.offdiagonal_scale[2] = (D(1, 2) + D(2, 1)) / 2;
+
+					// TODO:
+					//  B' = (I_{3x3} + D)B - b
+					//    D*(B+b) = D*B + D^-1*b
+					const Vector3f offsets = D.I() * Vector3f{mag_cal.bias_estimate};
+					offsets.copyTo(mag_cal.offsets);
+
+					mag_cal.timestamp = hrt_absolute_time();
+					_estimator_mag_cal_pub[mag_instance].publish(mag_cal);
+
+					// update calibration
+					if ((mag_cal.error < 0.20f) && (mag_cal.error < _calibration_error[mag_instance])) {
+						_calibration[mag_instance].set_device_id(mag.device_id, mag.is_external);
+						_calibration[mag_instance].set_offset(offsets);
+						_calibration[mag_instance].set_scale(Vector3f{mag_cal.diagonal_scale});
+						_calibration[mag_instance].set_offdiagonal(Vector3f{mag_cal.offdiagonal_scale});
+
+						if (!_armed && hrt_elapsed_time(&_last_calibration_save[mag_instance]) > 10_s) {
+							if (_calibration[mag_instance].calibration_index() < 0) {
+								_calibration[mag_instance].set_calibration_index(mag_instance);
+							}
+
+							_calibration[mag_instance].ParametersSave();
+							param_notify_changes();
+
+							_last_calibration_save[mag_instance] = hrt_absolute_time();
+							_calibration_error[mag_instance] = mag_cal.error;
+						}
+					}
+				}
+			}
+		}
+	}
+
+	perf_end(_loop_perf);
+}
+
+int MagCalibrator::print_status()
+{
+	for (int mag_instance = 0; mag_instance < MAX_SENSOR_COUNT; mag_instance++) {
+
+		if (_initialized[mag_instance]) {
+			_calibration[mag_instance].PrintStatus();
+		}
+	}
+
+	return 0;
+}
+
+int MagCalibrator::task_spawn(int argc, char *argv[])
+{
+	MagCalibrator *instance = new MagCalibrator();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int MagCalibrator::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int MagCalibrator::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("mag_calibrator", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int mag_calibrator_main(int argc, char *argv[])
+{
+	return MagCalibrator::main(argc, argv);
+}

src/modules/mag_calibrator/MagCalibrator.hpp

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. 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.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * 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 OWNER 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "TRICAL.h"
+
+#include <lib/sensor_calibration/Magnetometer.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/estimator_mag_cal.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_mag.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_status.h>
+
+class MagCalibrator : public ModuleBase<MagCalibrator>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	MagCalibrator();
+	~MagCalibrator() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+private:
+	void Run() override;
+
+	static constexpr int MAX_SENSOR_COUNT = 4;
+
+	TRICAL_instance_t _trical_instance[MAX_SENSOR_COUNT];
+
+	bool _initialized[MAX_SENSOR_COUNT] {};
+
+	bool _mag_earth_available{false};
+
+	float _mag_declination_gps{0.f}; // magnetic declination returned by the geo library using the last valid GPS position (rad)
+	float _mag_inclination_gps{0.f}; // magnetic inclination returned by the geo library using the last valid GPS position (rad)
+	float _mag_strength_gps{0.f};    // magnetic strength returned by the geo library using the last valid GPS position (T)
+
+	matrix::Vector3f _mag_earth_pred{};
+
+	calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
+
+	uORB::PublicationMulti<estimator_mag_cal_s> _estimator_mag_cal_pub[MAX_SENSOR_COUNT] {
+		ORB_ID(estimator_mag_cal), ORB_ID(estimator_mag_cal), ORB_ID(estimator_mag_cal), ORB_ID(estimator_mag_cal)
+	};
+
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	uORB::SubscriptionMultiArray<sensor_mag_s, MAX_SENSOR_COUNT> _sensor_mag_subs{ORB_ID::sensor_mag};
+
+	hrt_abstime _last_calibration_save[MAX_SENSOR_COUNT] {};
+	float _calibration_error[MAX_SENSOR_COUNT] {INFINITY, INFINITY, INFINITY, INFINITY};
+
+	bool _armed{false};
+
+	perf_counter_t	_loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+};

src/modules/mag_calibrator/TRICAL.cpp

@@ -0,0 +1,187 @@
+/*
+Copyright (C) 2013 Ben Dyer
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#include <assert.h>
+#include <math.h>
+#include <float.h>
+#include <string.h>
+
+#include "TRICAL.h"
+#include "filter.h"
+
+/*
+TRICAL_init:
+Initializes `instance`. Must be called prior to any other TRICAL procedures
+taking `instance` as a parameter.
+
+If called on an instance that has already been initialized, TRICAL_init will
+reset that instance to its default state.
+*/
+void TRICAL_init(TRICAL_instance_t *instance)
+{
+	memset(instance, 0, sizeof(TRICAL_instance_t));
+
+	instance->field_norm = 1.0f;
+	instance->measurement_noise = 1e-6f;
+
+	/*
+	Set the state covariance diagonal to a small value, so that we can run the
+	Cholesky decomposition without blowing up
+	*/
+	for (unsigned i = 0; i < TRICAL_STATE_DIM * TRICAL_STATE_DIM; i += (TRICAL_STATE_DIM + 1)) {
+		instance->state_covariance[i] = 1e-2f;
+	}
+}
+
+/*
+TRICAL_reset:
+Resets the state and state covariance of `instance`.
+*/
+void TRICAL_reset(TRICAL_instance_t *instance)
+{
+	memset(instance->state, 0, sizeof(instance->state));
+	memset(instance->state_covariance, 0, sizeof(instance->state_covariance));
+
+	/*
+	Set the state covariance diagonal to a small value, so that we can run the
+	Cholesky decomposition without blowing up
+	*/
+	for (unsigned i = 0; i < TRICAL_STATE_DIM * TRICAL_STATE_DIM; i += (TRICAL_STATE_DIM + 1)) {
+		instance->state_covariance[i] = 1e-2f;
+	}
+}
+
+/*
+TRICAL_norm_set:
+Sets the expected field norm (magnitude) of `instance` to `norm`.
+*/
+void TRICAL_norm_set(TRICAL_instance_t *instance, float norm)
+{
+	instance->field_norm = norm;
+}
+
+/*
+TRICAL_norm_get:
+Returns the expected field norm (magnitude) of `instance`.
+*/
+float TRICAL_norm_get(TRICAL_instance_t *instance)
+{
+	return instance->field_norm;
+}
+
+/*
+TRICAL_noise_set:
+Sets the standard deviation in measurement supplied to `instance` to `noise`.
+*/
+void TRICAL_noise_set(TRICAL_instance_t *instance, float noise)
+{
+	instance->measurement_noise = noise;
+}
+
+/*
+TRICAL_noise_get:
+Returns the standard deviation in measurements supplied to `instance`.
+*/
+float TRICAL_noise_get(TRICAL_instance_t *instance)
+{
+	return instance->measurement_noise;
+}
+
+/*
+TRICAL_measurement_count_get:
+Returns the number of measurements previously provided to `instance` via
+TRICAL_estimate_update.
+*/
+unsigned int TRICAL_measurement_count_get(TRICAL_instance_t *instance)
+{
+	return instance->measurement_count;
+}
+
+/*
+TRICAL_estimate_update
+Updates the calibration estimate of `instance` based on the new data in
+`measurement`, and the current field direction estimate `reference_field`.
+Call this function with each reading you receive from your sensor.
+*/
+void TRICAL_estimate_update(TRICAL_instance_t *instance, float measurement[3], float reference_field[3])
+{
+	_trical_filter_iterate(instance, measurement, reference_field);
+	instance->measurement_count++;
+}
+
+/*
+TRICAL_estimate_get
+Copies the calibration bias and scale esimates of `instance` to
+`bias_estimate` and `scale_estimate` respectively. A new calibration estimate
+will be available after every call to TRICAL_estimate_update.
+*/
+void TRICAL_estimate_get(TRICAL_instance_t *instance, float bias_estimate[3], float scale_estimate[9])
+{
+	/* Copy bias estimate from state[0:3] to the destination vector */
+	memcpy(bias_estimate, instance->state, 3 * sizeof(float));
+
+	/* Copy the scale estimate to the destination matrix */
+	memcpy(scale_estimate, &instance->state[3], 9 * sizeof(float));
+}
+
+/*
+TRICAL_estimate_get_ext
+Same as TRICAL_estimate_get, but additionally copies the bias and scale
+estimate variances to `bias_estimate_variance` and `scale_estimate_variance`.
+*/
+void TRICAL_estimate_get_ext(TRICAL_instance_t *instance, float bias_estimate[3], float scale_estimate[9], float bias_estimate_variance[3], float scale_estimate_variance[9])
+{
+	TRICAL_estimate_get(instance, bias_estimate, scale_estimate);
+
+	/* Copy bias estimate covariance from the state covariance diagonal */
+	bias_estimate_variance[0] = instance->state_covariance[0 * 12 + 0];
+	bias_estimate_variance[1] = instance->state_covariance[1 * 12 + 1];
+	bias_estimate_variance[2] = instance->state_covariance[2 * 12 + 2];
+
+	/* Now copy scale estimate covariance. */
+	scale_estimate_variance[0] = instance->state_covariance[3 * 12 + 3];
+	scale_estimate_variance[1] = instance->state_covariance[4 * 12 + 4];
+	scale_estimate_variance[2] = instance->state_covariance[5 * 12 + 5];
+
+	scale_estimate_variance[3] = instance->state_covariance[6 * 12 + 6];
+	scale_estimate_variance[4] = instance->state_covariance[7 * 12 + 7];
+	scale_estimate_variance[5] = instance->state_covariance[8 * 12 + 8];
+
+	scale_estimate_variance[6] = instance->state_covariance[9 * 12 + 9];
+	scale_estimate_variance[7] = instance->state_covariance[10 * 12 + 10];
+	scale_estimate_variance[8] = instance->state_covariance[11 * 12 + 11];
+}
+
+/*
+TRICAL_measurement_calibrate
+Calibrates `measurement` based on the current calibration estimates, and
+copies the result to `calibrated_measurement`. The `measurement` and
+`calibrated_measurement` parameters may be pointers to the same vector.
+
+DO NOT pass the calibrated measurement into TRICAL_estimate_update, as it
+needs the raw measurement values to work.
+*/
+void TRICAL_measurement_calibrate(TRICAL_instance_t * instance, float measurement[3], float calibrated_measurement[3])
+{
+	/* Pass off to the internal function */
+	_trical_measurement_calibrate(instance->state, measurement, calibrated_measurement);
+}

src/modules/mag_calibrator/TRICAL.h

@@ -0,0 +1,132 @@
+/*
+Copyright (C) 2013 Ben Dyer
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#ifndef _TRICAL_H_
+#define _TRICAL_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define TRICAL_STATE_DIM 12
+
+typedef struct {
+	float field_norm;
+	float measurement_noise;
+
+	float state[TRICAL_STATE_DIM];
+	float state_covariance[TRICAL_STATE_DIM * TRICAL_STATE_DIM];
+	unsigned int measurement_count;
+} TRICAL_instance_t;
+
+/*
+TRICAL_init:
+Initializes `instance`. Must be called prior to any other TRICAL procedures
+taking `instance` as a parameter.
+
+If called on an instance that has already been initialized, TRICAL_init will
+reset that instance to its default state.
+*/
+void TRICAL_init(TRICAL_instance_t *instance);
+
+/*
+TRICAL_reset:
+Resets the state and state covariance of `instance`.
+*/
+void TRICAL_reset(TRICAL_instance_t *instance);
+
+/*
+TRICAL_norm_set:
+Sets the expected field norm (magnitude) of `instance` to `norm`. If `norm`
+differs from the instance's current field norm, all estimates are multiplied
+by the ratio of new norm:old norm.
+*/
+void TRICAL_norm_set(TRICAL_instance_t *instance, float norm);
+
+/*
+TRICAL_norm_get:
+Returns the expected field norm (magnitude) of `instance`.
+*/
+float TRICAL_norm_get(TRICAL_instance_t *instance);
+
+/*
+TRICAL_noise_set:
+Sets the standard deviation in measurement supplied to `instance` to `noise`.
+*/
+void TRICAL_noise_set(TRICAL_instance_t *instance, float noise);
+
+/*
+TRICAL_noise_get:
+Returns the standard deviation in measurements supplied to `instance`.
+*/
+float TRICAL_noise_get(TRICAL_instance_t *instance);
+
+/*
+TRICAL_measurement_count_get:
+Returns the number of measurements previously provided to `instance` via
+TRICAL_estimate_update.
+*/
+unsigned int TRICAL_measurement_count_get(TRICAL_instance_t *instance);
+
+/*
+TRICAL_estimate_update
+Updates the calibration estimate of `instance` based on the new data in
+`measurement`, and the current field direction estimate `reference_field`.
+Call this function with each reading you receive from your sensor.
+*/
+void TRICAL_estimate_update(TRICAL_instance_t *instance,
+			    float measurement[3], float reference_field[3]);
+
+/*
+TRICAL_estimate_get
+Copies the calibration bias and scale esimates of `instance` to
+`bias_estimate` and `scale_estimate` respectively. A new calibration estimate
+will be available after every call to TRICAL_estimate_update.
+*/
+void TRICAL_estimate_get(TRICAL_instance_t *instance, float bias_estimate[3],
+			 float scale_estimate[9]);
+
+/*
+TRICAL_estimate_get_ext
+Same as TRICAL_estimate_get, but additionally copies the bias and scale
+estimate variances to `bias_estimate_variance` and `scale_estimate_variance`.
+*/
+void TRICAL_estimate_get_ext(TRICAL_instance_t *instance,
+			     float bias_estimate[3], float scale_estimate[9],
+			     float bias_estimate_variance[3], float scale_estimate_variance[9]);
+
+/*
+TRICAL_measurement_calibrate
+Calibrates `measurement` based on the current calibration estimates, and
+copies the result to `calibrated_measurement`.
+
+DO NOT pass the calibrated measurement into TRICAL_estimate_update, as it
+needs the raw measurement values to work.
+*/
+void TRICAL_measurement_calibrate(TRICAL_instance_t *instance,
+				  float measurement[3], float calibrated_measurement[3]);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/modules/mag_calibrator/filter.cpp

@@ -0,0 +1,337 @@
+/*
+Copyright (C) 2013 Ben Dyer
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#include <assert.h>
+#include <math.h>
+#include <float.h>
+#include <string.h>
+
+#include "TRICAL.h"
+#include "filter.h"
+
+#define X 0
+#define Y 1
+#define Z 2
+#define W 3
+
+#ifdef DEBUG
+#include <stdio.h>
+
+static void _print_matrix(const char *label, float mat[], size_t rows,
+			  size_t cols);
+
+static void _print_matrix(const char *label, float mat[], size_t rows,
+			  size_t cols)
+{
+	printf("%s", label);
+
+	for (size_t i = 0; i < cols; i++) {
+		for (size_t j = 0; j < rows; j++) {
+			printf("%12.6f ", mat[j * cols + i]);
+		}
+
+		printf("\n");
+	}
+}
+#else
+#define _print_matrix(a, b, c, d)
+#endif
+
+/*
+A bit about the UKF formulation in this file: main references are
+[1]: http://www.acsu.buffalo.edu/~johnc/mag_cal05.pdf
+[2]: http://malcolmdshuster.com/Pub_2002c_J_scale_scan.pdf
+
+Since we don't know the attitude matrix, we use scalar values for measurements
+and scalar measurement noise.
+
+The relevant equations are (2a) and (2b) in [2]. Basically, the measurement
+value is:
+
+z = -Bt x (2D + D^2) x B + 2Bt x (I + D) x b - |b|^2
+
+where B is the 3-axis measurement vector, and H is the reference field (making
+|H| the norm of the reference field, which we know). Here, t is used as the
+transpose operator.
+
+The measurement noise is a scalar:
+
+v = 2[(I + D) x B - b]t x E - |E|^2
+
+where B is the 3-axis measurement vector, b is the 3-axis bias estimate,
+E is the measurement noise, I is the 3x3 identity matrix, and D is the 3x3
+scale estimate.
+
+The implementation follows the general approach of https://github.com/sfwa/ukf
+however there a number of simplifications due to the restricted problem
+domain.
+
+Since there is no process model, the apriori mean is the same as the state at
+the start of the update step, and W' is just the set of sigma points. That in
+turn means that there's no need to calculate propagated state covariance from
+W'; it's the same as the covariance was at the start of the update step.
+
+The scalar measurement model also simplifies a lot of the cross-correlation
+and Kalman gain calculation.
+
+Like the full SFWA UKF, and unlike the papers upon which this approach is
+based, we're actually using the scaled unscented Kalman filter. The main
+differences are in the choices of scaling parameters, including alpha, beta
+and kappa. Given the relatively small dimensionality of the filter it's
+probably not strictly necessary to use the scaled formulation, but we'd
+already implemented that so it seemed easier to continue with that approach.
+*/
+
+/*
+_trical_measurement_reduce
+Reduces `measurement` to a scalar value based on the calibration estimate in
+`state` and the current field direction estimate `field` (if no absolute
+orientation calibration is required, the same vector can be supplied for
+`measurement` and `field`).
+
+FIXME: It's not clear to me why the measurement model should be as in (2a)
+above, when we can just apply the current calibration estimate to the
+measurement and take its magnitude. Look into this further if the below
+approach doesn't work.
+*/
+float _trical_measurement_reduce(float state[TRICAL_STATE_DIM], float measurement[3], float field[3])
+{
+	float temp[3];
+	_trical_measurement_calibrate(state, measurement, temp);
+
+	return sqrtf(fabsf(temp[X] * field[X] + temp[Y] * field[Y] + temp[Z] * field[Z]));
+}
+
+/*
+_trical_measurement_calibrate
+Calibrates `measurement` based on the calibration estimate in `state` and
+copies the result to `calibrated_measurement`. The `measurement` and
+`calibrated_measurement` parameters may be pointers to the same vector.
+
+Implements
+B' = (I_{3x3} + D)B - b
+
+where B' is the calibrated measurement, I_{3x3} is the 3x3 identity matrix,
+D is the scale calibration matrix, B is the raw measurement, and b is the bias
+vector.
+*/
+void _trical_measurement_calibrate(float state[TRICAL_STATE_DIM], float measurement[3], float calibrated_measurement[3])
+{
+	float v[3];
+	float *s = state;
+
+	v[0] = measurement[0] - s[0];
+	v[1] = measurement[1] - s[1];
+	v[2] = measurement[2] - s[2];
+
+	/* 3x3 matrix multiply */
+	float *c = calibrated_measurement;
+	c[0] = v[0] * (s[3] + 1.0f) + v[1] * s[4] + v[2] * s[5];
+	c[1] = v[0] * s[6] + v[1] * (s[7] + 1.0f) + v[2] * s[8];
+	c[2] = v[0] * s[9] + v[1] * s[10] + v[2] * (s[11] + 1.0f);
+}
+
+static void matrix_cholesky_decomp_scale_f(unsigned int dim, float L[], const float A[], const float mul)
+{
+	/*
+	9x9:
+	900 mult
+	72 div
+	9 sqrt
+	*/
+	unsigned i;
+	unsigned in;
+	for (i = 0, in = 0; i < dim; i++, in += dim) {
+
+		if (i == 0) {
+			L[i + 0] = sqrtf(A[i + in] * mul);
+		} else {
+			L[i + 0] = (1.f / L[0]) * (A[i] * mul);
+		}
+
+		unsigned j;
+		unsigned jn;
+		for (j = 1, jn = dim; j <= i; j++, jn += dim) {
+			float s = 0;
+
+			for (unsigned kn = 0; kn < j * dim; kn += dim) {
+				s += L[i + kn] * L[j + kn];
+			}
+
+			if (i == j) {
+				L[i + jn] = sqrtf(A[i + in] * mul - s);
+
+			} else {
+				L[i + jn] = (1.f / (L[j + jn])) * (A[i + jn] * mul - s);
+			}
+		}
+	}
+}
+
+/*
+_trical_filter_iterate
+Generates a new calibration estimate for `instance` incorporating the raw
+sensor readings in `measurement`.
+*/
+void _trical_filter_iterate(TRICAL_instance_t *instance, float measurement[3], float field[3])
+{
+	/*
+	LLT decomposition on state covariance matrix, with result multiplied by
+	TRICAL_DIM_PLUS_LAMBDA
+	*/
+	float *covariance = instance->state_covariance;
+	float covariance_llt[TRICAL_STATE_DIM * TRICAL_STATE_DIM]{};
+
+	matrix_cholesky_decomp_scale_f(TRICAL_STATE_DIM, covariance_llt, covariance, TRICAL_DIM_PLUS_LAMBDA);
+
+	_print_matrix("LLT:\n", covariance_llt, TRICAL_STATE_DIM, TRICAL_STATE_DIM);
+
+	/*
+	Generate the sigma points, and use them as the basis of the measurement
+	estimates
+	*/
+	float temp_sigma[TRICAL_STATE_DIM]{};
+	float temp;
+	float measurement_estimates[TRICAL_NUM_SIGMA]{};
+	float measurement_estimate_mean = 0.0;
+
+	/*
+	Handle central sigma point -- process the measurement based on the current
+	state vector
+	*/
+	float *state = instance->state;
+	measurement_estimates[0] = _trical_measurement_reduce(state, measurement, field);
+
+	unsigned i;
+	unsigned col;
+	for (i = 0, col = 0; i < TRICAL_STATE_DIM; i++, col += TRICAL_STATE_DIM) {
+		/*
+		Handle the positive sigma point -- perturb the state vector based on
+		the current column of the covariance matrix, and process the
+		measurement based on the resulting state estimate
+		*/
+		int k;
+		int l;
+		for (k = col, l = 0; l < TRICAL_STATE_DIM; k++, l++) {
+			temp_sigma[l] = state[l] + covariance_llt[k];
+		}
+
+		measurement_estimates[i + 1] = _trical_measurement_reduce(temp_sigma, measurement, field);
+
+		/* Handle the negative sigma point -- mirror of the above */
+		for (k = col, l = 0; l < TRICAL_STATE_DIM; k++, l++) {
+			temp_sigma[l] = state[l] - covariance_llt[k];
+		}
+
+		measurement_estimates[i + 1 + TRICAL_STATE_DIM] = _trical_measurement_reduce(temp_sigma, measurement, field);
+
+		/* Calculate the measurement estimate sum as we go */
+		temp = measurement_estimates[i + 1] + measurement_estimates[i + 1 + TRICAL_STATE_DIM];
+		measurement_estimate_mean += temp;
+	}
+
+	measurement_estimate_mean = measurement_estimate_mean * TRICAL_SIGMA_WMI + measurement_estimates[0] * TRICAL_SIGMA_WM0;
+
+	/*
+	Convert estimates to deviation from mean (so measurement_estimates
+	effectively becomes Z').
+
+	While we're at it, calculate the measurement estimate covariance (which
+	is a scalar quantity).
+	*/
+	float measurement_estimate_covariance = 0.0;
+
+	for (i = 0; i < TRICAL_NUM_SIGMA; i++) {
+		measurement_estimates[i] -= measurement_estimate_mean;
+
+		temp = measurement_estimates[i] * measurement_estimates[i];
+		measurement_estimate_covariance += temp;
+	}
+
+	_print_matrix("Measurement estimates:\n", measurement_estimates, 1, TRICAL_NUM_SIGMA);
+
+	/* Add the sensor noise to the measurement estimate covariance */
+	temp = instance->measurement_noise * instance->measurement_noise;
+	measurement_estimate_covariance += temp;
+
+	/* Calculate cross-correlation matrix (1 x TRICAL_STATE_DIM) */
+	float cross_correlation[TRICAL_STATE_DIM]{};
+
+	// Calculate the innovation (difference between the expected value, i.e. the
+	// field norm, and the measurement estimate mean).
+	float innovation = instance->field_norm - measurement_estimate_mean;
+
+	/* Iterate over sigma points, two at a time */
+	for (i = 0; i < TRICAL_STATE_DIM; i++) {
+		/* Iterate over the cross-correlation matrix */
+		for (int j = 0; j < TRICAL_STATE_DIM; j++) {
+			// We're regenerating the sigma points as we go, so that we don't need to store W'
+			temp = measurement_estimates[i + 1] * (state[j] + covariance_llt[i * TRICAL_STATE_DIM + j]);
+			cross_correlation[j] += temp;
+
+			temp = measurement_estimates[i + 1 + TRICAL_STATE_DIM] * (state[j] - covariance_llt[i * TRICAL_STATE_DIM + j]);
+			cross_correlation[j] += temp;
+		}
+	}
+
+	// Scale the results of the previous step, and add in the scaled central sigma point
+	for (int j = 0; j < TRICAL_STATE_DIM; j++) {
+		temp = TRICAL_SIGMA_WC0 * measurement_estimates[0] * state[j];
+		cross_correlation[j] = TRICAL_SIGMA_WCI * cross_correlation[j] + temp;
+	}
+
+	_print_matrix("Cross-correlation:\n", cross_correlation, 1, TRICAL_STATE_DIM);
+
+	// Update the state -- since the measurement is a scalar, we can calculate
+	// the Kalman gain and update in a single pass.
+	temp = 1.f / measurement_estimate_covariance;
+
+	for (i = 0; i < TRICAL_STATE_DIM; i++) {
+		float kalman_gain = cross_correlation[i] * temp;
+		state[i] += kalman_gain * innovation;
+	}
+
+	_print_matrix("State:\n", state, 1, TRICAL_STATE_DIM);
+
+	/*
+	Update the state covariance:
+	covariance = covariance - kalman gain * measurement estimate covariance * (transpose of kalman gain)
+
+	Since kalman gain is a 1 x TRICAL_STATE_DIM matrix, multiplying by its
+	transpose is just a vector outer product accumulating onto state
+	covariance.
+
+	And, of course, since kalman gain is cross correlation *
+	(1 / measurement estimate covariance), and we multiply by measurement
+	estimate covariance during the outer product, we can skip that whole step
+	and just use cross correlation instead.
+	*/
+	for (i = 0; i < TRICAL_STATE_DIM; i++) {
+		temp = -cross_correlation[i];
+
+		for (int j = 0; j < TRICAL_STATE_DIM; j++) {
+			covariance[i * TRICAL_STATE_DIM + j] += temp * cross_correlation[j];
+		}
+	}
+
+	_print_matrix("State covariance:\n", covariance, TRICAL_STATE_DIM, TRICAL_STATE_DIM);
+}

src/modules/mag_calibrator/filter.h

@@ -0,0 +1,73 @@
+/*
+Copyright (C) 2013 Ben Dyer
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#ifndef _FILTER_H_
+#define _FILTER_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Unscented Kalman filter sigma point and scaling parameters. */
+#define TRICAL_NUM_SIGMA (2 * TRICAL_STATE_DIM + 1)
+
+#define TRICAL_ALPHA_2 (1.0f)
+#define TRICAL_BETA (0.0f)
+#define TRICAL_KAPPA (1.0f)
+#define TRICAL_LAMBDA (TRICAL_ALPHA_2 * (TRICAL_STATE_DIM + TRICAL_KAPPA) - TRICAL_STATE_DIM)
+#define TRICAL_DIM_PLUS_LAMBDA (TRICAL_ALPHA_2 * (TRICAL_STATE_DIM + TRICAL_KAPPA))
+
+#define TRICAL_SIGMA_WM0 (TRICAL_LAMBDA / TRICAL_DIM_PLUS_LAMBDA)
+#define TRICAL_SIGMA_WC0 (TRICAL_SIGMA_WM0 + (1.0f - TRICAL_ALPHA_2 + TRICAL_BETA))
+#define TRICAL_SIGMA_WMI (1.0f / (2.0f * TRICAL_DIM_PLUS_LAMBDA))
+#define TRICAL_SIGMA_WCI (TRICAL_SIGMA_WMI)
+
+/* Internal function prototypes */
+
+/*
+_trical_measurement_reduce
+Reduces `measurement` to a scalar value based on the calibration estimate in
+`state`.
+*/
+float _trical_measurement_reduce(float state[TRICAL_STATE_DIM], float measurement[3], float field[3]);
+
+/*
+_trical_measurement_calibrate
+Calibrates `measurement` based on the calibration estimate in `state` and
+copies the result to `calibrated_measurement`. The `measurement` and
+`calibrated_measurement` parameters may be pointers to the same vector.
+*/
+void _trical_measurement_calibrate(float state[TRICAL_STATE_DIM], float measurement[3],
+				   float calibrated_measurement[3]);
+
+/*
+_trical_filter_iterate
+Generates a new calibration estimate for `instance` incorporating the raw
+sensor readings in `measurement`.
+*/
+void _trical_filter_iterate(TRICAL_instance_t *instance, float measurement[3], float field[3]);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/modules/mag_calibrator/test/CMakeLists.txt

@@ -0,0 +1,49 @@
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8.7 FATAL_ERROR)
+PROJECT(unittest)
+
+# Enable ExternalProject CMake module
+INCLUDE(ExternalProject)
+
+set(CMAKE_CXX_FLAGS "-O3 -mssse3 -oaddmul")
+set(CMAKE_C_FLAGS "-O3 -mssse3 -oaddmul-Weverything -Werror -Wno-documentation -Wno-padded -Wno-unknown-pragmas")
+
+# Set default ExternalProject root directory
+SET_DIRECTORY_PROPERTIES(PROPERTIES EP_PREFIX .)
+
+# Add googletest
+ExternalProject_Add(googletest
+SVN_REPOSITORY http://googletest.googlecode.com/svn/trunk/
+TIMEOUT 30
+# Disable install step
+INSTALL_COMMAND ""
+# Wrap download, configure and build steps in a script to log output
+LOG_DOWNLOAD ON
+LOG_CONFIGURE ON
+LOG_BUILD ON)
+
+# Specify googletest include dir
+ExternalProject_Get_Property(googletest source_dir)
+SET(googletest_dir ${source_dir})
+
+INCLUDE_DIRECTORIES(${googletest_dir}/include ../)
+
+# Add test executable target
+ADD_EXECUTABLE(unittest
+    ../src/filter.c
+    ../src/TRICAL.c
+    test_TRICAL.cpp
+    test_3dmath.cpp
+    test_filter.cpp)
+
+# Create dependency of test on googletest
+ADD_DEPENDENCIES(unittest googletest)
+
+# Specify test's link libraries
+ExternalProject_Get_Property(googletest binary_dir)
+TARGET_LINK_LIBRARIES(unittest
+    ${binary_dir}/${CMAKE_FIND_LIBRARY_PREFIXES}gtest.a
+    ${binary_dir}/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main.a
+    pthread)
+
+ADD_TEST(unittest unittest)
+ADD_CUSTOM_TARGET(check COMMAND ${CMAKE_CTEST_COMMAND} DEPENDS unittest)

src/modules/mag_calibrator/test/test_3dmath.cpp

@@ -0,0 +1,64 @@
+/*
+Copyright (C) 2013 Ben Dyer
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#include <gtest/gtest.h>
+#include <cmath>
+#include <cfloat>
+
+/* C++ complains about the C99 'restrict' qualifier. Just ignore it. */
+#define restrict
+
+#include "TRICAL.h"
+
+/*
+Test LLT decomposition of a square positive definite matrix via the Cholesky
+method.
+
+The expected output is a lower-triangle matrix in column-major format.
+*/
+TEST(MatrixMath, CholeskyLLT)
+{
+	float m1[16] = {
+		18, 22,  54,  42,
+		22, 70,  86,  62,
+		54, 86, 174, 134,
+		42, 62, 134, 106
+	};
+	float expected[16] = {
+		4.24264, 5.18545, 12.72792, 9.8994951,
+		0, 6.5659051, 3.0460384, 1.6245539,
+		0, 0, 1.6497422, 1.8497111,
+		0, 0, 0, 1.3926213
+	};
+
+	matrix_cholesky_decomp_scale_f(4, m1, m1, 1.0);
+	EXPECT_NEAR(expected[0], m1[0], 1e-5);
+	EXPECT_NEAR(expected[1], m1[1], 1e-5);
+	EXPECT_NEAR(expected[2], m1[2], 1e-5);
+	EXPECT_NEAR(expected[3], m1[3], 1e-5);
+	EXPECT_NEAR(expected[5], m1[5], 1e-5);
+	EXPECT_NEAR(expected[6], m1[6], 1e-5);
+	EXPECT_NEAR(expected[7], m1[7], 1e-5);
+	EXPECT_NEAR(expected[10], m1[10], 1e-5);
+	EXPECT_NEAR(expected[11], m1[11], 1e-5);
+	EXPECT_NEAR(expected[15], m1[15], 1e-5);
+}

src/modules/mag_calibrator/test/test_TRICAL.cpp

@@ -0,0 +1,353 @@
+/*
+Copyright (C) 2013 Ben Dyer
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#include <gtest/gtest.h>
+
+/* C++ complains about the C99 'restrict' qualifier. Just ignore it. */
+#define restrict
+
+#include "TRICAL.h"
+
+
+TEST(TRICAL, Initialisation)
+{
+	TRICAL_instance_t cal;
+
+	TRICAL_init(&cal);
+	EXPECT_FLOAT_EQ(1.0f, TRICAL_norm_get(&cal));
+}
+
+/* Check that the field norm can be set and read */
+TEST(TRICAL, NormGetSet)
+{
+	TRICAL_instance_t cal;
+
+	TRICAL_init(&cal);
+	TRICAL_norm_set(&cal, 2.0f);
+	EXPECT_FLOAT_EQ(2.0f, TRICAL_norm_get(&cal));
+}
+
+/* Check that the measurement noise can be set and read */
+TEST(TRICAL, NoiseGetSet)
+{
+	TRICAL_instance_t cal;
+
+	TRICAL_init(&cal);
+	TRICAL_noise_set(&cal, 2.0f);
+	EXPECT_FLOAT_EQ(2.0f, TRICAL_noise_get(&cal));
+}
+
+/* Check that the measurement count can be read */
+TEST(TRICAL, MeasurementCountGet)
+{
+	TRICAL_instance_t cal;
+
+	TRICAL_init(&cal);
+	cal.measurement_count = 15;
+	EXPECT_EQ(15, TRICAL_measurement_count_get(&cal));
+}
+
+/*
+Check that estimate update works with a series of measurements that should
+result in the identity matrix with zero bias
+*/
+TEST(TRICAL, EstimateUpdateIdentity)
+{
+	TRICAL_instance_t cal;
+
+	TRICAL_init(&cal);
+
+	float measurement[3];
+	unsigned int i;
+
+	for (i = 0; i < 100; i++) {
+		measurement[0] = 1.0;
+		measurement[1] = 0.0;
+		measurement[2] = 0.0;
+		TRICAL_estimate_update(&cal, measurement, measurement);
+
+		measurement[0] = 0.0;
+		measurement[1] = 1.0;
+		measurement[2] = 0.0;
+		TRICAL_estimate_update(&cal, measurement, measurement);
+
+		measurement[0] = 0.0;
+		measurement[1] = 0.0;
+		measurement[2] = 1.0;
+		TRICAL_estimate_update(&cal, measurement, measurement);
+
+		measurement[0] = -1.0;
+		measurement[1] = 0.0;
+		measurement[2] = 0.0;
+		TRICAL_estimate_update(&cal, measurement, measurement);
+
+		measurement[0] = 0.0;
+		measurement[1] = -1.0;
+		measurement[2] = 0.0;
+		TRICAL_estimate_update(&cal, measurement, measurement);
+
+		measurement[0] = 0.0;
+		measurement[1] = 0.0;
+		measurement[2] = -1.0;
+		TRICAL_estimate_update(&cal, measurement, measurement);
+	}
+
+	float bias_estimate[3], scale_estimate[9];
+	TRICAL_estimate_get(&cal, bias_estimate, scale_estimate);
+	EXPECT_NEAR(0.0, bias_estimate[0], 2e-2);
+	EXPECT_NEAR(0.0, bias_estimate[1], 2e-2);
+	EXPECT_NEAR(0.0, bias_estimate[2], 2e-2);
+
+	EXPECT_NEAR(0.0, scale_estimate[0], 2e-2);
+	EXPECT_NEAR(0.0, scale_estimate[1], 2e-2);
+	EXPECT_NEAR(0.0, scale_estimate[2], 2e-2);
+
+	EXPECT_NEAR(0.0, scale_estimate[3], 2e-2);
+	EXPECT_NEAR(0.0, scale_estimate[4], 2e-2);
+	EXPECT_NEAR(0.0, scale_estimate[5], 2e-2);
+
+	EXPECT_NEAR(0.0, scale_estimate[6], 2e-2);
+	EXPECT_NEAR(0.0, scale_estimate[7], 2e-2);
+	EXPECT_NEAR(0.0, scale_estimate[8], 2e-2);
+}
+
+/*
+Check that estimate update works with a series of measurements that should
+result in the identity matrix with a bias in the positive X axis
+*/
+TEST(TRICAL, EstimateUpdateBias)
+{
+	TRICAL_instance_t cal;
+
+	TRICAL_init(&cal);
+
+	float measurement[3], ref[3];
+	unsigned int i;
+
+	for (i = 0; i < 200; i++) {
+		measurement[0] = 2.0;
+		measurement[1] = 0.0;
+		measurement[2] = 0.0;
+		ref[0] = 1.0;
+		ref[1] = 0.0;
+		ref[2] = 0.0;
+		TRICAL_estimate_update(&cal, measurement, ref);
+
+		measurement[0] = 1.0;
+		measurement[1] = 1.0;
+		measurement[2] = 0.0;
+		ref[0] = 0.0;
+		ref[1] = 1.0;
+		ref[2] = 0.0;
+		TRICAL_estimate_update(&cal, measurement, ref);
+
+		measurement[0] = 1.0;
+		measurement[1] = 0.0;
+		measurement[2] = 1.0;
+		ref[0] = 0.0;
+		ref[1] = 0.0;
+		ref[2] = 1.0;
+		TRICAL_estimate_update(&cal, measurement, ref);
+
+		measurement[0] = 0.0;
+		measurement[1] = 0.0;
+		measurement[2] = 0.0;
+		ref[0] = -1.0;
+		ref[1] = 0.0;
+		ref[2] = 0.0;
+		TRICAL_estimate_update(&cal, measurement, ref);
+
+		measurement[0] = 1.0;
+		measurement[1] = -1.0;
+		measurement[2] = 0.0;
+		ref[0] = 0.0;
+		ref[1] = -1.0;
+		ref[2] = 0.0;
+		TRICAL_estimate_update(&cal, measurement, ref);
+
+		measurement[0] = 1.0;
+		measurement[1] = 0.0;
+		measurement[2] = -1.0;
+		ref[0] = 0.0;
+		ref[1] = 0.0;
+		ref[2] = -1.0;
+		TRICAL_estimate_update(&cal, measurement, ref);
+	}
+
+	float bias_estimate[3], scale_estimate[9];
+	TRICAL_estimate_get(&cal, bias_estimate, scale_estimate);
+	EXPECT_NEAR(1.0, bias_estimate[0], 2e-2);
+	EXPECT_NEAR(0.0, bias_estimate[1], 2e-2);
+	EXPECT_NEAR(0.0, bias_estimate[2], 2e-2);
+
+	EXPECT_NEAR(0.0, scale_estimate[0], 2e-2);
+	EXPECT_NEAR(0.0, scale_estimate[1], 2e-2);
+	EXPECT_NEAR(0.0, scale_estimate[2], 2e-2);
+
+	EXPECT_NEAR(0.0, scale_estimate[3], 2e-2);
+	EXPECT_NEAR(0.0, scale_estimate[4], 2e-2);
+	EXPECT_NEAR(0.0, scale_estimate[5], 2e-2);
+
+	EXPECT_NEAR(0.0, scale_estimate[6], 2e-2);
+	EXPECT_NEAR(0.0, scale_estimate[7], 2e-2);
+	EXPECT_NEAR(0.0, scale_estimate[8], 2e-2);
+}
+
+/* Check that state estimates can be accessed */
+TEST(TRICAL, EstimateGet)
+{
+	TRICAL_instance_t cal;
+
+	TRICAL_init(&cal);
+	cal.state[0] = 3.0;
+	cal.state[1] = 2.0;
+	cal.state[2] = 1.0;
+	cal.state[3] = 1.0;
+	cal.state[4] = 0.5;
+	cal.state[5] = 0.1;
+	cal.state[6] = 0.5;
+	cal.state[7] = 1.0;
+	cal.state[8] = 0.5;
+	cal.state[9] = 0.1;
+	cal.state[10] = 0.5;
+	cal.state[11] = 1.0;
+
+	float bias_estimate[3], scale_estimate[9];
+	TRICAL_estimate_get(&cal, bias_estimate, scale_estimate);
+	EXPECT_FLOAT_EQ(3.0, bias_estimate[0]);
+	EXPECT_FLOAT_EQ(2.0, bias_estimate[1]);
+	EXPECT_FLOAT_EQ(1.0, bias_estimate[2]);
+
+	EXPECT_FLOAT_EQ(1.0, scale_estimate[0]);
+	EXPECT_FLOAT_EQ(0.5, scale_estimate[1]);
+	EXPECT_FLOAT_EQ(0.1, scale_estimate[2]);
+
+	EXPECT_FLOAT_EQ(0.5, scale_estimate[3]);
+	EXPECT_FLOAT_EQ(1.0, scale_estimate[4]);
+	EXPECT_FLOAT_EQ(0.5, scale_estimate[5]);
+
+	EXPECT_FLOAT_EQ(0.1, scale_estimate[6]);
+	EXPECT_FLOAT_EQ(0.5, scale_estimate[7]);
+	EXPECT_FLOAT_EQ(1.0, scale_estimate[8]);
+}
+
+/* Check that state and covariance estimates can be accessed */
+TEST(TRICAL, EstimateGetExt)
+{
+	TRICAL_instance_t cal;
+
+	TRICAL_init(&cal);
+
+	cal.state[0] = 3.0;
+	cal.state[1] = 2.0;
+	cal.state[2] = 1.0;
+	cal.state[3] = 1.0;
+	cal.state[4] = 0.5;
+	cal.state[5] = 0.1;
+	cal.state[6] = 0.5;
+	cal.state[7] = 1.0;
+	cal.state[8] = 0.5;
+	cal.state[9] = 0.1;
+	cal.state[10] = 0.5;
+	cal.state[11] = 1.0;
+
+	cal.state_covariance[0] = 10.0;
+	cal.state_covariance[13] = 20.0;
+	cal.state_covariance[26] = 30.0;
+	cal.state_covariance[39] = 40.0;
+	cal.state_covariance[52] = 50.0;
+	cal.state_covariance[65] = 60.0;
+	cal.state_covariance[78] = 70.0;
+	cal.state_covariance[91] = 80.0;
+	cal.state_covariance[104] = 90.0;
+	cal.state_covariance[117] = 100.0;
+	cal.state_covariance[130] = 110.0;
+	cal.state_covariance[143] = 120.0;
+
+	float bias_estimate[3], scale_estimate[9], bias_estimate_variance[3],
+	      scale_estimate_variance[9];
+	TRICAL_estimate_get_ext(&cal, bias_estimate, scale_estimate,
+				bias_estimate_variance, scale_estimate_variance);
+
+	EXPECT_FLOAT_EQ(3.0, bias_estimate[0]);
+	EXPECT_FLOAT_EQ(2.0, bias_estimate[1]);
+	EXPECT_FLOAT_EQ(1.0, bias_estimate[2]);
+
+	EXPECT_FLOAT_EQ(1.0, scale_estimate[0]);
+	EXPECT_FLOAT_EQ(0.5, scale_estimate[1]);
+	EXPECT_FLOAT_EQ(0.1, scale_estimate[2]);
+
+	EXPECT_FLOAT_EQ(0.5, scale_estimate[3]);
+	EXPECT_FLOAT_EQ(1.0, scale_estimate[4]);
+	EXPECT_FLOAT_EQ(0.5, scale_estimate[5]);
+
+	EXPECT_FLOAT_EQ(0.1, scale_estimate[6]);
+	EXPECT_FLOAT_EQ(0.5, scale_estimate[7]);
+	EXPECT_FLOAT_EQ(1.0, scale_estimate[8]);
+
+	EXPECT_FLOAT_EQ(10.0, bias_estimate_variance[0]);
+	EXPECT_FLOAT_EQ(20.0, bias_estimate_variance[1]);
+	EXPECT_FLOAT_EQ(30.0, bias_estimate_variance[2]);
+
+	EXPECT_FLOAT_EQ(40.0, scale_estimate_variance[0]);
+	EXPECT_FLOAT_EQ(50.0, scale_estimate_variance[1]);
+	EXPECT_FLOAT_EQ(60.0, scale_estimate_variance[2]);
+
+	EXPECT_FLOAT_EQ(70.0, scale_estimate_variance[3]);
+	EXPECT_FLOAT_EQ(80.0, scale_estimate_variance[4]);
+	EXPECT_FLOAT_EQ(90.0, scale_estimate_variance[5]);
+
+	EXPECT_FLOAT_EQ(100.0, scale_estimate_variance[6]);
+	EXPECT_FLOAT_EQ(110.0, scale_estimate_variance[7]);
+	EXPECT_FLOAT_EQ(120.0, scale_estimate_variance[8]);
+}
+
+/*
+Check that measurement calibration can be performed based on the instance
+state.
+
+This mirrors Filter.CalibrateFull.
+*/
+TEST(TRICAL, Calibrate)
+{
+	TRICAL_instance_t cal;
+
+	TRICAL_init(&cal);
+	cal.state[0] = 3.0;
+	cal.state[1] = 2.0;
+	cal.state[2] = 1.0;
+	cal.state[3] = 1.0;
+	cal.state[4] = 0.5;
+	cal.state[5] = 0.1;
+	cal.state[6] = 0.5;
+	cal.state[7] = 1.0;
+	cal.state[8] = 0.5;
+	cal.state[9] = 0.1;
+	cal.state[10] = 0.5;
+	cal.state[11] = 1.0;
+
+	float measurement[3] = { 1.0, 2.0, 3.0 }, result[3];
+	TRICAL_measurement_calibrate(&cal, measurement, result);
+	EXPECT_FLOAT_EQ(-3.8, result[0]);
+	EXPECT_FLOAT_EQ(0.0, result[1]);
+	EXPECT_FLOAT_EQ(3.8, result[2]);
+}

src/modules/mag_calibrator/test/test_filter.cpp

@@ -0,0 +1,165 @@
+/*
+Copyright (C) 2013 Ben Dyer
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#include <gtest/gtest.h>
+
+/* C++ complains about the C99 'restrict' qualifier. Just ignore it. */
+#define restrict
+
+#include "TRICAL.h"
+#include "filter.h"
+
+/*
+Test measurement calibration with the identity matrix -- all state values are
+zero because the identity matrix is added to the symmetric scale factor matrix
+during measurement calibration.
+*/
+TEST(Filter, CalibrateIdentity)
+{
+	float state[] = {
+		0.0, 0.0, 0.0,
+		0.0, 0.0, 0.0,
+		0.0, 0.0, 0.0,
+		0.0, 0.0, 0.0
+	};
+	float measurement[] = { 1.0, 2.0, 3.0 }, result[3];
+
+	_trical_measurement_calibrate(state, measurement, result);
+	EXPECT_FLOAT_EQ(measurement[0], result[0]);
+	EXPECT_FLOAT_EQ(measurement[1], result[1]);
+	EXPECT_FLOAT_EQ(measurement[2], result[2]);
+}
+
+/*
+Test measurement calibration with a bias, but the identity matrix for scale
+factors.
+
+This test would permit calibration for zero-point bias and hard iron
+distortion only.
+*/
+TEST(Filter, CalibrateBias)
+{
+	float state[] = {
+		1.0, 2.0, 3.0,
+		0.0, 0.0, 0.0,
+		0.0, 0.0, 0.0,
+		0.0, 0.0, 0.0
+	};
+	float measurement[] = { 1.0, 2.0, 3.0 }, result[3];
+
+	_trical_measurement_calibrate(state, measurement, result);
+	EXPECT_FLOAT_EQ(0.0, result[0]);
+	EXPECT_FLOAT_EQ(0.0, result[1]);
+	EXPECT_FLOAT_EQ(0.0, result[2]);
+}
+
+/*
+Test measurement calibration with an orthogonal scale factor matrix -- this
+is added to the identity matrix during the calculation, so the actual scale
+factor matrix used will be:
+2  0  0
+0  2  0
+0  0  2
+
+This test would permit calibration for soft iron distortion of the magnetic
+field, as well as scale factor error in the magnetometer.
+*/
+TEST(Filter, CalibrateOrthogonal)
+{
+	float state[] = {
+		0.0, 0.0, 0.0,
+		1.0, 0.0, 0.0,
+		0.0, 1.0, 0.0,
+		0.0, 0.0, 1.0
+	};
+	float measurement[] = { 1.0, 2.0, 3.0 }, result[3];
+
+	_trical_measurement_calibrate(state, measurement, result);
+	EXPECT_FLOAT_EQ(2.0, result[0]);
+	EXPECT_FLOAT_EQ(4.0, result[1]);
+	EXPECT_FLOAT_EQ(6.0, result[2]);
+}
+
+/*
+Test measurement calibration with a full symmetric scale factor matrix. This
+test would permit calibration for soft iron distortion, as well as scale
+factor error and axis misalignment in the sensor.
+*/
+TEST(Filter, CalibrateSymmetric)
+{
+	float state[] = {
+		0.0, 0.0, 0.0,
+		1.0, 0.5, 0.1,
+		0.5, 1.0, 0.5,
+		0.1, 0.5, 1.0
+	};
+	float measurement[] = { 1.0, 2.0, 3.0 }, result[3];
+
+	_trical_measurement_calibrate(state, measurement, result);
+	EXPECT_FLOAT_EQ(3.3, result[0]);
+	EXPECT_FLOAT_EQ(6.0, result[1]);
+	EXPECT_FLOAT_EQ(7.1, result[2]);
+}
+
+/*
+Test measurement calibration with a full symmetric scale factor matrix as well
+as bias. This test would permit calibration for hard and soft iron distortion,
+as well as bias, scale factor error and axis misalignment in the sensor.
+
+(The general case, including body/sensor axis misalignment, would require
+non-symmetric matrices as well as knowledge of the body attitude at each
+calibration step, and is easier to do elsewhere -- e.g. by measurement.)
+*/
+TEST(Filter, CalibrateFull)
+{
+	float state[] = {
+		3.0, 2.0, 1.0,
+		1.0, 0.5, 0.1,
+		0.5, 1.0, 0.5,
+		0.1, 0.5, 1.0
+	};
+	float measurement[] = { 1.0, 2.0, 3.0 }, result[3];
+
+	_trical_measurement_calibrate(state, measurement, result);
+	EXPECT_FLOAT_EQ(-3.8, result[0]);
+	EXPECT_FLOAT_EQ(0.0, result[1]);
+	EXPECT_FLOAT_EQ(3.8, result[2]);
+}
+
+/*
+Now test measurement reduction, from a raw 3-axis sensor reading to a scalar
+output based on the current calibration estimate. This is used in the UKF
+implementation to generate a measurement estimate for each sigma point.
+*/
+TEST(Filter, MeasurementReduction)
+{
+	float state[] = {
+		0.0, 0.0, 0.0,
+		1.0, 0.5, 0.1,
+		0.5, 1.0, 0.5,
+		0.1, 0.5, 1.0
+	};
+	float measurement[] = { 1.0, 2.0, 3.0 }, result;
+
+	result = _trical_measurement_reduce(state, measurement, measurement);
+	EXPECT_FLOAT_EQ(6.0497932, result);
+}