fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 08:43:34 +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-08 08:51:51 +10:00
committed by GitHub
parent d6b6276cdc
commit 47624a0f02
5 changed files with 25 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/imu_down_sampler.cpp
+6
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;
}