ekf2: move EKF out of ecl

src/modules/ekf2/test/sensor_simulator/.gitignore

@@ -0,0 +1 @@
+__pycache__/

src/modules/ekf2/test/sensor_simulator/CMakeLists.txt

@@ -0,0 +1,51 @@
+############################################################################
+#
+#   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 ECL 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.
+#
+############################################################################
+
+
+set(SRCS
+	sensor_simulator.cpp
+	ekf_wrapper.cpp
+	ekf_logger.cpp
+	sensor.cpp
+	imu.cpp
+	mag.cpp
+	baro.cpp
+	gps.cpp
+	flow.cpp
+	range_finder.cpp
+	vio.cpp
+	airspeed.cpp
+   )
+
+add_library(ecl_sensor_sim ${SRCS})
+target_link_libraries(ecl_sensor_sim ecl_EKF)

src/modules/ekf2/test/sensor_simulator/airspeed.cpp

@@ -0,0 +1,34 @@
+#include "airspeed.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+Airspeed::Airspeed(std::shared_ptr<Ekf> ekf): Sensor(ekf)
+{
+}
+
+Airspeed::~Airspeed()
+{
+}
+
+void Airspeed::send(uint64_t time)
+{
+	if (_true_airspeed_data > FLT_EPSILON && _indicated_airspeed_data > FLT_EPSILON) {
+		airspeedSample airspeed_sample;
+		airspeed_sample.time_us = time;
+		airspeed_sample.eas2tas = _true_airspeed_data / _indicated_airspeed_data;
+		airspeed_sample.true_airspeed = _true_airspeed_data;
+		_ekf->setAirspeedData(airspeed_sample);
+	}
+}
+
+void Airspeed::setData(float true_airspeed, float indicated_airspeed)
+{
+	_true_airspeed_data = true_airspeed;
+	_indicated_airspeed_data = indicated_airspeed;
+}
+
+} // namespace sensor
+} // namespace sensor_simulator

src/modules/ekf2/test/sensor_simulator/airspeed.h

@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Feeds Ekf with airspeed data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#ifndef EKF_AIRSPEED_H
+#define EKF_AIRSPEED_H
+
+#include "sensor.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+class Airspeed: public Sensor
+{
+public:
+	Airspeed(std::shared_ptr<Ekf> ekf);
+	~Airspeed();
+
+	void setData(float true_airspeed, float eas2tas);
+
+private:
+	float _true_airspeed_data{0.0f};
+	float _indicated_airspeed_data{0.0f};
+
+	void send(uint64_t time) override;
+
+};
+
+} // namespace sensor
+} // namespace sensor_simulator
+#endif // !EKF_AIRSPEED_H

src/modules/ekf2/test/sensor_simulator/baro.cpp

@@ -0,0 +1,27 @@
+#include "baro.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+Baro::Baro(std::shared_ptr<Ekf> ekf): Sensor(ekf)
+{
+}
+
+Baro::~Baro()
+{
+}
+
+void Baro::send(uint64_t time)
+{
+	_ekf->setBaroData(baroSample{time, _baro_data});
+}
+
+void Baro::setData(float baro)
+{
+	_baro_data = baro;
+}
+
+} // namespace sensor
+} // namespace sensor_simulator

src/modules/ekf2/test/sensor_simulator/baro.h

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Feeds Ekf with Mag data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#ifndef EKF_BARO_H
+#define EKF_BARO_H
+
+#include "sensor.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+class Baro: public Sensor
+{
+public:
+	Baro(std::shared_ptr<Ekf> ekf);
+	~Baro();
+
+	void setData(float baro);
+
+private:
+	float _baro_data{0.0f};
+
+	void send(uint64_t time) override;
+
+};
+
+} // namespace sensor
+} // namespace sensor_simulator
+#endif // !EKF_BARO_H

src/modules/ekf2/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

src/modules/ekf2/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()

src/modules/ekf2/test/sensor_simulator/ekf_logger.cpp

@@ -0,0 +1,73 @@
+#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;
+		system_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->getStateAtFusionHorizonAsVector();
+
+			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;
+	}
+}

src/modules/ekf2/test/sensor_simulator/ekf_logger.h

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   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>
+ */
+#ifndef EKF_EKF_LOGGER_H
+#define EKF_EKF_LOGGER_H
+
+#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();
+
+};
+#endif // !EKF_EKF_LOGGER_H

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -0,0 +1,239 @@
+#include "ekf_wrapper.h"
+
+EkfWrapper::EkfWrapper(std::shared_ptr<Ekf> ekf):
+	_ekf{ekf}
+{
+	_ekf_params = _ekf->getParamHandle();
+}
+
+EkfWrapper::~EkfWrapper()
+{
+}
+
+void EkfWrapper::setBaroHeight()
+{
+	_ekf_params->vdist_sensor_type = VDIST_SENSOR_BARO;
+}
+
+bool EkfWrapper::isIntendingBaroHeightFusion() const
+{
+	return _ekf->control_status_flags().baro_hgt;
+}
+
+void EkfWrapper::setGpsHeight()
+{
+	_ekf_params->vdist_sensor_type = VDIST_SENSOR_GPS;
+}
+
+bool EkfWrapper::isIntendingGpsHeightFusion() const
+{
+	return _ekf->control_status_flags().gps_hgt;
+}
+
+void EkfWrapper::setRangeHeight()
+{
+	_ekf_params->vdist_sensor_type = VDIST_SENSOR_RANGE;
+}
+
+bool EkfWrapper::isIntendingRangeHeightFusion() const
+{
+	return _ekf->control_status_flags().rng_hgt;
+}
+
+void EkfWrapper::setVisionHeight()
+{
+	_ekf_params->vdist_sensor_type = VDIST_SENSOR_EV;
+}
+
+bool EkfWrapper::isIntendingVisionHeightFusion() const
+{
+	return _ekf->control_status_flags().ev_hgt;
+}
+
+void EkfWrapper::enableGpsFusion()
+{
+	_ekf_params->fusion_mode |= MASK_USE_GPS;
+}
+
+void EkfWrapper::disableGpsFusion()
+{
+	_ekf_params->fusion_mode &= ~MASK_USE_GPS;
+}
+
+bool EkfWrapper::isIntendingGpsFusion() const
+{
+	return _ekf->control_status_flags().gps;
+}
+
+void EkfWrapper::enableGpsHeadingFusion()
+{
+	_ekf_params->fusion_mode |= MASK_USE_GPSYAW;
+}
+
+void EkfWrapper::disableGpsHeadingFusion()
+{
+	_ekf_params->fusion_mode &= ~MASK_USE_GPSYAW;
+}
+
+bool EkfWrapper::isIntendingGpsHeadingFusion() const
+{
+	return _ekf->control_status_flags().gps_yaw;
+}
+
+void EkfWrapper::enableFlowFusion()
+{
+	_ekf_params->fusion_mode |= MASK_USE_OF;
+}
+
+void EkfWrapper::disableFlowFusion()
+{
+	_ekf_params->fusion_mode &= ~MASK_USE_OF;
+}
+
+bool EkfWrapper::isIntendingFlowFusion() const
+{
+	return _ekf->control_status_flags().opt_flow;
+}
+
+void EkfWrapper::setFlowOffset(const Vector3f &offset)
+{
+	_ekf_params->flow_pos_body = offset;
+}
+
+void EkfWrapper::enableExternalVisionPositionFusion()
+{
+	_ekf_params->fusion_mode |= MASK_USE_EVPOS;
+}
+
+void EkfWrapper::disableExternalVisionPositionFusion()
+{
+	_ekf_params->fusion_mode &= ~MASK_USE_EVPOS;
+}
+
+bool EkfWrapper::isIntendingExternalVisionPositionFusion() const
+{
+	return _ekf->control_status_flags().ev_pos;
+}
+
+void EkfWrapper::enableExternalVisionVelocityFusion()
+{
+	_ekf_params->fusion_mode |= MASK_USE_EVVEL;
+}
+
+void EkfWrapper::disableExternalVisionVelocityFusion()
+{
+	_ekf_params->fusion_mode &= ~MASK_USE_EVVEL;
+}
+
+bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
+{
+	return _ekf->control_status_flags().ev_vel;
+}
+
+void EkfWrapper::enableExternalVisionHeadingFusion()
+{
+	_ekf_params->fusion_mode |= MASK_USE_EVYAW;
+}
+
+void EkfWrapper::disableExternalVisionHeadingFusion()
+{
+	_ekf_params->fusion_mode &= ~MASK_USE_EVYAW;
+}
+
+bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
+{
+	return _ekf->control_status_flags().ev_yaw;
+}
+
+void EkfWrapper::enableExternalVisionAlignment()
+{
+	_ekf_params->fusion_mode |= MASK_ROTATE_EV;
+}
+
+void EkfWrapper::disableExternalVisionAlignment()
+{
+	_ekf_params->fusion_mode &= ~MASK_ROTATE_EV;
+}
+
+bool EkfWrapper::isIntendingMagHeadingFusion() const
+{
+	return _ekf->control_status_flags().mag_hdg;
+}
+
+bool EkfWrapper::isIntendingMag3DFusion() const
+{
+	return _ekf->control_status_flags().mag_3D;
+}
+
+void EkfWrapper::setMagFuseTypeNone()
+{
+	_ekf_params->mag_fusion_type = MAG_FUSE_TYPE_NONE;
+}
+
+bool EkfWrapper::isWindVelocityEstimated() const
+{
+	return _ekf->control_status_flags().wind;
+}
+
+void EkfWrapper::enableTerrainRngFusion()
+{
+	_ekf_params->terrain_fusion_mode |= TerrainFusionMask::TerrainFuseRangeFinder;
+}
+
+void EkfWrapper::disableTerrainRngFusion()
+{
+	_ekf_params->terrain_fusion_mode &= ~TerrainFusionMask::TerrainFuseRangeFinder;
+}
+
+bool EkfWrapper::isIntendingTerrainRngFusion() const
+{
+	terrain_fusion_status_u terrain_status;
+	terrain_status.value = _ekf->getTerrainEstimateSensorBitfield();
+	return terrain_status.flags.range_finder;
+}
+
+void EkfWrapper::enableTerrainFlowFusion()
+{
+	_ekf_params->terrain_fusion_mode |= TerrainFusionMask::TerrainFuseOpticalFlow;
+}
+
+void EkfWrapper::disableTerrainFlowFusion()
+{
+	_ekf_params->terrain_fusion_mode &= ~TerrainFusionMask::TerrainFuseOpticalFlow;
+}
+
+bool EkfWrapper::isIntendingTerrainFlowFusion() const
+{
+	terrain_fusion_status_u terrain_status;
+	terrain_status.value = _ekf->getTerrainEstimateSensorBitfield();
+	return terrain_status.flags.flow;
+}
+
+Eulerf EkfWrapper::getEulerAngles() const
+{
+	return Eulerf(_ekf->getQuaternion());
+}
+
+float EkfWrapper::getYawAngle() const
+{
+	const Eulerf euler(_ekf->getQuaternion());
+	return euler(2);
+}
+
+matrix::Vector<float, 4> EkfWrapper::getQuaternionVariance() const
+{
+	return matrix::Vector<float, 4>(_ekf->orientation_covariances().diag());
+}
+
+int EkfWrapper::getQuaternionResetCounter() const
+{
+	float tmp[4];
+	uint8_t counter;
+	_ekf->get_quat_reset(tmp, &counter);
+	return static_cast<int>(counter);
+}
+
+matrix::Vector3f EkfWrapper::getDeltaVelBiasVariance() const
+{
+	return _ekf->covariances_diagonal().slice<3, 1>(13, 0);
+}

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h

@@ -0,0 +1,120 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Wrapper class for Ekf object to set behavior or check status
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#ifndef EKF_EKF_WRAPPER_H
+#define EKF_EKF_WRAPPER_H
+
+#include <memory>
+#include "EKF/ekf.h"
+#include "EKF/estimator_interface.h"
+
+class EkfWrapper
+{
+public:
+	EkfWrapper(std::shared_ptr<Ekf> ekf);
+	~EkfWrapper();
+
+
+	void setBaroHeight();
+	bool isIntendingBaroHeightFusion() const;
+
+	void setGpsHeight();
+	bool isIntendingGpsHeightFusion() const;
+
+	void setRangeHeight();
+	bool isIntendingRangeHeightFusion() const;
+
+	void setVisionHeight();
+	bool isIntendingVisionHeightFusion() const;
+
+	void enableGpsFusion();
+	void disableGpsFusion();
+	bool isIntendingGpsFusion() const;
+
+	void enableGpsHeadingFusion();
+	void disableGpsHeadingFusion();
+	bool isIntendingGpsHeadingFusion() const;
+
+	void enableFlowFusion();
+	void disableFlowFusion();
+	bool isIntendingFlowFusion() const;
+	void setFlowOffset(const matrix::Vector3f &offset);
+
+	void enableExternalVisionPositionFusion();
+	void disableExternalVisionPositionFusion();
+	bool isIntendingExternalVisionPositionFusion() const;
+
+	void enableExternalVisionVelocityFusion();
+	void disableExternalVisionVelocityFusion();
+	bool isIntendingExternalVisionVelocityFusion() const;
+
+	void enableExternalVisionHeadingFusion();
+	void disableExternalVisionHeadingFusion();
+	bool isIntendingExternalVisionHeadingFusion() const;
+
+	bool isIntendingMagHeadingFusion() const;
+	bool isIntendingMag3DFusion() const;
+	void setMagFuseTypeNone();
+
+	void enableExternalVisionAlignment();
+	void disableExternalVisionAlignment();
+
+	bool isWindVelocityEstimated() const;
+
+	void enableTerrainRngFusion();
+	void disableTerrainRngFusion();
+	bool isIntendingTerrainRngFusion() const;
+
+	void enableTerrainFlowFusion();
+	void disableTerrainFlowFusion();
+	bool isIntendingTerrainFlowFusion() const;
+
+	Eulerf getEulerAngles() const;
+	float getYawAngle() const;
+	matrix::Vector<float, 4> getQuaternionVariance() const;
+	int getQuaternionResetCounter() const;
+
+	matrix::Vector3f getDeltaVelBiasVariance() const;
+
+private:
+	std::shared_ptr<Ekf> _ekf;
+
+	// Pointer to Ekf internal param struct
+	parameters *_ekf_params;
+
+};
+#endif // !EKF_EKF_WRAPPER_H

src/modules/ekf2/test/sensor_simulator/flow.cpp

@@ -0,0 +1,39 @@
+#include "flow.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+Flow::Flow(std::shared_ptr<Ekf> ekf): Sensor(ekf)
+{
+}
+
+Flow::~Flow()
+{
+}
+
+void Flow::send(uint64_t time)
+{
+	_flow_data.time_us = time;
+	_ekf->setOpticalFlowData(_flow_data);
+}
+
+void Flow::setData(const flowSample &flow)
+{
+	_flow_data = flow;
+
+}
+
+flowSample Flow::dataAtRest()
+{
+	flowSample flow_at_rest;
+	flow_at_rest.dt = static_cast<float>(_update_period) * 1e-6f;
+	flow_at_rest.flow_xy_rad = Vector2f{0.0f, 0.0f};
+	flow_at_rest.gyro_xyz = Vector3f{0.0f, 0.0f, 0.0f};
+	flow_at_rest.quality = 255;
+	return flow_at_rest;
+}
+
+} // namespace sensor
+} // namespace sensor_simulator

src/modules/ekf2/test/sensor_simulator/flow.h

@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Feeds Ekf with optical flow data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#ifndef EKF_FLOW_H
+#define EKF_FLOW_H
+
+#include "sensor.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+class Flow: public Sensor
+{
+public:
+	Flow(std::shared_ptr<Ekf> ekf);
+	~Flow();
+
+	void setData(const flowSample &flow);
+	flowSample dataAtRest();
+
+private:
+	flowSample _flow_data;
+
+	void send(uint64_t time) override;
+
+};
+
+} // namespace sensor
+} // namespace sensor_simulator
+#endif // !EKF_FLOW_H

src/modules/ekf2/test/sensor_simulator/gps.cpp

@@ -0,0 +1,134 @@
+#include "gps.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+Gps::Gps(std::shared_ptr<Ekf> ekf): Sensor(ekf)
+{
+}
+
+Gps::~Gps()
+{
+}
+
+void Gps::send(const uint64_t time)
+{
+	const float dt = static_cast<float>(time - _gps_data.time_usec) * 1e-6f;
+
+	_gps_data.time_usec = time;
+
+	if (fabsf(_gps_pos_rate(0)) > FLT_EPSILON || fabsf(_gps_pos_rate(1)) > FLT_EPSILON) {
+		stepHorizontalPositionByMeters(Vector2f(_gps_pos_rate) * dt);
+	}
+
+	if (fabsf(_gps_pos_rate(2)) > FLT_EPSILON) {
+		stepHeightByMeters(-_gps_pos_rate(2) * dt);
+	}
+
+	_ekf->setGpsData(_gps_data);
+}
+
+void Gps::setData(const gps_message &gps)
+{
+	_gps_data = gps;
+}
+
+void Gps::setAltitude(const int32_t alt)
+{
+	_gps_data.alt = alt;
+}
+
+void Gps::setLatitude(const int32_t lat)
+{
+	_gps_data.lat = lat;
+}
+
+void Gps::setLongitude(const int32_t lon)
+{
+	_gps_data.lon = lon;
+}
+
+void Gps::setVelocity(const Vector3f &vel)
+{
+	_gps_data.vel_ned = vel;
+}
+
+void Gps::setYaw(const float yaw)
+{
+	_gps_data.yaw = yaw;
+}
+
+void Gps::setYawOffset(const float yaw_offset)
+{
+	_gps_data.yaw_offset = yaw_offset;
+}
+
+void Gps::setFixType(const int fix_type)
+{
+	_gps_data.fix_type = fix_type;
+}
+
+void Gps::setNumberOfSatellites(const int num_satellites)
+{
+	_gps_data.nsats = num_satellites;
+}
+
+void Gps::setPdop(const float pdop)
+{
+	_gps_data.pdop = pdop;
+}
+
+void Gps::setPositionRateNED(const Vector3f &rate)
+{
+	_gps_pos_rate = rate;
+}
+
+void Gps::stepHeightByMeters(const float hgt_change)
+{
+	_gps_data.alt += hgt_change * 1e3f;
+}
+
+void Gps::stepHorizontalPositionByMeters(const Vector2f hpos_change)
+{
+	float hposN_curr {0.f};
+	float hposE_curr {0.f};
+
+	double lat_new {0.0};
+	double lon_new {0.0};
+
+	map_projection_project(&_ekf->global_origin(), _gps_data.lat * 1e-7, _gps_data.lon * 1e-7, &hposN_curr, &hposE_curr);
+
+	Vector2f hpos_new = Vector2f{hposN_curr, hposE_curr} + hpos_change;
+
+	map_projection_reproject(&_ekf->global_origin(), hpos_new(0), hpos_new(1), &lat_new, &lon_new);
+
+	_gps_data.lon = static_cast<int32_t>(lon_new * 1e7);
+	_gps_data.lat = static_cast<int32_t>(lat_new * 1e7);
+}
+
+gps_message Gps::getDefaultGpsData()
+{
+	gps_message gps_data{};
+	gps_data.time_usec = 0;
+	gps_data.lat = 473566094;
+	gps_data.lon = 85190237;
+	gps_data.alt = 422056;
+	gps_data.yaw = NAN;
+	gps_data.yaw_offset = 0.0f;
+	gps_data.fix_type = 3;
+	gps_data.eph = 0.5f;
+	gps_data.epv = 0.8f;
+	gps_data.sacc = 0.2f;
+	gps_data.vel_m_s = 0.0;
+	gps_data.vel_ned.setZero();
+	gps_data.vel_ned_valid = 1;
+	gps_data.nsats = 16;
+	gps_data.pdop = 0.0f;
+
+	return gps_data;
+}
+
+} // namespace sensor
+} // namespace sensor_simulator

src/modules/ekf2/test/sensor_simulator/gps.h

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Feeds Ekf with Gps data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#ifndef EKF_GPS_H
+#define EKF_GPS_H
+
+#include "sensor.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+class Gps: public Sensor
+{
+public:
+	Gps(std::shared_ptr<Ekf> ekf);
+	~Gps();
+
+	void setData(const gps_message &gps);
+	void stepHeightByMeters(const float hgt_change);
+	void stepHorizontalPositionByMeters(const Vector2f hpos_change);
+	void setPositionRateNED(const Vector3f &rate);
+	void setAltitude(const int32_t alt);
+	void setLatitude(const int32_t lat);
+	void setLongitude(const int32_t lon);
+	void setVelocity(const Vector3f &vel);
+	void setYaw(const float yaw);
+	void setYawOffset(const float yaw);
+	void setFixType(const int fix_type);
+	void setNumberOfSatellites(const int num_satellites);
+	void setPdop(const float pdop);
+
+	gps_message getDefaultGpsData();
+
+private:
+	void send(uint64_t time) override;
+
+	gps_message _gps_data{};
+	Vector3f _gps_pos_rate{};
+};
+
+} // namespace sensor
+} // namespace sensor_simulator
+#endif // EKF_GPS_H

src/modules/ekf2/test/sensor_simulator/imu.cpp

@@ -0,0 +1,46 @@
+#include "imu.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+Imu::Imu(std::shared_ptr<Ekf> ekf): Sensor(ekf)
+{
+}
+
+Imu::~Imu()
+{
+}
+
+void Imu::send(uint64_t time)
+{
+	const float dt = float((time - _time_last_data_sent) * 1.e-6f);
+	imuSample imu_sample{};
+	imu_sample.time_us = time;
+	imu_sample.delta_ang_dt = dt;
+	imu_sample.delta_ang = _gyro_data * imu_sample.delta_ang_dt;
+	imu_sample.delta_vel_dt = dt;
+	imu_sample.delta_vel = _accel_data * imu_sample.delta_vel_dt;
+
+	_ekf->setIMUData(imu_sample);
+}
+
+void Imu::setData(const Vector3f &accel, const Vector3f &gyro)
+{
+	setAccelData(accel);
+	setGyroData(gyro);
+}
+
+void Imu::setAccelData(const Vector3f &accel)
+{
+	_accel_data = accel;
+}
+
+void Imu::setGyroData(const Vector3f &gyro)
+{
+	_gyro_data = gyro;
+}
+
+} // namespace sensor
+} // namespace sensor_simulator

src/modules/ekf2/test/sensor_simulator/imu.h

@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Feeds Ekf with Imu data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#ifndef EKF_IMU_H
+#define EKF_IMU_H
+
+#include "sensor.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+class Imu: public Sensor
+{
+public:
+	Imu(std::shared_ptr<Ekf> ekf);
+	~Imu();
+
+	void setData(const Vector3f &accel, const Vector3f &gyro);
+	void setAccelData(const Vector3f &accel);
+	void setGyroData(const Vector3f &gyro);
+
+private:
+	Vector3f _accel_data;
+	Vector3f _gyro_data;
+
+	void send(uint64_t time) override;
+
+};
+
+} // namespace sensor
+} // namespace sensor_simulator
+#endif // EKF_IMU_H

src/modules/ekf2/test/sensor_simulator/mag.cpp

@@ -0,0 +1,27 @@
+#include "mag.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+Mag::Mag(std::shared_ptr<Ekf> ekf): Sensor(ekf)
+{
+}
+
+Mag::~Mag()
+{
+}
+
+void Mag::send(uint64_t time)
+{
+	_ekf->setMagData(magSample{time, _mag_data});
+}
+
+void Mag::setData(const Vector3f &mag)
+{
+	_mag_data = mag;
+}
+
+} // namespace sensor
+} // namespace sensor_simulator

