dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Rename IMU biases

This commit is contained in:
kamilritz 2019-10-29 10:58:51 +01:00 committed by Paul Riseborough
parent dae8c2f8dc
commit 07e804676c
7 changed files with 46 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/common.h
View File

@ -368,8 +368,8 @@ struct stateSample {
Quatf quat_nominal; ///< quaternion defining the rotation from body to earth frame
Vector3f vel; ///< NED velocity in earth frame in m/s
Vector3f pos; ///< NED position in earth frame in m
Vector3f gyro_bias; ///< delta angle bias estimate in rad
Vector3f accel_bias; ///< delta velocity bias estimate in m/s
Vector3f delta_ang_bias; ///< delta angle bias estimate in rad
Vector3f delta_vel_bias; ///< delta velocity bias estimate in m/s
Vector3f mag_I; ///< NED earth magnetic field in gauss
Vector3f mag_B; ///< magnetometer bias estimate in body frame in gauss
Vector2f wind_vel; ///< wind velocity in m/s

14
EKF/control.cpp
View File

@ -424,7 +424,7 @@ void Ekf::controlOpticalFlowFusion()
}
// Accumulate autopilot gyro data across the same time interval as the flow sensor
_imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias;
_imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.delta_ang_bias;
_delta_time_of += _imu_sample_delayed.delta_ang_dt;
// New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon
@ -1383,8 +1383,8 @@ void Ekf::controlFakePosFusion()
// Fuse synthetic position observations every 200msec
if (((_time_last_imu - _time_last_fake_pos) > (uint64_t)2e5) || _fuse_height) {
Vector3f fake_gps_pos_obs_var{};
Vector2f fake_gps_pos_innov_gate{};
Vector3f fake_pos_obs_var{};
Vector2f fake_pos_innov_gate{};
// Reset position and velocity states if we re-commence this aiding method
@ -1401,19 +1401,19 @@ void Ekf::controlFakePosFusion()
_time_last_fake_pos = _time_last_imu;
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
fake_gps_pos_obs_var(0) = fake_gps_pos_obs_var(1) = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
fake_pos_obs_var(0) = fake_pos_obs_var(1) = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
} else {
fake_gps_pos_obs_var(0) = fake_gps_pos_obs_var(1) = 0.5f;
fake_pos_obs_var(0) = fake_pos_obs_var(1) = 0.5f;
}
_gps_pos_innov(0) = _state.pos(0) - _last_known_posNE(0);
_gps_pos_innov(1) = _state.pos(1) - _last_known_posNE(1);
// glitch protection is not required so set gate to a large value
fake_gps_pos_innov_gate(0) = 100.0f;
fake_pos_innov_gate(0) = 100.0f;
fuseHorizontalPosition(_gps_pos_innov, fake_gps_pos_innov_gate, fake_gps_pos_obs_var,
fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var,
_gps_pos_innov_var, _gps_pos_test_ratio);
}

21
EKF/covariance.cpp
View File

@ -148,13 +148,13 @@ void Ekf::predictCovariance()
float dvy = _imu_sample_delayed.delta_vel(1);
float dvz = _imu_sample_delayed.delta_vel(2);
float dax_b = _state.gyro_bias(0);
float day_b = _state.gyro_bias(1);
float daz_b = _state.gyro_bias(2);
float dax_b = _state.delta_ang_bias(0);
float day_b = _state.delta_ang_bias(1);
float daz_b = _state.delta_ang_bias(2);
float dvx_b = _state.accel_bias(0);
float dvy_b = _state.accel_bias(1);
float dvz_b = _state.accel_bias(2);
float dvx_b = _state.delta_vel_bias(0);
float dvy_b = _state.delta_vel_bias(1);
float dvz_b = _state.delta_vel_bias(2);
float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
float dt_inv = 1.0f / dt;
@ -337,7 +337,7 @@ void Ekf::predictCovariance()
SPP[10] = SF[16];
// covariance update
float nextP[24][24];
float nextP[24][24] = {};
// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
nextP[0][0] = P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10] + (daxVar*SQ[10])/4 + SF[9]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[11]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[10]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) + SF[15]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) + SPP[10]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + (dayVar*sq(q2))/4 + (dazVar*sq(q3))/4;
@ -822,13 +822,14 @@ void Ekf::fixCovarianceErrors()
float down_dvel_bias = 0.0f;
for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
down_dvel_bias += _state.accel_bias(axis_index) * _R_to_earth(2, axis_index);
down_dvel_bias += _state.delta_vel_bias(axis_index) * _R_to_earth(2, axis_index);
}
// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim
&& down_dvel_bias * _vel_pos_innov[2] < 0.0f
&& down_dvel_bias * _vel_pos_innov[5] < 0.0f);
&& ( (down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps)
|| (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel) )
&& down_dvel_bias * _gps_pos_innov(2) < 0.0f);
// record the pass/fail
if (!bad_acc_bias) {

4
EKF/drag_fusion.cpp
View File

@ -90,7 +90,7 @@ void Ekf::fuseDrag()
// calculate observation jacobiam and Kalman gain vectors
if (axis_index == 0) {
// Estimate the airspeed from the measured drag force and ballistic coefficient
float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.accel_bias(axis_index) / _dt_ekf_avg;
float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_x * rho));
// Estimate the derivative of specific force wrt airspeed along the X axis
@ -163,7 +163,7 @@ void Ekf::fuseDrag()
} else if (axis_index == 1) {
// Estimate the airspeed from the measured drag force and ballistic coefficient
float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.accel_bias(axis_index) / _dt_ekf_avg;
float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_y * rho));
// Estimate the derivative of specific force wrt airspeed along the X axis

18
EKF/ekf.cpp
View File

@ -100,8 +100,8 @@ void Ekf::resetStates()
{
_state.vel.setZero();
_state.pos.setZero();
_state.gyro_bias.setZero();
_state.accel_bias.setZero();
_state.delta_ang_bias.setZero();
_state.delta_vel_bias.setZero();
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
@ -241,8 +241,8 @@ bool Ekf::initialiseFilter()
// Zero all of the states
_state.vel.setZero();
_state.pos.setZero();
_state.gyro_bias.setZero();
_state.accel_bias.setZero();
_state.delta_ang_bias.setZero();
_state.delta_vel_bias.setZero();
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
@ -325,8 +325,8 @@ void Ekf::predictState()
}
// apply imu bias corrections
Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.gyro_bias;
Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.accel_bias;
Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.delta_ang_bias;
Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias;
// correct delta angles for earth rotation rate
corrected_delta_ang -= -_R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
@ -464,7 +464,7 @@ void Ekf::calculateOutputStates()
const float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg;
// 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.gyro_bias * dt_scale_correction + _delta_angle_corr};
const Vector3f delta_angle{imu.delta_ang - _state.delta_ang_bias * dt_scale_correction + _delta_angle_corr};
// calculate a yaw change about the earth frame vertical
const float spin_del_ang_D = _R_to_earth_now(2, 0) * delta_angle(0) +
@ -491,7 +491,7 @@ void Ekf::calculateOutputStates()
_R_to_earth_now = Dcmf(_output_new.quat_nominal);
// correct delta velocity for bias offsets
const Vector3f delta_vel{imu.delta_vel - _state.accel_bias * dt_scale_correction};
const Vector3f delta_vel{imu.delta_vel - _state.delta_vel_bias * dt_scale_correction};
// rotate the delta velocity to earth frame
Vector3f delta_vel_NED{_R_to_earth_now * delta_vel};
@ -681,7 +681,7 @@ 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{_imu_sample_new.delta_ang - _state.gyro_bias * (_dt_imu_avg / _dt_ekf_avg) + _delta_angle_corr};
const Vector3f delta_angle{_imu_sample_new.delta_ang - _state.delta_ang_bias * (_dt_imu_avg / _dt_ekf_avg) + _delta_angle_corr};
// increment the quaternions using the corrected delta angle vector
// the quaternions must always be normalised after modification

30
EKF/ekf_helper.cpp
View File

