diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index e0fa783d97..a200569b40 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 3d60adc206..0e78473fdc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -139,3 +139,5 @@ fi ############################################################################### sensors start + +mag_calibrator start diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index d779e0cae7..c23733ac5e 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/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 diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 4375d2ab10..34d2e48554 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/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 diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index a9e32f6b27..4ba6205c80 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/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 diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index ea4664dcd7..fa907674f0 100644 --- a/boards/px4/sitl/default.cmake +++ b/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 diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index b33fed50e4..0d73efad6b 100644 --- a/boards/px4/sitl/test.cmake +++ b/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 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 263d12f8ec..0cbc253a06 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/estimator_mag_cal.msg b/msg/estimator_mag_cal.msg new file mode 100644 index 0000000000..51962cff40 --- /dev/null +++ b/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 diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 41edbd228f..c08d44fb33 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/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", 10700, -50}; static constexpr wq_config_t test1{"wq:test1", 2000, 0}; static constexpr wq_config_t test2{"wq:test2", 2000, 0}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index a108a12819..7a6bf19780 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/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"); diff --git a/src/modules/mag_calibrator/3dmath.h b/src/modules/mag_calibrator/3dmath.h new file mode 100644 index 0000000000..acebb7634e --- /dev/null +++ b/src/modules/mag_calibrator/3dmath.h @@ -0,0 +1,93 @@ +/* +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 _3DMATH_H_ +#define _3DMATH_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#define X 0 +#define Y 1 +#define Z 2 +#define W 3 + +#ifndef absval +#define absval(x) ((x) < 0 ? -x : x) +#endif + +#ifndef min +#define min(a,b) (((a) < (b)) ? (a) : (b)) +#endif + +#ifndef max +#define max(a,b) (((a) > (b)) ? (a) : (b)) +#endif + +#ifndef M_PI +#define M_PI ((real_t)3.14159265358979323846) +#define M_PI_2 (M_PI * 0.5) +#define M_PI_4 (M_PI * 0.25) +#endif + +#define sqrt_inv(x) (1.0f / (float)sqrt((x))) +#define divide(a, b) ((a) / (b)) +#define recip(a) (1.0f / (a)) +#define _nassert(x) + +static void matrix_cholesky_decomp_scale_f(unsigned int dim, float L[], + const float A[], const float mul) +{ + assert(L && A && dim); + _nassert((size_t)L % 8 == 0); + _nassert((size_t)A % 8 == 0); + + /* + 9x9: + 900 mult + 72 div + 9 sqrt + */ + + unsigned int i, j, kn, in, jn; + + for (i = 0, in = 0; i < dim; i++, in += dim) { + L[i + 0] = (i == 0) ? sqrtf(A[i + in] * mul) : recip(L[0]) * (A[i] * mul); + + for (j = 1, jn = dim; j <= i; j++, jn += dim) { + float s = 0; + + for (kn = 0; kn < j * dim; kn += dim) { + s += L[i + kn] * L[j + kn]; + } + + L[i + jn] = (i == j) ? sqrtf(A[i + in] * mul - s) : recip(L[j + jn]) * (A[i + jn] * mul - s); + } + } +} + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/modules/mag_calibrator/CMakeLists.txt b/src/modules/mag_calibrator/CMakeLists.txt new file mode 100644 index 0000000000..02b8684583 --- /dev/null +++ b/src/modules/mag_calibrator/CMakeLists.txt @@ -0,0 +1,56 @@ +############################################################################ +# +# 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 + 3dmath.h + filter.c + filter.h + TRICAL.c + TRICAL.h + DEPENDS + ecl_geo_lookup + mathlib + px4_work_queue + #UNITY_BUILD + ) diff --git a/src/modules/mag_calibrator/MagCalibrator.cpp b/src/modules/mag_calibrator/MagCalibrator.cpp new file mode 100644 index 0000000000..402b0329ca --- /dev/null +++ b/src/modules/mag_calibrator/MagCalibrator.cpp @@ -0,0 +1,303 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include + +#include + +using namespace matrix; +using namespace time_literals; +using math::radians; + +MagCalibrator::MagCalibrator() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + 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], 1e-4f); + } +} + +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(¶m_update); + + updateParams(); + //parameters_updated(); + } + + 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 + //_calibration.set_offsets(offsets); + } + } + } + } + + perf_end(_loop_perf); +} + +int MagCalibrator::print_status() +{ + for (int mag_instance = 0; mag_instance < MAX_SENSOR_COUNT; mag_instance++) { + + if (_initialized[mag_instance]) { + + float bias_estimate[3]; + float scale_estimate[9]; + float bias_estimate_variance[3]; + float scale_estimate_variance[9]; + + TRICAL_estimate_get_ext(&_trical_instance[mag_instance], bias_estimate, scale_estimate, bias_estimate_variance, + scale_estimate_variance); + + printf("bias_estimate: %d\n", mag_instance); + + for (int i = 0; i < 3; i++) { + printf("%05.4f ", (double)bias_estimate[i]); + } + + printf("\n\n"); + + printf("scale_estimate: %d\n", mag_instance); + + for (int c = 0; c < 3; c++) { + for (int r = 0; r < 3; r++) { + printf("%05.4f ", (double)scale_estimate[r + 3 * c]); + } + + printf("\n"); + } + + printf("\n"); + } + } + + 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); +} diff --git a/src/modules/mag_calibrator/MagCalibrator.hpp b/src/modules/mag_calibrator/MagCalibrator.hpp new file mode 100644 index 0000000000..5745db2983 --- /dev/null +++ b/src/modules/mag_calibrator/MagCalibrator.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class MagCalibrator : public ModuleBase, 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_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::SubscriptionMultiArray _sensor_mag_subs{ORB_ID::sensor_mag}; + + perf_counter_t _loop_perf; /**< loop duration performance counter */ +}; diff --git a/src/modules/mag_calibrator/TRICAL.c b/src/modules/mag_calibrator/TRICAL.c new file mode 100644 index 0000000000..46d0cb86ec --- /dev/null +++ b/src/modules/mag_calibrator/TRICAL.c @@ -0,0 +1,237 @@ +/* +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 +#include +#include +#include + +#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) +{ + assert(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 + */ + unsigned int i; + + for (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) +{ + assert(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 + */ + unsigned int i; + + for (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) +{ + assert(instance); + assert(norm > FLT_EPSILON); + + instance->field_norm = norm; +} + +/* +TRICAL_norm_get: +Returns the expected field norm (magnitude) of `instance`. +*/ +float TRICAL_norm_get(TRICAL_instance_t *instance) +{ + assert(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) +{ + assert(instance); + assert(noise > FLT_EPSILON); + + instance->measurement_noise = noise; +} + +/* +TRICAL_noise_get: +Returns the standard deviation in measurements supplied to `instance`. +*/ +float TRICAL_noise_get(TRICAL_instance_t *instance) +{ + assert(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) +{ + assert(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]) +{ + assert(instance); + assert(measurement); + assert(reference_field); + + _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 *restrict instance, + float bias_estimate[3], float scale_estimate[9]) +{ + assert(instance); + assert(bias_estimate); + assert(scale_estimate); + assert(bias_estimate != scale_estimate); + + /* 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 *restrict 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); + + /* A bit of paranoia to avoid potential undefined behaviour */ + assert(bias_estimate_variance); + assert(scale_estimate_variance); + assert(bias_estimate != bias_estimate_variance); + assert(bias_estimate != scale_estimate_variance); + assert(scale_estimate != bias_estimate_variance); + assert(scale_estimate != scale_estimate_variance); + assert(bias_estimate_variance != scale_estimate_variance); + + /* 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 *restrict instance, + float measurement[3], float calibrated_measurement[3]) +{ + assert(instance); + assert(measurement); + assert(calibrated_measurement); + + /* Pass off to the internal function */ + _trical_measurement_calibrate(instance->state, measurement, + calibrated_measurement); +} diff --git a/src/modules/mag_calibrator/TRICAL.h b/src/modules/mag_calibrator/TRICAL.h new file mode 100644 index 0000000000..ab291bd660 --- /dev/null +++ b/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 diff --git a/src/modules/mag_calibrator/filter.c b/src/modules/mag_calibrator/filter.c new file mode 100644 index 0000000000..3807b617f0 --- /dev/null +++ b/src/modules/mag_calibrator/filter.c @@ -0,0 +1,346 @@ +/* +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 +#include +#include +#include + +#include "TRICAL.h" +#include "filter.h" +#include "3dmath.h" + +#ifdef DEBUG +#include + +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]) +{ + assert(state && measurement && calibrated_measurement); + + float v[3]; + float *restrict s = state; + float *restrict c = calibrated_measurement; + + v[0] = measurement[0] - s[0]; + v[1] = measurement[1] - s[1]; + v[2] = measurement[2] - s[2]; + + /* 3x3 matrix multiply */ + 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); +} + +/* +_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]) +{ + unsigned int i, j, k, l, col; + + float *restrict covariance = instance->state_covariance; + float *restrict state = instance->state; + + /* + LLT decomposition on state covariance matrix, with result multiplied by + TRICAL_DIM_PLUS_LAMBDA + */ + float covariance_llt[TRICAL_STATE_DIM * TRICAL_STATE_DIM]; + memset(covariance_llt, 0, sizeof(covariance_llt)); + 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], temp; + float measurement_estimates[TRICAL_NUM_SIGMA], measurement_estimate_mean; + + measurement_estimate_mean = 0.0; + + /* + Handle central sigma point -- process the measurement based on the current + state vector + */ + measurement_estimates[0] = _trical_measurement_reduce(state, measurement, + field); + + __asm__ __volatile__(""); + + 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 + */ + __asm__ __volatile__(""); + + 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 */ + __asm__ __volatile__(""); + + 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; + + __asm__ __volatile__(""); + + 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]; + memset(cross_correlation, 0, sizeof(cross_correlation)); + + /* + Calculate the innovation (difference between the expected value, i.e. the + field norm, and the measurement estimate mean). + */ + float innovation; + innovation = instance->field_norm - measurement_estimate_mean; + + /* Iterate over sigma points, two at a time */ + __asm__ __volatile__(""); + + for (i = 0; i < TRICAL_STATE_DIM; i++) { + /* Iterate over the cross-correlation matrix */ + __asm__ __volatile__(""); + + for (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 + */ + __asm__ __volatile__(""); + + for (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. + */ + float kalman_gain; + temp = recip(measurement_estimate_covariance); + __asm__ __volatile__(""); + + for (i = 0; i < TRICAL_STATE_DIM; i++) { + 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. + */ + __asm__ __volatile__(""); + + for (i = 0; i < TRICAL_STATE_DIM; i++) { + temp = -cross_correlation[i]; + + __asm__ __volatile__(""); + + for (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); +} diff --git a/src/modules/mag_calibrator/filter.h b/src/modules/mag_calibrator/filter.h new file mode 100644 index 0000000000..7e00ec0b35 --- /dev/null +++ b/src/modules/mag_calibrator/filter.h @@ -0,0 +1,78 @@ +/* +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 diff --git a/src/modules/mag_calibrator/test/CMakeLists.txt b/src/modules/mag_calibrator/test/CMakeLists.txt new file mode 100644 index 0000000000..d508fe6a78 --- /dev/null +++ b/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) diff --git a/src/modules/mag_calibrator/test/test_3dmath.cpp b/src/modules/mag_calibrator/test/test_3dmath.cpp new file mode 100644 index 0000000000..6ea2c60118 --- /dev/null +++ b/src/modules/mag_calibrator/test/test_3dmath.cpp @@ -0,0 +1,65 @@ +/* +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 +#include +#include + +/* C++ complains about the C99 'restrict' qualifier. Just ignore it. */ +#define restrict + +#include "TRICAL.h" +#include "3dmath.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); +} diff --git a/src/modules/mag_calibrator/test/test_TRICAL.cpp b/src/modules/mag_calibrator/test/test_TRICAL.cpp new file mode 100644 index 0000000000..c74aadd061 --- /dev/null +++ b/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 + +/* 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]); +} diff --git a/src/modules/mag_calibrator/test/test_filter.cpp b/src/modules/mag_calibrator/test/test_filter.cpp new file mode 100644 index 0000000000..03f9df0a6c --- /dev/null +++ b/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 + +/* 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); +}