diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 7702ec2491..b844cda724 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -50,6 +50,7 @@ add_subdirectory(hysteresis) add_subdirectory(l1) add_subdirectory(landing_slope) add_subdirectory(led) +add_subdirectory(magnetometer_bias_estimator) add_subdirectory(mathlib) add_subdirectory(mixer) add_subdirectory(mixer_module) diff --git a/src/lib/magnetometer_bias_estimator/CMakeLists.txt b/src/lib/magnetometer_bias_estimator/CMakeLists.txt new file mode 100644 index 0000000000..4d2594ee40 --- /dev/null +++ b/src/lib/magnetometer_bias_estimator/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_subdirectory(FieldSensorBiasEstimator) + +px4_add_library(MagnetometerBiasEstimator + MagnetometerBiasEstimator.cpp +) +target_include_directories(MagnetometerBiasEstimator + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) +target_link_libraries(MagnetometerBiasEstimator PUBLIC FieldSensorBiasEstimator) diff --git a/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/CMakeLists.txt b/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/CMakeLists.txt new file mode 100644 index 0000000000..9e359ab3ad --- /dev/null +++ b/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_library(FieldSensorBiasEstimator INTERFACE) +target_include_directories(FieldSensorBiasEstimator INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC FieldSensorBiasEstimatorTest.cpp LINKLIBS FieldSensorBiasEstimator) diff --git a/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimator.hpp b/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimator.hpp new file mode 100644 index 0000000000..fb75453a7f --- /dev/null +++ b/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimator.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 DAfieldES (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 DAfieldE. + * + ****************************************************************************/ + +/** + * @file FieldSensorBiasEstimator.hpp + * + * Estimator for the magnetometer/accelerometer bias calibration parameters to run online with the help of Gyroscope data. + * + * @author Matthias Grob + * @author Mathieu Bresciani + * + * Publication documenting the algorithm: + * Adaptive Estimation of Measurement Bias in Three-Dimensional Field Sensors with Angular-Rate Sensors: Theory and Comparative Experimental Evaluation + * Giancarlo Troni and Louis L. Whitcomb, Department of Mechanical Engineering, Johns Hopkins University, Baltimore, Maryland 21218, USA + * + * http://www.roboticsproceedings.org/rss09/p50.pdf + */ + +#pragma once + +#include + +class FieldSensorBiasEstimator +{ +public: + FieldSensorBiasEstimator() = default; + ~FieldSensorBiasEstimator() = default; + + // Set initial states + void setField(const matrix::Vector3f &field) { _field_prev = field; } + void setBias(const matrix::Vector3f &bias) { _state_bias = bias; } + + /** + * Update the estimator and extract updated biases. + * @param gyro bias corrected gyroscope data in the same coordinate frame as the field sensor data + * @param field biased field sensor data + * @param dt time in seconds since the last update + */ + void updateEstimate(const matrix::Vector3f &gyro, const matrix::Vector3f &field, const float dt) + { + const matrix::Vector3f field_pred = _field_prev + (-gyro % (_field_prev - _state_bias)) * dt; + const matrix::Vector3f field_innov = field - field_pred; + _state_bias += GAIN_BIAS * (-gyro % field_innov); + _field_prev = field; + } + + const matrix::Vector3f &getField() { return _field_prev; } + const matrix::Vector3f &getBias() { return _state_bias; } + +private: + // gains + static constexpr float GAIN_BIAS = 100.f; + + // states + matrix::Vector3f _field_prev{}; + matrix::Vector3f _state_bias{}; +}; diff --git a/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimatorTest.cpp b/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimatorTest.cpp new file mode 100644 index 0000000000..b1ed252558 --- /dev/null +++ b/src/lib/magnetometer_bias_estimator/FieldSensorBiasEstimator/FieldSensorBiasEstimatorTest.cpp @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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 +#include + +using namespace matrix; + +TEST(MagnetometerBiasEstimatorTest, AllZeroCase) +{ + FieldSensorBiasEstimator field_sensor_bias_estimator; + const Vector3f virtual_gyro = Vector3f(0.f, 0.f, 0.1f); + Vector3f virtual_unbiased_mag = Vector3f(0.9f, 0.f, 1.79f); // taken from SITL jmavsim initialization + const Vector3f virtual_bias(0.2f, -0.4f, 0.5f); + Vector3f virtual_mag = virtual_unbiased_mag + virtual_bias; + + // Initialize with the current measurement + field_sensor_bias_estimator.setField(virtual_mag); + + for (int i = 0; i <= 1000; i++) { + float dt = .01f; + + // turn the mag values according to the gyro + + virtual_mag = virtual_unbiased_mag + virtual_bias; + // printf("---- %i\n", i); + // printf("virtual_gyro\n"); virtual_gyro.print(); + // printf("virtual_mag\n"); virtual_mag.print(); + + field_sensor_bias_estimator.updateEstimate(virtual_gyro, virtual_mag, dt); + virtual_unbiased_mag = Dcmf(AxisAnglef(-virtual_gyro * dt)) * virtual_unbiased_mag; + } + + const Vector3f bias_est = field_sensor_bias_estimator.getBias(); + EXPECT_NEAR(bias_est(0), virtual_bias(0), 1e-3f) << "Estimated X bias " << bias_est(0); + EXPECT_NEAR(bias_est(1), virtual_bias(1), 1e-3f) << "Estimated Y bias " << bias_est(1); + // The Z bias is not observable due to pure yaw rotation + EXPECT_NEAR(bias_est(2), 0.f, 1e-3f) << "Estimated Z bias " << bias_est(2); +} diff --git a/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.cpp b/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.cpp new file mode 100644 index 0000000000..c0754ddc34 --- /dev/null +++ b/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file MagnetometerBiasEstimator.cpp + */ + +#include "MagnetometerBiasEstimator.hpp" + +#include +#include +#include + +using namespace matrix; +using namespace time_literals; + +MagnetometerBiasEstimator::MagnetometerBiasEstimator(const matrix::Dcmf &board_rotation) : + ModuleParams(nullptr), + _board_rotation(board_rotation) +{} + +void MagnetometerBiasEstimator::extractBias(vehicle_magnetometer_s &mag_raw, const sensor_combined_s &gyro_raw) +{ + // fill in vectors + Vector3f gyro(gyro_raw.gyro_rad); + Vector3f mag(mag_raw.magnetometer_ga); + + // prepare delta time in seconds + hrt_abstime time_stamp_current = hrt_absolute_time(); + float dt = math::min((time_stamp_current - _time_stamp_last_loop), hrt_abstime(500_ms)) / 1e6f; + _time_stamp_last_loop = time_stamp_current; + + _field_sensor_bias_estimator.updateEstimate(gyro, mag, dt); + const Vector3f &bias = _field_sensor_bias_estimator.getBias(); + + // save the bias to the parameters to apply it given the right circumstances + const bool longer_than_10_sec = (time_stamp_current - _time_stamp_last_save) > hrt_abstime(10_s); + const bool bias_significant = bias.norm_squared() > (0.01f * 0.01f); + + _actuator_armed_sub.update(); + + if (!_actuator_armed_sub.get().armed && bias_significant && longer_than_10_sec) { + saveBias(bias); + _time_stamp_last_save = time_stamp_current; + } + + (mag - bias).copyTo(mag_raw.magnetometer_ga); +} + +void MagnetometerBiasEstimator::saveBias(const matrix::Vector3f &bias) +{ + // estimated bias is in body frame, but the driver needs sensor frame + const Vector3f transformed_bias = _board_rotation.transpose() * bias; + + // get current calibration + updateParams(); + Vector3f calibration_bias(_param_cal_mag0_xoff.get(), + _param_cal_mag0_yoff.get(), + _param_cal_mag0_zoff.get()); + + // estimated bias comes on top of existing calibration + calibration_bias += transformed_bias; + + // save new calibration + _param_cal_mag0_xoff.set(calibration_bias(0)); + _param_cal_mag0_yoff.set(calibration_bias(1)); + _param_cal_mag0_zoff.set(calibration_bias(2)); + _param_cal_mag0_xoff.commit(); + _param_cal_mag0_yoff.commit(); + _param_cal_mag0_zoff.commit(); + + // reset internal bias to zero because from now the driver will correct + _field_sensor_bias_estimator.setBias(Vector3f()); +} diff --git a/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.hpp b/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.hpp new file mode 100644 index 0000000000..4fb0867463 --- /dev/null +++ b/src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file MagnetometerBiasEstimator.hpp + * + * Runs the FieldSensorBiasEstimator with PX4 magnetometer and gyroscope data and saves the result to the calibration parameters. + * + * @author Matthias Grob + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +class MagnetometerBiasEstimator : public ModuleParams +{ +public: + MagnetometerBiasEstimator(const matrix::Dcmf &board_rotation); + ~MagnetometerBiasEstimator() = default; + + /** + * Update the estimator and extract updated magnetometer biases. + * @param mag_raw struct containing the magnetometer data to operate on (gets adjusted with current estimate) + * @param raw struct containing the gyroscope data to use + */ + void extractBias(vehicle_magnetometer_s &mag_raw, const sensor_combined_s &gyro_raw); + +private: + void updateEstimate(const matrix::Vector3f &gyro, const matrix::Vector3f &mag, const float dt); + void saveBias(const matrix::Vector3f &bias); + + FieldSensorBiasEstimator _field_sensor_bias_estimator{}; + const matrix::Dcmf &_board_rotation; + + hrt_abstime _time_stamp_last_loop{0}; + hrt_abstime _time_stamp_last_save{0}; + + uORB::SubscriptionData _actuator_armed_sub{ORB_ID(actuator_armed)}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_cal_mag0_xoff, + (ParamFloat) _param_cal_mag0_yoff, + (ParamFloat) _param_cal_mag0_zoff + ) +};