diff --git a/EKF/control.cpp b/EKF/control.cpp index ba3d880fe3..0f3afa78f8 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -62,7 +62,7 @@ void Ekf::controlFusionModes() } } - // control use of various external souces for position and velocity aiding + // control use of various external sources for position and velocity aiding controlExternalVisionAiding(); controlOpticalFlowAiding(); controlGpsAiding(); @@ -151,7 +151,7 @@ void Ekf::controlOpticalFlowAiding() float range = heightAboveGndEst / _R_to_earth(2, 2); if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) { - // we should ahve reliable OF measurements so + // we should have reliable OF measurements so // calculate X and Y body relative velocities from OF measurements Vector3f vel_optflow_body; vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt; @@ -183,7 +183,7 @@ void Ekf::controlOpticalFlowAiding() _state.pos(0) = 0.0f; _state.pos(1) = 0.0f; - // reset the coresponding covariances + // reset the corresponding covariances // we are by definition at the origin at commencement so variances are also zeroed zeroRows(P,7,8); zeroCols(P,7,8); @@ -246,7 +246,7 @@ void Ekf::controlGpsAiding() // We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) { if (_time_last_imu - _time_last_gps > 5e5) { - // if we don't have gps then we need to switch to the non-aiding mode, zero the veloity states + // if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states // and set the synthetic GPS position to the current estimate _control_status.flags.gps = false; _last_known_posNE(0) = _state.pos(0); @@ -361,7 +361,7 @@ void Ekf::controlHeightSensorTimeouts() // if GPS height is unavailable and baro data is available, reset height to baro reset_to_baro = reset_to_baro || (!gps_hgt_available && baro_data_fresh); - // if we cannot switch to baro and GPs data is available, reset height to GPS + // if we cannot switch to baro and GPS data is available, reset height to GPS bool reset_to_gps = !reset_to_baro && gps_hgt_available; if (reset_to_baro) { diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index aee2a825cf..dd2a8dfa0b 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -53,7 +53,7 @@ void Ekf::initialiseCovariance() } // calculate average prediction time step in sec - float dt = 0.001f * (float)FILTER_UPDATE_PERRIOD_MS; + float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS; // define the initial angle uncertainty as variances for a rotation vector Vector3f rot_vec_var; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index f37adb933d..e77d8df3cc 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -178,7 +178,7 @@ bool Ekf::init(uint64_t timestamp) _control_status.value = 0; _control_status_prev.value = 0; - _dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERRIOD_MS); + _dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS); return ret; } @@ -214,7 +214,7 @@ bool Ekf::update() // measurement updates - // Fuse magnetometer data using the selected fuson method and only if angular alignment is complete + // Fuse magnetometer data using the selected fusion method and only if angular alignment is complete if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) { fuseMag(); @@ -224,7 +224,7 @@ bool Ekf::update() } } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) { - // fusion of a Euler yaw angle from either a 321 or 312 rotation sequence + // fusion of an Euler yaw angle from either a 321 or 312 rotation sequence fuseHeading(); } else { @@ -232,7 +232,7 @@ bool Ekf::update() } } - // determine if range finder data has fallen behind the fusin time horizon fuse it if we are + // determine if range finder data has fallen behind the fusion time horizon fuse it if we are // not tilted too much to use it if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) && (_R_to_earth(2, 2) > 0.7071f)) { @@ -261,14 +261,14 @@ bool Ekf::update() _fuse_height = true; } - // determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled + // determine if baro data has fallen behind the fusion time horizon and fuse it in the main filter if enabled uint64_t last_baro_time_us = _baro_sample_delayed.time_us; if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { if (_control_status.flags.baro_hgt) { _fuse_height = true; } else { - // calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference + // calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us); local_time_step = math::constrain(local_time_step,0.0f,1.0f); last_baro_time_us = _baro_sample_delayed.time_us; @@ -589,13 +589,13 @@ void Ekf::predictState() float input = 0.5f*(_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt); // filter and limit input between -50% and +100% of nominal value - input = math::constrain(input,0.0005f * (float)(FILTER_UPDATE_PERRIOD_MS),0.002f * (float)(FILTER_UPDATE_PERRIOD_MS)); + input = math::constrain(input,0.0005f * (float)(FILTER_UPDATE_PERIOD_MS),0.002f * (float)(FILTER_UPDATE_PERIOD_MS)); _dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input; } bool Ekf::collect_imu(imuSample &imu) { - // accumulate and downsample IMU data across a period FILTER_UPDATE_PERRIOD_MS long + // accumulate and downsample IMU data across a period FILTER_UPDATE_PERIOD_MS long // copy imu data to local variables _imu_sample_new.delta_ang = imu.delta_ang; @@ -625,7 +625,7 @@ bool Ekf::collect_imu(imuSample &imu) // if the target time delta between filter prediction steps has been exceeded // write the accumulated IMU data to the ring buffer - float target_dt = (float)(FILTER_UPDATE_PERRIOD_MS) / 1000; + float target_dt = (float)(FILTER_UPDATE_PERIOD_MS) / 1000; if (_imu_down_sampled.delta_ang_dt >= target_dt - _last_dt_overrun) { // store the amount we have over-run the target update rate by @@ -734,7 +734,7 @@ void Ekf::calculateOutputStates() delta_ang_error(2) = scalar * q_error(3); // calculate a gain that provides tight tracking of the estimator attitude states and - // adjust for changes in time delay to mantain consistent damping ratio of ~0.7 + // adjust for changes in time delay to maintain consistent damping ratio of ~0.7 float time_delay = 1e-6f * (float)(_imu_sample_new.time_us - _imu_sample_delayed.time_us); time_delay = fmaxf(time_delay, _dt_imu_avg); float att_gain = 0.5f * _dt_imu_avg / time_delay; @@ -747,13 +747,13 @@ void Ekf::calculateOutputStates() float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f); float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f); - // calculate velocity and position corrections at the EKF fusion time horizon + // calculate velocity and position corrections at the EKF fusion time horizon Vector3f vel_delta = (_state.vel - _output_sample_delayed.vel) * vel_gain; Vector3f pos_delta = (_state.pos - _output_sample_delayed.pos) * pos_gain; // loop through the output filter state history and apply the corrections to the translational states // this method is too expensive to use for the attitude states due to the quaternion operations required - // but does not introudce a time dela in the 'correction loop' and allows smaller trackin gtime constants + // but does not introudce a time dela in the 'correction loop' and allows smaller tracking time constants // to be used outputSample output_states; unsigned output_length = _output_buffer.get_length(); diff --git a/EKF/ekf.h b/EKF/ekf.h index f276f6ed75..69b7c1df8a 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -159,7 +159,7 @@ private: uint64_t _time_last_arsp_fuse; // time the last fusion of airspeed measurements were performed (usec) Vector2f _last_known_posNE; // last known local NE position vector (m) float _last_disarmed_posD; // vertical position recorded at arming (m) - float _last_dt_overrun; // the amount of time the last IMU collection over-ran the target set by FILTER_UPDATE_PERRIOD_MS (sec) + float _last_dt_overrun; // the amount of time the last IMU collection over-ran the target set by FILTER_UPDATE_PERIOD_MS (sec) Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index fe2dc0ba76..5d0132a3ac 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -131,7 +131,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float *data) magSample mag_sample_new = {}; mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000; - mag_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; + mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; _time_last_mag = time_usec; @@ -148,7 +148,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) gpsSample gps_sample_new = {}; gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000; - gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; + gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; _time_last_gps = time_usec; gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us); @@ -191,7 +191,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float *data) baro_sample_new.hgt = *data; baro_sample_new.time_us = time_usec - _params.baro_delay_ms * 1000; - baro_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; + baro_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; _time_last_baro = time_usec; baro_sample_new.time_us = math::max(baro_sample_new.time_us, _imu_sample_delayed.time_us); @@ -211,7 +211,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *true_airspee 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_PERRIOD_MS * 1000 / 2; //typo PeRRiod + airspeed_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; //typo PeRRiod _time_last_airspeed = time_usec; _airspeed_buffer.push(airspeed_sample_new); diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index ddc06aa5d4..c2694c9b8f 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -235,7 +235,7 @@ protected: static const uint8_t OBS_BUFFER_LENGTH = 10; // defines how many measurement samples we can buffer static const uint8_t IMU_BUFFER_LENGTH = 30; // defines how many imu samples we can buffer - static const unsigned FILTER_UPDATE_PERRIOD_MS = 10; // ekf prediction period in milliseconds + static const unsigned FILTER_UPDATE_PERIOD_MS = 10; // ekf prediction period in milliseconds float _dt_imu_avg; // average imu update period in s diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index d93e416159..4356b7a159 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -86,7 +86,7 @@ void Ekf::fuseMag() float H_MAG[3][24] = {}; float Kfusion[24] = {}; - // Calculate observation Jacobians and kalman gains for each magentoemter axis + // Calculate observation Jacobians and kalman gains for each magnetometer axis // X Axis H_MAG[0][0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; H_MAG[0][1] = SH_MAG[0]; @@ -108,7 +108,7 @@ void Ekf::fuseMag() _fault_status.flags.bad_mag_x = false; } else { - // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned + // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned _fault_status.flags.bad_mag_x = true; // we need to reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); @@ -190,7 +190,7 @@ void Ekf::fuseMag() return; } - // update the states and covariance usinng sequential fusion of the magnetometer components + // update the states and covariance using sequential fusion of the magnetometer components for (uint8_t index = 0; index <= 2; index++) { // Calculate Kalman gains if (index == 0) { diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 6ad7a02e0a..69f0d5534a 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -46,7 +46,7 @@ void Ekf::fuseVelPosHeight() { - bool fuse_map[6] = {}; // map of booelans true when [VN,VE,VD,PN,PE,PD] observations are available + bool fuse_map[6] = {}; // map of booleans true when [VN,VE,VD,PN,PE,PD] observations are available bool innov_check_pass_map[6] = {}; // true when innovations consistency checks pass for [VN,VE,VD,PN,PE,PD] observations float R[6] = {}; // observation variances for [VN,VE,VD,PN,PE,PD] float gate_size[6] = {}; // innovation consistency check gate sizes for [VN,VE,VD,PN,PE,PD] observations