-added comments

-removed unused print functions
-removed false _imu_time_last variable (correct is _time_last_imu)

EKF/RingBuffer.h

@@ -82,13 +82,8 @@ public:
 		}
 	}
 
-	inline void push(data_type sample, bool debug = false)
+	inline void push(data_type sample)
 	{
-		if (debug) {
-			printf("elapsed %" PRIu64 "\n", sample.time_us - _time_last);
-			_time_last = sample.time_us;
-		}
-
 		int head_new = _head;
 
 		if (_first_write) {

EKF/common.h

@@ -64,45 +64,45 @@ typedef matrix::Quaternion<float> Quaternion;
 typedef matrix::Matrix<float, 3, 3> Matrix3f;
 
 struct outputSample {
-	Quaternion  quat_nominal;
-	Vector3f    vel;
-	Vector3f    pos;
-	uint64_t 	time_us;
+	Quaternion  quat_nominal;	// nominal quaternion describing vehicle attitude
+	Vector3f    vel;	// NED velocity estimate in earth frame in m/s
+	Vector3f    pos;	// NED position estimate in earth frame in m/s
+	uint64_t 	time_us;	// timestamp in microseconds
 };
 
 struct imuSample {
-	Vector3f    delta_ang;
-	Vector3f    delta_vel;
-	float       delta_ang_dt;
-	float       delta_vel_dt;
-	uint64_t    time_us;
+	Vector3f    delta_ang;	// delta angle in body frame (integrated gyro measurements)
+	Vector3f    delta_vel;	// delta velocity in body frame (integrated accelerometer measurements)
+	float       delta_ang_dt;	// delta angle integration period in seconds
+	float       delta_vel_dt;	// delta velocity integration period in seconds
+	uint64_t    time_us;	// timestamp in microseconds
 };
 
 struct gpsSample {
-	Vector2f    pos;
-	float       hgt;
-	Vector3f    vel;
-	uint64_t    time_us;
+	Vector2f    pos;	// NE earth frame gps horizontal position measurement in m
+	float       hgt;	// gps height measurement in m
+	Vector3f    vel;	// NED earth frame gps velocity measurement in m/s
+	uint64_t    time_us;	// timestamp in microseconds
 };
 
 struct magSample {
-	Vector3f    mag;
-	uint64_t    time_us;
+	Vector3f    mag;	// NED magnetometer body frame measurements
+	uint64_t    time_us;	// timestamp in microseconds
 };
 
 struct baroSample {
-	float       hgt;
-	uint64_t    time_us;
+	float       hgt;	// barometer height above sea level measurement in m
+	uint64_t    time_us;	// timestamp in microseconds
 };
 
 struct rangeSample {
-	float       rng;
-	uint64_t    time_us;
+	float       rng;	// range (distance to ground) measurement in m
+	uint64_t    time_us;	// timestamp in microseconds
 };
 
 struct airspeedSample {
-	float       airspeed;
-	uint64_t    time_us;
+	float       airspeed;	// airspeed measurement in m/s
+	uint64_t    time_us;	// timestamp in microseconds
 };
 
 struct flowSample {
@@ -155,16 +155,16 @@ struct parameters {
 };
 
 struct stateSample {
-	Vector3f    ang_error;
-	Vector3f    vel;
-	Vector3f    pos;
-	Vector3f    gyro_bias;
-	Vector3f    gyro_scale;
-	float       accel_z_bias;
-	Vector3f    mag_I;
-	Vector3f    mag_B;
-	Vector2f    wind_vel;
-	Quaternion  quat_nominal;
+	Vector3f    ang_error;	// attitude axis angle error (error state formulation)
+	Vector3f    vel;	// NED velocity in earth frame in m/s
+	Vector3f    pos;	// NED position in earth frame in m
+	Vector3f    gyro_bias;	// gyro bias estimate in rad/s
+	Vector3f    gyro_scale;	// gyro scale estimate
+	float       accel_z_bias;	// accelerometer z axis bias estimate
+	Vector3f    mag_I;	// NED earth magnetic field in gauss
+	Vector3f    mag_B;	// magnetometer bias estimate in body frame in gauss
+	Vector2f    wind_vel;	// wind velocity in m/s
+	Quaternion  quat_nominal;	// nominal quaternion describing vehicle attitude
 };
 
 struct fault_status_t {

EKF/ekf.cpp

@@ -40,7 +40,6 @@
  */
 
 #include "ekf.h"
-#include <drivers/drv_hrt.h>
 
 Ekf::Ekf():
 	_control_status{},
@@ -443,83 +442,3 @@ void Ekf::fuseRange()
 {
 
 }
-
-void Ekf::printStates()
-{
-	static int counter = 0;
-
-	if (counter % 50 == 0) {
-		printf("quaternion\n");
-
-		for (int i = 0; i < 4; i++) {
-			printf("quat %i %.5f\n", i, (double)_state.quat_nominal(i));
-		}
-
-		matrix::Euler<float> euler(_state.quat_nominal);
-		printf("yaw pitch roll %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0));
-
-		printf("vel\n");
-
-		for (int i = 0; i < 3; i++) {
-			printf("v %i %.5f\n", i, (double)_state.vel(i));
-		}
-
-		printf("pos\n");
-
-		for (int i = 0; i < 3; i++) {
-			printf("p %i %.5f\n", i, (double)_state.pos(i));
-		}
-
-		printf("gyro_scale\n");
-
-		for (int i = 0; i < 3; i++) {
-			printf("gs %i %.5f\n", i, (double)_state.gyro_scale(i));
-		}
-
-		printf("mag earth\n");
-
-		for (int i = 0; i < 3; i++) {
-			printf("mI %i %.5f\n", i, (double)_state.mag_I(i));
-		}
-
-		printf("mag bias\n");
-
-		for (int i = 0; i < 3; i++) {
-			printf("mB %i %.5f\n", i, (double)_state.mag_B(i));
-		}
-
-		counter = 0;
-	}
-
-	counter++;
-
-}
-
-void Ekf::printStatesFast()
-{
-	static int counter_fast = 0;
-
-	if (counter_fast % 50 == 0) {
-		printf("quaternion\n");
-
-		for (int i = 0; i < 4; i++) {
-			printf("quat %i %.5f\n", i, (double)_output_new.quat_nominal(i));
-		}
-
-		printf("vel\n");
-
-		for (int i = 0; i < 3; i++) {
-			printf("v %i %.5f\n", i, (double)_output_new.vel(i));
-		}
-
-		printf("pos\n");
-
-		for (int i = 0; i < 3; i++) {
-			printf("p %i %.5f\n", i, (double)_output_new.pos(i));
-		}
-
-		counter_fast = 0;
-	}
-
-	counter_fast++;
-}

EKF/ekf.h

@@ -49,7 +49,10 @@ public:
 	Ekf();
 	~Ekf();
 
+	// initialise variables to sane values (also interface class)
 	bool init(uint64_t timestamp);
+
+	// should be called every time new data is pushed into the filter
 	bool update();
 
 	// gets the innovations of velocity and position measurements
@@ -92,7 +95,7 @@ private:
 	static const uint8_t _k_num_states = 24;
 	static constexpr float _k_earth_rate = 0.000072921f;
 
-	stateSample _state;
+	stateSample _state;		// state struct of the ekf running at the delayed time horizon
 
 	bool _filter_initialised;
 	bool _earth_rate_initialised;
@@ -102,7 +105,7 @@ private:
 	bool _fuse_hor_vel;		// gps horizontal velocity measurement should be fused
 	bool _fuse_vert_vel;	// gps vertical velocity measurement should be fused
 
-	uint64_t _time_last_fake_gps;
+	uint64_t _time_last_fake_gps;	// last time in us at which we have faked gps measurement for static mode
 
 	uint64_t _time_last_pos_fuse;   // time the last fusion of horizotal position measurements was performed (usec)
 	uint64_t _time_last_vel_fuse;   // time the last fusion of velocity measurements was performed (usec)
@@ -111,9 +114,9 @@ private:
 	Vector2f _last_known_posNE;     // last known local NE position vector (m)
 	float _last_disarmed_posD;      // vertical position recorded at arming (m)
 
-	Vector3f _earth_rate_NED;
+	Vector3f _earth_rate_NED;	// earth rotation vector (NED) in rad/s
 
-	matrix::Dcm<float> _R_prev;
+	matrix::Dcm<float> _R_prev;	// transformation matrix from earth frame to body frame of previous ekf step
 
 	float P[_k_num_states][_k_num_states];	// state covariance matrix
 
@@ -126,11 +129,11 @@ private:
 	float _heading_innov_var; // heading measurement innovation variance
 
 	// complementary filter states
-	Vector3f _delta_angle_corr;
-	Vector3f _delta_vel_corr;
-	Vector3f _vel_corr;
-	imuSample _imu_down_sampled;
-	Quaternion _q_down_sampled;
+	Vector3f _delta_angle_corr;	// delta angle correction vector
+	Vector3f _delta_vel_corr;	// delta velocity correction vector
+	Vector3f _vel_corr;			// velocity correction vector
+	imuSample _imu_down_sampled;	// down sampled imu data (sensor rate -> filter update rate)
+	Quaternion _q_down_sampled;		// down sampled quaternion (tracking delta angles between ekf update steps)
 
 	// variables used for the GPS quality checks
 	float _gpsDriftVelN = 0.0f;     // GPS north position derivative (m/s)
@@ -155,52 +158,65 @@ private:
 
 	gps_check_fail_status_u _gps_check_fail_status;
 
+	// update the real time complementary filter states. This includes the prediction
+	// and the correction step
 	void calculateOutputStates();
 
+	// initialise filter states of both the delayed ekf and the real time complementary filter
 	bool initialiseFilter(void);
 
+	// initialise ekf covariance matrix
 	void initialiseCovariance();
 
+	// predict ekf state
 	void predictState();
 
+	// predict ekf covariance
 	void predictCovariance();
 
+	// ekf sequential fusion of magnetometer measurements
 	void fuseMag();
 
+	// fuse magnetometer heading measurement
 	void fuseHeading();
 
+	// fuse magnetometer declination measurement
 	void fuseDeclination();
 
+	// fuse airspeed measurement
 	void fuseAirspeed();
 
+	// fuse range measurements
 	void fuseRange();
 
+	// fuse velocity and position measurements (also barometer height)
 	void fuseVelPosHeight();
 
+	// reset velocity states of the ekf
 	void resetVelocity();
 
+	// reset position states of the ekf (only vertical position)
 	void resetPosition();
 
+	// reset height state of the ekf
 	void resetHeight();
 
 	void makeCovSymetrical();
 
+	// limit the diagonal of the covariance matrix
 	void limitCov();
 
-	void printCovToFile(char const *filename);
-
-	void assertCovNiceness();
-
+	// make ekf covariance matrix symmetric
 	void makeSymmetrical();
 
+	// constrain the ekf states
 	void constrainStates();
 
+	// generic function which will perform a fusion step given a kalman gain K
+	// and a scalar innovation value
 	void fuse(float *K, float innovation);
 
-	void printStates();
-
-	void printStatesFast();
-
+	// calculate the earth rotation vector from a given latitude
 	void calcEarthRateNED(Vector3f &omega, double lat_rad) const;
 
 	// return true id the GPS quality is good enough to set an origin and start aiding

EKF/ekf_helper.cpp

@@ -94,39 +94,6 @@ void Ekf::resetHeight()
 	}
 }
 
-#if defined(__PX4_POSIX) && !defined(__PX4_QURT)
-void Ekf::printCovToFile(char const *filename)
-{
-	std::ofstream myfile;
-	myfile.open(filename);
-	myfile << "Covariance matrix\n";
-	myfile << std::setprecision(1);
-
-	for (int i = 0; i < _k_num_states; i++) {
-		for (int j = 0; j < _k_num_states; j++) {
-			myfile << std::to_string(P[i][j]) << std::setprecision(1) << "          ";
-		}
-
-		myfile << "\n\n\n\n\n\n\n\n\n\n";
-	}
-}
-#endif
-
-// This checks if the diagonal of the covariance matrix is non-negative
-// and that the matrix is symmetric
-void Ekf::assertCovNiceness()
-{
-	for (int row = 0; row < _k_num_states; row++) {
-		for (int column = 0; column < row; column++) {
-			assert(fabsf(P[row][column] - P[column][row]) < 0.00001f);
-		}
-	}
-
-	for (int i = 0; i < _k_num_states; i++) {
-		assert(P[i][i] > -0.000001f);
-	}
-}
-
 // This function forces the covariance matrix to be symmetric
 void Ekf::makeSymmetrical()
 {

EKF/estimator_interface.cpp

@@ -223,7 +223,6 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 
 
 	_dt_imu_avg = 0.0f;
-	_imu_time_last = timestamp;
 
 	_imu_sample_delayed.delta_ang.setZero();
 	_imu_sample_delayed.delta_vel.setZero();
@@ -265,78 +264,3 @@ bool EstimatorInterface::position_is_valid()
 	// TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status
 	return _NED_origin_initialised && (_time_last_imu - _time_last_gps) < 5e6;
 }
-
-void EstimatorInterface::printStoredIMU()
-{
-	printf("---------Printing IMU data buffer------------\n");
-
-	for (int i = 0; i < IMU_BUFFER_LENGTH; i++) {
-		printIMU(&_imu_buffer[i]);
-	}
-}
-
-void EstimatorInterface::printIMU(struct imuSample *data)
-{
-	printf("time %" PRIu64 "\n", data->time_us);
-	printf("delta_ang_dt %.5f\n", (double)data->delta_ang_dt);
-	printf("delta_vel_dt %.5f\n", (double)data->delta_vel_dt);
-	printf("dA: %.5f %.5f %.5f \n", (double)data->delta_ang(0), (double)data->delta_ang(1), (double)data->delta_ang(2));
-	printf("dV: %.5f %.5f %.5f \n\n", (double)data->delta_vel(0), (double)data->delta_vel(1), (double)data->delta_vel(2));
-}
-
-void EstimatorInterface::printQuaternion(Quaternion &q)
-{
-	printf("q1 %.5f q2 %.5f q3 %.5f q4 %.5f\n", (double)q(0), (double)q(1), (double)q(2), (double)q(3));
-}
-
-void EstimatorInterface::print_imu_avg_time()
-{
-	printf("dt_avg: %.5f\n", (double)_dt_imu_avg);
-}
-
-void EstimatorInterface::printStoredMag()
-{
-	printf("---------Printing mag data buffer------------\n");
-
-	for (int i = 0; i < OBS_BUFFER_LENGTH; i++) {
-		printMag(&_mag_buffer[i]);
-	}
-}
-
-void EstimatorInterface::printMag(struct magSample *data)
-{
-	printf("time %" PRIu64 "\n", data->time_us);
-	printf("mag: %.5f %.5f %.5f \n\n", (double)data->mag(0), (double)data->mag(1), (double)data->mag(2));
-
-}
-
-void EstimatorInterface::printBaro(struct baroSample *data)
-{
-	printf("time %" PRIu64 "\n", data->time_us);
-	printf("baro: %.5f\n\n", (double)data->hgt);
-}
-
-void EstimatorInterface::printStoredBaro()
-{
-	printf("---------Printing baro data buffer------------\n");
-
-	for (int i = 0; i < OBS_BUFFER_LENGTH; i++) {
-		printBaro(&_baro_buffer[i]);
-	}
-}
-
-void EstimatorInterface::printGps(struct gpsSample *data)
-{
-	printf("time %" PRIu64 "\n", data->time_us);
-	printf("gps pos: %.5f %.5f %.5f\n", (double)data->pos(0), (double)data->pos(1), (double)data->hgt);
-	printf("gps vel %.5f %.5f %.5f\n\n", (double)data->vel(0), (double)data->vel(1), (double)data->vel(2));
-}
-
-void EstimatorInterface::printStoredGps()
-{
-	printf("---------Printing GPS data buffer------------\n");
-
-	for (int i = 0; i < OBS_BUFFER_LENGTH; i++) {
-		printGps(&_gps_buffer[i]);
-	}
-}

EKF/estimator_interface.h

@@ -128,17 +128,6 @@ public:
 	// set vehicle arm status data
 	void set_arm_status(bool data) { _vehicle_armed = data; }
 
-	void printIMU(struct imuSample *data);
-	void printStoredIMU();
-	void printQuaternion(Quaternion &q);
-	void print_imu_avg_time();
-	void printMag(struct magSample *data);
-	void printStoredMag();
-	void printBaro(struct baroSample *data);
-	void printStoredBaro();
-	void printGps(struct gpsSample *data);
-	void printStoredGps();
-
 	bool position_is_valid();
 
 
@@ -162,22 +151,22 @@ public:
 	}
 	void copy_timestamp(uint64_t *time_us)
 	{
-		*time_us = _imu_time_last;
+		*time_us = _time_last_imu;
 	}
 
 protected:
 
 	parameters _params;		// filter parameters
 
-	static const uint8_t OBS_BUFFER_LENGTH = 10;
-	static const uint8_t IMU_BUFFER_LENGTH = 30;
-	static const unsigned FILTER_UPDATE_PERRIOD_MS = 10;
+	static const uint8_t OBS_BUFFER_LENGTH = 10;	// defines how many measurement samples we can buffer
+	static const uint8_t IMU_BUFFER_LENGTH = 30;	// defines how many imu samples we can buffer
+	static const unsigned FILTER_UPDATE_PERRIOD_MS = 10;	// ekf prediction period in milliseconds
 
-	float _dt_imu_avg;
-	uint64_t _imu_time_last;
+	float _dt_imu_avg;	// average imu update period in s
 
-	imuSample _imu_sample_delayed;
+	imuSample _imu_sample_delayed;	// captures the imu sample on the delayed time horizon
 
+	// measurement samples capturing measurements on the delayed time horizon
 	magSample _mag_sample_delayed;
 	baroSample _baro_sample_delayed;
 	gpsSample _gps_sample_delayed;
@@ -185,14 +174,14 @@ protected:
 	airspeedSample _airspeed_sample_delayed;
 	flowSample _flow_sample_delayed;
 
-	outputSample _output_sample_delayed;
-	outputSample _output_new;
-	imuSample _imu_sample_new;
+	outputSample _output_sample_delayed;	// filter output on the delayed time horizon
+	outputSample _output_new;	// filter output on the non-delayed time horizon
+	imuSample _imu_sample_new;	// imu sample capturing the newest imu data
 
-	uint64_t _imu_ticks;
+	uint64_t _imu_ticks;	// counter for imu updates
 
-	bool _imu_updated = false;
-	bool _initialised = false;
+	bool _imu_updated = false;	// true if the ekf should update (completed downsampling process)
+	bool _initialised = false;	// true if ekf interface instance (data buffering) is initialised
 	bool _vehicle_armed = false;     // vehicle arm status used to turn off functionality used on the ground
 
 	bool _NED_origin_initialised = false;
@@ -206,6 +195,7 @@ protected:
 
 	float _vel_pos_test_ratio[6];   // velocity and position innovation consistency check ratios
 
+	// data buffer instances
 	RingBuffer<imuSample> _imu_buffer;
 	RingBuffer<gpsSample> _gps_buffer;
 	RingBuffer<magSample> _mag_buffer;
@@ -215,15 +205,19 @@ protected:
 	RingBuffer<flowSample> 	_flow_buffer;
 	RingBuffer<outputSample> _output_buffer;
 
-	uint64_t _time_last_imu;
-	uint64_t _time_last_gps;
-	uint64_t _time_last_mag;
-	uint64_t _time_last_baro;
-	uint64_t _time_last_range;
-	uint64_t _time_last_airspeed;
+	uint64_t _time_last_imu;	// timestamp of last imu sample in microseconds
+	uint64_t _time_last_gps;	// timestamp of last gps measurement in microseconds
+	uint64_t _time_last_mag;	// timestamp of last magnetometer measurement in microseconds
+	uint64_t _time_last_baro;	// timestamp of last barometer measurement in microseconds
+	uint64_t _time_last_range;	// timestamp of last range measurement in microseconds
+	uint64_t _time_last_airspeed;	// timestamp of last airspeed measurement in microseconds
 
 
 	fault_status_t _fault_status;
+
+	// allocate data buffers and intialise interface variables
 	bool initialise_interface(uint64_t timestamp);
+
+	// free buffer memory
 	void unallocate_buffers();
 };