forked from Archive/PX4-Autopilot
[WIP] per sensor noise defaults
This commit is contained in:
Daniel Agar
parent
774ad80ba0
commit
3ccbe8f8b6
|
|
@ -146,6 +146,8 @@ public:
|
|||
|
||||
void set(float val) { _val = val; }
|
||||
|
||||
bool is_default() { return param_value_is_default(handle()); }
|
||||
|
||||
void reset()
|
||||
{
|
||||
param_reset_no_notification(handle());
|
||||
|
|
@ -198,6 +200,8 @@ public:
|
|||
|
||||
void set(float val) { _val = val; }
|
||||
|
||||
bool is_default() { return param_value_is_default(handle()); }
|
||||
|
||||
void reset()
|
||||
{
|
||||
param_reset_no_notification(handle());
|
||||
|
|
@ -248,6 +252,8 @@ public:
|
|||
|
||||
void set(int32_t val) { _val = val; }
|
||||
|
||||
bool is_default() { return param_value_is_default(handle()); }
|
||||
|
||||
void reset()
|
||||
{
|
||||
param_reset_no_notification(handle());
|
||||
|
|
@ -300,6 +306,8 @@ public:
|
|||
|
||||
void set(int32_t val) { _val = val; }
|
||||
|
||||
bool is_default() { return param_value_is_default(handle()); }
|
||||
|
||||
void reset()
|
||||
{
|
||||
param_reset_no_notification(handle());
|
||||
|
|
@ -358,6 +366,8 @@ public:
|
|||
|
||||
void set(bool val) { _val = val; }
|
||||
|
||||
bool is_default() { return param_value_is_default(handle()); }
|
||||
|
||||
void reset()
|
||||
{
|
||||
param_reset_no_notification(handle());
|
||||
|
|
|
|||
|
|
@ -0,0 +1,97 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
#include <lib/drivers/device/Device.hpp>
|
||||
|
||||
namespace sensors
|
||||
{
|
||||
|
||||
struct AccelNoiseParameters {
|
||||
float noise{0.35f}; // Accelerometer noise for covariance prediction (m/s^2)
|
||||
float bias_noise{0.003f}; // Process noise for IMU accelerometer bias prediction (m/s^2)
|
||||
float switch_on_bias{0.2f}; // 1-sigma IMU accelerometer switch-on bias (m/s^3)
|
||||
uint8_t type{DRV_DEVTYPE_UNUSED};
|
||||
};
|
||||
|
||||
AccelNoiseParameters getAccelNoise(uint32_t sensor_device_id)
|
||||
{
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid = sensor_device_id;
|
||||
|
||||
switch (device_id.devid_s.devtype) {
|
||||
case DRV_ACC_DEVTYPE_BMI055: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_ACC_DEVTYPE_BMI088: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_ACC_DEVTYPE_FXOS8701C: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_ACC_DEVTYPE_LSM303AGR: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ADIS16448: return AccelNoiseParameters{ .noise = 0.30f, .bias_noise = 0.002f, .switch_on_bias = 0.1f};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ADIS16470: return AccelNoiseParameters{ .noise = 0.20f, .bias_noise = 0.001f, .switch_on_bias = 0.05f};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ADIS16477: return AccelNoiseParameters{ .noise = 0.20f, .bias_noise = 0.001f, .switch_on_bias = 0.05f};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ADIS16497: return AccelNoiseParameters{ .noise = 0.20f, .bias_noise = 0.001f, .switch_on_bias = 0.05f};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM20602: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM20608G: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM20649: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM20689: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM20948: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM40609D: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM42605: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM42688P: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_MPU6000: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_MPU6500: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_MPU9250: return AccelNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ST_LSM9DS1_AG : return AccelNoiseParameters{};
|
||||
}
|
||||
|
||||
return AccelNoiseParameters{};
|
||||
}
|
||||
|
||||
}; // namespace sensors
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
#include <lib/drivers/device/Device.hpp>
|
||||
|
||||
namespace sensors
|
||||
{
|
||||
|
||||
struct GyroNoiseParameters {
|
||||
float noise{0.015f}; // Rate gyro noise for covariance prediction (rad/s)
|
||||
float bias_noise{0.001f}; // Process noise for IMU rate gyro bias prediction (rad/s^2)
|
||||
float switch_on_bias{0.1f}; // 1-sigma IMU gyro switch-on bias (rad/s)
|
||||
uint8_t type{DRV_DEVTYPE_UNUSED};
|
||||
};
|
||||
|
||||
GyroNoiseParameters getGyroNoise(uint32_t sensor_device_id)
|
||||
{
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid = sensor_device_id;
|
||||
|
||||
switch (device_id.devid_s.devtype) {
|
||||
case DRV_GYR_DEVTYPE_BMI055: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_GYR_DEVTYPE_BMI088: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_GYR_DEVTYPE_FXAS2100C: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_GYR_DEVTYPE_L3GD20: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ADIS16448: return GyroNoiseParameters{ .noise = 0.012f, .bias_noise = 0.0005f, .switch_on_bias = 0.05f};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ADIS16470: return GyroNoiseParameters{ .noise = 0.010f, .bias_noise = 0.0002f, .switch_on_bias = 0.02f};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ADIS16477: return GyroNoiseParameters{ .noise = 0.010f, .bias_noise = 0.0002f, .switch_on_bias = 0.02f};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ADIS16497: return GyroNoiseParameters{ .noise = 0.010f, .bias_noise = 0.0002f, .switch_on_bias = 0.02f};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM20602: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM20608G: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM20649: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM20689: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM20948: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM40609D: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM42605: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ICM42688P: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_MPU6000: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_MPU6500: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_MPU9250: return GyroNoiseParameters{};
|
||||
|
||||
case DRV_IMU_DEVTYPE_ST_LSM9DS1_AG : return GyroNoiseParameters{};
|
||||
}
|
||||
|
||||
return GyroNoiseParameters{};
|
||||
}
|
||||
|
||||
}; // namespace sensors
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
#include <lib/drivers/device/Device.hpp>
|
||||
|
||||
namespace sensors
|
||||
{
|
||||
|
||||
static constexpr float DEFAULT_MAG_NOISE = 0.005f; // gauss
|
||||
|
||||
float getMagNoise(uint32_t sensor_device_id)
|
||||
{
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid = sensor_device_id;
|
||||
|
||||
switch (device_id.devid_s.devtype) {
|
||||
case DRV_GYR_DEVTYPE_BMI055: return DEFAULT_MAG_NOISE;
|
||||
|
||||
case DRV_MAG_DEVTYPE_HMC5883: return DEFAULT_MAG_NOISE;
|
||||
|
||||
case DRV_MAG_DEVTYPE_AK8963: return DEFAULT_MAG_NOISE;
|
||||
|
||||
case DRV_MAG_DEVTYPE_LIS3MDL: return DEFAULT_MAG_NOISE;
|
||||
|
||||
case DRV_MAG_DEVTYPE_IST8310: return DEFAULT_MAG_NOISE;
|
||||
|
||||
case DRV_MAG_DEVTYPE_RM3100: return DEFAULT_MAG_NOISE;
|
||||
|
||||
case DRV_MAG_DEVTYPE_QMC5883L: return DEFAULT_MAG_NOISE;
|
||||
|
||||
case DRV_MAG_DEVTYPE_AK09916: return DEFAULT_MAG_NOISE;
|
||||
|
||||
case DRV_MAG_DEVTYPE_IST8308: return DEFAULT_MAG_NOISE;
|
||||
|
||||
case DRV_MAG_DEVTYPE_LIS2MDL: return DEFAULT_MAG_NOISE;
|
||||
|
||||
case DRV_MAG_DEVTYPE_BMM150: return DEFAULT_MAG_NOISE;
|
||||
|
||||
case DRV_MAG_DEVTYPE_ST_LSM9DS1_M: return DEFAULT_MAG_NOISE;
|
||||
|
||||
case DRV_MAG_DEVTYPE_LSM303AGR: return DEFAULT_MAG_NOISE;
|
||||
}
|
||||
|
||||
return DEFAULT_MAG_NOISE;
|
||||
}
|
||||
}; // namespace sensors
|
||||
|
|
@ -33,6 +33,10 @@
|
|||
|
||||
#include "EKF2.hpp"
|
||||
|
||||
#include <lib/sensor_defaults/AccelDefaults.hpp>
|
||||
#include <lib/sensor_defaults/GyroDefaults.hpp>
|
||||
#include <lib/sensor_defaults/MagDefaults.hpp>
|
||||
|
||||
using namespace time_literals;
|
||||
using math::constrain;
|
||||
using matrix::Eulerf;
|
||||
|
|
@ -261,6 +265,24 @@ void EKF2::Run()
|
|||
// update parameters from storage
|
||||
updateParams();
|
||||
|
||||
if (_param_ekf2_acc_noise.is_default()) {
|
||||
const auto noise = sensors::getAccelNoise(_device_id_accel);
|
||||
_param_ekf2_acc_noise.set(noise.noise);
|
||||
_param_ekf2_acc_b_noise.set(noise.bias_noise);
|
||||
_param_ekf2_abias_init.set(noise.switch_on_bias);
|
||||
}
|
||||
|
||||
if (_param_ekf2_gyr_noise.is_default()) {
|
||||
const auto noise = sensors::getGyroNoise(_device_id_gyro);
|
||||
_param_ekf2_gyr_noise.set(noise.noise);
|
||||
_param_ekf2_gyr_b_noise.set(noise.bias_noise);
|
||||
_param_ekf2_gbias_init.set(noise.switch_on_bias);
|
||||
}
|
||||
|
||||
if (_param_ekf2_mag_noise.is_default()) {
|
||||
_param_ekf2_mag_noise.set(sensors::getMagNoise(_device_id_mag));
|
||||
}
|
||||
|
||||
_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
|
||||
|
||||
// The airspeed scale factor correcton is only available via parameter as used by the airspeed module
|
||||
|
|
@ -336,6 +358,38 @@ void EKF2::Run()
|
|||
|
||||
imu_dt = imu.delta_angle_dt;
|
||||
|
||||
if (_device_id_accel != imu.accel_device_id) {
|
||||
if (_param_ekf2_acc_noise.is_default()) {
|
||||
const float old_noise = _param_ekf2_acc_noise.get();
|
||||
|
||||
const auto noise = sensors::getAccelNoise(imu.gyro_device_id);
|
||||
_param_ekf2_acc_noise.set(noise.noise);
|
||||
_param_ekf2_acc_b_noise.set(noise.bias_noise);
|
||||
_param_ekf2_abias_init.set(noise.switch_on_bias);
|
||||
|
||||
if (fabsf(old_noise - _param_ekf2_acc_noise.get()) > 0.f) {
|
||||
PX4_INFO("%d updating accel (%d) noise %.4f -> %4f", _instance, imu.accel_device_id, (double)old_noise,
|
||||
(double)_param_ekf2_acc_noise.get());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_device_id_gyro != imu.gyro_device_id) {
|
||||
if (_param_ekf2_gyr_noise.is_default()) {
|
||||
const float old_noise = _param_ekf2_gyr_noise.get();
|
||||
|
||||
const auto noise = sensors::getGyroNoise(imu.gyro_device_id);
|
||||
_param_ekf2_gyr_noise.set(noise.noise);
|
||||
_param_ekf2_gyr_b_noise.set(noise.bias_noise);
|
||||
_param_ekf2_gbias_init.set(noise.switch_on_bias);
|
||||
|
||||
if (fabsf(old_noise - _param_ekf2_gyr_noise.get()) > 0.f) {
|
||||
PX4_INFO("%d updating gyro (%d) noise %.4f -> %4f", _instance, imu.gyro_device_id, (double)old_noise,
|
||||
(double)_param_ekf2_gyr_noise.get());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ((_device_id_accel == 0) || (_device_id_gyro == 0)) {
|
||||
_device_id_accel = imu.accel_device_id;
|
||||
_device_id_gyro = imu.gyro_device_id;
|
||||
|
|
@ -1566,6 +1620,18 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
PX4_WARN("%d - mag sensor ID changed %" PRIu32 " -> %" PRIu32, _instance, _device_id_mag, magnetometer.device_id);
|
||||
}
|
||||
|
||||
if (_param_ekf2_mag_noise.is_default()) {
|
||||
const float old_noise = _param_ekf2_mag_noise.get();
|
||||
|
||||
const auto noise = sensors::getMagNoise(magnetometer.device_id);
|
||||
_param_ekf2_mag_noise.set(noise);
|
||||
|
||||
if (fabsf(old_noise - _param_ekf2_mag_noise.get()) > 0.f) {
|
||||
PX4_INFO("%d updating mag (%d) noise %.4f -> %4f", _instance, magnetometer.device_id, (double)old_noise,
|
||||
(double)_param_ekf2_mag_noise.get());
|
||||
}
|
||||
}
|
||||
|
||||
reset = true;
|
||||
|
||||
} else if (magnetometer.calibration_count > _mag_calibration_count) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue