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/CMakeLists.txt

@@ -35,6 +35,7 @@
 set(SRCS
 	sensor_simulator.cpp
 	ekf_wrapper.cpp
+	ekf_logger.cpp
 	sensor.cpp
 	imu.cpp
 	mag.cpp

test/sensor_simulator/convertULogToSensorData.py

@@ -0,0 +1,141 @@
+from pyulog import ULog
+import pandas as pd
+
+def getVioData(ulog: ULog) -> pd.DataFrame:
+
+	vehicle_visual_odometry = ulog.get_dataset("vehicle_visual_odometry").data
+	vio = pd.DataFrame({'timestamp': vehicle_visual_odometry['timestamp'],
+		'sensor' : 'vio',
+		'x': vehicle_visual_odometry["x"],
+		'y': vehicle_visual_odometry["y"],
+		'z': vehicle_visual_odometry["z"],
+		'qw': vehicle_visual_odometry["q[0]"],
+		'qx': vehicle_visual_odometry["q[1]"],
+		'qy': vehicle_visual_odometry["q[2]"],
+		'qz': vehicle_visual_odometry["q[3]"],
+		'vx': vehicle_visual_odometry["vx"],
+		'vy': vehicle_visual_odometry["vy"],
+		'vz': vehicle_visual_odometry["vz"]
+		})
+	return vio
+
+
+def getOpticalFlowData(ulog: ULog) -> pd.DataFrame:
+
+	optical_flow = ulog.get_dataset("optical_flow").data
+	flow = pd.DataFrame({'timestamp': optical_flow['timestamp'],
+	'sensor' : 'flow',
+	'pixel_flow_x_integral': optical_flow["pixel_flow_x_integral"],
+	'pixel_flow_y_integral': optical_flow["pixel_flow_y_integral"],
+	'gyro_x_rate_integral': optical_flow["gyro_x_rate_integral"],
+	'gyro_y_rate_integral': optical_flow["gyro_y_rate_integral"],
+	'gyro_z_rate_integral': optical_flow["gyro_z_rate_integral"],
+	'quality': optical_flow["quality"]
+		})
+	return flow
+
+
+def getAirspeedData(ulog: ULog) -> pd.DataFrame:
+
+	airspeed = ulog.get_dataset("airspeed").data
+	airspeed = pd.DataFrame({'timestamp': airspeed['timestamp'],
+		'sensor' : 'airspeed',
+		'true_as': airspeed["true_airspeed_m_s"],
+		'indicated_as': airspeed["indicated_airspeed_m_s"]
+		})
+	return airspeed
+
+
+def getRangeFinderData(ulog: ULog) -> pd.DataFrame:
+
+	range = pd.DataFrame()
+	try:
+		range_0 = ulog.get_dataset("distance_sensor", 0).data
+		rng_0 = pd.DataFrame({'timestamp': range_0['timestamp'],
+			'sensor' : 'range',
+			'data': range_0["current_distance"],
+			'quality': range_0["signal_quality"]
+			})
+		range = pd.concat([range, rng_0], ignore_index=True, sort=False)
+	except:
+		pass
+
+	try:
+		range_1 = ulog.get_dataset("distance_sensor", 1).data
+		rng_1 = pd.DataFrame({'timestamp': range_1['timestamp'],
+			'sensor' : 'range',
+			'data': range_1["current_distance"],
+			'quality': range_1["signal_quality"]
+			})
+		range = pd.concat([range, rng_1], ignore_index=True, sort=False)
+	except:
+		pass
+
+	try:
+		range_2 = ulog.get_dataset("distance_sensor", 2).data
+		rng_2 = pd.DataFrame({'timestamp': range_2['timestamp'],
+			'sensor' : 'range',
+			'data': range_2["current_distance"],
+			'quality': range_2["signal_quality"]
+			})
+		range = pd.concat([range, rng_2], ignore_index=True, sort=False)
+	except:
+		pass
+
+	return range
+
+
+def getGpsData(ulog: ULog) -> pd.DataFrame:
+
+	vehicle_gps_position = ulog.get_dataset("vehicle_gps_position").data
+	gps = pd.DataFrame({'timestamp': vehicle_gps_position['timestamp'],
+		'sensor' : 'gps',
+		'alt': vehicle_gps_position["alt"],
+		'lon': vehicle_gps_position["lon"],
+		'lat': vehicle_gps_position["lat"],
+		'vel_N': vehicle_gps_position["vel_n_m_s"],
+		'vel_E': vehicle_gps_position["vel_e_m_s"],
+		'vel_D': vehicle_gps_position["vel_d_m_s"],
+		})
+	return gps
+
+
+def getBarometerData(ulog: ULog) -> pd.DataFrame:
+
+	vehicle_air_data = ulog.get_dataset("vehicle_air_data").data
+	baro = pd.DataFrame({'timestamp': vehicle_air_data['timestamp'],
+		'sensor' : 'baro',
+		'baro_alt_meter': vehicle_air_data["baro_alt_meter"]})
+	return baro
+
+
+def getMagnetometerData(ulog: ULog) -> pd.DataFrame:
+
+	vehicle_magnetometer = ulog.get_dataset("vehicle_magnetometer").data
+	mag = pd.DataFrame({'timestamp': vehicle_magnetometer['timestamp'],
+		'sensor' : 'mag',
+		'magnetometer_ga[0]': vehicle_magnetometer["magnetometer_ga[0]"],
+		'magnetometer_ga[1]': vehicle_magnetometer["magnetometer_ga[1]"],
+		'magnetometer_ga[2]': vehicle_magnetometer["magnetometer_ga[2]"]})
+	return mag
+
+
+def getImuData(ulog: ULog) -> pd.DataFrame:
+
+	sensor_combined = ulog.get_dataset("sensor_combined").data
+	imu = pd.DataFrame({'timestamp': sensor_combined['timestamp'],
+		'sensor' : 'imu',
+		'accel_m_s2[0]': sensor_combined["accelerometer_m_s2[0]"],
+		'accel_m_s2[1]': sensor_combined["accelerometer_m_s2[1]"],
+		'accel_m_s2[2]': sensor_combined["accelerometer_m_s2[2]"],
+		'gyro_rad[0]': sensor_combined["gyro_rad[0]"],
+		'gyro_rad[1]': sensor_combined["gyro_rad[1]"],
+		'gyro_rad[2]': sensor_combined["gyro_rad[2]"]})
+	return imu
+
+def getVehicleLandingStatus(ulog: ULog) -> pd.DataFrame:
+	vehicle_land_detected = ulog.get_dataset("vehicle_land_detected").data
+	land = pd.DataFrame({'timestamp': vehicle_land_detected['timestamp'],
+		'sensor' : 'landed',
+		'landed': vehicle_land_detected["landed"]})
+	return land

test/sensor_simulator/createSensorDataFile.py

@@ -0,0 +1,124 @@
+from pyulog import ULog
+import argparse
+import pandas as pd
+import csv
+import convertULogToSensorData as util
+
+path = "/home/kamil/Documents/QGroundControl/Logs/iris_vision.ulg"
+output_path = "/home/kamil/Documents/QGroundControl/Logs/sensor_data_iris_vision.csv"
+
+
+def get_arguments():
+	"""
+	parses the command line arguments
+	:return:
+	"""
+	parser = argparse.ArgumentParser(
+	description='Create a csv file with sensor data values in chronological order from a single'
+			'ulog file.')
+	parser.add_argument('input_file', metavar='file.ulg', help='ULog input file path')
+	parser.add_argument('output_file', metavar='file.ulg', help='csv output file path')
+	return parser.parse_args()
+
+def main() -> None:
+	"""
+	main entry point
+	:return:
+	"""
+	args = get_arguments()
+
+	table = pd.DataFrame()
+
+	try:
+		ulog = ULog(args.input_file)
+	except:
+		print("Could not find ulog file")
+		exit(-1)
+
+	try:
+		imu = util.getImuData(ulog)
+		print("IMU data detected")
+		table = pd.concat([table, imu], ignore_index=True, sort=False)
+	except:
+		print("IMU data not detected")
+
+	try:
+		mag = util.getMagnetometerData(ulog)
+		print("Mag data detected")
+		table = pd.concat([table, mag], ignore_index=True, sort=False)
+	except:
+		print("Mag data not detected")
+
+	try:
+		baro = util.getBarometerData(ulog)
+		print("Baro data detected")
+		table = pd.concat([table, baro], ignore_index=True, sort=False)
+	except:
+		print("Baro data not detected")
+
+	try:
+		gps = util.getGpsData(ulog)
+		print("GPS data detected")
+		table = pd.concat([table, gps], ignore_index=True, sort=False)
+	except:
+		print("GPS data not detected")
+
+	try:
+		airspeed = util.getAirspeedData(ulog)
+		print("Airspeed data detected")
+		table = pd.concat([table, airspeed], ignore_index=True, sort=False)
+	except:
+		print("Airspeed data not detected")
+
+	try:
+		flow = util.getOpticalFlowData(ulog)
+		print("Flow data detected")
+		table = pd.concat([table, flow], ignore_index=True, sort=False)
+	except:
+		print("Flow data not detected")
+
+	try:
+		range = util.getRangeFinderData(ulog)
+		print("Range data detected")
+		table = pd.concat([table, range], ignore_index=True, sort=False)
+	except:
+		print("Range data not detected")
+
+	try:
+		vio = util.getVioData(ulog)
+		print("VIO data detected")
+		table = pd.concat([table, vio], ignore_index=True, sort=False)
+	except:
+		print("VIO data not detected")
+
+	try:
+		land = util.getVehicleLandingStatus(ulog)
+		print("Landing data detected")
+		table = pd.concat([table, land], ignore_index=True, sort=False)
+	except:
+		print("Landing data not detected")
+
+	table = table.sort_values('timestamp', axis=0, ascending=True)
+	table['timestamp'] = table['timestamp'] - table['timestamp'].iloc[0]
+	# remove the first 0.5 seconds of data to be robust against faulty initialized data
+	table = table[table.timestamp > 500000]
+	table.timestamp = table.timestamp - 500000
+
+	try:
+		table.to_csv(args.output_file, index=None, header=None)
+
+		# post processing remove empty cells from csv
+		result = []
+		with open(args.output_file, "r") as in_file:
+			reader = csv.reader(in_file)
+			result = [[item for item in row if item != ''] for row in reader]
+
+		with open(args.output_file, "w") as out_file:
+			csv_writer = csv.writer(out_file)
+			csv_writer.writerows(result)
+	except:
+		print("Could not write to specified output file")
+
+
+if __name__ == '__main__':
+	main()

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;
+	}
+}

test/sensor_simulator/ekf_logger.h

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 ECL Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Class to write EKF state to file
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#pragma once
+
+#include "EKF/ekf.h"
+#include "EKF/estimator_interface.h"
+#include "ekf_wrapper.h"
+#include <fstream>
+#include <iostream>
+
+class EkfLogger
+{
+public:
+	EkfLogger(std::shared_ptr<Ekf> ekf);
+	~EkfLogger();
+	void setFilePath(std::string file_path);
+
+	void enableStateLogging(){ _state_logging_enabled = true; };
+	void disableStateLogging(){ _state_logging_enabled = false; };
+	void enableVarianceLogging(){ _variance_logging_enabled = true; };
+	void disableVarianceLogging(){ _variance_logging_enabled = false; };
+
+	void writeStateToFile();
+
+private:
+	std::shared_ptr<Ekf> _ekf;
+	EkfWrapper _ekf_wrapper;
+
+	std::string _file_path;
+	std::ofstream _file;
+
+	bool _file_opened {false};
+
+	bool _state_logging_enabled {true};
+	bool _variance_logging_enabled {true};
+
+	void writeState();
+
+};

test/sensor_simulator/sensor.h

@@ -62,6 +62,8 @@ public:
 
 	void stop(){ _is_running = false; }
 
