Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Spell in comments

Browse Source
This commit is contained in:
Dr.-Ing. Amilcar Do Carmo Lucas 2017-05-18 00:22:51 +02:00 committed by Randy Mackay
parent f004f4b499
commit c1eb9fb9c3
3 changed files with 21 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -80,7 +80,7 @@ public:
// An out of range instance (eg -1) returns data for the the primary instance
void getVelNED(int8_t instance, Vector3f &vel);
// Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s for the specified 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 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

12
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -94,10 +94,10 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
maxTimeDelay_ms = MAX(maxTimeDelay_ms , _frontend->tasDelay_ms);
}
// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
// calculate the IMU buffer length required to accommodate the maximum delay with some allowance for jitter
imu_buffer_length = (maxTimeDelay_ms / (uint16_t)(EKF_TARGET_DT_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 = maxTimeDelay_ms + (int)(ceil((float)maxTimeDelay_ms * 0.5f));
@ -680,7 +680,7 @@ void NavEKF3_core::calcOutputStates()
correctDeltaAngle(delAngNewCorrected, imuDataNew.delAngDT);
correctDeltaVelocity(delVelNewCorrected, imuDataNew.delVelDT);
// apply corections to track EKF solution
// apply corrections to track EKF solution
Vector3f delAng = delAngNewCorrected + delAngCorrection;
// convert the rotation vector to its equivalent quaternion
@ -762,7 +762,7 @@ void NavEKF3_core::calcOutputStates()
timeDelay = MAX(timeDelay, dtIMUavg);
float errorGain = 0.5f / timeDelay;
// calculate a corrrection to the delta angle
// calculate a correction to the delta angle
// that will cause the INS to track the EKF quaternions
delAngCorrection = deltaAngErr * errorGain * dtIMUavg;
@ -1430,7 +1430,7 @@ void NavEKF3_core::ConstrainVariances()
// constrain states to prevent ill-conditioning
void NavEKF3_core::ConstrainStates()
{
// quaternionsare limited between +-1
// quaternions are limited between +-1
for (uint8_t i=0; i<=3; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f);
// velocity limit 500 m/sec (could set this based on some multiple of max airspeed * EAS2TAS)
for (uint8_t i=4; i<=6; i++) statesArray[i] = constrain_float(statesArray[i],-5.0e2f,5.0e2f);
@ -1504,7 +1504,7 @@ Quaternion NavEKF3_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 correlations will now be invalid
zeroAttCovOnly();
// calculate initial Tbn matrix and rotate Mag measurements into NED

28
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -100,7 +100,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;
@ -873,7 +873,7 @@ private:
uint32_t lastYawReset_ms; // System time at which the last yaw reset occurred. Returned by getLastYawResetAngle
bool tiltAlignComplete; // true when tilt alignment is complete
bool yawAlignComplete; // true when yaw alignment is complete
bool magStateInitComplete; // true when the magnetic field sttes have been initialised
bool magStateInitComplete; // true when the magnetic field states have been initialised
uint8_t stateIndexLim; // Max state index used during matrix and array operations
imu_elements imuDataDelayed; // IMU data at the fusion time horizon
imu_elements imuDataNew; // IMU data at the current time horizon
@ -927,7 +927,7 @@ private:
uint8_t magSelectIndex; // Index of the magnetometer that is being used by the EKF
bool runUpdates; // boolean true when the EKF updates can be run
uint32_t framesSincePredict; // number of frames lapsed since EKF instance did a state prediction
bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycele
bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycle
uint8_t localFilterTimeStep_ms; // average number of msec between filter updates
float posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2)
Vector3f delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad)
@ -943,7 +943,7 @@ private:
Vector3f velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
Vector3f posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
uint32_t firstInitTime_ms; // First time the initialise function was called (msec)
uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report wass sent (msec)
uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report was sent (msec)
// Specify source of data to be used for a partial state reset
// Checking the availability and quality of the data source specified is the responsibility of the caller
@ -959,21 +959,21 @@ private:
resetDataSource posResetSource; // preferred soure 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 verical position
float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.
// variables used to calculate a vertical velocity that is kinematically consistent with the vertical position
float posDownDerivative; // Rate of change of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.
float posDown; // Down position state used in calculation of posDownRate
// variables used by the pre-initialisation GPS checks
struct Location gpsloc_prev; // LLH location of previous GPS measurement
uint32_t lastPreAlignGpsCheckTime_ms; // last time in msec the GPS quality was checked during pre alignment checks
float gpsDriftNE; // amount of drift detected in the GPS position during pre-flight GPs checks
float gpsVertVelFilt; // amount of filterred vertical GPS velocity detected durng pre-flight GPS checks
float gpsVertVelFilt; // amount of filtered vertical GPS velocity detected during pre-flight GPS checks
float gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
// variable used by the in-flight GPS quality check
bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks
bool ekfInnovationsPass; // true when GPS innovations pass in-flight checks
float sAccFilterState1; // state variable for LPF applid to reported GPS speed accuracy
float sAccFilterState1; // state variable for LPF applied to reported GPS speed accuracy
float sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed
uint32_t lastGpsCheckTime_ms; // last time in msec the GPS quality was checked
uint32_t lastInnovPassTime_ms; // last time in msec the GPS innovations passed
@ -1051,7 +1051,7 @@ private:
bfodm_elements bodyOdmDataNew; // Body frame odometry data at the current time horizon
bfodm_elements bodyOdmDataDelayed; // Body frame odometry data at the fusion time horizon
uint8_t bodyOdmStoreIndex; // Body frame odometry data storage index
uint32_t lastbodyVelPassTime_ms; // time stamp when the body velocity measurement last passed innvovation consistency checks (msec)
uint32_t lastbodyVelPassTime_ms; // time stamp when the body velocity measurement last passed innovation consistency checks (msec)
Vector3 bodyVelTestRatio; // Innovation test ratios for body velocity XYZ measurements
Vector3 varInnovBodyVel; // Body velocity XYZ innovation variances (rad/sec)^2
Vector3 innovBodyVel; // Body velocity XYZ innovations (rad/sec)
@ -1066,10 +1066,10 @@ private:
rng_bcn_elements rngBcnDataNew; // Range beacon data at the current time horizon
rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon
uint8_t rngBcnStoreIndex; // Range beacon data storage index
uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innvovation consistency checks (msec)
uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec)
float rngBcnTestRatio; // Innovation test ratio for range beacon measurements
bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check
bool rngBcnTimeout; // boolean true if range beacon measurements have faled innovation consistency checks for too long
bool rngBcnTimeout; // boolean true if range beacon measurements have failed innovation consistency checks for too long
float varInnovRngBcn; // range beacon observation innovation variance (m^2)
float innovRngBcn; // range beacon observation innovation (m)
uint32_t lastTimeRngBcn_ms[10]; // last time we received a range beacon measurement (msec)
@ -1090,7 +1090,7 @@ private:
uint8_t N_beacons; // Number of range beacons in use
float maxBcnPosD; // maximum position of all beacons in the down direction (m)
float minBcnPosD; // minimum position of all beacons in the down direction (m)
bool usingMinHypothesis; // true when the min beacon constellation offset hyopthesis is being used
bool usingMinHypothesis; // true when the min beacon constellation offset hypothesis is being used
float bcnPosDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMaxVar; // Variance of the bcnPosDownOffsetMax state (m)
@ -1131,7 +1131,7 @@ private:
// control of post takeoff magentic field and heading resets
bool finalInflightYawInit; // true when the final post takeoff initialisation of yaw angle has been performed
bool finalInflightMagInit; // true when the final post takeoff initialisation of magnetic field states been performed
bool magStateResetRequest; // true if magnetic field states need to be reset using the magneteomter measurements
bool magStateResetRequest; // true if magnetic field states need to be reset using the magnetomter measurements
bool magYawResetRequest; // true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements
bool gpsYawResetRequest; // true if the vehicle yaw needs to be reset to the GPS course
float posDownAtLastMagReset; // vertical position last time the mag states were reset (m)
@ -1176,7 +1176,7 @@ private:
bool bad_horiz_vel:1;
} gpsCheckStatus;
// states held by magnetomter fusion across time steps
// states held by magnetometer fusion across time steps
// magnetometer X,Y,Z measurements are fused across three time steps
// to level computational load as this is an expensive operation
struct {