src/modules/ekf2/test/sensor_simulator/mag.h

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Feeds Ekf with Mag data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#ifndef EKF_MAG_H
+#define EKF_MAG_H
+
+#include "sensor.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+class Mag: public Sensor
+{
+public:
+	Mag(std::shared_ptr<Ekf> ekf);
+	~Mag();
+
+	void setData(const Vector3f &mag);
+
+private:
+	Vector3f _mag_data;
+
+	void send(uint64_t time) override;
+
+};
+
+} // namespace sensor
+} // namespace sensor_simulator
+#endif // !EKF_MAG_H

src/modules/ekf2/test/sensor_simulator/range_finder.cpp

@@ -0,0 +1,36 @@
+#include "range_finder.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+RangeFinder::RangeFinder(std::shared_ptr<Ekf> ekf): Sensor(ekf)
+{
+}
+
+RangeFinder::~RangeFinder()
+{
+}
+
+void RangeFinder::send(uint64_t time)
+{
+	_range_sample.time_us = time;
+	_ekf->setRangeData(_range_sample);
+	_ekf->set_rangefinder_limits(_min_distance, _max_distance);
+}
+
+void RangeFinder::setData(float range_data_meters, int8_t range_quality)
+{
+	_range_sample.rng = range_data_meters;
+	_range_sample.quality = range_quality;
+}
+
+void RangeFinder::setLimits(float min_distance_m, float max_distance_m)
+{
+	_min_distance = min_distance_m;
+	_max_distance = max_distance_m;
+}
+
+} // namespace sensor
+} // namespace sensor_simulator

src/modules/ekf2/test/sensor_simulator/range_finder.h

@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Feeds Ekf with range finder data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#ifndef EKF_RANGE_FINDER_H
+#define EKF_RANGE_FINDER_H
+
+#include "sensor.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+class RangeFinder: public Sensor
+{
+public:
+	RangeFinder(std::shared_ptr<Ekf> ekf);
+	~RangeFinder();
+
+	void setData(float range_data, int8_t range_quality);
+	void setLimits(float min_distance_m, float max_distance_m);
+
+private:
+	rangeSample _range_sample {};
+	float _min_distance {0.2f};
+	float _max_distance {20.0f};
+
+	void send(uint64_t time) override;
+};
+
+} // namespace sensor
+} // namespace sensor_simulator
+#endif // !EKF_RANGE_FINDER_H

src/modules/ekf2/test/sensor_simulator/sensor.cpp

@@ -0,0 +1,32 @@
+#include "sensor.h"
+
+namespace sensor_simulator
+{
+
+Sensor::Sensor(std::shared_ptr<Ekf> ekf): _ekf{ekf}
+{
+}
+
+Sensor::~Sensor()
+{
+}
+
+void Sensor::update(uint64_t time)
+{
+	if (should_send(time)) {
+		send(time);
+		_time_last_data_sent = time;
+	}
+}
+
+bool Sensor::should_send(uint64_t time) const
+{
+	return _is_running && is_time_to_send(time);
+}
+
+bool Sensor::is_time_to_send(uint64_t time) const
+{
+	return (time >= _time_last_data_sent) && ((time - _time_last_data_sent) >= _update_period);
+}
+
+} // namespace sensor_simulator

src/modules/ekf2/test/sensor_simulator/sensor.h

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Base class for defining the interface for simulaton of a sensor
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+
+#ifndef EKF_SENSOR_H
+#define EKF_SENSOR_H
+
+#include "EKF/ekf.h"
+#include <math.h>
+#include <memory>
+
+namespace sensor_simulator
+{
+
+class Sensor
+{
+public:
+
+	Sensor(std::shared_ptr<Ekf> ekf);
+	virtual ~Sensor();
+
+	void update(uint64_t time);
+
+	void setRateHz(uint32_t rate) { _update_period = uint32_t(1000000) / rate; }
+
+	bool isRunning() const { return _is_running; }
+
+	void start() { _is_running = true; }
+
+	void stop() { _is_running = false; }
+
+	bool should_send(uint64_t time) const;
+
+protected:
+
+	std::shared_ptr<Ekf> _ekf;
+	// time in microseconds
+	uint32_t _update_period;
+	uint64_t _time_last_data_sent{0};
+
+	bool _is_running{false};
+
+	// Checks that the right amount time passed since last send data to fulfill rate
+	bool is_time_to_send(uint64_t time) const;
+
+	// call set*Data function of Ekf
+	virtual void send(uint64_t time) = 0;
+
+};
+
+} // namespace sensor_simulator
+#endif // !EKF_SENSOR_H

src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp

@@ -0,0 +1,327 @@
+#include "sensor_simulator.h"
+
+
+SensorSimulator::SensorSimulator(std::shared_ptr<Ekf> ekf):
+	_airspeed(ekf),
+	_baro(ekf),
+	_flow(ekf),
+	_gps(ekf),
+	_imu(ekf),
+	_mag(ekf),
+	_rng(ekf),
+	_vio(ekf),
+	_ekf{ekf}
+{
+	setSensorRateToDefault();
+	setSensorDataToDefault();
+	startBasicSensor();
+}
+
+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;
+				system_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;
+			system_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;
+				system_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::setSensorRateToDefault()
+{
+	_imu.setRateHz(200);
+	_mag.setRateHz(80);
+	_baro.setRateHz(80);
+	_gps.setRateHz(5);
+	_flow.setRateHz(50);
+	_rng.setRateHz(30);
+	_vio.setRateHz(30);
+	_airspeed.setRateHz(100);
+}
+
+void SensorSimulator::setSensorDataToDefault()
+{
+	_airspeed.setData(0.0f, 0.0f);
+	_baro.setData(122.2f);
+	_flow.setData(_flow.dataAtRest());
+	_gps.setData(_gps.getDefaultGpsData());
+	_imu.setData(Vector3f{0.0f, 0.0f, -CONSTANTS_ONE_G}, Vector3f{0.0f, 0.0f, 0.0f});
+	_mag.setData(Vector3f{0.2f, 0.0f, 0.4f});
+	_rng.setData(0.2f, 100);
+	_vio.setData(_vio.dataAtRest());
+}
+
+void SensorSimulator::startBasicSensor()
+{
+	_baro.start();
+	_imu.start();
+	_mag.start();
+}
+
+void SensorSimulator::runSeconds(float duration_seconds)
+{
+	runMicroseconds(uint32_t(duration_seconds * 1e6f));
+}
+
+void SensorSimulator::runMicroseconds(uint32_t duration)
+{
+	// simulate in 1000us steps
+	const uint64_t start_time = _time;
+
+	for (; _time < start_time + (uint64_t)duration; _time += 1000) {
+		bool update_imu = _imu.should_send(_time);
+		updateSensors();
+
+		if (update_imu) {
+			// Update at IMU rate
+			_ekf->update();
+		}
+	}
+}
+
+void SensorSimulator::updateSensors()
+{
+	_imu.update(_time);
+	_mag.update(_time);
+	_baro.update(_time);
+	_gps.update(_time);
+	_flow.update(_time);
+	_rng.update(_time);
+	_vio.update(_time);
+	_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;
+		system_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;
+		system_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 = fabsf(sample.sensor_data[0]) <= 0.f;
+		_ekf->set_in_air_status(!landed);
+
+	} else {
+		printf("Unknown sensor type, can not set replay sample");
+		system_exit(-1);
+	}
+}
+
+void SensorSimulator::setGpsLatitude(const double latitude)
+{
+	int32_t lat = static_cast<int32_t>(latitude * 1e7);
+	_gps.setLatitude(lat);
+}
+
+void SensorSimulator::setGpsLongitude(const double longitude)
+{
+	int32_t lon = static_cast<int32_t>(longitude * 1e7);
+	_gps.setLongitude(lon);
+}
+
+void SensorSimulator::setGpsAltitude(const float altitude)
+{
+	int32_t alt = static_cast<int32_t>(altitude * 1e3f);
+	_gps.setAltitude(alt);
+}
+
+void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias)
+{
+	_imu.setData(Vector3f{0.0f, 0.0f, -CONSTANTS_ONE_G} + accel_bias,
+		     Vector3f{0.0f, 0.0f, 0.0f} + gyro_bias);
+}
+
+void SensorSimulator::simulateOrientation(Quatf orientation)
+{
+	const Vector3f world_sensed_gravity = {0.0f, 0.0f, -CONSTANTS_ONE_G};
+	const Vector3f world_mag_field = Vector3f{0.2f, 0.0f, 0.4f};
+	const Dcmf R_bodyToWorld(orientation);
+	const Vector3f sensed_gravity_body = R_bodyToWorld.transpose() * world_sensed_gravity;
+	const Vector3f body_mag_field = R_bodyToWorld.transpose() * world_mag_field;
+
+	_imu.setData(sensed_gravity_body, Vector3f{0.0f, 0.0f, 0.0f});
+	_mag.setData(body_mag_field);
+}

src/modules/ekf2/test/sensor_simulator/sensor_simulator.h

