fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 13:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

create Welford mean Vector with covariance and improve precision with Kahan summation (#20676)

Browse Source 
- 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
2022-11-30 14:50:13 -05:00
committed by GitHub
parent 5155346d60
commit 89b81b0bd6
9 changed files with 527 additions and 248 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gyro_calibration/GyroCalibration.cpp
+43 -40
View File
@@ -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();
}
}
}
src/modules/gyro_calibration/GyroCalibration.hpp
+11 -4
View File
@@ -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] {};