AP_NavEKF2: Improve comments, typos
This commit is contained in:
parent
a6eb337438
commit
ce37517268
@ -667,7 +667,7 @@ bool NavEKF2::InitialiseFilter(void)
|
||||
primary = 0;
|
||||
}
|
||||
|
||||
// initialse the cores. We return success only if all cores
|
||||
// initialise the cores. We return success only if all cores
|
||||
// initialise successfully
|
||||
bool ret = true;
|
||||
for (uint8_t i=0; i<num_cores; i++) {
|
||||
|
@ -162,7 +162,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);
|
||||
|
@ -427,7 +427,7 @@ void NavEKF2_core::FuseSideslip()
|
||||
}
|
||||
}
|
||||
|
||||
// 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-condiioning.
|
||||
ForceSymmetry();
|
||||
ConstrainVariances();
|
||||
|
||||
|
@ -70,7 +70,7 @@ public:
|
||||
if (success) {
|
||||
element = buffer[bestIndex].element;
|
||||
_tail = (bestIndex+1)%_size;
|
||||
//make time zero to stop using it again,
|
||||
//make time zero to stop using it again,
|
||||
//resolves corner case of reusing the element when head == tail
|
||||
buffer[bestIndex].element.time_ms = 0;
|
||||
return true;
|
||||
|
@ -163,7 +163,7 @@ void NavEKF2_core::realignYawGPS()
|
||||
|
||||
// calculate new filter quaternion states from Euler angles
|
||||
stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw);
|
||||
// reset the velocity and posiiton states as they will be inaccurate due to bad yaw
|
||||
// reset the velocity and position states as they will be inaccurate due to bad yaw
|
||||
ResetVelocity();
|
||||
ResetPosition();
|
||||
|
||||
|
@ -609,7 +609,7 @@ void NavEKF2_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];
|
||||
|
||||
@ -661,7 +661,7 @@ void NavEKF2_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 NavEKF2_core::readRngBcnData()
|
||||
{
|
||||
// get the location of the beacon data
|
||||
|
@ -249,8 +249,8 @@ void NavEKF2_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 NavEKF2_core::FuseRngBcnStatic()
|
||||
@ -260,7 +260,7 @@ void NavEKF2_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.
|
||||
*/
|
||||
|
@ -159,7 +159,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);
|
||||
|
Loading…
Reference in New Issue
Block a user