@@ -0,0 +1,143 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * This class is providing methods to feed the ECL EKF with measurement.
+ * It takes a pointer to the Ekf object and will manipulate the object
+ * by call set*Data functions.
+ * It simulates the time to allow for sensor data being set at certain rate
+ * and also calls the update method of the EKF
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+
+#ifndef EKF_SENSOR_SIMULATOR_H
+#define EKF_SENSOR_SIMULATOR_H
+
+#include <memory>
+#include <fstream>
+#include <iostream>
+#include <sstream>
+#include <vector>
+#include <array>
+
+#include "imu.h"
+#include "mag.h"
+#include "baro.h"
+#include "gps.h"
+#include "flow.h"
+#include "range_finder.h"
+#include "vio.h"
+#include "airspeed.h"
+#include "EKF/ekf.h"
+
+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
+{
+
+public:
+	SensorSimulator(std::shared_ptr<Ekf> ekf);
+	~SensorSimulator();
+
+	uint64_t getTime() const { return _time; };
+
+	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(); }
+
+	void startGps() { _gps.start(); }
+	void stopGps() { _gps.stop(); }
+
+	void startFlow() { _flow.start(); }
+	void stopFlow() { _flow.stop(); }
+
+	void startRangeFinder() { _rng.start(); }
+	void stopRangeFinder() { _rng.stop(); }
+
+	void startExternalVision() { _vio.start(); }
+	void stopExternalVision() { _vio.stop(); }
+
+	void startAirspeedSensor() { _airspeed.start(); }
+	void stopAirspeedSensor() { _airspeed.stop(); }
+
+	void setGpsLatitude(const double latitude);
+	void setGpsLongitude(const double longitude);
+	void setGpsAltitude(const float altitude);
+
+	void setImuBias(Vector3f accel_bias, Vector3f gyro_bias);
+
+	void simulateOrientation(Quatf orientation);
+
+	void loadSensorDataFromFile(std::string filename);
+
+	Airspeed    _airspeed;
+	Baro        _baro;
+	Flow        _flow;
+	Gps         _gps;
+	Imu         _imu;
+	Mag         _mag;
+	RangeFinder _rng;
+	Vio         _vio;
+
+protected:
+
+private:
+	void setSensorDataToDefault();
+	void setSensorDataFromReplayData();
+	void setSensorRateToDefault();
+	void setSingleReplaySample(const sensor_info &sample);
+	void startBasicSensor();
+	void updateSensors();
+
+	std::shared_ptr<Ekf> _ekf {nullptr};
+
+	std::vector<sensor_info> _replay_data {};
+
+	bool _has_replay_data {false};
+
+	uint64_t _current_replay_data_index {0};
+	uint64_t _time {0};	// microseconds
+
+};
+#endif // !EKF_SENSOR_SIMULATOR_H

src/modules/ekf2/test/sensor_simulator/vio.cpp

@@ -0,0 +1,86 @@
+#include "vio.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+Vio::Vio(std::shared_ptr<Ekf> ekf): Sensor(ekf)
+{
+}
+
+Vio::~Vio()
+{
+}
+
+void Vio::send(uint64_t time)
+{
+	_vio_data.time_us = time;
+	_ekf->setExtVisionData(_vio_data);
+}
+
+void Vio::setData(const extVisionSample &vio_data)
+{
+	_vio_data = vio_data;
+}
+
+void Vio::setVelocityVariance(const Vector3f &velVar)
+{
+	setVelocityCovariance(matrix::diag(velVar));
+}
+
+void Vio::setVelocityCovariance(const Matrix3f &velCov)
+{
+	_vio_data.velCov = velCov;
+}
+
+void Vio::setPositionVariance(const Vector3f &posVar)
+{
+	_vio_data.posVar = posVar;
+}
+
+void Vio::setAngularVariance(float angVar)
+{
+	_vio_data.angVar = angVar;
+}
+
+void Vio::setVelocity(const Vector3f &vel)
+{
+	_vio_data.vel = vel;
+}
+
+void Vio::setPosition(const Vector3f &pos)
+{
+	_vio_data.pos = pos;
+}
+
+void Vio::setOrientation(const Quatf &quat)
+{
+	_vio_data.quat = quat;
+}
+
+void Vio::setVelocityFrameToBody()
+{
+	_vio_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD;
+}
+
+void Vio::setVelocityFrameToLocal()
+{
+	_vio_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
+}
+
+extVisionSample Vio::dataAtRest()
+{
+	extVisionSample vio_data;
+	vio_data.pos = Vector3f{0.0f, 0.0f, 0.0f};;
+	vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};;
+	vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f};
+	vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
+	vio_data.velCov = matrix::eye<float, 3>() * 0.1f;
+	vio_data.angVar = 0.05f;
+	vio_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
+	return vio_data;
+}
+
+} // namespace sensor
+} // namespace sensor_simulator

src/modules/ekf2/test/sensor_simulator/vio.h

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Feeds Ekf external vision data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#ifndef EKF_VIO_H
+#define EKF_VIO_H
+
+#include "sensor.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+class Vio: public Sensor
+{
+public:
+	Vio(std::shared_ptr<Ekf> ekf);
+	~Vio();
+
+	void setData(const extVisionSample &vio_data);
+	void setVelocityVariance(const Vector3f &velVar);
+	void setVelocityCovariance(const Matrix3f &velCov);
+	void setPositionVariance(const Vector3f &posVar);
+	void setAngularVariance(float angVar);
+	void setVelocity(const Vector3f &vel);
+	void setPosition(const Vector3f &pos);
+	void setOrientation(const Quatf &quat);
+	void setVelocityFrameToLocal();
+	void setVelocityFrameToBody();
+
+	extVisionSample dataAtRest();
+
+private:
+	extVisionSample _vio_data;
+
+	void send(uint64_t time) override;
+
+};
+
+} // namespace sensor
+} // namespace sensor_simulator
+#endif // !EKF_VIO_H