Sensor Replay (#717)

* Add primitive logging for Ekf

* Add python script to extract sensor data from ULog

* Add primitive sensor replay

* Add iris_gps data for sensor replay

* Add CI for functional change indication

* Update sensor replay flow data type

* update iris_gps_change indication

* test: Update EKF replay test documentation

Co-authored-by: Paul Riseborough <priseborough@users.noreply.github.com>

test/sensor_simulator/ekf_logger.cpp

@@ -0,0 +1,77 @@
+#include "ekf_logger.h"
+
+EkfLogger::EkfLogger(std::shared_ptr<Ekf> ekf):
+_ekf{ekf},
+_ekf_wrapper(ekf)
+{
+
+}
+
+EkfLogger::~EkfLogger() {}
+
+void EkfLogger::setFilePath(std::string file_path)
+{
+	_file_path = file_path;
+}
+
+void EkfLogger::writeStateToFile()
+{
+	if(!_file_opened)
+	{
+		_file.open(_file_path);
+		_file_opened = true;
+		_file << "Timestamp";
+		if(_state_logging_enabled)
+		{
+			for(int i = 0; i < 24; i++)
+			{
+				_file << ",state[" << i << "]";
+			}
+		}
+		if(_variance_logging_enabled)
+		{
+			for(int i = 0; i < 24; i++)
+			{
+				_file << ",variance[" << i << "]";
+			}
+		}
+		_file << std::endl;
+	}
+
+	if (_file)
+	{
+		writeState();
+	}
+	else
+	{
+		std::cerr << "Can not write to output file" << std::endl;
+		std::exit(-1);
+	}
+
+}
+
+void EkfLogger::writeState()
+{
+	if(_state_logging_enabled)
+	{
+		uint64_t time = _ekf->get_imu_sample_delayed().time_us;
+		_file << time;
+		if(_state_logging_enabled)
+		{
+			matrix::Vector<float, 24> state = _ekf_wrapper.getState();
+			for(int i = 0; i < 24; i++)
+			{
+				_file << "," << state(i);
+			}
+		}
+		if(_variance_logging_enabled)
+		{
+			matrix::Vector<float, 24> variance = _ekf->covariances_diagonal();
+			for(int i = 0; i < 24; i++)
+			{
+				_file << "," << variance(i);
+			}
+		}
+		_file << std::endl;
+	}
+}