From 89b81b0bd613dc20ad00f7a6c2808408d6500a85 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 30 Nov 2022 14:50:13 -0500 Subject: [PATCH] create Welford mean Vector with covariance and improve precision with Kahan summation (#20676) - WelfordMeanVector now computes covariance - use Kahan summation for Welford mean (but continue using float32 for actual mean, etc) - WelfordMean and WelfordMeanVector handle initial value and count roll over - Welford mean count rollover at 16 bit max to prevent numerical issues and shift weight to newer samples - sensors/vehicle_imu: update Welford mean usage (now simplified with resets removed) - fix vehicle_imu_status accel var, now properly rotated with full covariance matrix - gyro_calibration: update Welford mean usage --- src/lib/mathlib/CMakeLists.txt | 1 + src/lib/mathlib/math/WelfordMean.hpp | 83 ++++- src/lib/mathlib/math/WelfordMeanTest.cpp | 22 +- src/lib/mathlib/math/WelfordMeanVector.hpp | 161 +++++++++ .../mathlib/math/WelfordMeanVectorTest.cpp | 67 ++++ .../gyro_calibration/GyroCalibration.cpp | 83 ++--- .../gyro_calibration/GyroCalibration.hpp | 15 +- .../sensors/vehicle_imu/VehicleIMU.cpp | 322 +++++++++--------- .../sensors/vehicle_imu/VehicleIMU.hpp | 21 +- 9 files changed, 527 insertions(+), 248 deletions(-) create mode 100644 src/lib/mathlib/math/WelfordMeanVector.hpp create mode 100644 src/lib/mathlib/math/WelfordMeanVectorTest.cpp diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index 3b4377932a..4c63ebd17b 100644 --- a/src/lib/mathlib/CMakeLists.txt +++ b/src/lib/mathlib/CMakeLists.txt @@ -47,3 +47,4 @@ px4_add_unit_gtest(SRC math/test/second_order_reference_model_test.cpp) px4_add_unit_gtest(SRC math/FunctionsTest.cpp) px4_add_unit_gtest(SRC math/test/UtilitiesTest.cpp) px4_add_unit_gtest(SRC math/WelfordMeanTest.cpp) +px4_add_unit_gtest(SRC math/WelfordMeanVectorTest.cpp) diff --git a/src/lib/mathlib/math/WelfordMean.hpp b/src/lib/mathlib/math/WelfordMean.hpp index 80d6b68a60..30c4253811 100644 --- a/src/lib/mathlib/math/WelfordMean.hpp +++ b/src/lib/mathlib/math/WelfordMean.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2022 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 @@ -39,48 +39,97 @@ #pragma once +#include + namespace math { -template +template class WelfordMean { public: // For a new value, compute the new count, new mean, the new M2. - void update(const matrix::Vector &new_value) + bool update(const Type &new_value) { - _count++; + if (_count == 0) { + reset(); + _count = 1; + _mean = new_value; + return false; + + } else if (_count == UINT16_MAX) { + // count overflow + // reset count, but maintain mean and variance + _M2 = _M2 / _count; + _M2_accum = 0; + + _count = 1; + + } else { + _count++; + } // mean accumulates the mean of the entire dataset - const matrix::Vector delta{new_value - _mean}; - _mean += delta / _count; + // delta can be very small compared to the mean, use algorithm to minimise numerical error + const Type delta{new_value - _mean}; + const Type mean_change = delta / _count; + _mean = kahanSummation(_mean, mean_change, _mean_accum); // M2 aggregates the squared distance from the mean // count aggregates the number of samples seen so far - _M2 += delta.emult(new_value - _mean); + const Type M2_change = delta * (new_value - _mean); + _M2 = kahanSummation(_M2, M2_change, _M2_accum); // protect against floating point precision causing negative variances - _M2 = matrix::max(_M2, {}); + _M2 = math::max(_M2, 0.f); + + if (!PX4_ISFINITE(_mean) || !PX4_ISFINITE(_M2)) { + reset(); + return false; + } + + return valid(); } bool valid() const { return _count > 2; } - unsigned count() const { return _count; } + auto count() const { return _count; } void reset() { _count = 0; - _mean = {}; - _M2 = {}; + _mean = 0; + _M2 = 0; + + _mean_accum = 0; + _M2_accum = 0; } // Retrieve the mean, variance and sample variance - matrix::Vector mean() const { return _mean; } - matrix::Vector variance() const { return _M2 / _count; } - matrix::Vector sample_variance() const { return _M2 / (_count - 1); } + Type mean() const { return _mean; } + Type variance() const { return _M2 / (_count - 1); } + Type standard_deviation() const { return std::sqrt(variance()); } + private: - matrix::Vector _mean{}; - matrix::Vector _M2{}; - unsigned _count{0}; + + // Use Kahan summation algorithm to get the sum of "sum_previous" and "input". + // This function relies on the caller to be responsible for keeping a copy of + // "accumulator" and passing this value at the next iteration. + // Ref: https://en.wikipedia.org/wiki/Kahan_summation_algorithm + inline Type kahanSummation(Type sum_previous, Type input, Type &accumulator) + { + const Type y = input - accumulator; + const Type t = sum_previous + y; + accumulator = (t - sum_previous) - y; + return t; + } + + Type _mean{}; + Type _M2{}; + + Type _mean_accum{}; ///< kahan summation algorithm accumulator for mean + Type _M2_accum{}; ///< kahan summation algorithm accumulator for M2 + + uint16_t _count{0}; }; } // namespace math diff --git a/src/lib/mathlib/math/WelfordMeanTest.cpp b/src/lib/mathlib/math/WelfordMeanTest.cpp index 15abbe10c4..f36b25a61b 100644 --- a/src/lib/mathlib/math/WelfordMeanTest.cpp +++ b/src/lib/mathlib/math/WelfordMeanTest.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2021-2022 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 @@ -45,22 +45,20 @@ TEST(WelfordMeanTest, NoisySignal) std::normal_distribution standard_normal_distribution{0.f, std_dev}; std::default_random_engine random_generator{}; // Pseudo-random generator with constant seed random_generator.seed(42); - WelfordMean welford{}; + WelfordMean welford{}; - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 1000; i++) { const float noisy_value = standard_normal_distribution(random_generator); - welford.update(Vector3f(noisy_value, noisy_value - 1.f, noisy_value + 1.f)); + welford.update(noisy_value); } EXPECT_TRUE(welford.valid()); - const Vector3f mean = welford.mean(); - const Vector3f var = welford.variance(); + const float mean = welford.mean(); + const float var = welford.variance(); const float var_real = std_dev * std_dev; - EXPECT_NEAR(mean(0), 0.f, 0.7f); - EXPECT_NEAR(mean(1), -1.f, 0.7f); - EXPECT_NEAR(mean(2), 1.f, 0.7f); - EXPECT_NEAR(var(0), var_real, 0.1f); - EXPECT_NEAR(var(1), var_real, 0.1f); - EXPECT_NEAR(var(2), var_real, 0.1f); + EXPECT_NEAR(mean, 0.f, 0.7f); + + EXPECT_NEAR(var, var_real, 0.1f); + } diff --git a/src/lib/mathlib/math/WelfordMeanVector.hpp b/src/lib/mathlib/math/WelfordMeanVector.hpp new file mode 100644 index 0000000000..d642128dc9 --- /dev/null +++ b/src/lib/mathlib/math/WelfordMeanVector.hpp @@ -0,0 +1,161 @@ +/**************************************************************************** + * + * Copyright (c) 2021-2022 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 WelfordMeanVector.hpp + * + * Welford's online algorithm for computing mean and covariance of a vector. + */ + +#pragma once + +namespace math +{ + +template +class WelfordMeanVector +{ +public: + // For a new value, compute the new count, new mean, the new M2. + bool update(const matrix::Vector &new_value) + { + if (_count == 0) { + reset(); + _count = 1; + _mean = new_value; + return false; + + } else if (_count == UINT16_MAX) { + // count overflow + // reset count, but maintain mean and variance + _M2 = _M2 / _count; + _M2_accum.zero(); + + _count = 1; + + } else { + _count++; + } + + // mean + // accumulates the mean of the entire dataset + // use Kahan summation because delta can be very small compared to the mean + const matrix::Vector delta{new_value - _mean}; + { + const matrix::Vector y = (delta / _count) - _mean_accum; + const matrix::Vector t = _mean + y; + _mean_accum = (t - _mean) - y; + _mean = t; + } + + if (!_mean.isAllFinite()) { + reset(); + return false; + } + + // covariance + // Kahan summation (upper triangle only) + { + // eg C(x,y) += dx * (y - mean_y) + matrix::SquareMatrix m2_change{}; + + for (size_t r = 0; r < N; r++) { + for (size_t c = r; c < N; c++) { + m2_change(r, c) = delta(r) * (new_value(c) - _mean(c)); + } + } + + for (size_t r = 0; r < N; r++) { + for (size_t c = r; c < N; c++) { + + const Type y = m2_change(r, c) - _M2_accum(r, c); + const Type t = _M2(r, c) + y; + _M2_accum(r, c) = (t - _M2(r, c)) - y; + + _M2(r, c) = t; + } + + // protect against floating point precision causing negative variances + if (_M2(r, r) < 0) { + _M2(r, r) = 0; + } + } + + // make symmetric + for (size_t r = 0; r < N; r++) { + for (size_t c = r + 1; c < N; c++) { + _M2(c, r) = _M2(r, c); + } + } + } + + if (!_M2.isAllFinite()) { + reset(); + return false; + } + + return valid(); + } + + bool valid() const { return _count > 2; } + auto count() const { return _count; } + + void reset() + { + _count = 0; + _mean.zero(); + _M2.zero(); + + _mean_accum.zero(); + _M2_accum.zero(); + } + + // Retrieve the mean, variance and sample variance + matrix::Vector mean() const { return _mean; } + matrix::Vector variance() const { return _M2.diag() / (_count - 1); } + matrix::SquareMatrix covariance() const { return _M2 / (_count - 1); } + + Type covariance(int x, int y) const { return _M2(x, y) / (_count - 1); } + +private: + + matrix::Vector _mean{}; + matrix::Vector _mean_accum{}; ///< kahan summation algorithm accumulator for mean + + matrix::SquareMatrix _M2{}; + matrix::SquareMatrix _M2_accum{}; ///< kahan summation algorithm accumulator for M2 + + uint16_t _count{0}; +}; + +} // namespace math diff --git a/src/lib/mathlib/math/WelfordMeanVectorTest.cpp b/src/lib/mathlib/math/WelfordMeanVectorTest.cpp new file mode 100644 index 0000000000..c3dce5e387 --- /dev/null +++ b/src/lib/mathlib/math/WelfordMeanVectorTest.cpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 +#include +#include "WelfordMeanVector.hpp" + +using namespace math; +using matrix::Vector3f; + +TEST(WelfordMeanVectorTest, NoisySignal) +{ + const float std_dev = 3.f; + std::normal_distribution standard_normal_distribution{0.f, std_dev}; + std::default_random_engine random_generator{}; // Pseudo-random generator with constant seed + random_generator.seed(42); + WelfordMeanVector welford{}; + + for (int i = 0; i < 1000; i++) { + const float noisy_value = standard_normal_distribution(random_generator); + welford.update(Vector3f(noisy_value, noisy_value - 1.f, noisy_value + 1.f)); + } + + EXPECT_TRUE(welford.valid()); + const Vector3f mean = welford.mean(); + const Vector3f var = welford.variance(); + + const float var_real = std_dev * std_dev; + + EXPECT_NEAR(mean(0), 0.f, 0.7f); + EXPECT_NEAR(mean(1), -1.f, 0.7f); + EXPECT_NEAR(mean(2), 1.f, 0.7f); + EXPECT_NEAR(var(0), var_real, 0.1f); + EXPECT_NEAR(var(1), var_real, 0.1f); + EXPECT_NEAR(var(2), var_real, 0.1f); +} diff --git a/src/modules/gyro_calibration/GyroCalibration.cpp b/src/modules/gyro_calibration/GyroCalibration.cpp index 026a38ecee..7bca0b6869 100644 --- a/src/modules/gyro_calibration/GyroCalibration.cpp +++ b/src/modules/gyro_calibration/GyroCalibration.cpp @@ -130,7 +130,7 @@ void GyroCalibration::Run() for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { sensor_gyro_s sensor_gyro; - while (_sensor_gyro_subs[gyro].update(&sensor_gyro)) { + if (_sensor_gyro_subs[gyro].update(&sensor_gyro)) { if (PX4_ISFINITE(sensor_gyro.temperature)) { if ((fabsf(_temperature[gyro] - sensor_gyro.temperature) > 1.f) || !PX4_ISFINITE(_temperature[gyro])) { PX4_DEBUG("gyro %d temperature change, resetting all %.6f -> %.6f", gyro, (double)_temperature[gyro], @@ -140,6 +140,7 @@ void GyroCalibration::Run() // reset all on any temperature change Reset(); + return; } } else { @@ -156,13 +157,16 @@ void GyroCalibration::Run() // setting device id, reset all _gyro_calibration[gyro].set_device_id(sensor_gyro.device_id); Reset(); + return; } } if ((_gyro_last_update[gyro] != 0) && (hrt_elapsed_time(&_gyro_last_update[gyro]) > 100_ms)) { - // reset on any timeout + // remove sensor and reset on any timeout + _gyro_calibration[gyro].set_device_id(0); + _gyro_calibration[gyro].Reset(); + Reset(); - _gyro_last_update[gyro] = 0; return; } } @@ -190,19 +194,14 @@ void GyroCalibration::Run() } } + // update calibrations for all available gyros + if (hrt_elapsed_time(&_last_calibration_update) > 5_s) { - // check if sufficient data has been gathered to update calibration - bool sufficient_samples = false; + // check variance again before saving + for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { + if (_gyro_calibration[gyro].device_id() != 0) { - for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { - if ((_gyro_calibration[gyro].device_id() != 0) && _gyro_mean[gyro].valid()) { - // periodically check variance - if (_gyro_mean[gyro].count() % 100 == 0) { - PX4_DEBUG("gyro %d (%" PRIu32 ") variance, [%.9f, %.9f, %.9f] %.9f", gyro, _gyro_calibration[gyro].device_id(), - (double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2), - (double)_gyro_mean[gyro].variance().length()); - - if (_gyro_mean[gyro].variance().longerThan(0.001f)) { + if (!_gyro_mean[gyro].valid() || _gyro_mean[gyro].variance().longerThan(0.001f)) { // reset all PX4_DEBUG("gyro %d variance longer than 0.001f (%.3f), resetting all", gyro, (double)_gyro_mean[gyro].variance().length()); @@ -210,44 +209,49 @@ void GyroCalibration::Run() return; } } - - if (_gyro_mean[gyro].count() > 5000) { - sufficient_samples = true; - - } else { - sufficient_samples = false; - return; - } } - } - - // update calibrations for all available gyros - if (sufficient_samples && (hrt_elapsed_time(&_last_calibration_update) > 10_s)) { bool calibration_updated = false; for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { - if (_gyro_calibration[gyro].device_id() != 0 && _gyro_mean[gyro].valid()) { - // check variance again before saving - if (_gyro_mean[gyro].variance().longerThan(0.001f)) { - // reset all - PX4_DEBUG("gyro %d variance longer than 0.001f (%.3f), resetting all", - gyro, (double)_gyro_mean[gyro].variance().length()); - Reset(); - return; - } + if ((_gyro_calibration[gyro].device_id() != 0) + && _gyro_mean[gyro].valid() && (_gyro_mean[gyro].count() > 100) + ) { const Vector3f old_offset{_gyro_calibration[gyro].offset()}; + const Vector3f new_offset{_gyro_mean[gyro].mean()}; - if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean()) || !_gyro_calibration[gyro].calibrated()) { + bool change_exceeds_stddev = false; + bool variance_significantly_better = false; + + const Vector3f variance = _gyro_mean[gyro].variance(); + + for (int i = 0; i < 3; i++) { + // check if offset changed by more than 1 standard deviation + if (sq(new_offset(i) - old_offset(i)) > variance(i)) { + change_exceeds_stddev = true; + } + + // check if current variance is significantly better than previous cal + if (variance(i) < 0.1f * _gyro_cal_variance[gyro](i)) { + variance_significantly_better = true; + } + } + + // update if offset changed by more than 1 standard deviation or currently uncalibrated + if ((change_exceeds_stddev || variance_significantly_better || !_gyro_calibration[gyro].calibrated()) + && _gyro_calibration[gyro].set_offset(new_offset) + ) { calibration_updated = true; + _gyro_cal_variance[gyro] = variance; + PX4_INFO("gyro %d (%" PRIu32 ") updating offsets [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f] %.1f degC", gyro, _gyro_calibration[gyro].device_id(), (double)old_offset(0), (double)old_offset(1), (double)old_offset(2), - (double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2), + (double)new_offset(0), (double)new_offset(1), (double)new_offset(2), (double)_temperature[gyro]); perf_count(_calibration_updated_perf); @@ -269,11 +273,10 @@ void GyroCalibration::Run() if (param_save) { param_notify_changes(); - _last_calibration_update = hrt_absolute_time(); } - } - Reset(); + Reset(); + } } } diff --git a/src/modules/gyro_calibration/GyroCalibration.hpp b/src/modules/gyro_calibration/GyroCalibration.hpp index 10d922bb50..53092855d0 100644 --- a/src/modules/gyro_calibration/GyroCalibration.hpp +++ b/src/modules/gyro_calibration/GyroCalibration.hpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #include #include @@ -79,11 +79,17 @@ private: void Reset() { - for (auto &m : _gyro_mean) { - m.reset(); + for (int gyro = 0; gyro < MAX_SENSORS; gyro++) { + _gyro_mean[gyro].reset(); + _gyro_last_update[gyro] = 0; } + + _last_calibration_update = hrt_absolute_time(); } + // return the square of two floating point numbers + static constexpr float sq(float var) { return var * var; } + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_status_sub{ORB_ID::vehicle_status}; @@ -91,7 +97,8 @@ private: uORB::SubscriptionMultiArray _sensor_gyro_subs{ORB_ID::sensor_gyro}; calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {}; - math::WelfordMean _gyro_mean[MAX_SENSORS] {}; + math::WelfordMeanVector _gyro_mean[MAX_SENSORS] {}; + matrix::Vector3f _gyro_cal_variance[MAX_SENSORS] {}; float _temperature[MAX_SENSORS] {}; hrt_abstime _gyro_last_update[MAX_SENSORS] {}; diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index a86d79808e..8ca2464b5e 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -198,12 +198,15 @@ void VehicleIMU::Run() bool consume_all_gyro = !_intervals_configured || _data_gap; // monitor scheduling latency and force catch up with latest gyro if falling behind - if (_sensor_gyro_sub.updated() && (_gyro_update_latency_mean.count() > 100) - && (_gyro_update_latency_mean.mean()(1) > _gyro_interval_us * 1e-6f)) { - - PX4_DEBUG("gyro update mean sample latency: %.6f, publish latency %.6f", - (double)_gyro_update_latency_mean.mean()(0), - (double)_gyro_update_latency_mean.mean()(1)); + if (_intervals_configured && !consume_all_gyro && _sensor_gyro_sub.updated() + && _gyro_publish_latency_mean_us.valid() && _gyro_mean_interval_us.valid() + && (_gyro_publish_latency_mean_us.mean() > _gyro_mean_interval_us.mean()) + ) { + PX4_DEBUG("%d gyro publish latency %.6f > %.6f us (mean interval)", + _instance, + (double)_gyro_publish_latency_mean_us.mean(), + (double)_gyro_mean_interval_us.mean() + ); consume_all_gyro = true; } @@ -219,7 +222,8 @@ void VehicleIMU::Run() // update accel until integrator ready and caught up to gyro while (_sensor_accel_sub.updated() && (!_accel_integrator.integral_ready() || !_intervals_configured || _data_gap - || (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us)))) { + || (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us))) + ) { if (UpdateAccel()) { updated = true; @@ -239,16 +243,6 @@ void VehicleIMU::Run() // publish if both accel & gyro integrators are ready if (_intervals_configured && _accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) { if (Publish()) { - // record gyro publication latency and integrated samples - if (_gyro_update_latency_mean.count() > 10000) { - // reset periodically to avoid numerical issues - _gyro_update_latency_mean.reset(); - } - - const float time_run_s = now_us * 1e-6f; - - _gyro_update_latency_mean.update(Vector2f{time_run_s - _gyro_timestamp_sample_last * 1e-6f, time_run_s - _gyro_timestamp_last * 1e-6f}); - break; } } @@ -285,15 +279,28 @@ bool VehicleIMU::UpdateAccel() _data_gap = true; perf_count(_accel_generation_gap_perf); - // reset average sample measurement - _accel_interval_mean.reset(); - } else { // collect sample interval average for filters if (accel.timestamp_sample > _accel_timestamp_sample_last) { - if ((_accel_timestamp_sample_last != 0) && (accel.samples > 0)) { - float interval_us = accel.timestamp_sample - _accel_timestamp_sample_last; - _accel_interval_mean.update(Vector2f{interval_us, interval_us / accel.samples}); + if (_accel_timestamp_sample_last != 0) { + const float interval_us = accel.timestamp_sample - _accel_timestamp_sample_last; + + _accel_mean_interval_us.update(interval_us); + _accel_fifo_mean_interval_us.update(interval_us / math::max(accel.samples, (uint8_t)1)); + + // check measured interval periodically + if (_accel_mean_interval_us.valid() && (_accel_mean_interval_us.count() % 10 == 0)) { + + const float interval_mean = _accel_mean_interval_us.mean(); + + // update sample rate if previously invalid or changed by more than 1 standard deviation + const bool diff_exceeds_stddev = sq(interval_mean - _accel_interval_us) > _accel_mean_interval_us.variance(); + + if (!PX4_ISFINITE(_accel_interval_us) || diff_exceeds_stddev) { + // update integrator configuration if interval has changed by more than 10% + _update_integrator_config = true; + } + } } } else { @@ -305,47 +312,6 @@ bool VehicleIMU::UpdateAccel() PX4_ERR("%d - accel %" PRIu32 " timestamp (%" PRIu64 ") < timestamp_sample (%" PRIu64 ")", _instance, accel.device_id, accel.timestamp, accel.timestamp_sample); } - - const int interval_count = _accel_interval_mean.count(); - const float interval_variance = _accel_interval_mean.variance()(0); - - // check measured interval periodically - if ((_accel_interval_mean.valid() && (interval_count % 10 == 0)) - && (!PX4_ISFINITE(_accel_interval_best_variance) - || (interval_variance < _accel_interval_best_variance) - || (interval_count > 1000))) { - - const float interval_mean = _accel_interval_mean.mean()(0); - const float interval_mean_fifo = _accel_interval_mean.mean()(1); - - // update sample rate if previously invalid or changed - const float interval_delta_us = fabsf(interval_mean - _accel_interval_us); - const float percent_changed = interval_delta_us / _accel_interval_us; - - if (!PX4_ISFINITE(_accel_interval_us) || (percent_changed > 0.001f)) { - if (PX4_ISFINITE(interval_mean) && PX4_ISFINITE(interval_mean_fifo) && PX4_ISFINITE(interval_variance)) { - // update integrator configuration if interval has changed by more than 10% - if (interval_delta_us > 0.1f * _accel_interval_us) { - _update_integrator_config = true; - } - - _accel_interval_us = interval_mean; - _accel_interval_best_variance = interval_variance; - - _status.accel_rate_hz = 1e6f / interval_mean; - _status.accel_raw_rate_hz = 1e6f / interval_mean_fifo; // FIFO - _publish_status = true; - - } else { - _accel_interval_mean.reset(); - } - } - } - - if (interval_count > 10000) { - // reset periodically to prevent numerical issues - _accel_interval_mean.reset(); - } } _accel_last_generation = _sensor_accel_sub.get_last_generation(); @@ -359,15 +325,17 @@ bool VehicleIMU::UpdateAccel() // temperature average - if (_accel_temperature_sum_count == 0) { - _accel_temperature_sum = accel.temperature; + if (PX4_ISFINITE(accel.temperature)) { + if ((_accel_temperature_sum_count == 0) || !PX4_ISFINITE(_accel_temperature_sum)) { + _accel_temperature_sum = accel.temperature; + _accel_temperature_sum_count = 1; - } else { - _accel_temperature_sum += accel.temperature; + } else { + _accel_temperature_sum += accel.temperature; + _accel_temperature_sum_count += 1; + } } - _accel_temperature_sum_count++; - const float dt = (accel.timestamp_sample - _accel_timestamp_sample_last) * 1e-6f; _accel_timestamp_sample_last = accel.timestamp_sample; @@ -440,15 +408,28 @@ bool VehicleIMU::UpdateGyro() _data_gap = true; perf_count(_gyro_generation_gap_perf); - // reset average sample measurement - _gyro_interval_mean.reset(); - } else { // collect sample interval average for filters if (gyro.timestamp_sample > _gyro_timestamp_sample_last) { - if ((_gyro_timestamp_sample_last != 0) && (gyro.samples > 0)) { - float interval_us = gyro.timestamp_sample - _gyro_timestamp_sample_last; - _gyro_interval_mean.update(Vector2f{interval_us, interval_us / gyro.samples}); + if (_gyro_timestamp_sample_last != 0) { + + const float interval_us = gyro.timestamp_sample - _gyro_timestamp_sample_last; + + _gyro_mean_interval_us.update(interval_us); + _gyro_fifo_mean_interval_us.update(interval_us / math::max(gyro.samples, (uint8_t)1)); + + // check measured interval periodically + if (_gyro_mean_interval_us.valid() && (_gyro_mean_interval_us.count() % 10 == 0)) { + const float interval_mean = _gyro_mean_interval_us.mean(); + + // update sample rate if previously invalid or changed by more than 1 standard deviation + const bool diff_exceeds_stddev = sq(interval_mean - _gyro_interval_us) > _gyro_mean_interval_us.variance(); + + if (!PX4_ISFINITE(_gyro_interval_us) || diff_exceeds_stddev) { + // update integrator configuration if interval has changed by more than 10% + _update_integrator_config = true; + } + } } } else { @@ -460,50 +441,13 @@ bool VehicleIMU::UpdateGyro() PX4_ERR("%d - gyro %" PRIu32 " timestamp (%" PRIu64 ") < timestamp_sample (%" PRIu64 ")", _instance, gyro.device_id, gyro.timestamp, gyro.timestamp_sample); } - - const int interval_count = _gyro_interval_mean.count(); - const float interval_variance = _gyro_interval_mean.variance()(0); - - // check measured interval periodically - if ((_gyro_interval_mean.valid() && (interval_count % 10 == 0)) - && (!PX4_ISFINITE(_gyro_interval_best_variance) - || (interval_variance < _gyro_interval_best_variance) - || (interval_count > 1000))) { - - const float interval_mean = _gyro_interval_mean.mean()(0); - const float interval_mean_fifo = _gyro_interval_mean.mean()(1); - - // update sample rate if previously invalid or changed - const float interval_delta_us = fabsf(interval_mean - _gyro_interval_us); - const float percent_changed = interval_delta_us / _gyro_interval_us; - - if (!PX4_ISFINITE(_gyro_interval_us) || (percent_changed > 0.001f)) { - if (PX4_ISFINITE(interval_mean) && PX4_ISFINITE(interval_mean_fifo) && PX4_ISFINITE(interval_variance)) { - // update integrator configuration if interval has changed by more than 10% - if (interval_delta_us > 0.1f * _gyro_interval_us) { - _update_integrator_config = true; - } - - _gyro_interval_us = interval_mean; - _gyro_interval_best_variance = interval_variance; - - _status.gyro_rate_hz = 1e6f / interval_mean; - _status.gyro_raw_rate_hz = 1e6f / interval_mean_fifo; // FIFO - _publish_status = true; - - } else { - _gyro_interval_mean.reset(); - } - } - } - - if (interval_count > 10000) { - // reset periodically to prevent numerical issues - _gyro_interval_mean.reset(); - } } _gyro_last_generation = _sensor_gyro_sub.get_last_generation(); + + const float dt = (gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f; + + _gyro_timestamp_sample_last = gyro.timestamp_sample; _gyro_timestamp_last = gyro.timestamp; _gyro_calibration.set_device_id(gyro.device_id); @@ -513,21 +457,18 @@ bool VehicleIMU::UpdateGyro() _status.gyro_error_count = gyro.error_count; } - // temperature average - if (_gyro_temperature_sum_count == 0) { - _gyro_temperature_sum = gyro.temperature; + if (PX4_ISFINITE(gyro.temperature)) { + if ((_gyro_temperature_sum_count == 0) || !PX4_ISFINITE(_gyro_temperature_sum)) { + _gyro_temperature_sum = gyro.temperature; + _gyro_temperature_sum_count = 1; - } else { - _gyro_temperature_sum += gyro.temperature; + } else { + _gyro_temperature_sum += gyro.temperature; + _gyro_temperature_sum_count += 1; + } } - _gyro_temperature_sum_count++; - - - const float dt = (gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f; - _gyro_timestamp_sample_last = gyro.timestamp_sample; - const Vector3f gyro_raw{gyro.x, gyro.y, gyro.z}; _raw_gyro_mean.update(gyro_raw); _gyro_integrator.put(gyro_raw, dt); @@ -620,41 +561,66 @@ bool VehicleIMU::Publish() // vehicle_imu_status // publish before vehicle_imu so that error counts are available synchronously if needed - const bool imu_status_publishing_interval_exceeded = hrt_elapsed_time(&_status.timestamp) >= - kIMUStatusPublishingInterval; + const bool status_publish_interval_exceeded = (hrt_elapsed_time(&_status.timestamp) >= kIMUStatusPublishingInterval); if (_raw_accel_mean.valid() && _raw_gyro_mean.valid() - && (_publish_status || imu_status_publishing_interval_exceeded)) { + && _accel_mean_interval_us.valid() && _gyro_mean_interval_us.valid() + && (_publish_status || status_publish_interval_exceeded) + ) { // Accel - _status.accel_device_id = _accel_calibration.device_id(); + { + _status.accel_device_id = _accel_calibration.device_id(); - // accel mean and variance - Vector3f(_accel_calibration.rotation() * _raw_accel_mean.mean()).copyTo(_status.mean_accel); - Vector3f(_accel_calibration.rotation() * _raw_accel_mean.variance()).copyTo(_status.var_accel); + _status.accel_rate_hz = 1e6f / _accel_mean_interval_us.mean(); + _status.accel_raw_rate_hz = 1e6f / _accel_fifo_mean_interval_us.mean(); // FIFO - // accel temperature - _status.temperature_accel = _accel_temperature_sum / _accel_temperature_sum_count; - _accel_temperature_sum = NAN; - _accel_temperature_sum_count = 0; + // accel mean and variance + const Dcmf &R = _accel_calibration.rotation(); + Vector3f(R * _raw_accel_mean.mean()).copyTo(_status.mean_accel); + // variance from R * COV * R^T + const Matrix3f cov = R * _raw_accel_mean.covariance() * R.transpose(); + cov.diag().copyTo(_status.var_accel); + + // temperature + if ((_accel_temperature_sum_count > 0) && PX4_ISFINITE(_accel_temperature_sum)) { + _status.temperature_accel = _accel_temperature_sum / _accel_temperature_sum_count; + + } else { + _status.temperature_accel = NAN; + } + } // Gyro - _status.gyro_device_id = _gyro_calibration.device_id(); + { + _status.gyro_device_id = _gyro_calibration.device_id(); - // gyro mean and variance - Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.mean()).copyTo(_status.mean_gyro); - Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.variance()).copyTo(_status.var_gyro); + _status.gyro_rate_hz = 1e6f / _gyro_mean_interval_us.mean(); + _status.gyro_raw_rate_hz = 1e6f / _gyro_fifo_mean_interval_us.mean(); // FIFO - // Gyro delta angle coning metric = length of coning corrections averaged since last status publication - _status.delta_angle_coning_metric = _coning_norm_accum / _coning_norm_accum_total_time_s; - _coning_norm_accum = 0; - _coning_norm_accum_total_time_s = 0; + // gyro mean and variance + const Dcmf &R = _gyro_calibration.rotation(); + Vector3f(R * _raw_gyro_mean.mean()).copyTo(_status.mean_gyro); - // gyro temperature - _status.temperature_gyro = _gyro_temperature_sum / _gyro_temperature_sum_count; - _gyro_temperature_sum = NAN; - _gyro_temperature_sum_count = 0; + // variance from R * COV * R^T + const Matrix3f cov = R * _raw_gyro_mean.covariance() * R.transpose(); + cov.diag().copyTo(_status.var_gyro); + + + // Gyro delta angle coning metric = length of coning corrections averaged since last status publication + _status.delta_angle_coning_metric = _coning_norm_accum / _coning_norm_accum_total_time_s; + _coning_norm_accum = 0; + _coning_norm_accum_total_time_s = 0; + + // temperature + if ((_gyro_temperature_sum_count > 0) && PX4_ISFINITE(_gyro_temperature_sum)) { + _status.temperature_gyro = _gyro_temperature_sum / _gyro_temperature_sum_count; + + } else { + _status.temperature_gyro = NAN; + } + } // publish _status.timestamp = hrt_absolute_time(); @@ -662,9 +628,14 @@ bool VehicleIMU::Publish() _publish_status = false; - if (imu_status_publishing_interval_exceeded) { + if (status_publish_interval_exceeded) { _raw_accel_mean.reset(); + _accel_temperature_sum = NAN; + _accel_temperature_sum_count = 0; + _raw_gyro_mean.reset(); + _gyro_temperature_sum = NAN; + _gyro_temperature_sum_count = 0; } } @@ -683,6 +654,10 @@ bool VehicleIMU::Publish() // reset clip counts _delta_velocity_clipping = 0; + // record gyro publication latency and integrated samples + _gyro_publish_latency_mean_us.update(imu.timestamp - _gyro_timestamp_last); + _gyro_update_latency_mean_us.update(imu.timestamp - _gyro_timestamp_sample_last); + updated = true; } } @@ -692,31 +667,34 @@ bool VehicleIMU::Publish() void VehicleIMU::UpdateIntegratorConfiguration() { - if (PX4_ISFINITE(_accel_interval_us) && PX4_ISFINITE(_gyro_interval_us)) { + if (_accel_mean_interval_us.valid() && _gyro_mean_interval_us.valid()) { + + const float accel_interval_us = _accel_mean_interval_us.mean(); + const float gyro_interval_us = _gyro_mean_interval_us.mean(); // determine number of sensor samples that will get closest to the desired integration interval - uint8_t gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / _gyro_interval_us)); + uint8_t gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / gyro_interval_us)); // if gyro samples exceeds queue depth, instead round to nearest even integer to improve scheduling options if (gyro_integral_samples > sensor_gyro_s::ORB_QUEUE_LENGTH) { - gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / _gyro_interval_us / 2) * 2); + gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / gyro_interval_us / 2) * 2); } - uint32_t integration_interval_us = roundf(gyro_integral_samples * _gyro_interval_us); + uint32_t integration_interval_us = roundf(gyro_integral_samples * gyro_interval_us); // accel follows gyro as closely as possible - uint8_t accel_integral_samples = math::max(1, (int)roundf(integration_interval_us / _accel_interval_us)); + uint8_t accel_integral_samples = math::max(1, (int)roundf(integration_interval_us / accel_interval_us)); // let the gyro set the configuration and scheduling // relaxed minimum integration time required - _accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * _accel_interval_us)); + _accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * accel_interval_us)); _accel_integrator.set_reset_samples(accel_integral_samples); - _gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * _gyro_interval_us)); + _gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * gyro_interval_us)); _gyro_integrator.set_reset_samples(gyro_integral_samples); - _backup_schedule_timeout_us = math::constrain((int)math::min(sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us, - sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_interval_us) / 2, 1000, 20000); + _backup_schedule_timeout_us = math::constrain((int)math::min(sensor_accel_s::ORB_QUEUE_LENGTH * accel_interval_us, + sensor_gyro_s::ORB_QUEUE_LENGTH * gyro_interval_us) / 2, 1000, 20000); // gyro: find largest integer multiple of gyro_integral_samples for (int n = sensor_gyro_s::ORB_QUEUE_LENGTH; n > 0; n--) { @@ -734,7 +712,10 @@ void VehicleIMU::UpdateIntegratorConfiguration() PX4_DEBUG("accel (%" PRIu32 "), gyro (%" PRIu32 "), accel samples: %" PRIu8 ", gyro samples: %" PRIu8 ", accel interval: %.1f, gyro interval: %.1f sub samples: %d", _accel_calibration.device_id(), _gyro_calibration.device_id(), accel_integral_samples, gyro_integral_samples, - (double)_accel_interval_us, (double)_gyro_interval_us, n); + (double)accel_interval_us, (double)_gyro_interval_us, n); + + _accel_interval_us = accel_interval_us; + _gyro_interval_us = gyro_interval_us; break; } @@ -765,12 +746,19 @@ void VehicleIMU::PrintStatus() PX4_INFO_RAW("[vehicle_imu] %" PRIu8 " - Accel: %" PRIu32 ", interval: %.1f us (SD %.1f us), Gyro: %" PRIu32 ", interval: %.1f us (SD %.1f us)\n", _instance, - _accel_calibration.device_id(), (double)_accel_interval_us, (double)sqrtf(_accel_interval_best_variance), - _gyro_calibration.device_id(), (double)_gyro_interval_us, (double)sqrtf(_gyro_interval_best_variance)); + _accel_calibration.device_id(), (double)_accel_mean_interval_us.mean(), + (double)_accel_mean_interval_us.standard_deviation(), + _gyro_calibration.device_id(), (double)_gyro_mean_interval_us.mean(), + (double)_gyro_mean_interval_us.standard_deviation()); - PX4_DEBUG("gyro update mean sample latency: %.6f s, publish latency %.6f s, gyro interval %.6f s", - (double)_gyro_update_latency_mean.mean()(0), (double)_gyro_update_latency_mean.mean()(1), - (double)(_gyro_interval_us * 1e-6f)); +#if defined(DEBUG_BUILD) + PX4_INFO_RAW("[vehicle_imu] %" PRIu8 + " - gyro update sample latency: %.1f us (SD %.1f us), publish latency %.1f us (SD %.1f us)\n", + _instance, + (double)_gyro_update_latency_mean_us.mean(), (double)_gyro_update_latency_mean_us.standard_deviation(), + (double)_gyro_publish_latency_mean_us.mean(), (double)_gyro_publish_latency_mean_us.standard_deviation() + ); +#endif // DEBUG_BUILD perf_print_counter(_accel_generation_gap_perf); perf_print_counter(_gyro_generation_gap_perf); diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 130efc390f..ab06dbf03e 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -92,6 +93,9 @@ private: void SensorCalibrationSaveAccel(); void SensorCalibrationSaveGyro(); + // return the square of two floating point numbers + static constexpr float sq(float var) { return var * var; } + uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; uORB::PublicationMulti _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; @@ -119,16 +123,17 @@ private: hrt_abstime _gyro_timestamp_sample_last{0}; hrt_abstime _gyro_timestamp_last{0}; - math::WelfordMean _raw_accel_mean{}; - math::WelfordMean _raw_gyro_mean{}; + math::WelfordMeanVector _raw_accel_mean{}; + math::WelfordMeanVector _raw_gyro_mean{}; - math::WelfordMean _accel_interval_mean{}; - math::WelfordMean _gyro_interval_mean{}; + math::WelfordMean _accel_mean_interval_us{}; + math::WelfordMean _accel_fifo_mean_interval_us{}; - math::WelfordMean _gyro_update_latency_mean{}; + math::WelfordMean _gyro_mean_interval_us{}; + math::WelfordMean _gyro_fifo_mean_interval_us{}; - float _accel_interval_best_variance{(float)INFINITY}; - float _gyro_interval_best_variance{(float)INFINITY}; + math::WelfordMean _gyro_update_latency_mean_us{}; + math::WelfordMean _gyro_publish_latency_mean_us{}; float _accel_interval_us{NAN}; float _gyro_interval_us{NAN}; @@ -166,7 +171,7 @@ private: bool _data_gap{false}; bool _update_integrator_config{true}; bool _intervals_configured{false}; - bool _publish_status{false}; + bool _publish_status{true}; const uint8_t _instance;