fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF increase delta velocity process noise per axis if clipping (#663)

Browse Source
This commit is contained in:
Daniel Agar 2020-04-07 18:51:51 -04:00 committed by GitHub
parent d6b6276cdc
commit 47624a0f02
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 25 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/common.h
View File

@ -93,6 +93,7 @@ struct imuSample {
float delta_ang_dt; ///< delta angle integration period (sec)
float delta_vel_dt; ///< delta velocity integration period (sec)
uint64_t time_us; ///< timestamp of the measurement (uSec)
bool delta_vel_clipping[3]{}; ///< true (per axis) if this sample contained any accelerometer clipping
};
struct gpsSample {

14
EKF/covariance.cpp
View File

@ -246,6 +246,20 @@ void Ekf::predictCovariance()
dvxVar = dvyVar = dvzVar = sq(dt * accel_noise);
// Accelerometer Clipping
// delta velocity X: increase process noise if sample contained any X axis clipping
if (_imu_sample_delayed.delta_vel_clipping[0]) {
dvxVar = sq(dt * BADACC_BIAS_PNOISE);
}
// delta velocity Y: increase process noise if sample contained any Y axis clipping
if (_imu_sample_delayed.delta_vel_clipping[1]) {
dvyVar = sq(dt * BADACC_BIAS_PNOISE);
}
// delta velocity Z: increase process noise if sample contained any Z axis clipping
if (_imu_sample_delayed.delta_vel_clipping[2]) {
dvzVar = sq(dt * BADACC_BIAS_PNOISE);
}
// predict the covariance
// intermediate calculations

3
EKF/estimator_interface.cpp
View File

@ -540,6 +540,9 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
}
_imu_sample_delayed.time_us = timestamp;
_imu_sample_delayed.delta_vel_clipping[0] = false;
_imu_sample_delayed.delta_vel_clipping[1] = false;
_imu_sample_delayed.delta_vel_clipping[2] = false;
_fault_status.value = 0;

6
EKF/imu_down_sampler.cpp
View File

@ -14,6 +14,9 @@ bool ImuDownSampler::update(const imuSample &imu_sample_new) {
_imu_down_sampled.delta_ang_dt += imu_sample_new.delta_ang_dt;
_imu_down_sampled.delta_vel_dt += imu_sample_new.delta_vel_dt;
_imu_down_sampled.time_us = imu_sample_new.time_us;
_imu_down_sampled.delta_vel_clipping[0] += imu_sample_new.delta_vel_clipping[0];
_imu_down_sampled.delta_vel_clipping[1] += imu_sample_new.delta_vel_clipping[1];
_imu_down_sampled.delta_vel_clipping[2] += imu_sample_new.delta_vel_clipping[2];
// use a quaternion to accumulate delta angle data
// this quaternion represents the rotation from the start to end of the accumulation period
@ -51,6 +54,9 @@ void ImuDownSampler::reset() {
_imu_down_sampled.delta_vel.setZero();
_imu_down_sampled.delta_ang_dt = 0.0f;
_imu_down_sampled.delta_vel_dt = 0.0f;
_imu_down_sampled.delta_vel_clipping[0] = false;
_imu_down_sampled.delta_vel_clipping[1] = false;
_imu_down_sampled.delta_vel_clipping[2] = false;
_delta_angle_accumulated.setIdentity();
_do_reset = false;
}

2
test/sensor_simulator/imu.cpp
View File

@ -15,7 +15,7 @@ Imu::~Imu()
void Imu::send(uint64_t time)
{
imuSample imu_sample;
imuSample imu_sample{};
imu_sample.time_us = time;
imu_sample.delta_ang_dt = (time - _time_last_data_sent) * 1.e-6f;
imu_sample.delta_ang = _gyro_data * imu_sample.delta_ang_dt;