+	bool should_send(uint64_t time) const;
+
 protected:
 
 	std::shared_ptr<Ekf> _ekf;
@@ -71,8 +73,6 @@ protected:
 
 	bool _is_running{false};
 
-	bool should_send(uint64_t time) const;
-
 	// Checks that the right amount time passed since last send data to fulfill rate
 	bool is_time_to_send(uint64_t time) const;
 

test/sensor_simulator/sensor_simulator.cpp

@@ -22,6 +22,88 @@ SensorSimulator::~SensorSimulator()
 
 }
 
+void SensorSimulator::loadSensorDataFromFile(std::string file_name)
+{
+	std::ifstream file(file_name);
+	std::string line;
+
+	while (!file.eof()) {
+		std::string timestamp;
+		std::string sensor_type;
+		std::string sensor_data;
+		sensor_info sensor_sample;
+
+		getline(file, timestamp, ',');
+
+		if (!timestamp.compare("")){ // empty line at end of file
+			break;
+		}
+		sensor_sample.timestamp = std::stoul(timestamp);
+
+		if(_replay_data.size() > 0) {
+			sensor_info last_sample = _replay_data.back();
+			if (sensor_sample.timestamp < last_sample.timestamp)
+			{
+				std::cout << "Timestamps not sorted ascendingly" << std::endl;
+				exit(-1);
+			}
+		}
+
+		getline(file, sensor_type, ',');
+		if (!sensor_type.compare("imu")) {
+			sensor_sample.sensor_type = sensor_info::IMU;
+		} else if (!sensor_type.compare("mag")) {
+			sensor_sample.sensor_type = sensor_info::MAG;
+
+		} else if (!sensor_type.compare("baro")) {
+			sensor_sample.sensor_type = sensor_info::BARO;
+
+		} else if (!sensor_type.compare("gps")) {
+			sensor_sample.sensor_type = sensor_info::GPS;
+
+		} else if (!sensor_type.compare("airspeed")) {
+			sensor_sample.sensor_type = sensor_info::AIRSPEED;
+
+		} else if (!sensor_type.compare("range")) {
+			sensor_sample.sensor_type = sensor_info::RANGE;
+
+		} else if (!sensor_type.compare("flow")) {
+			sensor_sample.sensor_type = sensor_info::FLOW;
+
+		} else if (!sensor_type.compare("vio")) {
+			sensor_sample.sensor_type = sensor_info::VISION;
+
+		} else if (!sensor_type.compare("landed")) {
+			sensor_sample.sensor_type = sensor_info::LANDING_STATUS;
+
+		} else {
+			std::cout << "Sensor type in file unknown" << std::endl;
+			exit(-1);
+		}
+
+		getline(file, sensor_data);
+		std::stringstream ss(sensor_data);
+		int8_t i = 0;
+		while( ss.good() )
+		{
+			if(i>=10){
+				std::cout << "sensor data bigger than expected" << std::endl;
+				exit(-1);
+			}
+			std::string value_string;
+			getline( ss, value_string, ',' );
+			if(!value_string.compare("")){
+				continue;
+			}
+			sensor_sample.sensor_data[i] = std::stod(value_string);
+			i++;
+		}
+		_replay_data.emplace_back(sensor_sample);
+	}
+	file.close();
+	_has_replay_data = true;
+}
+
 void SensorSimulator::setSensorDataToDefault()
 {
 	_imu.setRateHz(250);
@@ -82,6 +164,118 @@ void SensorSimulator::updateSensors()
 	_airspeed.update(_time);
 }
 
+void SensorSimulator::runReplaySeconds(float duration_seconds)
+{
+	runReplayMicroseconds( uint32_t(duration_seconds * 1e6f) );
+}
+
+void SensorSimulator::runReplayMicroseconds(uint32_t duration)
+{
+	if(!_has_replay_data) {
+		std::cout << "Can not run replay without replay data" << std::endl;
+		exit(-1);
+	}
+	// simulate in 1000us steps
+	const uint64_t start_time = _time;
+
+	for(;_time < start_time + duration; _time+=1000)
+	{
+		setSensorDataFromReplayData();
+
+		bool update_imu = _imu.should_send(_time);
+		updateSensors();
+
+		if(update_imu)
+		{
+			_ekf->update();
+		}
+	}
+}
+
+void SensorSimulator::setSensorDataFromReplayData()
+{
+	if(_replay_data.size() > 0) {
+		sensor_info sample = _replay_data[_current_replay_data_index];
+		while(sample.timestamp < _time)
+		{
+			setSingleReplaySample(sample);
+			if(_current_replay_data_index < _replay_data.size())
+			{
+				_current_replay_data_index ++;
+			} else {
+				break;
+			}
+			sample = _replay_data[_current_replay_data_index];
+		}
+	} else {
+		std::cerr << "Loaded replay data empty. Likely could not load replay data" << std::endl;
+		exit(-1);
+	}
+}
+
+void SensorSimulator::setSingleReplaySample(const sensor_info& sample)
+{
+	if (sample.sensor_type == sensor_info::IMU) {
+		Vector3f accel{(float) sample.sensor_data[0],
+				(float) sample.sensor_data[1],
+				(float) sample.sensor_data[2]};
+		Vector3f gyro{(float) sample.sensor_data[3],
+				(float) sample.sensor_data[4],
+				(float) sample.sensor_data[5]};
+		_imu.setData(accel, gyro);
+
+	} else if (sample.sensor_type == sensor_info::MAG) {
+		Vector3f mag{(float) sample.sensor_data[0],
+				(float) sample.sensor_data[1],
+				(float) sample.sensor_data[2]};
+		_mag.setData(mag);
+
+	} else if (sample.sensor_type == sensor_info::BARO) {
+		_baro.setData((float) sample.sensor_data[0]);
+
+	} else if (sample.sensor_type == sensor_info::GPS) {
+		_gps.setAltitude((int32_t) sample.sensor_data[0]);
+		_gps.setLatitude((int32_t) sample.sensor_data[1]);
+		_gps.setLongitude((int32_t) sample.sensor_data[2]);
+		_gps.setVelocity(Vector3f((float) sample.sensor_data[3],
+					  (float) sample.sensor_data[4],
+					  (float) sample.sensor_data[5]));
+
+	} else if (sample.sensor_type == sensor_info::AIRSPEED) {
+			_airspeed.setData((float) sample.sensor_data[0], (float) sample.sensor_data[1]);
+
+	} else if (sample.sensor_type == sensor_info::RANGE) {
+			_rng.setData((float) sample.sensor_data[0], (float) sample.sensor_data[1]);
+
+	} else if (sample.sensor_type == sensor_info::FLOW) {
+		flowSample flow_sample;
+		flow_sample.flow_xy_rad = Vector2f(sample.sensor_data[0],
+						 sample.sensor_data[1]);
+		flow_sample.gyro_xyz = Vector3f(sample.sensor_data[2],
+					       sample.sensor_data[3],
+					       sample.sensor_data[4]);
+		flow_sample.quality = sample.sensor_data[5];
+		_flow.setData(flow_sample);
+
+	} else if (sample.sensor_type == sensor_info::VISION) {
+		// sensor not yet implemented
+
+		// extVisionSample vision_sample;
+		// vision_sample.pos;
+		// vision_sample.quat;
+		// vision_sample.vel;
+		// _vio.setData((float) sample.sensor_data[0], (float) sample.sensor_data[1]);
+
+	} else if (sample.sensor_type == sensor_info::LANDING_STATUS) {
+		bool landed = sample.sensor_data[0];
+		_ekf->set_in_air_status(!landed);
+
+	} else {
+		printf("Unknown sensor type, can not set replay sample");
+		exit(-1);
+	}
+}
+
 void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias)
 {
 	_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G} + accel_bias,

test/sensor_simulator/sensor_simulator.h

@@ -43,6 +43,10 @@
 #pragma once
 
 #include <memory>
+#include <fstream>
+#include <iostream>
+#include <sstream>
+#include <vector>
 
 #include "imu.h"
 #include "mag.h"
@@ -56,6 +60,12 @@
 
 using namespace sensor_simulator::sensor;
 
+struct sensor_info {
+uint64_t timestamp {};
+enum measurement_t {IMU, MAG, BARO, GPS, AIRSPEED, RANGE, FLOW, VISION, LANDING_STATUS} sensor_type = IMU;
+std::array<double, 10> sensor_data {};
+};
+
 class SensorSimulator
 {
 
@@ -68,6 +78,9 @@ private:
 	void setSensorRateToDefault();
 	void startBasicSensor();
 	void updateSensors();
+	void setSensorDataFromReplayData();
+	void setSingleReplaySample(const sensor_info& sample);
+
 
 public:
 	SensorSimulator(std::shared_ptr<Ekf> ekf);
@@ -78,6 +91,9 @@ public:
 	void runSeconds(float duration_seconds);
 	void runMicroseconds(uint32_t duration);
 
+	void runReplaySeconds(float duration_seconds);
+	void runReplayMicroseconds(uint32_t duration);
+
 	void startBaro(){ _baro.start(); }
 	void stopBaro(){ _baro.stop(); }
 
@@ -99,6 +115,8 @@ public:
 	void setImuBias(Vector3f accel_bias, Vector3f gyro_bias);
 	void simulateOrientation(Quatf orientation);
 
+	void loadSensorDataFromFile(std::string filename);
+
 	Imu _imu;
 	Mag _mag;
 	Baro _baro;
@@ -107,4 +125,9 @@ public:
 	RangeFinder _rng;
 	Vio _vio;
 	Airspeed _airspeed;
+
+	bool _has_replay_data {false};
+	std::vector<sensor_info> _replay_data;
+	uint64_t _current_replay_data_index {0};
+
 };