From 23b7f1e645db708cacb0407338b20d48d0c60fd1 Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar Do Carmo Lucas" Date: Mon, 1 May 2017 12:15:42 +0200 Subject: [PATCH] AP_NavEKF3: Improve comments, typos --- libraries/AP_NavEKF3/AP_NavEKF3.h | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 10 +++++----- libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp | 6 +++--- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 4 ++-- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index ab68243e66..3bbc6f9af8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -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); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 7650c70d67..486a778084 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index d9e495f377..fd927d9078 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -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. */ diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 1c2f51e362..05dd719eb8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -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)