@ -805,11 +805,11 @@ void Ekf::constrainStates()
}
for (int i = 0; i < 3; i++) {
_state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -math::radians(20.f) * _dt_ekf_avg, math::radians(20.f) * _dt_ekf_avg);
_state.delta_ang_bias(i) = math::constrain(_state.delta_ang_bias(i), -math::radians(20.f) * _dt_ekf_avg, math::radians(20.f) * _dt_ekf_avg);
}
for (int i = 0; i < 3; i++) {
_state.accel_bias(i) = math::constrain(_state.accel_bias(i), -_params.acc_bias_lim * _dt_ekf_avg, _params.acc_bias_lim * _dt_ekf_avg);
_state.delta_vel_bias(i) = math::constrain(_state.delta_vel_bias(i), -_params.acc_bias_lim * _dt_ekf_avg, _params.acc_bias_lim * _dt_ekf_avg);
}
for (int i = 0; i < 3; i++) {
@ -953,7 +953,7 @@ void Ekf::getMagInnovRatio(float &mag_innov_ratio)
void Ekf::getDragInnov(float drag_innov[2])
{
memcpy(&drag_innov, _drag_innov, sizeof(_drag_innov));
memcpy(drag_innov, _drag_innov, sizeof(_drag_innov));
}
void Ekf::getDragInnovVar(float drag_innov_var[2])
@ -1033,11 +1033,11 @@ void Ekf::get_state_delayed(float *state)
}
for (int i = 0; i < 3; i++) {
state[i + 10] = _state.gyro_bias(i);
state[i + 10] = _state.delta_ang_bias(i);
}
for (int i = 0; i < 3; i++) {
state[i + 13] = _state.accel_bias(i);
state[i + 13] = _state.delta_vel_bias(i);
}
for (int i = 0; i < 3; i++) {
@ -1057,9 +1057,9 @@ void Ekf::get_state_delayed(float *state)
void Ekf::get_accel_bias(float bias[3])
{
float temp[3];
temp[0] = _state.accel_bias(0) / _dt_ekf_avg;
temp[1] = _state.accel_bias(1) / _dt_ekf_avg;
temp[2] = _state.accel_bias(2) / _dt_ekf_avg;
temp[0] = _state.delta_vel_bias(0) / _dt_ekf_avg;
temp[1] = _state.delta_vel_bias(1) / _dt_ekf_avg;
temp[2] = _state.delta_vel_bias(2) / _dt_ekf_avg;
memcpy(bias, temp, 3 * sizeof(float));
}
@ -1067,9 +1067,9 @@ void Ekf::get_accel_bias(float bias[3])
void Ekf::get_gyro_bias(float bias[3])
{
float temp[3];
temp[0] = _state.gyro_bias(0) / _dt_ekf_avg;
temp[1] = _state.gyro_bias(1) / _dt_ekf_avg;
temp[2] = _state.gyro_bias(2) / _dt_ekf_avg;
temp[0] = _state.delta_ang_bias(0) / _dt_ekf_avg;
temp[1] = _state.delta_ang_bias(1) / _dt_ekf_avg;
temp[2] = _state.delta_ang_bias(2) / _dt_ekf_avg;
memcpy(bias, temp, 3 * sizeof(float));
}
@ -1250,8 +1250,8 @@ bool Ekf::reset_imu_bias()
}
// Zero the delta angle and delta velocity bias states
_state.gyro_bias.zero();
_state.accel_bias.zero();
_state.delta_ang_bias.zero();
_state.delta_vel_bias.zero();
// Zero the corresponding covariances
zeroCols(P, 10, 15);
@ -1342,11 +1342,11 @@ void Ekf::fuse(float *K, float innovation)
}
for (unsigned i = 0; i < 3; i++) {
_state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 10] * innovation;
_state.delta_ang_bias(i) = _state.delta_ang_bias(i) - K[i + 10] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.accel_bias(i) = _state.accel_bias(i) - K[i + 13] * innovation;
_state.delta_vel_bias(i) = _state.delta_vel_bias(i) - K[i + 13] * innovation;
}
for (unsigned i = 0; i < 3; i++) {

2
EKF/terrain_estimator.cpp
View File

@ -307,5 +307,3 @@ void Ekf::getTerrainVertPos(float *ret)
{
memcpy(ret, &_terrain_vpos, sizeof(float));
}
}