diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 8fbc6ca1eb..3facb6d448 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -813,7 +813,7 @@ void NavEKF2::getVelNED(int8_t instance, Vector3f &vel) const float NavEKF2::getPosDownDerivative(int8_t instance) const { if (instance < 0 || instance >= num_cores) instance = primary; - // return the value calculated from a complementary filer applied to the EKF height and vertical acceleration + // return the value calculated from a complementary filter applied to the EKF height and vertical acceleration if (core) { return core[instance].getPosDownDerivative(); } @@ -990,7 +990,7 @@ bool NavEKF2::getLLH(struct Location &loc) const } // Return the latitude and longitude and height used to set the NED origin for the specified instance -// An out of range instance (eg -1) returns data for the the primary instance +// An out of range instance (eg -1) returns data for the primary instance // All NED positions calculated by the filter are relative to this location // Returns false if the origin has not been set bool NavEKF2::getOriginLLH(int8_t instance, struct Location &loc) const @@ -1476,7 +1476,7 @@ void NavEKF2::getTimingStatistics(int8_t instance, struct ekf_timing &timing) co * Write position and quaternion data from an external navigation system * * pos : XYZ position (m) in a RH navigation frame with the Z axis pointing down and XY axes horizontal. Frame must be aligned with NED if the magnetomer is being used for yaw. - * quat : quaternion describing the the rotation from navigation frame to body frame + * quat : quaternion describing the rotation from navigation frame to body frame * posErr : 1-sigma spherical position error (m) * angErr : 1-sigma spherical angle error (rad) * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index d8a7dd83f6..47b02a6297 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -72,23 +72,23 @@ public: int8_t getPrimaryCoreIMUIndex(void) const; // Write the last calculated NE position relative to the reference point (m) for the specified instance. - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control bool getPosNE(int8_t instance, Vector2f &posNE) const; // Write the last calculated D position relative to the reference point (m) for the specified instance. - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control bool getPosD(int8_t instance, float &posD) const; // return NED velocity in m/s for the specified instance - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance void getVelNED(int8_t instance, Vector3f &vel) const; - // Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s for the specified instance - // An out of range instance (eg -1) returns data for the the primary instance + // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s for the specified instance + // An out of range instance (eg -1) returns data for the primary instance // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF // but will always be kinematically consistent with the z component of the EKF position state float getPosDownDerivative(int8_t instance) const; @@ -97,15 +97,15 @@ public: void getAccelNED(Vector3f &accelNED) const; // return body axis gyro bias estimates in rad/sec for the specified instance - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance void getGyroBias(int8_t instance, Vector3f &gyroBias) const; // return body axis gyro scale factor error as a percentage for the specified instance - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance void getGyroScaleErrorPercentage(int8_t instance, Vector3f &gyroScale) const; // return tilt error convergence metric for the specified instance - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance void getTiltError(int8_t instance, float &ang) const; // reset body axis gyro bias estimates @@ -135,23 +135,23 @@ public: void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const; // return the Z-accel bias estimate in m/s^2 for the specified instance - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance void getAccelZBias(int8_t instance, float &zbias) const; // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance void getWind(int8_t instance, Vector3f &wind) const; // return earth magnetic field estimates in measurement units / 1000 for the specified instance - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance void getMagNED(int8_t instance, Vector3f &magNED) const; // return body magnetic field estimates in measurement units / 1000 for the specified instance - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance void getMagXYZ(int8_t instance, Vector3f &magXYZ) const; // return the magnetometer in use for the specified instance - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance uint8_t getActiveMag(int8_t instance) const; // Return estimated magnetometer offsets @@ -165,7 +165,7 @@ public: bool getLLH(struct Location &loc) const; // Return the latitude and longitude and height used to set the NED origin for the specified instance - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance // All NED positions calculated by the filter are relative to this location // Returns false if the origin has not been set bool getOriginLLH(int8_t instance, struct Location &loc) const; @@ -181,7 +181,7 @@ public: bool getHAGL(float &HAGL) const; // return the Euler roll, pitch and yaw angle in radians for the specified instance - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance void getEulerAngles(int8_t instance, Vector3f &eulers) const; // return the transformation matrix from XYZ (body) to NED axes @@ -191,14 +191,14 @@ public: void getQuaternion(int8_t instance, Quaternion &quat) const; // return the innovations for the specified instance - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const; // publish output observer angular, velocity and position tracking error void getOutputTrackingError(int8_t instance, Vector3f &error) const; // return the innovation consistency test ratios for the specified instance - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const; // should we use the compass? This is public so it can be used for @@ -215,12 +215,12 @@ public: void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset); // return data for debugging optical flow fusion for the specified instance - // An out of range instance (eg -1) returns data for the the primary instance + // An out of range instance (eg -1) returns data for the primary instance void getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const; /* Returns the following data for debugging range beacon fusion from the specified instance - An out of range instance (eg -1) returns data for the the primary instance + An out of range instance (eg -1) returns data for the primary instance ID : beacon identifier rng : measured range to beacon (m) innov : range innovation (m) @@ -246,7 +246,7 @@ public: /* return the filter fault status as a bitmasked integer for the specified instance - An out of range instance (eg -1) returns data for the the primary instance + An out of range instance (eg -1) returns data for the primary instance 0 = quaternions are NaN 1 = velocities are NaN 2 = badly conditioned X magnetometer fusion @@ -260,7 +260,7 @@ public: /* return filter timeout status as a bitmasked integer for the specified instance - An out of range instance (eg -1) returns data for the the primary instance + An out of range instance (eg -1) returns data for the primary instance 0 = position measurement timeout 1 = velocity measurement timeout 2 = height measurement timeout @@ -274,13 +274,13 @@ public: /* return filter gps quality check status for the specified instance - An out of range instance (eg -1) returns data for the the primary instance + An out of range instance (eg -1) returns data for the primary instance */ void getFilterGpsStatus(int8_t instance, nav_gps_status &faults) const; /* return filter status flags for the specified instance - An out of range instance (eg -1) returns data for the the primary instance + An out of range instance (eg -1) returns data for the primary instance */ void getFilterStatus(int8_t instance, nav_filter_status &status) const; @@ -327,7 +327,7 @@ public: * Write position and quaternion data from an external navigation system * * pos : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m) - * quat : quaternion desribing the the rotation from navigation frame to body frame + * quat : quaternion desribing the rotation from navigation frame to body frame * posErr : 1-sigma spherical position error (m) * angErr : 1-sigma spherical angle error (rad) * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp index 5250f2d2f5..9214a491b8 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp @@ -178,7 +178,7 @@ void NavEKF2_core::FuseAirspeed() } } - // force the covariance matrix to me symmetrical and limit the variances to prevent ill-condiioning. + // force the covariance matrix to me symmetrical and limit the variances to prevent ill-conditioning. ForceSymmetry(); ConstrainVariances(); @@ -425,7 +425,7 @@ void NavEKF2_core::FuseSideslip() } } - // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning. ForceSymmetry(); ConstrainVariances(); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index e81c1dd9a5..2e3c82a5b2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -83,7 +83,7 @@ void NavEKF2_core::setWindMagStateLearningMode() } } - // determine if the vehicle is manoevring + // determine if the vehicle is manoeuvring if (accNavMagHoriz > 0.5f) { manoeuvring = true; } else { @@ -261,7 +261,7 @@ void NavEKF2_core::setAidingMode() // check to see if we are starting or stopping aiding and set states and modes as required if (PV_AidingMode != PV_AidingModePrev) { - // set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped. + // set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped. switch (PV_AidingMode) { case AID_NONE: // We have ceased aiding diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 6e5a5803a6..a84c6724bc 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -112,8 +112,8 @@ void NavEKF2_core::controlMagYawReset() gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u ext nav yaw alignment complete",(unsigned)imu_index); } - // record the reset as complete and also record the in-flight reset as complete to stop further resets when hight is gained - // in-flight reset is unnecessary because we do not need to consider groudn based magnetic anomaly effects + // record the reset as complete and also record the in-flight reset as complete to stop further resets when height is gained + // in-flight reset is unnecessary because we do not need to consider ground based magnetic anomaly effects yawAlignComplete = true; finalInflightYawInit = true; @@ -202,7 +202,7 @@ void NavEKF2_core::realignYawGPS() // send yaw alignment information to console gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index); - // zero the attitude covariances becasue the corelations will now be invalid + // zero the attitude covariances because the correlations will now be invalid zeroAttCovOnly(); // record the yaw reset event @@ -213,7 +213,7 @@ void NavEKF2_core::realignYawGPS() magYawResetRequest = false; if (use_compass()) { - // request a mag field reset which may enable us to use the magnetoemter if the previous fault was due to bad initialisation + // request a mag field reset which may enable us to use the magnetometer if the previous fault was due to bad initialisation magStateResetRequest = true; // clear the all sensors failed status so that the magnetometers sensors get a second chance now that we are flying allMagSensorsFailed = false; @@ -589,7 +589,7 @@ void NavEKF2_core::FuseMagnetometer() } } - // set flags to indicate to other processes that fusion has been performede and is required on the next frame + // set flags to indicate to other processes that fusion has been performed and is required on the next frame // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step magFusePerformed = true; magFuseRequired = true; @@ -654,7 +654,7 @@ void NavEKF2_core::FuseMagnetometer() } } - // set flags to indicate to other processes that fusion has been performede and is required on the next frame + // set flags to indicate to other processes that fusion has been performed and is required on the next frame // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step magFusePerformed = true; magFuseRequired = false; @@ -712,7 +712,7 @@ void NavEKF2_core::FuseMagnetometer() } } - // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning. ForceSymmetry(); ConstrainVariances(); @@ -755,7 +755,7 @@ void NavEKF2_core::FuseMagnetometer() * This fusion method only modifies the orientation, does not require use of the magnetic field states and is computationally cheaper. * It is suitable for use when the external magnetic field environment is disturbed (eg close to metal structures, on ground). * It is not as robust to magnetometer failures. - * It is not suitable for operation where the horizontal magnetic field strength is weak (within 30 degrees latitude of the the magnetic poles) + * It is not suitable for operation where the horizontal magnetic field strength is weak (within 30 degrees latitude of the magnetic poles) */ void NavEKF2_core::fuseEulerYaw() { @@ -960,7 +960,7 @@ void NavEKF2_core::fuseEulerYaw() } } - // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning. ForceSymmetry(); ConstrainVariances(); @@ -1090,7 +1090,7 @@ void NavEKF2_core::FuseDeclination(float declErr) } } - // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning. ForceSymmetry(); ConstrainVariances(); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index 2ca2c4ceb6..4181ff4cd9 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -74,7 +74,7 @@ void NavEKF2_core::SelectFlowFusion() /* Estimation of terrain offset using a single state EKF -The filter can fuse motion compensated optiocal flow rates and range finder measurements +The filter can fuse motion compensated optical flow rates and range finder measurements */ void NavEKF2_core::EstimateTerrainOffset() { @@ -679,7 +679,7 @@ void NavEKF2_core::FuseOptFlow() } } - // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning. ForceSymmetry(); ConstrainVariances(); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 947170ac98..5968e267a2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -211,7 +211,7 @@ void NavEKF2_core::getVelNED(Vector3f &vel) const vel = outputDataNew.velocity + velOffsetNED; } -// Return the rate of change of vertical position in the down diection (dPosD/dt) of the body frame origin in m/s +// Return the rate of change of vertical position in the down direction (dPosD/dt) of the body frame origin in m/s float NavEKF2_core::getPosDownDerivative(void) const { // return the value calculated from a complementary filter applied to the EKF height and vertical acceleration diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 5bcbfd9c54..62eb3e17ae 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -353,7 +353,7 @@ void NavEKF2_core::SelectVelPosFusion() // store the time of the reset lastPosReset_ms = imuSampleTime_ms; - // If we are alseo using GPS as the height reference, reset the height + // If we are also using GPS as the height reference, reset the height if (activeHgtSource == HGT_SOURCE_GPS) { // Store the position before the reset so that we can record the reset delta posResetD = stateStruct.position.z; @@ -467,7 +467,7 @@ void NavEKF2_core::FuseVelPosNED() } R_OBS[4] = R_OBS[3]; // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity - // For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance + // For horizontal GPS velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPS perfomrance // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); } @@ -709,7 +709,7 @@ void NavEKF2_core::FuseVelPosNED() } } - // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning. ForceSymmetry(); ConstrainVariances(); @@ -801,7 +801,7 @@ void NavEKF2_core::selectHeightForFusion() // select height source if (extNavUsedForPos) { - // always use external vision as the hight source if using for position. + // always use external vision as the height source if using for position. activeHgtSource = HGT_SOURCE_EV; } else if (((frontend->_useRngSwHgt > 0) || (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) { if (frontend->_altSource == 1) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp index 29be5dd524..192dc7e6a9 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp @@ -210,7 +210,7 @@ void NavEKF2_core::FuseRngBcn() } } - // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning. ForceSymmetry(); ConstrainVariances(); @@ -306,7 +306,7 @@ void NavEKF2_core::FuseRngBcnStatic() // We are using a different height reference for the main EKF so need to estimate a vertical // position offset to be applied to the beacon system that minimises the range innovations // The position estimate should be stable after 100 iterations so we use a simple dual - // hypothesis 1-state EKF to estiate the offset + // hypothesis 1-state EKF to estimate the offset Vector3f refPosNED; refPosNED.x = receiverPos.x; refPosNED.y = receiverPos.y; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index ebefb2b136..2fc77d742d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -12,7 +12,7 @@ extern const AP_HAL::HAL& hal; /* Monitor GPS data to see if quality is good enough to initialise the EKF - Monitor magnetometer innovations to to see if the heading is good enough to use GPS + Monitor magnetometer innovations to see if the heading is good enough to use GPS Return true if all criteria pass for 10 seconds We also record the failure reason so that prearm_failure_reason() @@ -55,7 +55,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) gpsloc_prev = gpsloc; // Decay distance moved exponentially to zero gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst); - // Clamp the fiter state to prevent excessive persistence of large transients + // Clamp the filter state to prevent excessive persistence of large transients gpsDriftNE = MIN(gpsDriftNE,10.0f); // Fail if more than 3 metres drift after filtering whilst on-ground // This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 6bed91053f..2364905448 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -606,7 +606,7 @@ void NavEKF2_core::correctDeltaVelocity(Vector3f &delVel, float delVelDT) * Update the quaternion, velocity and position states using delayed IMU measurements * because the EKF is running on a delayed time horizon. Note that the quaternion is * not used by the EKF equations, which instead estimate the error in the attitude of - * the vehicle when each observtion is fused. This attitude error is then used to correct + * the vehicle when each observation is fused. This attitude error is then used to correct * the quaternion. */ void NavEKF2_core::UpdateStrapdownEquationsNED() @@ -1521,7 +1521,7 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch) lastYawReset_ms = imuSampleTime_ms; // calculate an initial quaternion using the new yaw value initQuat.from_euler(roll, pitch, yaw); - // zero the attitude covariances becasue the corelations will now be invalid + // zero the attitude covariances because the corelations will now be invalid zeroAttCovOnly(); // calculate initial Tbn matrix and rotate Mag measurements into NED diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 04a1276189..4ff0c320e6 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -94,7 +94,7 @@ public: // return NED velocity in m/s void getVelNED(Vector3f &vel) const; - // Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s + // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF // but will always be kinematically consistent with the z component of the EKF position state float getPosDownDerivative(void) const; @@ -311,9 +311,9 @@ public: /* * Write position and quaternion data from an external navigation system * - * sensOffset : position of the external navigatoin sensor in body frame (m) + * sensOffset : position of the external navigation sensor in body frame (m) * pos : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m) - * quat : quaternion desribing the the rotation from navigation frame to body frame + * quat : quaternion desribing the rotation from navigation frame to body frame * posErr : 1-sigma spherical position error (m) * angErr : 1-sigma spherical angle error (rad) * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec) @@ -500,10 +500,10 @@ private: // fuse range beacon measurements void FuseRngBcn(); - // use range beaon measurements to calculate a static position + // use range beacon measurements to calculate a static position void FuseRngBcnStatic(); - // calculate the offset from EKF vetical position datum to the range beacon system datum + // calculate the offset from EKF vertical position datum to the range beacon system datum void CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning); // fuse magnetometer measurements @@ -512,7 +512,7 @@ private: // fuse true airspeed measurements void FuseAirspeed(); - // fuse sythetic sideslip measurement of zero + // fuse synthetic sideslip measurement of zero void FuseSideslip(); // zero specified range of rows in the state covariance matrix @@ -603,7 +603,7 @@ private: // determine when to perform fusion of GPS position and velocity measurements void SelectVelPosFusion(); - // determine when to perform fusion of range measurements take realtive to a beacon at a known NED position + // determine when to perform fusion of range measurements take relative to a beacon at a known NED position void SelectRngBcnFusion(); // determine when to perform fusion of magnetometer measurements @@ -807,8 +807,8 @@ private: Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements ftype innovVtas; // innovation output from fusion of airspeed measurements ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements - bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step - bool magFuseRequired; // boolean set to true when magnetometer fusion will be perfomred in the next time step + bool magFusePerformed; // boolean set to true when magnetometer fusion has been performed in that time step + bool magFuseRequired; // boolean set to true when magnetometer fusion will be performed in the next time step uint32_t prevTasStep_ms; // time stamp of last TAS fusion step uint32_t prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step uint32_t lastMagUpdate_us; // last time compass was updated in usec @@ -926,7 +926,7 @@ private: Vector3f bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2) bool delAngBiasLearned; // true when the gyro bias has been learned nav_filter_status filterStatus; // contains the status of various filter outputs - float ekfOriginHgtVar; // Variance of the the EKF WGS-84 origin height estimate (m^2) + float ekfOriginHgtVar; // Variance of the EKF WGS-84 origin height estimate (m^2) double ekfGpsRefHgt; // floating point representation of the WGS-84 reference height used to convert GPS height to local height (m) uint32_t lastOriginHgtTime_ms; // last time the ekf's WGS-84 origin height was corrected Vector3f outputTrackError; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer @@ -963,7 +963,7 @@ private: of_elements ofDataNew; // OF data at the current time horizon of_elements ofDataDelayed; // OF data at the fusion time horizon uint8_t ofStoreIndex; // OF data storage index - bool flowDataToFuse; // true when optical flow data has is ready for fusion + bool flowDataToFuse; // true when optical flow data is ready for fusion bool flowDataValid; // true while optical flow data is still fresh bool fuseOptFlowData; // this boolean causes the last optical flow measurement to be fused float auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator