diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 741a2a2dc1..a1b34b371d 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -33,7 +33,7 @@
 
 include(gtest.cmake)
 
-add_subdirectory(SensorSimulator)
+add_subdirectory(sensor_simulator)
 
 set(SRCS
 	main.cpp
diff --git a/test/sensor_simulator/baro.cpp b/test/sensor_simulator/baro.cpp
index 452262467a..7915b2719a 100644
--- a/test/sensor_simulator/baro.cpp
+++ b/test/sensor_simulator/baro.cpp
@@ -1,9 +1,11 @@
-#include "Baro.h"
+#include "baro.h"
 
-namespace sensor_simulator::sensor
+namespace sensor_simulator
+{
+namespace sensor
 {
 
-Baro::Baro(Ekf* ekf):Sensor(ekf)
+Baro::Baro(std::shared_ptr<Ekf> ekf):Sensor(ekf)
 {
 }
 
@@ -21,3 +23,6 @@ void Baro::setData(float baro)
 {
 	_baro_data = baro;
 }
+
+} // namespace sensor
+} // namespace sensor_simulator
diff --git a/test/sensor_simulator/baro.h b/test/sensor_simulator/baro.h
index 368c397012..46515d575b 100644
--- a/test/sensor_simulator/baro.h
+++ b/test/sensor_simulator/baro.h
@@ -39,13 +39,15 @@
 
 #include "sensor.h"
 
-namespace sensor_simulator::sensor
+namespace sensor_simulator
+{
+namespace sensor
 {
 
 class Baro: public Sensor
 {
 public:
-	Baro(Ekf* ekf);
+	Baro(std::shared_ptr<Ekf> ekf);
 	~Baro();
 
 	void setData(float baro);
@@ -57,4 +59,5 @@ private:
 
 };
 
+} // namespace sensor
 } // namespace sensor_simulator::sensor
diff --git a/test/sensor_simulator/gps.cpp b/test/sensor_simulator/gps.cpp
index cdbb0ea770..4b2776956b 100644
--- a/test/sensor_simulator/gps.cpp
+++ b/test/sensor_simulator/gps.cpp
@@ -1,9 +1,11 @@
-#include "Gps.h"
+#include "gps.h"
 
-namespace sensor_simulator::sensor
+namespace sensor_simulator
+{
+namespace sensor
 {
 
-Gps::Gps(Ekf* ekf):Sensor(ekf)
+Gps::Gps(std::shared_ptr<Ekf> ekf):Sensor(ekf)
 {
 }
 
@@ -17,9 +19,10 @@ void Gps::send(uint32_t time)
 	_ekf->setGpsData(time, _gps_data);
 }
 
-void Gps::setData(gps_message gps)
+void Gps::setData(const gps_message& gps)
 {
 	_gps_data = gps;
 }
 
+} // namespace sensor
 } // namespace sensor_simulator::sensor
diff --git a/test/sensor_simulator/gps.h b/test/sensor_simulator/gps.h
index 13c1e61f57..575d10ea78 100644
--- a/test/sensor_simulator/gps.h
+++ b/test/sensor_simulator/gps.h
@@ -39,16 +39,18 @@
 
 #include "sensor.h"
 
-namespace sensor_simulator::sensor
+namespace sensor_simulator
+{
+namespace sensor
 {
 
 class Gps: public Sensor
 {
 public:
-	Gps(Ekf* ekf);
+	Gps(std::shared_ptr<Ekf> ekf);
 	~Gps();
 
-	void setData(gps_message gps);
+	void setData(const gps_message& gps);
 
 private:
 	gps_message _gps_data;
@@ -57,4 +59,5 @@ private:
 
 };
 
+} // namespace sensor
 } // namespace sensor_simulator::sensor
diff --git a/test/sensor_simulator/imu.cpp b/test/sensor_simulator/imu.cpp
index f63499174c..a15c6f6117 100644
--- a/test/sensor_simulator/imu.cpp
+++ b/test/sensor_simulator/imu.cpp
@@ -1,9 +1,11 @@
-#include "Imu.h"
+#include "imu.h"
 
-namespace sensor_simulator::sensor
+namespace sensor_simulator
+{
+namespace sensor
 {
 
-Imu::Imu(Ekf* ekf):Sensor(ekf)
+Imu::Imu(std::shared_ptr<Ekf> ekf):Sensor(ekf)
 {
 }
 
@@ -25,20 +27,21 @@ void Imu::send(uint32_t time)
 	_time_last_data_sent = time;
 }
 
-void Imu::setData(Vector3f accel, Vector3f gyro)
+void Imu::setData(const Vector3f& accel, const Vector3f& gyro)
 {
 	setAccelData(accel);
 	setGyroData(gyro);
 }
 
-void Imu::setAccelData(Vector3f accel)
+void Imu::setAccelData(const Vector3f& accel)
 {
 	_accel_data = accel;
 }
 
-void Imu::setGyroData(Vector3f gyro)
+void Imu::setGyroData(const Vector3f& gyro)
 {
 	_gyro_data = gyro;
 }
 
+} // namespace sensor
 } // namespace sensor_simulator::sensor
diff --git a/test/sensor_simulator/imu.h b/test/sensor_simulator/imu.h
index bc397250b0..c4f26e29ca 100644
--- a/test/sensor_simulator/imu.h
+++ b/test/sensor_simulator/imu.h
@@ -39,18 +39,20 @@
 
 #include "sensor.h"
 
-namespace sensor_simulator::sensor
+namespace sensor_simulator
+{
+namespace sensor
 {
 
 class Imu: public Sensor
 {
 public:
-	Imu(Ekf* ekf);
+	Imu(std::shared_ptr<Ekf> ekf);
 	~Imu();
 
-	void setData(Vector3f accel, Vector3f gyro);
-	void setAccelData(Vector3f accel);
-	void setGyroData(Vector3f gyro);
+	void setData(const Vector3f& accel, const Vector3f& gyro);
+	void setAccelData(const Vector3f& accel);
+	void setGyroData(const Vector3f& gyro);
 
 private:
 	Vector3f _accel_data;
@@ -60,4 +62,5 @@ private:
 
 };
 
+} // namespace sensor
 } // namespace sensor_simulator::sensor
diff --git a/test/sensor_simulator/mag.cpp b/test/sensor_simulator/mag.cpp
index 83e96e466d..32734bf7a4 100644
--- a/test/sensor_simulator/mag.cpp
+++ b/test/sensor_simulator/mag.cpp
@@ -1,9 +1,11 @@
-#include "Mag.h"
+#include "mag.h"
 
-namespace sensor_simulator::sensor
+namespace sensor_simulator
+{
+namespace sensor
 {
 
-Mag::Mag(Ekf* ekf):Sensor(ekf)
+Mag::Mag(std::shared_ptr<Ekf> ekf):Sensor(ekf)
 {
 }
 
@@ -19,9 +21,10 @@ void Mag::send(uint32_t time)
 	_time_last_data_sent = time;
 }
 
-void Mag::setData(Vector3f mag)
+void Mag::setData(const Vector3f& mag)
 {
 	_mag_data = mag;
 }
 
+} // namespace sensor
 } // namespace sensor_simulator::sensor
diff --git a/test/sensor_simulator/mag.h b/test/sensor_simulator/mag.h
index 137d5abb41..016b65c565 100644
--- a/test/sensor_simulator/mag.h
+++ b/test/sensor_simulator/mag.h
@@ -39,16 +39,18 @@
 
 #include "sensor.h"
 
-namespace sensor_simulator::sensor
+namespace sensor_simulator
+{
+namespace sensor
 {
 
 class Mag: public Sensor
 {
 public:
-	Mag(Ekf* ekf);
+	Mag(std::shared_ptr<Ekf> ekf);
 	~Mag();
 
-	void setData(Vector3f mag);
+	void setData(const Vector3f& mag);
 
 private:
 	Vector3f _mag_data;
@@ -57,4 +59,5 @@ private:
 
 };
 
+} // namespace sensor
 } // namespace sensor_simulator::sensor
diff --git a/test/sensor_simulator/sensor.cpp b/test/sensor_simulator/sensor.cpp
index 74bbf42e5f..80b8e281bc 100644
--- a/test/sensor_simulator/sensor.cpp
+++ b/test/sensor_simulator/sensor.cpp
@@ -3,9 +3,8 @@
 namespace sensor_simulator
 {
 
-Sensor::Sensor(Ekf* ekf)
+Sensor::Sensor(std::shared_ptr<Ekf> ekf): _ekf{ekf}
 {
-	_ekf = ekf;
 }
 
 Sensor::~Sensor()
@@ -20,12 +19,12 @@ void Sensor::update(uint32_t time)
 	}
 }
 
-bool Sensor::should_send(uint32_t time)
+bool Sensor::should_send(uint32_t time) const
 {
 	return _is_running && is_time_to_send(time);
 }
 
-bool Sensor::is_time_to_send(uint32_t time)
+bool Sensor::is_time_to_send(uint32_t time) const
 {
 	return (time >= _time_last_data_sent) && ((time - _time_last_data_sent) >= _update_period);
 }
diff --git a/test/sensor_simulator/sensor.h b/test/sensor_simulator/sensor.h
index a5bdad77c8..6690851913 100644
--- a/test/sensor_simulator/sensor.h
+++ b/test/sensor_simulator/sensor.h
@@ -40,6 +40,7 @@
 
 #include "EKF/ekf.h"
 #include <math.h>
