forked from Archive/PX4-Autopilot
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
This commit is contained in:
Daniel Agar
GitHub
parent
5155346d60
commit
89b81b0bd6
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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 <lib/mathlib/mathlib.h>
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
template <typename Type, size_t N>
|
||||
template <typename Type = float>
|
||||
class WelfordMean
|
||||
{
|
||||
public:
|
||||
// For a new value, compute the new count, new mean, the new M2.
|
||||
void update(const matrix::Vector<Type, N> &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<Type, N> 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<Type, N> mean() const { return _mean; }
|
||||
matrix::Vector<Type, N> variance() const { return _M2 / _count; }
|
||||
matrix::Vector<Type, N> 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<Type, N> _mean{};
|
||||
matrix::Vector<Type, N> _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
|
||||
|
|
|
|||
|
|
@ -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<float> standard_normal_distribution{0.f, std_dev};
|
||||
std::default_random_engine random_generator{}; // Pseudo-random generator with constant seed
|
||||
random_generator.seed(42);
|
||||
WelfordMean<float, 3> welford{};
|
||||
WelfordMean<float> 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);
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 <typename Type, size_t N>
|
||||
class WelfordMeanVector
|
||||
{
|
||||
public:
|
||||
// For a new value, compute the new count, new mean, the new M2.
|
||||
bool update(const matrix::Vector<Type, N> &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<Type, N> delta{new_value - _mean};
|
||||
{
|
||||
const matrix::Vector<Type, N> y = (delta / _count) - _mean_accum;
|
||||
const matrix::Vector<Type, N> 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<Type, N> 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<Type, N> mean() const { return _mean; }
|
||||
matrix::Vector<Type, N> variance() const { return _M2.diag() / (_count - 1); }
|
||||
matrix::SquareMatrix<Type, N> covariance() const { return _M2 / (_count - 1); }
|
||||
|
||||
Type covariance(int x, int y) const { return _M2(x, y) / (_count - 1); }
|
||||
|
||||
private:
|
||||
|
||||
matrix::Vector<Type, N> _mean{};
|
||||
matrix::Vector<Type, N> _mean_accum{}; ///< kahan summation algorithm accumulator for mean
|
||||
|
||||
matrix::SquareMatrix<Type, N> _M2{};
|
||||
matrix::SquareMatrix<Type, N> _M2_accum{}; ///< kahan summation algorithm accumulator for M2
|
||||
|
||||
uint16_t _count{0};
|
||||
};
|
||||
|
||||
} // namespace math
|
||||
|
|
@ -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 <gtest/gtest.h>
|
||||
#include <random>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include "WelfordMeanVector.hpp"
|
||||
|
||||
using namespace math;
|
||||
using matrix::Vector3f;
|
||||
|
||||
TEST(WelfordMeanVectorTest, NoisySignal)
|
||||
{
|
||||
const float std_dev = 3.f;
|
||||
std::normal_distribution<float> standard_normal_distribution{0.f, std_dev};
|
||||
std::default_random_engine random_generator{}; // Pseudo-random generator with constant seed
|
||||
random_generator.seed(42);
|
||||
WelfordMeanVector<float, 3> 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);
|
||||
}
|
||||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@
|
|||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/mathlib/math/WelfordMean.hpp>
|
||||
#include <lib/mathlib/math/WelfordMeanVector.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/sensor_calibration/Gyroscope.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
|
@ -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_s, MAX_SENSORS> _sensor_gyro_subs{ORB_ID::sensor_gyro};
|
||||
|
||||
calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {};
|
||||
math::WelfordMean<float, 3> _gyro_mean[MAX_SENSORS] {};
|
||||
math::WelfordMeanVector<float, 3> _gyro_mean[MAX_SENSORS] {};
|
||||
matrix::Vector3f _gyro_cal_variance[MAX_SENSORS] {};
|
||||
float _temperature[MAX_SENSORS] {};
|
||||
hrt_abstime _gyro_last_update[MAX_SENSORS] {};
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -37,6 +37,7 @@
|
|||
|
||||
#include <lib/mathlib/math/Limits.hpp>
|
||||
#include <lib/mathlib/math/WelfordMean.hpp>
|
||||
#include <lib/mathlib/math/WelfordMeanVector.hpp>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/sensor_calibration/Accelerometer.hpp>
|
||||
|
|
@ -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_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
|
||||
uORB::PublicationMulti<vehicle_imu_status_s> _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<float, 3> _raw_accel_mean{};
|
||||
math::WelfordMean<float, 3> _raw_gyro_mean{};
|
||||
math::WelfordMeanVector<float, 3> _raw_accel_mean{};
|
||||
math::WelfordMeanVector<float, 3> _raw_gyro_mean{};
|
||||
|
||||
math::WelfordMean<float, 2> _accel_interval_mean{};
|
||||
math::WelfordMean<float, 2> _gyro_interval_mean{};
|
||||
math::WelfordMean<float> _accel_mean_interval_us{};
|
||||
math::WelfordMean<float> _accel_fifo_mean_interval_us{};
|
||||
|
||||
math::WelfordMean<float, 2> _gyro_update_latency_mean{};
|
||||
math::WelfordMean<float> _gyro_mean_interval_us{};
|
||||
math::WelfordMean<float> _gyro_fifo_mean_interval_us{};
|
||||
|
||||
float _accel_interval_best_variance{(float)INFINITY};
|
||||
float _gyro_interval_best_variance{(float)INFINITY};
|
||||
math::WelfordMean<float> _gyro_update_latency_mean_us{};
|
||||
math::WelfordMean<float> _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;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue