forked from Archive/PX4-Autopilot
EKF: fixed some comment typos
Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
This commit is contained in:
Carl Olsson
Daniel Agar
parent
6e77b197c7
commit
f0889c1760
|
|
@ -55,9 +55,9 @@ void Ekf::fuseAirspeed()
|
|||
float v_tas_pred; // Predicted measurement
|
||||
float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f,
|
||||
10.0f)); // Variance for true airspeed measurement - (m/sec)^2
|
||||
float SH_TAS[3] = {}; // Varialbe used to optimise calculations of measurement jacobian
|
||||
float SH_TAS[3] = {}; // Variable used to optimise calculations of measurement jacobian
|
||||
float H_TAS[24] = {}; // Observation Jacobian
|
||||
float SK_TAS[2] = {}; // Varialbe used to optimise calculations of the Kalman gain vector
|
||||
float SK_TAS[2] = {}; // Variable used to optimise calculations of the Kalman gain vector
|
||||
float Kfusion[24] = {}; // Kalman gain vector
|
||||
|
||||
// Copy required states to local variable names
|
||||
|
|
@ -96,7 +96,7 @@ void Ekf::fuseAirspeed()
|
|||
SK_TAS[0] = 1.0f / _airspeed_innov_var_temp;
|
||||
_fault_status.flags.bad_airspeed = false;
|
||||
|
||||
} else { // Reset the estimator covarinace matrix
|
||||
} else { // Reset the estimator covariance matrix
|
||||
_fault_status.flags.bad_airspeed = true;
|
||||
|
||||
// if we are getting aiding from other sources, warn and reset the wind states and covariances only
|
||||
|
|
@ -198,7 +198,7 @@ void Ekf::fuseAirspeed()
|
|||
}
|
||||
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance marix is unhealthy and must be corrected
|
||||
// the covariance matrix is unhealthy and must be corrected
|
||||
bool healthy = true;
|
||||
_fault_status.flags.bad_airspeed = false;
|
||||
|
||||
|
|
@ -217,7 +217,7 @@ void Ekf::fuseAirspeed()
|
|||
}
|
||||
}
|
||||
|
||||
// only apply covariance and state corrrections if healthy
|
||||
// only apply covariance and state corrections if healthy
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
|
|
@ -226,7 +226,7 @@ void Ekf::fuseAirspeed()
|
|||
}
|
||||
}
|
||||
|
||||
// correct the covariance marix for gross errors
|
||||
// correct the covariance matrix for gross errors
|
||||
fixCovarianceErrors();
|
||||
|
||||
// apply the state corrections
|
||||
|
|
|
|||
20
EKF/common.h
20
EKF/common.h
|
|
@ -143,7 +143,7 @@ struct airspeedSample {
|
|||
|
||||
struct flowSample {
|
||||
uint8_t quality; ///< quality indicator between 0 and 255
|
||||
Vector2f flowRadXY; ///< measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive
|
||||
Vector2f flowRadXY; ///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive
|
||||
Vector3f gyroXYZ; ///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
|
||||
float dt; ///< amount of integration time (sec)
|
||||
uint64_t time_us; ///< timestamp of the integration period leading edge (uSec)
|
||||
|
|
@ -185,7 +185,7 @@ struct auxVelSample {
|
|||
#define MASK_USE_OF (1<<1) ///< set to true to use optical flow data
|
||||
#define MASK_INHIBIT_ACC_BIAS (1<<2) ///< set to true to inhibit estimation of accelerometer delta velocity bias
|
||||
#define MASK_USE_EVPOS (1<<3) ///< set to true to use external vision NED position data
|
||||
#define MASK_USE_EVYAW (1<<4) ///< set to true to use exernal vision quaternion data for yaw
|
||||
#define MASK_USE_EVYAW (1<<4) ///< set to true to use external vision quaternion data for yaw
|
||||
#define MASK_USE_DRAG (1<<5) ///< set to true to use the multi-rotor drag model to estimate wind
|
||||
#define MASK_ROTATE_EV (1<<6) ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
|
||||
#define MASK_USE_GPSYAW (1<<7) ///< set to true to use GPS yaw data if available
|
||||
|
|
@ -195,8 +195,8 @@ struct auxVelSample {
|
|||
#define MAG_FUSE_TYPE_HEADING 1 ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
|
||||
#define MAG_FUSE_TYPE_3D 2 ///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
|
||||
#define MAG_FUSE_TYPE_AUTOFW 3 ///< The same as option 0, but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states.
|
||||
#define MAG_FUSE_TYPE_INDOOR 4 ///< The same as option 0, but magnetomer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
|
||||
#define MAG_FUSE_TYPE_NONE 5 ///< Do not use magnetomer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
|
||||
#define MAG_FUSE_TYPE_INDOOR 4 ///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
|
||||
#define MAG_FUSE_TYPE_NONE 5 ///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
|
||||
|
||||
// Maximum sensor intervals in usec
|
||||
#define GPS_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec)
|
||||
|
|
@ -218,7 +218,7 @@ struct parameters {
|
|||
int32_t sensor_interval_min_ms{20}; ///< minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
|
||||
|
||||
// measurement time delays
|
||||
float min_delay_ms{0.0f}; ///< Maximmum time delay of any sensor used to increse buffer length to handle large timing jitter (mSec)
|
||||
float min_delay_ms{0.0f}; ///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec)
|
||||
float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec)
|
||||
float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec)
|
||||
float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec)
|
||||
|
|
@ -287,7 +287,7 @@ struct parameters {
|
|||
float vehicle_variance_scaler{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance
|
||||
float max_hagl_for_range_aid{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if range_aid == 1)
|
||||
float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if range_aid == 1)
|
||||
int32_t range_aid{0}; ///< allow switching primary height source to range finder if certian conditions are met
|
||||
int32_t range_aid{0}; ///< allow switching primary height source to range finder if certain conditions are met
|
||||
float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion
|
||||
float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
|
||||
float range_stuck_threshold{0.1f}; ///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m)
|
||||
|
|
@ -321,7 +321,7 @@ struct parameters {
|
|||
|
||||
// output complementary filter tuning
|
||||
float vel_Tau{0.25f}; ///< velocity state correction time constant (1/sec)
|
||||
float pos_Tau{0.25f}; ///< postion state correction time constant (1/sec)
|
||||
float pos_Tau{0.25f}; ///< position state correction time constant (1/sec)
|
||||
|
||||
// accel bias learning control
|
||||
float acc_bias_lim{0.4f}; ///< maximum accel bias magnitude (m/sec**2)
|
||||
|
|
@ -343,7 +343,7 @@ struct parameters {
|
|||
float vert_innov_test_lim{4.5f}; ///< Number of standard deviations allowed before the combined vertical velocity and position test is declared as failed
|
||||
int bad_acc_reset_delay_us{500000}; ///< Continuous time that the vertical position and velocity innovation test must fail before the states are reset (uSec)
|
||||
|
||||
// auxilliary velocity fusion
|
||||
// auxiliary velocity fusion
|
||||
float auxvel_noise{0.5f}; ///< minimum observation noise, uses reported noise if greater (m/s)
|
||||
float auxvel_gate{5.0f}; ///< velocity fusion innovation consistency gate size (STD)
|
||||
|
||||
|
|
@ -352,7 +352,7 @@ struct parameters {
|
|||
};
|
||||
|
||||
struct stateSample {
|
||||
Quatf quat_nominal; ///< quaternion defining the rotaton from earth to body frame
|
||||
Quatf quat_nominal; ///< quaternion defining the rotation from earth to body 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
|
||||
|
|
@ -443,7 +443,7 @@ union filter_control_status_u {
|
|||
uint32_t fuse_beta : 1; ///< 15 - true when synthetic sideslip measurements are being fused
|
||||
uint32_t update_mag_states_only : 1; ///< 16 - true when only the magnetometer states are updated by the magnetometer
|
||||
uint32_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
|
||||
uint32_t mag_fault : 1; ///< 18 - true when the magnetomer has been declared faulty and is no longer being used
|
||||
uint32_t mag_fault : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used
|
||||
uint32_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused
|
||||
uint32_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active
|
||||
uint32_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
|
||||
|
|
|
|||
|
|
@ -154,7 +154,7 @@ void Ekf::controlFusionModes()
|
|||
|
||||
void Ekf::controlExternalVisionFusion()
|
||||
{
|
||||
// Check for new exernal vision data
|
||||
// Check for new external vision data
|
||||
if (_ev_data_ready) {
|
||||
|
||||
// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
|
||||
|
|
@ -168,7 +168,7 @@ void Ekf::controlExternalVisionFusion()
|
|||
if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align
|
||||
&& _control_status.flags.yaw_align) {
|
||||
|
||||
// check for a exernal vision measurement that has fallen behind the fusion time horizon
|
||||
// check for a external vision measurement that has fallen behind the fusion time horizon
|
||||
if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL)) {
|
||||
// turn on use of external vision measurements for position
|
||||
_control_status.flags.ev_pos = true;
|
||||
|
|
@ -196,7 +196,7 @@ void Ekf::controlExternalVisionFusion()
|
|||
if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
|
||||
// don't start using EV data unless daa is arriving frequently
|
||||
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
|
||||
// reset the yaw angle to the value from the observaton quaternion
|
||||
// reset the yaw angle to the value from the observation quaternion
|
||||
// get the roll, pitch, yaw estimates from the quaternion states
|
||||
Quatf q_init(_state.quat_nominal);
|
||||
Eulerf euler_init(q_init);
|
||||
|
|
@ -707,7 +707,7 @@ void Ekf::controlHeightSensorTimeouts()
|
|||
// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical innovations being positive and not stale.
|
||||
// Clipping causes the average accel reading to move towards zero which makes the INS think it is falling and produces positive vertical innovations
|
||||
float var_product_lim = sq(_params.vert_innov_test_lim) * sq(_params.vert_innov_test_lim);
|
||||
bool bad_vert_accel = (_control_status.flags.baro_hgt && // we can only run this check if vertical position and velocity observations are indepedant
|
||||
bool bad_vert_accel = (_control_status.flags.baro_hgt && // we can only run this check if vertical position and velocity observations are independent
|
||||
(sq(_vel_pos_innov[5] * _vel_pos_innov[2]) > var_product_lim * (_vel_pos_innov_var[5] * _vel_pos_innov_var[2])) && // vertical position and velocity sensors are in agreement that we have a significant error
|
||||
(_vel_pos_innov[2] > 0.0f) && // positive innovation indicates that the inertial nav thinks it is falling
|
||||
((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) && // vertical position data is fresh
|
||||
|
|
@ -730,7 +730,7 @@ void Ekf::controlHeightSensorTimeouts()
|
|||
_bad_vert_accel_detected = bad_vert_accel;
|
||||
}
|
||||
|
||||
// check if height is continuously failing becasue of accel errors
|
||||
// check if height is continuously failing because of accel errors
|
||||
bool continuous_bad_accel_hgt = ((_time_last_imu - _time_good_vert_accel) > (unsigned)_params.bad_acc_reset_delay_us);
|
||||
|
||||
// check if height has been inertial deadreckoning for too long
|
||||
|
|
@ -962,7 +962,7 @@ void Ekf::controlHeightFusion()
|
|||
_fuse_height = true;
|
||||
|
||||
// we have just switched to using range finder, calculate height sensor offset such that current
|
||||
// measurment matches our current height estimate
|
||||
// measurement matches our current height estimate
|
||||
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
|
||||
if (get_terrain_valid()) {
|
||||
_hgt_sensor_offset = _terrain_vpos;
|
||||
|
|
@ -998,7 +998,7 @@ void Ekf::controlHeightFusion()
|
|||
_fuse_height = true;
|
||||
|
||||
// we have just switched to using gps height, calculate height sensor offset such that current
|
||||
// measurment matches our current height estimate
|
||||
// measurement matches our current height estimate
|
||||
if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
|
||||
_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
|
||||
}
|
||||
|
|
@ -1011,7 +1011,7 @@ void Ekf::controlHeightFusion()
|
|||
_fuse_height = _range_data_ready;
|
||||
|
||||
// we have just switched to using range finder, calculate height sensor offset such that current
|
||||
// measurment matches our current height estimate
|
||||
// measurement matches our current height estimate
|
||||
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
|
||||
// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
|
||||
if (_control_status.flags.in_air && get_terrain_valid()) {
|
||||
|
|
@ -1047,7 +1047,7 @@ void Ekf::controlHeightFusion()
|
|||
_fuse_height = true;
|
||||
|
||||
// we have just switched to using range finder, calculate height sensor offset such that current
|
||||
// measurment matches our current height estimate
|
||||
// measurement matches our current height estimate
|
||||
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
|
||||
if (get_terrain_valid()) {
|
||||
_hgt_sensor_offset = _terrain_vpos;
|
||||
|
|
@ -1062,7 +1062,7 @@ void Ekf::controlHeightFusion()
|
|||
_fuse_height = true;
|
||||
|
||||
// we have just switched to using gps height, calculate height sensor offset such that current
|
||||
// measurment matches our current height estimate
|
||||
// measurement matches our current height estimate
|
||||
if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
|
||||
_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
|
||||
}
|
||||
|
|
@ -1307,7 +1307,7 @@ void Ekf::controlBetaFusion()
|
|||
|
||||
// Perform synthetic sideslip fusion when in-air and sideslip fuson had been enabled externally in addition to the following criteria:
|
||||
|
||||
// Suffient time has lapsed sice the last fusion
|
||||
// Sufficient time has lapsed sice the last fusion
|
||||
bool beta_fusion_time_triggered = ((_time_last_imu - _time_last_beta_fuse) > _params.beta_avg_ft_us);
|
||||
|
||||
if (beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) {
|
||||
|
|
@ -1354,7 +1354,7 @@ void Ekf::controlMagFusion()
|
|||
{
|
||||
if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) {
|
||||
|
||||
// do not use the magnetomer and deactivate magnetic field states
|
||||
// do not use the magnetometer and deactivate magnetic field states
|
||||
// save covariance data for re-use if currently doing 3-axis fusion
|
||||
if (_control_status.flags.mag_3D) {
|
||||
save_mag_cov_data();
|
||||
|
|
@ -1456,9 +1456,9 @@ void Ekf::controlMagFusion()
|
|||
_time_last_movement = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
// decide whether 3-axis magnetomer fusion can be used
|
||||
// decide whether 3-axis magnetometer fusion can be used
|
||||
bool use_3D_fusion = _control_status.flags.tilt_align && // Use of 3D fusion requires valid tilt estimates
|
||||
_control_status.flags.in_air && // don't use when on the ground becasue of magnetic anomalies
|
||||
_control_status.flags.in_air && // don't use when on the ground because of magnetic anomalies
|
||||
_control_status.flags.mag_align_complete &&
|
||||
((_imu_sample_delayed.time_us - _time_last_movement) < 2 * 1000 * 1000); // Using 3-axis fusion for a minimum period after to allow for false negatives
|
||||
|
||||
|
|
@ -1566,7 +1566,7 @@ void Ekf::controlMagFusion()
|
|||
_control_status.flags.mag_dec = false;
|
||||
}
|
||||
|
||||
// If the user has selected auto protection against indoor magnetic field errors, only use the magnetomer
|
||||
// If the user has selected auto protection against indoor magnetic field errors, only use the magnetometer
|
||||
// if a yaw angle relative to true North is required for navigation. If no GPS or other earth frame aiding
|
||||
// is available, assume that we are operating indoors and the magnetometer should not be used.
|
||||
bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR);
|
||||
|
|
@ -1580,7 +1580,7 @@ void Ekf::controlMagFusion()
|
|||
_mag_use_not_inhibit_us = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
// If magnetomer use has been inhibited continuously then a yaw reset is required for a valid heading
|
||||
// If magnetometer use has been inhibited continuously then a yaw reset is required for a valid heading
|
||||
if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
|
||||
_mag_inhibit_yaw_reset_req = true;
|
||||
}
|
||||
|
|
@ -1628,7 +1628,7 @@ void Ekf::controlVelPosFusion()
|
|||
!_control_status.flags.ev_pos &&
|
||||
!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) {
|
||||
|
||||
// We now need to use a synthetic positon observation to prevent unconstrained drift of the INS states.
|
||||
// We now need to use a synthetic position observation to prevent unconstrained drift of the INS states.
|
||||
_using_synthetic_position = true;
|
||||
|
||||
// Fuse synthetic position observations every 200msec
|
||||
|
|
|
|||
|
|
@ -165,7 +165,7 @@ void Ekf::predictCovariance()
|
|||
// convert rate of change of accelerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update
|
||||
float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f);
|
||||
|
||||
// inhibit learning of imu acccel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
|
||||
// inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
|
||||
float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
|
||||
float beta = 1.0f - alpha;
|
||||
_ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_mag_filt);
|
||||
|
|
@ -744,10 +744,10 @@ void Ekf::fixCovarianceErrors()
|
|||
P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[3]);
|
||||
}
|
||||
|
||||
// force symmetry on the quaternion, velocity and positon state covariances
|
||||
// force symmetry on the quaternion, velocity and position state covariances
|
||||
makeSymmetrical(P, 0, 12);
|
||||
|
||||
// the following states are optional and are deactivaed when not required
|
||||
// the following states are optional and are deactivated when not required
|
||||
// by ensuring the corresponding covariance matrix values are kept at zero
|
||||
|
||||
// accelerometer bias states
|
||||
|
|
@ -807,7 +807,7 @@ void Ekf::fixCovarianceErrors()
|
|||
down_dvel_bias += _state.accel_bias(axis_index) * _R_to_earth(2, axis_index);
|
||||
}
|
||||
|
||||
// check that the vertical componenent of accel bias is consistent with both the vertical position and velocity innovation
|
||||
// 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);
|
||||
|
|
|
|||
|
|
@ -274,7 +274,7 @@ void Ekf::fuseDrag()
|
|||
}
|
||||
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance marix is unhealthy and must be corrected
|
||||
// the covariance matrix is unhealthy and must be corrected
|
||||
bool healthy = true;
|
||||
|
||||
//_fault_status.flags.bad_sideslip = false;
|
||||
|
|
@ -293,7 +293,7 @@ void Ekf::fuseDrag()
|
|||
}
|
||||
}
|
||||
|
||||
// only apply covariance and state corrrections if healthy
|
||||
// only apply covariance and state corrections if healthy
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
|
|
@ -302,7 +302,7 @@ void Ekf::fuseDrag()
|
|||
}
|
||||
}
|
||||
|
||||
// correct the covariance marix for gross errors
|
||||
// correct the covariance matrix for gross errors
|
||||
fixCovarianceErrors();
|
||||
|
||||
// apply the state corrections
|
||||
|
|
|
|||
|
|
@ -470,7 +470,7 @@ void Ekf::calculateOutputStates()
|
|||
_output_new.time_us = imu.time_us;
|
||||
_output_new.quat_nominal = _output_new.quat_nominal * dq;
|
||||
|
||||
// the quaternions must always be normalised afer modification
|
||||
// the quaternions must always be normalised after modification
|
||||
_output_new.quat_nominal.normalize();
|
||||
|
||||
// calculate the rotation matrix from body to earth frame
|
||||
|
|
|
|||
26
EKF/ekf.h
26
EKF/ekf.h
|
|
@ -209,7 +209,7 @@ public:
|
|||
// get the terrain variance
|
||||
float get_terrain_var() const { return _terrain_var; }
|
||||
|
||||
// get the accerometer bias in m/s/s
|
||||
// get the accelerometer bias in m/s/s
|
||||
void get_accel_bias(float bias[3]);
|
||||
|
||||
// get the gyroscope bias in rad/s
|
||||
|
|
@ -249,7 +249,7 @@ public:
|
|||
// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
|
||||
// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
|
||||
// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
|
||||
// Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned.
|
||||
// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
|
||||
void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl, float *beta);
|
||||
|
||||
// return a bitmask integer that describes which state estimates can be used for flight control
|
||||
|
|
@ -292,7 +292,7 @@ private:
|
|||
bool _fuse_vert_vel{false}; ///< true when gps vertical velocity measurement should be fused
|
||||
bool _fuse_hor_vel_aux{false}; ///< true when auxiliary horizontal velocity measurement should be fused
|
||||
|
||||
float _posObsNoiseNE{0.0f}; ///< 1-STD observtion noise used for the fusion of NE position data (m)
|
||||
float _posObsNoiseNE{0.0f}; ///< 1-STD observation noise used for the fusion of NE position data (m)
|
||||
float _posInnovGateNE{1.0f}; ///< Number of standard deviations used for the NE position fusion innovation consistency check
|
||||
|
||||
Vector2f _velObsVarNE; ///< 1-STD observation noise variance used for the fusion of NE velocity data (m/sec)**2
|
||||
|
|
@ -338,7 +338,7 @@ private:
|
|||
|
||||
Vector3f _earth_rate_NED; ///< earth rotation vector (NED) in rad/s
|
||||
|
||||
Dcmf _R_to_earth; ///< transformation matrix from body frame to earth frame from last EKF predition
|
||||
Dcmf _R_to_earth; ///< transformation matrix from body frame to earth frame from last EKF prediction
|
||||
|
||||
// used by magnetometer fusion mode selection
|
||||
Vector2f _accel_lpf_NE; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
|
||||
|
|
@ -346,15 +346,15 @@ private:
|
|||
float _yaw_rate_lpf_ef{0.0f}; ///< Filtered angular rate about earth frame D axis (rad/sec)
|
||||
bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable
|
||||
bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable
|
||||
uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation moaneouvre was detected
|
||||
uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected
|
||||
uint8_t _num_bad_flight_yaw_events{0}; ///< number of times a bad heading has been detected in flight and required a yaw reset
|
||||
uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetomer use was inhibited
|
||||
bool _mag_use_inhibit{false}; ///< true when magnetomer use is being inhibited
|
||||
bool _mag_use_inhibit_prev{false}; ///< true when magnetomer use was being inhibited the previous frame
|
||||
bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetomer inhibit has been active for long enough to require a yaw reset when conditons improve.
|
||||
uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetometer use was inhibited
|
||||
bool _mag_use_inhibit{false}; ///< true when magnetometer use is being inhibited
|
||||
bool _mag_use_inhibit_prev{false}; ///< true when magnetometer use was being inhibited the previous frame
|
||||
bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve.
|
||||
float _last_static_yaw{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad)
|
||||
bool _vehicle_at_rest_prev{false}; ///< true when the vehicle was at rest the previous time the status was checked
|
||||
bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetomer data has been requested
|
||||
bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested
|
||||
bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
|
||||
|
||||
float P[_k_num_states][_k_num_states] {}; ///< state covariance matrix
|
||||
|
|
@ -447,7 +447,7 @@ private:
|
|||
float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m)
|
||||
float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2)
|
||||
uint64_t _time_last_hagl_fuse{0}; ///< last system time that the hagl measurement failed it's checks (uSec)
|
||||
bool _terrain_initialised{false}; ///< true when the terrain estimator has been intialised
|
||||
bool _terrain_initialised{false}; ///< true when the terrain estimator has been initialized
|
||||
float _sin_tilt_rng{0.0f}; ///< sine of the range finder tilt rotation about the Y body axis
|
||||
float _cos_tilt_rng{0.0f}; ///< cosine of the range finder tilt rotation about the Y body axis
|
||||
float _R_rng_to_earth_2_2{0.0f}; ///< 2,2 element of the rotation matrix from sensor frame to earth frame
|
||||
|
|
@ -596,7 +596,7 @@ private:
|
|||
// control fusion of external vision observations
|
||||
void controlExternalVisionFusion();
|
||||
|
||||
// control fusion of optical flow observtions
|
||||
// control fusion of optical flow observations
|
||||
void controlOpticalFlowFusion();
|
||||
|
||||
// control fusion of GPS observations
|
||||
|
|
@ -632,7 +632,7 @@ private:
|
|||
// control for combined height fusion mode (implemented for switching between baro and range height)
|
||||
void controlHeightFusion();
|
||||
|
||||
// determine if flight condition is suitable so use range finder instead of the primary height senor
|
||||
// determine if flight condition is suitable so use range finder instead of the primary height sensor
|
||||
void rangeAidConditionsMet();
|
||||
|
||||
// check for "stuck" range finder measurements when rng was not valid for certain period
|
||||
|
|
|
|||
|
|
@ -168,7 +168,7 @@ bool Ekf::resetPosition()
|
|||
|
||||
}
|
||||
|
||||
// estimate is relative to initial positon in this mode, so we start with zero error.
|
||||
// estimate is relative to initial position in this mode, so we start with zero error.
|
||||
zeroCols(P,7,8);
|
||||
zeroRows(P,7,8);
|
||||
|
||||
|
|
@ -223,7 +223,7 @@ void Ekf::resetHeight()
|
|||
// calculate the new vertical position using range sensor
|
||||
float new_pos_down = _hgt_sensor_offset - range_newest.rng * _R_rng_to_earth_2_2;
|
||||
|
||||
// update the state and assoicated variance
|
||||
// update the state and associated variance
|
||||
_state.pos(2) = new_pos_down;
|
||||
|
||||
// reset the associated covariance values
|
||||
|
|
@ -269,7 +269,7 @@ void Ekf::resetHeight()
|
|||
if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) {
|
||||
_state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref;
|
||||
|
||||
// reset the associated covarince values
|
||||
// reset the associated covariance values
|
||||
zeroRows(P, 9, 9);
|
||||
zeroCols(P, 9, 9);
|
||||
|
||||
|
|
@ -382,12 +382,11 @@ void Ekf::alignOutputFilter()
|
|||
Quatf q_delta = _output_sample_delayed.quat_nominal.inversed() * _state.quat_nominal;
|
||||
q_delta.normalize();
|
||||
|
||||
// calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon
|
||||
// calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon
|
||||
const Vector3f vel_delta = _state.vel - _output_sample_delayed.vel;
|
||||
const Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
|
||||
|
||||
// loop through the output filter state history and add the deltas
|
||||
// Note q1 *= q2 is equivalent to q1 = q1 * q2
|
||||
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
|
||||
_output_buffer[i].quat_nominal *= q_delta;
|
||||
_output_buffer[i].quat_nominal.normalize();
|
||||
|
|
@ -434,7 +433,7 @@ bool Ekf::realignYawGPS()
|
|||
if (badMagYaw || !_control_status.flags.yaw_align) {
|
||||
ECL_WARN("EKF bad yaw corrected using GPS course");
|
||||
|
||||
// declare the magnetomer as failed if a bad yaw has occurred more than once
|
||||
// declare the magnetometer as failed if a bad yaw has occurred more than once
|
||||
if (_control_status.flags.mag_align_complete && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) {
|
||||
ECL_WARN("EKF stopping magnetometer use");
|
||||
_control_status.flags.mag_fault = true;
|
||||
|
|
@ -451,7 +450,7 @@ bool Ekf::realignYawGPS()
|
|||
|
||||
// apply yaw correction
|
||||
if (!_control_status.flags.mag_align_complete) {
|
||||
// This is our first flight aligment so we can assume that the recent change in velocity has occurred due to a
|
||||
// This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
|
||||
// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
|
||||
euler321(2) += courseYawError;
|
||||
_control_status.flags.mag_align_complete = true;
|
||||
|
|
@ -468,7 +467,7 @@ bool Ekf::realignYawGPS()
|
|||
|
||||
}
|
||||
|
||||
// calculate new filter quaternion states using corected yaw angle
|
||||
// calculate new filter quaternion states using corrected yaw angle
|
||||
_state.quat_nominal = Quatf(euler321);
|
||||
uncorrelateQuatStates();
|
||||
|
||||
|
|
@ -566,7 +565,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
|
|||
}
|
||||
|
||||
if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) {
|
||||
// do not use the magnetomer and deactivate magnetic field states
|
||||
// do not use the magnetometer and deactivate magnetic field states
|
||||
// save covariance data for re-use if currently doing 3-axis fusion
|
||||
if (_control_status.flags.mag_3D) {
|
||||
save_mag_cov_data();
|
||||
|
|
@ -851,7 +850,7 @@ void Ekf::get_vel_pos_innov(float vel_pos_innov[6])
|
|||
memcpy(vel_pos_innov, _vel_pos_innov, sizeof(float) * 6);
|
||||
}
|
||||
|
||||
// gets the innovations of the earth magnetic field measurements
|
||||
// gets the innovations for of the NE auxiliary velocity measurement
|
||||
void Ekf::get_aux_vel_innov(float aux_vel_innov[2])
|
||||
{
|
||||
memcpy(aux_vel_innov, _aux_vel_innov, sizeof(float) * 2);
|
||||
|
|
@ -863,7 +862,7 @@ void Ekf::get_mag_innov(float mag_innov[3])
|
|||
memcpy(mag_innov, _mag_innov, 3 * sizeof(float));
|
||||
}
|
||||
|
||||
// gets the innovations of the airspeed measnurement
|
||||
// gets the innovations of the airspeed measurement
|
||||
void Ekf::get_airspeed_innov(float *airspeed_innov)
|
||||
{
|
||||
memcpy(airspeed_innov, &_airspeed_innov, sizeof(float));
|
||||
|
|
@ -894,7 +893,7 @@ void Ekf::get_mag_innov_var(float mag_innov_var[3])
|
|||
memcpy(mag_innov_var, _mag_innov_var, sizeof(float) * 3);
|
||||
}
|
||||
|
||||
// gest the innovation variance of the airspeed measurement
|
||||
// gets the innovation variance of the airspeed measurement
|
||||
void Ekf::get_airspeed_innov_var(float *airspeed_innov_var)
|
||||
{
|
||||
memcpy(airspeed_innov_var, &_airspeed_innov_var, sizeof(float));
|
||||
|
|
@ -1168,10 +1167,10 @@ bool Ekf::reset_imu_bias()
|
|||
// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
|
||||
// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
|
||||
// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
|
||||
// Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned.
|
||||
// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
|
||||
void Ekf::get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl, float *beta)
|
||||
{
|
||||
// return the integer bitmask containing the consistency check pass/fail satus
|
||||
// return the integer bitmask containing the consistency check pass/fail status
|
||||
*status = _innov_check_fail_status.value;
|
||||
// return the largest magnetometer innovation test ratio
|
||||
*mag = sqrtf(math::max(_yaw_test_ratio, math::max(math::max(_mag_test_ratio[0], _mag_test_ratio[1]), _mag_test_ratio[2])));
|
||||
|
|
@ -1593,7 +1592,7 @@ void Ekf::setControlEVHeight()
|
|||
}
|
||||
|
||||
// update the estimated misalignment between the EV navigation frame and the EKF navigation frame
|
||||
// and calculate a rotation matrix which rotates EV measurements into the EKF's navigatin frame
|
||||
// and calculate a rotation matrix which rotates EV measurements into the EKF's navigation frame
|
||||
void Ekf::calcExtVisRotMat()
|
||||
{
|
||||
// calculate the quaternion delta between the EKF and EV reference frames at the EKF fusion time horizon
|
||||
|
|
|
|||
|
|
@ -65,12 +65,12 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
|
|||
Vector3f temp = cross_product(imu_sample.delta_ang, _delta_ang_prev);
|
||||
_vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.norm();
|
||||
|
||||
// calculate a metric which indiates the amount of high frequency gyro vibration
|
||||
// calculate a metric which indicates the amount of high frequency gyro vibration
|
||||
temp = imu_sample.delta_ang - _delta_ang_prev;
|
||||
_delta_ang_prev = imu_sample.delta_ang;
|
||||
_vibe_metrics[1] = 0.99f * _vibe_metrics[1] + 0.01f * temp.norm();
|
||||
|
||||
// calculate a metric which indicates the amount of high fequency accelerometer vibration
|
||||
// calculate a metric which indicates the amount of high frequency accelerometer vibration
|
||||
temp = imu_sample.delta_vel - _delta_vel_prev;
|
||||
_delta_vel_prev = imu_sample.delta_vel;
|
||||
_vibe_metrics[2] = 0.99f * _vibe_metrics[2] + 0.01f * temp.norm();
|
||||
|
|
@ -307,7 +307,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed
|
|||
airspeed_sample_new.true_airspeed = true_airspeed;
|
||||
airspeed_sample_new.eas2tas = eas2tas;
|
||||
airspeed_sample_new.time_us = time_usec - _params.airspeed_delay_ms * 1000;
|
||||
airspeed_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; //typo PeRRiod
|
||||
airspeed_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
|
||||
_time_last_airspeed = time_usec;
|
||||
|
||||
_airspeed_buffer.push(airspeed_sample_new);
|
||||
|
|
@ -369,7 +369,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
|
|||
bool delta_time_good = delta_time >= delta_time_min;
|
||||
|
||||
if (!delta_time_good) {
|
||||
// protect against overflow casued by division with very small delta_time
|
||||
// protect against overflow caused by division with very small delta_time
|
||||
delta_time = delta_time_min;
|
||||
}
|
||||
|
||||
|
|
@ -501,7 +501,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
|||
// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
|
||||
_imu_buffer_length = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1;
|
||||
|
||||
// set the observaton buffer length to handle the minimum time of arrival between observations in combination
|
||||
// set the observation buffer length to handle the minimum time of arrival between observations in combination
|
||||
// with the worst case delay from current time to ekf fusion time
|
||||
// allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
|
||||
uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceilf((float)max_time_delay_ms * 0.5f));
|
||||
|
|
|
|||
|
|
@ -382,7 +382,7 @@ public:
|
|||
// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
|
||||
// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
|
||||
// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
|
||||
// Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned.
|
||||
// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
|
||||
virtual void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl, float *beta) = 0;
|
||||
|
||||
// return a bitmask integer that describes which state estimates can be used for flight control
|
||||
|
|
@ -488,19 +488,19 @@ protected:
|
|||
float _tas_test_ratio{0.0f}; // tas innovation consistency check ratio
|
||||
float _terr_test_ratio{0.0f}; // height above terrain measurement innovation consistency check ratio
|
||||
float _beta_test_ratio{0.0f}; // sideslip innovation consistency check ratio
|
||||
float _drag_test_ratio[2] {}; // drag innovation cinsistency check ratio
|
||||
float _drag_test_ratio[2] {}; // drag innovation consistency check ratio
|
||||
innovation_fault_status_u _innov_check_fail_status{};
|
||||
|
||||
bool _is_dead_reckoning{false}; // true if we are no longer fusing measurements that constrain horizontal velocity drift
|
||||
bool _deadreckon_time_exceeded{false}; // true if the horizontal nav solution has been deadreckoning for too long and is invalid
|
||||
bool _is_wind_dead_reckoning{false}; // true if we are navigating reliant on wind relative measurements
|
||||
bool _is_wind_dead_reckoning{false}; // true if we are navigationg reliant on wind relative measurements
|
||||
|
||||
// IMU vibration and movement monitoring
|
||||
Vector3f _delta_ang_prev; // delta angle from the previous IMU measurement
|
||||
Vector3f _delta_vel_prev; // delta velocity from the previous IMU measurement
|
||||
float _vibe_metrics[3] {}; // IMU vibration metrics
|
||||
// [0] Level of coning vibration in the IMU delta angles (rad^2)
|
||||
// [1] high frequency vibraton level in the IMU delta angle data (rad)
|
||||
// [1] high frequency vibration level in the IMU delta angle data (rad)
|
||||
// [2] high frequency vibration level in the IMU delta velocity data (m/s)
|
||||
float _gps_drift_metrics[3] {}; // Array containing GPS drift metrics
|
||||
// [0] Horizontal position drift rate (m/s)
|
||||
|
|
@ -548,7 +548,7 @@ protected:
|
|||
|
||||
fault_status_u _fault_status{};
|
||||
|
||||
// allocate data buffers and intialise interface variables
|
||||
// allocate data buffers and initialize interface variables
|
||||
bool initialise_interface(uint64_t timestamp);
|
||||
|
||||
// free buffer memory
|
||||
|
|
|
|||
|
|
@ -66,7 +66,7 @@ bool Ekf::collect_gps(const gps_message &gps)
|
|||
double lon = gps.lon / 1.0e7;
|
||||
map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
|
||||
|
||||
// if we are already doing aiding, corect for the change in posiiton since the EKF started navigating
|
||||
// if we are already doing aiding, correct for the change in position since the EKF started navigationg
|
||||
if (_control_status.flags.opt_flow || _control_status.flags.gps || _control_status.flags.ev_pos) {
|
||||
double est_lat, est_lon;
|
||||
map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon);
|
||||
|
|
@ -197,7 +197,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
|
|||
|
||||
} else if (_control_status.flags.in_air) {
|
||||
// These checks are always declared as passed when flying
|
||||
// If on ground and moving, the last result before movemenent commenced is kept
|
||||
// If on ground and moving, the last result before movement commenced is kept
|
||||
_gps_check_fail_status.flags.hdrift = false;
|
||||
_gps_check_fail_status.flags.vdrift = false;
|
||||
_gps_check_fail_status.flags.hspeed = false;
|
||||
|
|
|
|||
|
|
@ -138,7 +138,7 @@ void Ekf::fuseGpsAntYaw()
|
|||
// wrap the heading to the interval between +-pi
|
||||
measured_hdg = wrap_pi(measured_hdg);
|
||||
|
||||
// calculate the innovation and define the innovaton gate
|
||||
// calculate the innovation and define the innovation gate
|
||||
float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
|
||||
_heading_innov = predicted_hdg - measured_hdg;
|
||||
|
||||
|
|
@ -146,7 +146,7 @@ void Ekf::fuseGpsAntYaw()
|
|||
_heading_innov = wrap_pi(_heading_innov);
|
||||
|
||||
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
|
||||
// calculate the innovaton variance
|
||||
// calculate the innovation variance
|
||||
float PH[4];
|
||||
_heading_innov_var = R_YAW;
|
||||
|
||||
|
|
@ -253,7 +253,7 @@ void Ekf::fuseGpsAntYaw()
|
|||
}
|
||||
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance marix is unhealthy and must be corrected
|
||||
// the covariance matrix is unhealthy and must be corrected
|
||||
bool healthy = true;
|
||||
_fault_status.flags.bad_hdg = false;
|
||||
|
||||
|
|
@ -272,7 +272,7 @@ void Ekf::fuseGpsAntYaw()
|
|||
}
|
||||
}
|
||||
|
||||
// only apply covariance and state corrrections if healthy
|
||||
// only apply covariance and state corrections if healthy
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
|
|
@ -281,7 +281,7 @@ void Ekf::fuseGpsAntYaw()
|
|||
}
|
||||
}
|
||||
|
||||
// correct the covariance marix for gross errors
|
||||
// correct the covariance matrix for gross errors
|
||||
fixCovarianceErrors();
|
||||
|
||||
// apply the state corrections
|
||||
|
|
@ -309,7 +309,7 @@ bool Ekf::resetGpsAntYaw()
|
|||
// get measurement and correct for antenna array yaw offset
|
||||
float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
|
||||
|
||||
// caclulate the amount the yaw needs to be rotated by
|
||||
// calculate the amount the yaw needs to be rotated by
|
||||
float yaw_delta = wrap_pi(measured_yaw - predicted_yaw);
|
||||
|
||||
// save a copy of the quaternion state for later use in calculating the amount of reset change
|
||||
|
|
|
|||
|
|
@ -377,7 +377,7 @@ void Ekf::fuseMag()
|
|||
}
|
||||
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance marix is unhealthy and must be corrected
|
||||
// the covariance matrix is unhealthy and must be corrected
|
||||
_fault_status.flags.bad_mag_x = false;
|
||||
_fault_status.flags.bad_mag_y = false;
|
||||
_fault_status.flags.bad_mag_z = false;
|
||||
|
|
@ -404,7 +404,7 @@ void Ekf::fuseMag()
|
|||
}
|
||||
}
|
||||
|
||||
// only apply covariance and state corrrections if healthy
|
||||
// only apply covariance and state corrections if healthy
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
|
|
@ -413,7 +413,7 @@ void Ekf::fuseMag()
|
|||
}
|
||||
}
|
||||
|
||||
// correct the covariance marix for gross errors
|
||||
// correct the covariance matrix for gross errors
|
||||
fixCovarianceErrors();
|
||||
|
||||
// apply the state corrections
|
||||
|
|
@ -506,7 +506,7 @@ void Ekf::fuseHeading()
|
|||
}
|
||||
|
||||
} else {
|
||||
// calculate observaton jacobian when we are observing a rotation in a 312 sequence
|
||||
// calculate observation jacobian when we are observing a rotation in a 312 sequence
|
||||
float t9 = q0*q3;
|
||||
float t10 = q1*q2;
|
||||
float t2 = t9-t10;
|
||||
|
|
@ -617,18 +617,18 @@ void Ekf::fuseHeading()
|
|||
// wrap the heading to the interval between +-pi
|
||||
measured_hdg = wrap_pi(measured_hdg);
|
||||
|
||||
// calculate the innovation and define the innovaton gate
|
||||
// calculate the innovation and define the innovation gate
|
||||
float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
|
||||
if (_mag_use_inhibit) {
|
||||
// The magnetomer cannot be trusted but we need to fuse a heading to prevent a badly
|
||||
// conditoned covariance matrix developing over time.
|
||||
// The magnetometer cannot be trusted but we need to fuse a heading to prevent a badly
|
||||
// conditioned covariance matrix developing over time.
|
||||
if (!_vehicle_at_rest) {
|
||||
// Vehicle is not at rest so fuse a zero innovation and record the
|
||||
// predicted heading to use as an observation when movement ceases.
|
||||
_heading_innov = 0.0f;
|
||||
_vehicle_at_rest_prev = false;
|
||||
} else {
|
||||
// Vehicle is at rest so use the last moving prediciton as an observation
|
||||
// Vehicle is at rest so use the last moving prediction as an observation
|
||||
// to prevent the heading from drifting and to enable yaw gyro bias learning
|
||||
// before takeoff.
|
||||
if (!_vehicle_at_rest_prev || !_mag_use_inhibit_prev) {
|
||||
|
|
@ -649,7 +649,7 @@ void Ekf::fuseHeading()
|
|||
_heading_innov = wrap_pi(_heading_innov);
|
||||
|
||||
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
|
||||
// calculate the innovaton variance
|
||||
// calculate the innovation variance
|
||||
float PH[4];
|
||||
_heading_innov_var = R_YAW;
|
||||
|
||||
|
|
@ -756,7 +756,7 @@ void Ekf::fuseHeading()
|
|||
}
|
||||
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance marix is unhealthy and must be corrected
|
||||
// the covariance matrix is unhealthy and must be corrected
|
||||
bool healthy = true;
|
||||
_fault_status.flags.bad_hdg = false;
|
||||
|
||||
|
|
@ -775,7 +775,7 @@ void Ekf::fuseHeading()
|
|||
}
|
||||
}
|
||||
|
||||
// only apply covariance and state corrrections if healthy
|
||||
// only apply covariance and state corrections if healthy
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
|
|
@ -784,7 +784,7 @@ void Ekf::fuseHeading()
|
|||
}
|
||||
}
|
||||
|
||||
// correct the covariance marix for gross errors
|
||||
// correct the covariance matrix for gross errors
|
||||
fixCovarianceErrors();
|
||||
|
||||
// apply the state corrections
|
||||
|
|
@ -895,7 +895,7 @@ void Ekf::fuseDeclination(float decl_sigma)
|
|||
}
|
||||
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance marix is unhealthy and must be corrected
|
||||
// the covariance matrix is unhealthy and must be corrected
|
||||
bool healthy = true;
|
||||
_fault_status.flags.bad_mag_decl = false;
|
||||
for (int i = 0; i < _k_num_states; i++) {
|
||||
|
|
@ -913,7 +913,7 @@ void Ekf::fuseDeclination(float decl_sigma)
|
|||
}
|
||||
}
|
||||
|
||||
// only apply covariance and state corrrections if healthy
|
||||
// only apply covariance and state corrections if healthy
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
|
|
|
|||
|
|
@ -21,7 +21,7 @@ imu_data.accel_dt = IMT(:,4);
|
|||
imu_data.del_vel = IMT(:,9:11);
|
||||
save imu_data.mat imu_data;
|
||||
|
||||
%% convert magnetomer data
|
||||
%% convert magnetometer data
|
||||
clear mag_data;
|
||||
last_time = 0;
|
||||
output_index = 1;
|
||||
|
|
|
|||
|
|
@ -25,7 +25,7 @@ imu_data.del_ang = [gyro_rad0.*imu_data.gyro_dt, gyro_rad1.*imu_data.gyro_dt, gy
|
|||
imu_data.accel_dt = accelerometer_integral_dt ./ 1e6;
|
||||
imu_data.del_vel = [accelerometer_m_s20.*imu_data.accel_dt, accelerometer_m_s21.*imu_data.accel_dt, accelerometer_m_s22.*imu_data.accel_dt];
|
||||
|
||||
%% convert magnetomer data
|
||||
%% convert magnetometer data
|
||||
clear mag_data;
|
||||
last_time = 0;
|
||||
output_index = 1;
|
||||
|
|
|
|||
|
|
@ -8,7 +8,7 @@ function [...
|
|||
P, ... % predicted covariance
|
||||
magMea, ... % body frame magnetic flux measurements
|
||||
testRatio, ... % Size of magnetometer innovation in standard deviations before measurements are rejected
|
||||
R_MAG) % magnetoemter measurement variance - gauss^2
|
||||
R_MAG) % magnetometer measurement variance - gauss^2
|
||||
|
||||
q0 = states(1);
|
||||
q1 = states(2);
|
||||
|
|
|
|||
|
|
@ -87,7 +87,7 @@ stateVectorNew = [quatNew;vNew;pNew;dAngBiasNew;dVelBiasNew;magNnew;magEnew;magD
|
|||
|
||||
% Define the control (disturbance) vector. Error growth in the inertial
|
||||
% solution is assumed to be driven by 'noise' in the delta angles and
|
||||
% velocities, after bias effects have been removed. This is OK becasue we
|
||||
% velocities, after bias effects have been removed. This is OK because we
|
||||
% have sensor bias accounted for in the state equations.
|
||||
distVector = [daxVar;dayVar;dazVar;dvxVar;dvyVar;dvzVar];
|
||||
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@ function P = PredictCovariance(deltaAngle, ...
|
|||
param) % tuning parameters
|
||||
|
||||
% Set filter state process noise other than IMU errors, which are already
|
||||
% built into the derived covariance predition equations.
|
||||
% built into the derived covariance prediction equations.
|
||||
% This process noise determines the rate of estimation of IMU bias errors
|
||||
dAngBiasSigma = dt*dt*param.prediction.dAngBiasPnoise;
|
||||
dVelBiasSigma = dt*dt*param.prediction.dVelBiasPnoise;
|
||||
|
|
|
|||
|
|
@ -249,7 +249,7 @@ for index = indexStart:indexStop
|
|||
end
|
||||
|
||||
elseif (param.fusion.magFuseMethod == 2)
|
||||
% fuse magnetomer data as a single magnetic heading measurement
|
||||
% fuse magnetometer data as a single magnetic heading measurement
|
||||
[states, covariance, hdgInnov, hdgInnovVar] = FuseMagHeading(states, covariance, mag_data.field_ga(latest_mag_index,:), param.fusion.magDeclDeg*deg2rad, param.fusion.magHdgGate, param.fusion.magHdgError^2);
|
||||
|
||||
% log data
|
||||
|
|
@ -317,7 +317,7 @@ for index = indexStart:indexStop
|
|||
viso_fuse_index = viso_fuse_index + 1;
|
||||
last_drift_constrain_time = local_time;
|
||||
|
||||
% convert delta positon measurements to velocity
|
||||
% convert delta position measurements to velocity
|
||||
relVelBodyMea = [viso_data.dPosX(latest_viso_index);viso_data.dPosY(latest_viso_index);viso_data.dPosZ(latest_viso_index)]./viso_data.dt(latest_viso_index);
|
||||
|
||||
% convert quality metric to equivalent observation error
|
||||
|
|
|
|||
|
|
@ -25,7 +25,7 @@ param.fusion.baroHgtGate = 5.0; % Size of the IMU velocity innovation consistenc
|
|||
param.fusion.baroHgtNoise = 2.0; % 1SD observation noise of the baro measurements (m)
|
||||
|
||||
%% Magnetometer measurement fusion
|
||||
param.fusion.magTimeDelay = 0.0; % Magnetomer time delay relative to IMU (sec)
|
||||
param.fusion.magTimeDelay = 0.0; % magnetometer time delay relative to IMU (sec)
|
||||
param.fusion.magFuseMethod = 1; % 0: 3-Axis field fusion with free declination, 1: 3-Axis field fusion with constrained declination, 2: magnetic heading fusion. (None)
|
||||
param.fusion.magFieldError = 0.05; % Magnetic field measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 0 or 1. (gauss)
|
||||
param.fusion.magHdgError = 0.1745; % Magnetic heading measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 2. (rad)
|
||||
|
|
|
|||
|
|
@ -25,7 +25,7 @@ param.fusion.baroHgtGate = 5.0; % Size of the IMU velocity innovation consistenc
|
|||
param.fusion.baroHgtNoise = 2.0; % 1SD observation noise of the baro measurements (m)
|
||||
|
||||
%% Magnetometer measurement fusion
|
||||
param.fusion.magTimeDelay = 0.0; % Magnetomer time delay relative to IMU (sec)
|
||||
param.fusion.magTimeDelay = 0.0; % magnetometer time delay relative to IMU (sec)
|
||||
param.fusion.magFuseMethod = 1; % 0: 3-Axis field fusion with free declination, 1: 3-Axis field fusion with constrained declination, 2: magnetic heading fusion. (None)
|
||||
param.fusion.magFieldError = 0.05; % Magnetic field measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 0 or 1. (gauss)
|
||||
param.fusion.magHdgError = 0.1745; % Magnetic heading measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 2. (rad)
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ syms R_DECL R_YAW real; % variance of declination or yaw angle observation
|
|||
syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes
|
||||
syms rho real % air density (kg/m^3)
|
||||
syms R_ACC real % variance of accelerometer measurements (m/s^2)^2
|
||||
syms Kaccx Kaccy real % derivative of X and Y body specific forces wrt componenent of true airspeed along each axis (1/s)
|
||||
syms Kaccx Kaccy real % derivative of X and Y body specific forces wrt component of true airspeed along each axis (1/s)
|
||||
|
||||
%% define the state prediction equations
|
||||
|
||||
|
|
|
|||
|
|
@ -67,14 +67,14 @@ void Ekf::fuseOptFlow()
|
|||
float H_LOS[2][24] = {}; // Optical flow observation Jacobians
|
||||
float Kfusion[24][2] = {}; // Optical flow Kalman gains
|
||||
|
||||
// get rotation nmatrix from earth to body
|
||||
// get rotation matrix from earth to body
|
||||
Dcmf earth_to_body(_state.quat_nominal);
|
||||
earth_to_body = earth_to_body.transpose();
|
||||
|
||||
// calculate the sensor position relative to the IMU
|
||||
Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
|
||||
|
||||
// calculate the velocity of the sensor reelative to the imu in body frame
|
||||
// calculate the velocity of the sensor relative to the imu in body frame
|
||||
// Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign
|
||||
Vector3f vel_rel_imu_body = cross_product(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt, pos_offset_body);
|
||||
|
||||
|
|
@ -91,7 +91,7 @@ void Ekf::fuseOptFlow()
|
|||
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
||||
|
||||
// calculate the height above the ground of the optical flow camera. Since earth frame is NED
|
||||
// a positve offset in earth frame leads to a a smaller height above the ground.
|
||||
// a positive offset in earth frame leads to a smaller height above the ground.
|
||||
heightAboveGndEst -= pos_offset_earth(2);
|
||||
|
||||
// constrain minimum height above ground
|
||||
|
|
@ -102,7 +102,7 @@ void Ekf::fuseOptFlow()
|
|||
|
||||
// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
|
||||
// correct for gyro bias errors in the data used to do the motion compensation
|
||||
// Note the sign convention used: A positive LOS rate is a RH rotaton of the scene about that axis.
|
||||
// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
|
||||
Vector2f opt_flow_rate;
|
||||
opt_flow_rate(0) = _flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0);
|
||||
opt_flow_rate(1) = _flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1);
|
||||
|
|
@ -132,7 +132,7 @@ void Ekf::fuseOptFlow()
|
|||
H_LOS[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
|
||||
H_LOS[0][6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
|
||||
|
||||
// calculate intermediate variables for the X observaton innovatoin variance and Kalman gains
|
||||
// calculate intermediate variables for the X observation innovatoin variance and Kalman gains
|
||||
float t3 = q1*vd*2.0f;
|
||||
float t4 = q0*ve*2.0f;
|
||||
float t11 = q3*vn*2.0f;
|
||||
|
|
@ -278,7 +278,7 @@ void Ekf::fuseOptFlow()
|
|||
H_LOS[1][5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
|
||||
H_LOS[1][6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
|
||||
|
||||
// calculate intermediate variables for the Y observaton innovatoin variance and Kalman gains
|
||||
// calculate intermediate variables for the Y observation innovatoin variance and Kalman gains
|
||||
float t3 = q3*ve*2.0f;
|
||||
float t4 = q0*vn*2.0f;
|
||||
float t11 = q2*vd*2.0f;
|
||||
|
|
@ -472,7 +472,7 @@ void Ekf::fuseOptFlow()
|
|||
}
|
||||
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance marix is unhealthy and must be corrected
|
||||
// the covariance matrix is unhealthy and must be corrected
|
||||
bool healthy = true;
|
||||
_fault_status.flags.bad_optflow_X = false;
|
||||
_fault_status.flags.bad_optflow_Y = false;
|
||||
|
|
@ -496,7 +496,7 @@ void Ekf::fuseOptFlow()
|
|||
}
|
||||
}
|
||||
|
||||
// only apply covariance and state corrrections if healthy
|
||||
// only apply covariance and state corrections if healthy
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
|
|
@ -505,7 +505,7 @@ void Ekf::fuseOptFlow()
|
|||
}
|
||||
}
|
||||
|
||||
// correct the covariance marix for gross errors
|
||||
// correct the covariance matrix for gross errors
|
||||
fixCovarianceErrors();
|
||||
|
||||
// apply the state corrections
|
||||
|
|
@ -604,7 +604,7 @@ float Ekf::calcOptFlowMeasVar()
|
|||
weighting = 0.0f;
|
||||
}
|
||||
|
||||
// take the weighted average of the observation noie for the best and wort flow quality
|
||||
// take the weighted average of the observation noise for the best and wort flow quality
|
||||
float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting));
|
||||
|
||||
return R_LOS;
|
||||
|
|
|
|||
|
|
@ -46,9 +46,9 @@
|
|||
|
||||
void Ekf::fuseSideslip()
|
||||
{
|
||||
float SH_BETA[13] = {}; // Varialbe used to optimise calculations of measurement jacobian
|
||||
float SH_BETA[13] = {}; // Variable used to optimise calculations of measurement jacobian
|
||||
float H_BETA[24] = {}; // Observation Jacobian
|
||||
float SK_BETA[8] = {}; // Varialbe used to optimise calculations of the Kalman gain vector
|
||||
float SK_BETA[8] = {}; // Variable used to optimise calculations of the Kalman gain vector
|
||||
float Kfusion[24] = {}; // Kalman gain vector
|
||||
float R_BETA = _params.beta_noise;
|
||||
|
||||
|
|
@ -248,7 +248,7 @@ void Ekf::fuseSideslip()
|
|||
}
|
||||
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance marix is unhealthy and must be corrected
|
||||
// the covariance matrix is unhealthy and must be corrected
|
||||
bool healthy = true;
|
||||
_fault_status.flags.bad_sideslip = false;
|
||||
|
||||
|
|
@ -266,7 +266,7 @@ void Ekf::fuseSideslip()
|
|||
}
|
||||
}
|
||||
|
||||
// only apply covariance and state corrrections if healthy
|
||||
// only apply covariance and state corrections if healthy
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
|
|
@ -275,7 +275,7 @@ void Ekf::fuseSideslip()
|
|||
}
|
||||
}
|
||||
|
||||
// correct the covariance marix for gross errors
|
||||
// correct the covariance matrix for gross errors
|
||||
fixCovarianceErrors();
|
||||
|
||||
// apply the state corrections
|
||||
|
|
|
|||
|
|
@ -61,7 +61,7 @@ bool Ekf::initHagl()
|
|||
_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
|
||||
// Use the ground clearance value as our uncertainty
|
||||
_terrain_var = sq(_params.rng_gnd_clearance);
|
||||
// ths is a guess
|
||||
// this is a guess
|
||||
return false;
|
||||
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -49,7 +49,7 @@ int main(int argc, char *argv[])
|
|||
|
||||
Ekf *base = new Ekf();
|
||||
|
||||
// Test1: feed in fake imu data and check if delta angles are summed correclty
|
||||
// Test1: feed in fake imu data and check if delta angles are summed correctly
|
||||
float delta_vel[3] = { 0.002f, 0.002f, 0.002f};
|
||||
float delta_ang[3] = { -0.1f, 0.2f, 0.3f};
|
||||
uint32_t time_usec = 1000;
|
||||
|
|
|
|||
|
|
@ -106,7 +106,7 @@ void Ekf::fuseVelPosHeight()
|
|||
gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
|
||||
|
||||
// Compensate for positive static pressure transients (negative vertical position innovations)
|
||||
// casued by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
|
||||
// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
|
||||
float deadzone_start = 0.0f;
|
||||
float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
|
||||
|
||||
|
|
@ -146,7 +146,7 @@ void Ekf::fuseVelPosHeight()
|
|||
|
||||
} else if (_control_status.flags.ev_hgt) {
|
||||
fuse_map[5] = true;
|
||||
// calculate the innovation assuming the external vision observaton is in local NED frame
|
||||
// calculate the innovation assuming the external vision observation is in local NED frame
|
||||
innovation[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);
|
||||
// observation variance - defined externally
|
||||
R[5] = fmaxf(_ev_sample_delayed.hgtErr, 0.01f);
|
||||
|
|
@ -234,7 +234,7 @@ void Ekf::fuseVelPosHeight()
|
|||
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
|
||||
}
|
||||
|
||||
// update covarinace matrix via Pnew = (I - KH)P
|
||||
// update covariance matrix via Pnew = (I - KH)P
|
||||
float KHP[_k_num_states][_k_num_states];
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
|
|
@ -244,7 +244,7 @@ void Ekf::fuseVelPosHeight()
|
|||
}
|
||||
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance marix is unhealthy and must be corrected
|
||||
// the covariance matrix is unhealthy and must be corrected
|
||||
bool healthy = true;
|
||||
|
||||
for (int i = 0; i < _k_num_states; i++) {
|
||||
|
|
@ -299,7 +299,7 @@ void Ekf::fuseVelPosHeight()
|
|||
}
|
||||
}
|
||||
|
||||
// only apply covariance and state corrrections if healthy
|
||||
// only apply covariance and state corrections if healthy
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
|
|
@ -308,7 +308,7 @@ void Ekf::fuseVelPosHeight()
|
|||
}
|
||||
}
|
||||
|
||||
// correct the covariance marix for gross errors
|
||||
// correct the covariance matrix for gross errors
|
||||
fixCovarianceErrors();
|
||||
|
||||
// apply the state corrections
|
||||
|
|
|
|||
Loading…
Reference in New Issue