+#include <memory>
 
 namespace sensor_simulator
 {
@@ -48,14 +49,14 @@ class Sensor
 {
 public:
 
-	Sensor(Ekf* ekf);
+	Sensor(std::shared_ptr<Ekf> ekf);
 	virtual ~Sensor();
 
 	void update(uint32_t time);
 
 	void setRate(uint32_t rate){ _update_period = uint32_t(1000000)/rate; }
 
-	bool isRunning(){ return _is_running; }
+	bool isRunning() const { return _is_running; }
 
 	void start(){ _is_running = true; }
 
@@ -63,17 +64,17 @@ public:
 
 protected:
 
-	Ekf* _ekf;
+	std::shared_ptr<Ekf> _ekf;
 	// time in microseconds
 	uint32_t _update_period;
 	uint32_t _time_last_data_sent{0};
 
 	bool _is_running{false};
 
-	bool should_send(uint32_t time);
+	bool should_send(uint32_t time) const;
 
 	// Checks that the right amount time passed since last send data to fulfill rate
-	bool is_time_to_send(uint32_t time);
+	bool is_time_to_send(uint32_t time) const;
 
 	// call set*Data function of Ekf
 	virtual void send(uint32_t time) = 0;
diff --git a/test/sensor_simulator/sensor_simulator.cpp b/test/sensor_simulator/sensor_simulator.cpp
index c643485ad8..8e185d4454 100644
--- a/test/sensor_simulator/sensor_simulator.cpp
+++ b/test/sensor_simulator/sensor_simulator.cpp
@@ -1,13 +1,13 @@
-#include "SensorSimulator.h"
+#include "sensor_simulator.h"
 
 
-SensorSimulator::SensorSimulator(Ekf* ekf):
+SensorSimulator::SensorSimulator(std::shared_ptr<Ekf> ekf):
+_ekf{ekf},
 _imu{ekf},
 _mag{ekf},
 _baro{ekf},
 _gps{ekf}
 {
-	_ekf = ekf;
 
 	// set default sensor rate in Hz
 	_imu.setRate(250);
diff --git a/test/sensor_simulator/sensor_simulator.h b/test/sensor_simulator/sensor_simulator.h
index 386543aeed..9f88c8d62b 100644
--- a/test/sensor_simulator/sensor_simulator.h
+++ b/test/sensor_simulator/sensor_simulator.h
@@ -42,17 +42,20 @@
 
 #pragma once
 
+#include <memory>
+
 #include "imu.h"
 #include "mag.h"
 #include "baro.h"
 #include "gps.h"
 #include "EKF/ekf.h"
 
+using namespace sensor_simulator::sensor;
 
 class SensorSimulator
 {
 public:
-	SensorSimulator(Ekf* ekf);
+	SensorSimulator(std::shared_ptr<Ekf> ekf);
 	~SensorSimulator();
 
 	void setImuRate(uint32_t rate){ _imu.setRate(rate); }
@@ -68,7 +71,7 @@ public:
 	void setImuBias(Vector3f accel_bias, Vector3f gyro_bias);
 
 private:
-	Ekf* _ekf;
+	std::shared_ptr<Ekf> _ekf;
 
 	Imu _imu;
 	Mag _mag;
diff --git a/test/test_EKF_basics.cpp b/test/test_EKF_basics.cpp
index 603b60abb5..a52bc3b8aa 100644
--- a/test/test_EKF_basics.cpp
+++ b/test/test_EKF_basics.cpp
@@ -33,14 +33,15 @@
 
 #include <gtest/gtest.h>
 #include <math.h>
+#include <memory>
 #include "EKF/ekf.h"
-#include "SensorSimulator/SensorSimulator.h"
+#include "sensor_simulator/sensor_simulator.h"
 
 
 class EkfInitializationTest : public ::testing::Test {
  public:
 
-	Ekf* _ekf;
+	std::shared_ptr<Ekf> _ekf;
 	SensorSimulator* _sensor_simulator;
 
 	// Duration of initalization with only providing baro,mag and IMU
@@ -49,7 +50,7 @@ class EkfInitializationTest : public ::testing::Test {
 	// Setup the Ekf with synthetic measurements
 	void SetUp() override
 	{
-		_ekf = new Ekf();
+		_ekf = std::make_shared<Ekf>();
 		_ekf->init(0);
 		_sensor_simulator = new SensorSimulator(_ekf);
 
@@ -60,7 +61,6 @@ class EkfInitializationTest : public ::testing::Test {
 	void TearDown() override
 	{
 		delete _sensor_simulator;
-		delete _ekf;
 	}
 };