Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

ekf2: scale delta angle and delta velocity bias to particular IMU sample

Browse Source
This commit is contained in:
Daniel Agar
2022-08-21 12:00:11 -04:00
parent 213d5dac2a
commit b58f70726f
1 changed files with 11 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.cpp
+11 -6
View File
@@ -261,7 +261,8 @@ bool Ekf::initialiseTilt()
void Ekf::predictState()
{
// apply imu bias corrections
Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.delta_ang_bias;
const Vector3f delta_ang_bias_scaled = (_state.delta_ang_bias / _dt_ekf_avg) * _imu_sample_delayed.delta_ang_dt;
Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - delta_ang_bias_scaled;
// subtract component of angular rate due to earth rotation
corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
@@ -273,7 +274,8 @@ void Ekf::predictState()
_R_to_earth = Dcmf(_state.quat_nominal);
// Calculate an earth frame delta velocity
const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias;
const Vector3f delta_vel_bias_scaled = (_state.delta_vel_bias / _dt_ekf_avg) * _imu_sample_delayed.delta_vel_dt;
const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - delta_vel_bias_scaled;
const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel;
// calculate a filtered horizontal acceleration with a 1 sec time constant
@@ -326,10 +328,10 @@ void Ekf::calculateOutputStates(const imuSample &imu)
// Use full rate IMU data at the current time horizon
// correct delta angles for bias offsets
const float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg;
const Vector3f delta_ang_bias_scaled = (_state.delta_ang_bias / _dt_ekf_avg) * imu.delta_ang_dt;
// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
const Vector3f delta_angle(imu.delta_ang - _state.delta_ang_bias * dt_scale_correction + _delta_angle_corr);
const Vector3f delta_angle(imu.delta_ang - delta_ang_bias_scaled + _delta_angle_corr);
// calculate a yaw change about the earth frame vertical
const float spin_del_ang_D = delta_angle.dot(Vector3f(_R_to_earth_now.row(2)));
@@ -355,7 +357,8 @@ void Ekf::calculateOutputStates(const imuSample &imu)
_R_to_earth_now = Dcmf(_output_new.quat_nominal);
// correct delta velocity for bias offsets
const Vector3f delta_vel_body{imu.delta_vel - _state.delta_vel_bias * dt_scale_correction};
const Vector3f delta_vel_bias_scaled = (_state.delta_vel_bias / _dt_ekf_avg) * _imu_sample_delayed.delta_vel_dt;
const Vector3f delta_vel_body{imu.delta_vel - delta_vel_bias_scaled};
// rotate the delta velocity to earth frame
Vector3f delta_vel_earth{_R_to_earth_now * delta_vel_body};
@@ -536,7 +539,9 @@ Quatf Ekf::calculate_quaternion() const
{
// Correct delta angle data for bias errors using bias state estimates from the EKF and also apply
// corrections required to track the EKF quaternion states
const Vector3f delta_angle{_newest_high_rate_imu_sample.delta_ang - _state.delta_ang_bias * (_dt_imu_avg / _dt_ekf_avg) + _delta_angle_corr};
const Vector3f delta_ang_bias_scaled = (_state.delta_ang_bias / _dt_ekf_avg) * _newest_high_rate_imu_sample.delta_ang_dt;
const Vector3f delta_angle{_newest_high_rate_imu_sample.delta_ang - delta_ang_bias_scaled + _delta_angle_corr};
// increment the quaternions using the corrected delta angle vector
// the quaternions must always be normalised after modification