Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: fixed typos

This commit is contained in:
Arjun Vinod 2019-02-22 18:35:24 -05:00 committed by Peter Barker
parent 78b165e36f
commit f382a657bd
13 changed files with 77 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -184,7 +184,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
AP_GROUPINFO("GLITCH_RAD", 7, NavEKF3, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
// 8 previously used for EKF3_GPS_DELAY parameter that has been deprecated.
// The EKF now takes its GPs delay form the GPS library with the default delays
// The EKF now takes its GPS delay form the GPS library with the default delays
// specified by the GPS_DELAY and GPS_DELAY2 parameters.
// Height measurement parameters
@ -681,7 +681,7 @@ bool NavEKF3::InitialiseFilter(void)
// Set up any cores that have been created
// This specifies the IMU to be used and creates the data buffers
// If we are unble to set up a core, return false and try again next time the function is called
// If we are unable to set up a core, return false and try again next time the function is called
bool core_setup_success = true;
for (uint8_t core_index=0; core_index<num_cores; core_index++) {
if (coreSetupRequired[core_index]) {
@ -849,7 +849,7 @@ void NavEKF3::getVelNED(int8_t instance, Vector3f &vel) const
float NavEKF3::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();
}
@ -903,7 +903,7 @@ void NavEKF3::resetGyroBias(void)
// Resets the baro so that it reads zero at the current height
// Resets the EKF height to zero
// Adjusts the EKf origin height so that the EKF height + origin height is the same as before
// Adjusts the EKF origin height so that the EKF height + origin height is the same as before
// Returns true if the height datum reset has been performed
// If using a range finder for height no reset is performed and it returns false
bool NavEKF3::resetHeightDatum(void)
@ -1015,7 +1015,7 @@ bool NavEKF3::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 NavEKF3::getOriginLLH(int8_t instance, struct Location &loc) const

44
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -69,23 +69,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 direction (dPosD/dt) 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
// 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;
@ -94,15 +94,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 accelerometer bias estimate in m/s/s
// 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 getAccelBias(int8_t instance, Vector3f &accelBias) 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
@ -110,7 +110,7 @@ public:
// Resets the baro so that it reads zero at the current height
// Resets the EKF height to zero
// Adjusts the EKf origin height so that the EKF height + origin height is the same as before
// Adjusts the EKF origin height so that the EKF height + origin height is the same as before
// Returns true if the height datum reset has been performed
// If using a range finder for height no reset is performed and it returns false
bool resetHeightDatum(void);
@ -130,19 +130,19 @@ public:
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) 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
@ -156,7 +156,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;
@ -172,7 +172,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
@ -182,14 +182,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;
// return the diagonals from the covariance matrix for the specified instance
@ -242,7 +242,7 @@ public:
uint32_t getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar) const;
// 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;
/*
@ -277,7 +277,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
@ -291,7 +291,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
@ -305,13 +305,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;
@ -411,7 +411,7 @@ private:
AP_Int8 _rngBcnDelay_ms; // effective average delay of range beacon measurements rel to IMU (msec)
AP_Float _useRngSwSpd; // Maximum horizontal ground speed to use range finder as the primary height source (m/s)
AP_Float _accBiasLim; // Accelerometer bias limit (m/s/s)
AP_Int8 _magMask; // Bitmask forcng specific EKF core instances to use simple heading magnetometer fusion.
AP_Int8 _magMask; // Bitmask forcing specific EKF core instances to use simple heading magnetometer fusion.
AP_Int8 _originHgtMode; // Bitmask controlling post alignment correction and reporting of the EKF origin height.
AP_Float _visOdmVelErrMax; // Observation 1-STD velocity error assumed for visual odometry sensor at lowest reported quality (m/s)
AP_Float _visOdmVelErrMin; // Observation 1-STD velocity error assumed for visual odometry sensor at highest reported quality (m/s)

4
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -186,7 +186,7 @@ void NavEKF3_core::FuseAirspeed()
}
}
// force the covariance matrix to me 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();
@ -454,7 +454,7 @@ void NavEKF3_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();

6
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -83,7 +83,7 @@ void NavEKF3_core::setWindMagStateLearningMode()
}
}
// determine if the vehicle is manoevring
// determine if the vehicle is manoeuvring
if (accNavMagHoriz > 0.5f) {
manoeuvring = true;
} else {
@ -301,7 +301,7 @@ void NavEKF3_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
@ -427,7 +427,7 @@ bool NavEKF3_core::readyToUseBodyOdm(void) const
bool wencDataGood = (imuSampleTime_ms - wheelOdmMeasTime_ms < 200);
// We require stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use odometry measurements
// becasue they are in a body frame of reference
// because they are in a body frame of reference
return (visoDataGood || wencDataGood)
&& tiltAlignComplete
&& delAngBiasLearned;

20
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -80,7 +80,7 @@ void NavEKF3_core::controlMagYawReset()
magYawResetRequest = magYawResetRequest || // an external request
initialResetRequest || // an initial alignment performed by all vehicle types using magnetometer
interimResetRequest || // an interim alignment required to recover from ground based magnetic anomaly
finalResetRequest; // the final reset when we have acheived enough height to be in stable magnetic field environment
finalResetRequest; // the final reset when we have achieved enough height to be in stable magnetic field environment
// Perform a reset of magnetic field states and reset yaw to corrected magnetic heading
if (magYawResetRequest || magStateResetRequest) {
@ -111,7 +111,7 @@ void NavEKF3_core::controlMagYawReset()
// reset the quaternion covariances using the rotation vector variances
initialiseQuatCovariances(angleErrVarVec);
// calculate the change in the quaternion state and apply it to the ouput history buffer
// calculate the change in the quaternion state and apply it to the output history buffer
prevQuat = stateStruct.quat/prevQuat;
StoreQuatRotate(prevQuat);
@ -200,7 +200,7 @@ void NavEKF3_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;
@ -219,7 +219,7 @@ void NavEKF3_core::SelectMagFusion()
// start performance timer
hal.util->perf_begin(_perf_FuseMagnetometer);
// clear the flag that lets other processes know that the expensive magnetometer fusion operation has been perfomred on that time step
// clear the flag that lets other processes know that the expensive magnetometer fusion operation has been performed on that time step
// used for load levelling
magFusePerformed = false;
@ -584,7 +584,7 @@ void NavEKF3_core::FuseMagnetometer()
memset(&Kfusion[22], 0, 8);
}
// 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;
@ -660,7 +660,7 @@ void NavEKF3_core::FuseMagnetometer()
memset(&Kfusion[22], 0, 8);
}
// 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;
}
@ -712,7 +712,7 @@ void NavEKF3_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();
@ -745,7 +745,7 @@ void NavEKF3_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 NavEKF3_core::fuseEulerYaw()
{
@ -946,7 +946,7 @@ void NavEKF3_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();
@ -1124,7 +1124,7 @@ void NavEKF3_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();

6
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -136,7 +136,7 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam
{
// This is a simple hack to get wheel encoder data into the EKF and verify the interface sign conventions and units
// It uses the exisiting body frame velocity fusion.
// TODO implement a dedicated wheel odometry observaton model
// TODO implement a dedicated wheel odometry observation model
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
@ -150,7 +150,7 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam
wheelOdmDataNew.delTime = delTime;
wheelOdmMeasTime_ms = timeStamp_ms;
// becasue we are currently converting to an equivalent velocity measurement before fusing
// because we are currently converting to an equivalent velocity measurement before fusing
// the measurement time is moved back to the middle of the sampling period
wheelOdmDataNew.time_ms = timeStamp_ms - (uint32_t)(500.0f * delTime);
@ -558,7 +558,7 @@ void NavEKF3_core::readGpsData()
// Read the GPS location in WGS-84 lat,long,height coordinates
const struct Location &gpsloc = gps.location();
// Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
// Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
if (gpsGoodToAlign && !validOrigin) {
setOrigin();

13
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -77,7 +77,7 @@ void NavEKF3_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 NavEKF3_core::EstimateTerrainOffset()
{
@ -315,7 +315,7 @@ void NavEKF3_core::FuseOptFlow()
// calculate relative velocity in sensor frame including the relative motion due to rotation
relVelSensor = (prevTnb * stateStruct.velocity) + (ofDataDelayed.bodyRadXYZ % posOffsetBody);
// divide velocity by range to get predicted angular LOS rates relative to X and Y axes
// divide velocity by range to get predicted angular LOS rates relative to X and Y axes
losPred[0] = relVelSensor.y/range;
losPred[1] = -relVelSensor.x/range;
@ -332,7 +332,7 @@ void NavEKF3_core::FuseOptFlow()
H_LOS[5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
H_LOS[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 innovation variance and Kalman gains
float t3 = q1*vd*2.0f;
float t4 = q0*ve*2.0f;
float t11 = q3*vn*2.0f;
@ -503,7 +503,7 @@ void NavEKF3_core::FuseOptFlow()
H_LOS[5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
H_LOS[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 innovation variance and Kalman gains
float t3 = q3*ve*2.0f;
float t4 = q0*vn*2.0f;
float t11 = q2*vd*2.0f;
@ -597,8 +597,7 @@ void NavEKF3_core::FuseOptFlow()
float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
float t78;
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
if (t77 > R_LOS) {
t78 = 1.0f/t77;
faultStatus.bad_yflow = false;
@ -717,7 +716,7 @@ void NavEKF3_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();

4
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -218,7 +218,7 @@ void NavEKF3_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 NavEKF3_core::getPosDownDerivative(void) const
{
// return the value calculated from a complementary filter applied to the EKF height and vertical acceleration
@ -328,7 +328,7 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
location_offset(loc, outputDataNew.position.x, outputDataNew.position.y);
return true;
} else {
// we could be in constant position mode because the vehicle has taken off without GPS, or has lost GPS
// we could be in constant position mode because the vehicle has taken off without GPS, or has lost GPS
// in this mode we cannot use the EKF states to estimate position so will return the best available data
if ((gps.status() >= AP_GPS::GPS_OK_FIX_2D)) {
// we have a GPS position fix to return

8
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -429,7 +429,7 @@ void NavEKF3_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 performance
// 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);
}
@ -636,7 +636,7 @@ void NavEKF3_core::FuseVelPosNED()
memset(&Kfusion[10], 0, 12);
}
// inhibit delta velocity bias state estmation by setting Kalman gains to zero
// inhibit delta velocity bias state estimation by setting Kalman gains to zero
if (!inhibitDelVelBiasStates) {
for (uint8_t i = 13; i<=15; i++) {
Kfusion[i] = P[i][stateIndex]*SK;
@ -688,7 +688,7 @@ void NavEKF3_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();
@ -1522,7 +1522,7 @@ void NavEKF3_core::FuseBodyVel()
}
}
// 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();

6
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -100,7 +100,7 @@ void NavEKF3_core::FuseRngBcn()
H_BCN[7] = -t4*t9;
H_BCN[8] = -t3*t9;
// If we are not using the beacons as a height reference, we pretend that the beacons
// are a the same height as the flight vehicle when calculating the observation derivatives
// are at the same height as the flight vehicle when calculating the observation derivatives
// and Kalman gains
// TODO - less hacky way of achieving this, preferably using an alternative derivation
if (activeHgtSource != HGT_SOURCE_BCN) {
@ -247,7 +247,7 @@ void NavEKF3_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();
@ -335,7 +335,7 @@ void NavEKF3_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;

6
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -10,7 +10,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()
@ -45,7 +45,7 @@ bool NavEKF3_core::calcGpsGoodToAlign(void)
const struct Location &gpsloc = gps.location(); // Current location
const float posFiltTimeConst = 10.0f; // time constant used to decay position drift
// calculate time lapsesd since last update and limit to prevent numerical errors
// calculate time lapsed since last update and limit to prevent numerical errors
float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms;
// Sum distance moved
@ -53,7 +53,7 @@ bool NavEKF3_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

10
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -616,7 +616,7 @@ void NavEKF3_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 NavEKF3_core::UpdateStrapdownEquationsNED()
@ -1374,7 +1374,7 @@ void NavEKF3_core::StoreOutputReset()
storedOutput[i] = outputDataNew;
}
outputDataDelayed = outputDataNew;
// reset the states for the complementary filter used to provide a vertical position dervative output
// reset the states for the complementary filter used to provide a vertical position derivative output
posDown = stateStruct.position.z;
posDownDerivative = stateStruct.velocity.z;
}
@ -1497,7 +1497,7 @@ void NavEKF3_core::ConstrainStates()
for (uint8_t i=4; i<=6; i++) statesArray[i] = constrain_float(statesArray[i],-5.0e2f,5.0e2f);
// position limit 1000 km - TODO apply circular limit
for (uint8_t i=7; i<=8; i++) statesArray[i] = constrain_float(statesArray[i],-1.0e6f,1.0e6f);
// height limit covers home alt on everest through to home alt at SL and ballon drop
// height limit covers home alt on everest through to home alt at SL and balloon drop
stateStruct.position.z = constrain_float(stateStruct.position.z,-4.0e4f,1.0e4f);
// gyro bias limit (this needs to be set based on manufacturers specs)
for (uint8_t i=10; i<=12; i++) statesArray[i] = constrain_float(statesArray[i],-GYRO_BIAS_LIMIT*dtEkfAvg,GYRO_BIAS_LIMIT*dtEkfAvg);
@ -1524,7 +1524,7 @@ void NavEKF3_core::calcEarthRateNED(Vector3f &omega, int32_t latitude) const
omega.z = -earthRate*sinf(lat_rad);
}
// initialise the earth magnetic field states using declination, suppled roll/pitch
// initialise the earth magnetic field states using declination, supplied roll/pitch
// and magnetometer measurements and return initial attitude quaternion
Quaternion NavEKF3_core::calcQuatAndFieldStates(float roll, float pitch)
{
@ -1596,7 +1596,7 @@ Quaternion NavEKF3_core::calcQuatAndFieldStates(float roll, float pitch)
magStateResetRequest = false;
} else {
// this function should not be called if there is no compass data but if is is, return the
// this function should not be called if there is no compass data but if it is, return the
// current attitude
initQuat = stateStruct.quat;
}

18
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -121,7 +121,7 @@ public:
// Resets the baro so that it reads zero at the current height
// Resets the EKF height to zero
// Adjusts the EKf origin height so that the EKF height + origin height is the same as before
// Adjusts the EKF origin height so that the EKF height + origin height is the same as before
// Returns true if the height datum reset has been performed
// If using a range finder for height no reset is performed and it returns false
bool resetHeightDatum(void);
@ -211,7 +211,7 @@ public:
* Write body frame linear and angular displacement measurements from a visual odometry sensor
*
* quality is a normalised confidence value from 0 to 100
* delPos is the XYZ change in linear position meaasured in body frame and relative to the inertial reference at time_ms (m)
* delPos is the XYZ change in linear position measured in body frame and relative to the inertial reference at time_ms (m)
* delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at time_ms (rad)
* delTime is the time interval for the measurement of delPos and delAng (sec)
* timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec)
@ -533,7 +533,7 @@ 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 vertical position datum to the range beacon system datum
@ -545,7 +545,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
@ -636,7 +636,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
@ -651,7 +651,7 @@ private:
// force alignment of the yaw angle using GPS velocity data
void realignYawGPS();
// initialise the earth magnetic field states using declination and current attitude and magnetometer meaasurements
// initialise the earth magnetic field states using declination and current attitude and magnetometer measurements
// and return attitude quaternion
Quaternion calcQuatAndFieldStates(float roll, float pitch);
@ -712,7 +712,7 @@ private:
// Determine if we are flying or on the ground
void detectFlight();
// Set inertial navigaton aiding mode
// Set inertial navigation aiding mode
void setAidingMode();
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
@ -961,7 +961,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
@ -981,7 +981,7 @@ private:
MAG=5, // Use magnetometer data
RNGFND=6 // Use rangefinder data
};
resetDataSource posResetSource; // preferred soure of data for position reset
resetDataSource posResetSource; // preferred source of data for position reset
resetDataSource velResetSource; // preferred source of data for a velocity reset
// variables used to calculate a vertical velocity that is kinematically consistent with the vertical position