mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-26 18:48:30 -04:00
AP_NavEKF3: Improve comments, typos
This commit is contained in:
parent
ce37517268
commit
23b7f1e645
@ -153,7 +153,7 @@ public:
|
||||
bool getOriginLLH(struct Location &loc) const;
|
||||
|
||||
// set the latitude and longitude and height used to set the NED origin
|
||||
// All NED positions calcualted by the filter will be relative to this location
|
||||
// All NED positions calculated by the filter will be relative to this location
|
||||
// The origin cannot be set if the filter is in a flight mode (eg vehicle armed)
|
||||
// Returns false if the filter has rejected the attempt to set the origin
|
||||
bool setOriginLLH(const Location &loc);
|
||||
|
@ -380,7 +380,7 @@ void NavEKF3_core::readIMUData()
|
||||
// reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle
|
||||
framesSincePredict = 0;
|
||||
|
||||
// set the flag to let the filter know it has new IMU data nad needs to run
|
||||
// set the flag to let the filter know it has new IMU data and needs to run
|
||||
runUpdates = true;
|
||||
|
||||
// extract the oldest available data from the FIFO buffer
|
||||
@ -510,7 +510,7 @@ void NavEKF3_core::readGpsData()
|
||||
// Post-alignment checks
|
||||
calcGpsGoodForFlight();
|
||||
|
||||
// Read the GPS locaton in WGS-84 lat,long,height coordinates
|
||||
// Read the GPS location in WGS-84 lat,long,height coordinates
|
||||
const struct Location &gpsloc = _ahrs->get_gps().location();
|
||||
|
||||
// Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
|
||||
@ -524,7 +524,7 @@ void NavEKF3_core::readGpsData()
|
||||
// Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
|
||||
EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
|
||||
|
||||
// Set the uncertinty of the GPS origin height
|
||||
// Set the uncertainty of the GPS origin height
|
||||
ekfOriginHgtVar = sq(gpsHgtAccuracy);
|
||||
|
||||
}
|
||||
@ -631,7 +631,7 @@ void NavEKF3_core::calcFiltGpsHgtOffset()
|
||||
}
|
||||
lastOriginHgtTime_ms = imuDataDelayed.time_ms;
|
||||
|
||||
// calculate the observation variance assuming EKF error relative to datum is independant of GPS observation error
|
||||
// calculate the observation variance assuming EKF error relative to datum is independent of GPS observation error
|
||||
// when not using GPS as height source
|
||||
float originHgtObsVar = sq(gpsHgtAccuracy) + P[8][8];
|
||||
|
||||
@ -683,7 +683,7 @@ void NavEKF3_core::readAirSpdData()
|
||||
* Range Beacon Measurements *
|
||||
********************************************************/
|
||||
|
||||
// check for new airspeed data and update stored measurements if available
|
||||
// check for new range beacon data and push to data buffer if available
|
||||
void NavEKF3_core::readRngBcnData()
|
||||
{
|
||||
// get the location of the beacon data
|
||||
|
@ -259,8 +259,8 @@ void NavEKF3_core::FuseRngBcn()
|
||||
}
|
||||
|
||||
/*
|
||||
Use range beaon measurements to calculate a static position using a 3-state EKF algorithm.
|
||||
Algorihtm based on the following:
|
||||
Use range beacon measurements to calculate a static position using a 3-state EKF algorithm.
|
||||
Algorithm based on the following:
|
||||
https://github.com/priseborough/InertialNav/blob/master/derivations/range_beacon.m
|
||||
*/
|
||||
void NavEKF3_core::FuseRngBcnStatic()
|
||||
@ -270,7 +270,7 @@ void NavEKF3_core::FuseRngBcnStatic()
|
||||
|
||||
/*
|
||||
The first thing to do is to check if we have started the alignment and if not, initialise the
|
||||
states and covariance to a first guess. To do this iterate through the avilable beacons and then
|
||||
states and covariance to a first guess. To do this iterate through the available beacons and then
|
||||
initialise the initial position to the mean beacon position. The initial position uncertainty
|
||||
is set to the mean range measurement.
|
||||
*/
|
||||
|
@ -166,7 +166,7 @@ public:
|
||||
bool getOriginLLH(struct Location &loc) const;
|
||||
|
||||
// set the latitude and longitude and height used to set the NED origin
|
||||
// All NED positions calcualted by the filter will be relative to this location
|
||||
// All NED positions calculated by the filter will be relative to this location
|
||||
// The origin cannot be set if the filter is in a flight mode (eg vehicle armed)
|
||||
// Returns false if the filter has rejected the attempt to set the origin
|
||||
bool setOriginLLH(const Location &loc);
|
||||
@ -1091,7 +1091,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 beacob constellatio offset hyopthesis is being used
|
||||
bool usingMinHypothesis; // true when the min beacon constellation offset hyopthesis 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)
|
||||
|
Loading…
Reference in New Issue
Block a user