Add orientation initialization tests

test/sensor_simulator/ekf_wrapper.cpp

@@ -69,3 +69,35 @@ Vector3f EkfWrapper::getGyroBias() const
 	_ekf->get_gyro_bias(temp);
 	return Vector3f(temp);
 }
+
+Quatf EkfWrapper::getQuaternion() const
+{
+	return _ekf->get_quaternion();
+}
+
+Eulerf EkfWrapper::getEulerAngles() const
+{
+	return Eulerf(getQuaternion());
+}
+
+matrix::Vector<float, 24> EkfWrapper::getState() const
+{
+	float state[24];
+	_ekf->get_state_delayed(state);
+	return matrix::Vector<float, 24>{state};
+}
+
+matrix::Vector<float, 4> EkfWrapper::getQuaternionVariance() const
+{
+	return matrix::Vector<float, 4>(_ekf->orientation_covariances().diag());
+}
+
+Vector3f EkfWrapper::getPositionVariance() const
+{
+	return Vector3f(_ekf->position_covariances().diag());
+}
+
+Vector3f EkfWrapper::getVelocityVariance() const
+{
+	return Vector3f(_ekf->velocity_covariances().diag());
+}

test/sensor_simulator/ekf_wrapper.h

@@ -63,7 +63,12 @@ public:
 	Vector3f getVelocity() const;
 	Vector3f getAccelBias() const;
 	Vector3f getGyroBias() const;
-
+	Quatf getQuaternion() const;
+	Eulerf getEulerAngles() const;
+	matrix::Vector<float, 24> getState() const;
+	matrix::Vector<float, 4> getQuaternionVariance() const;
+	Vector3f getPositionVariance() const;
+	Vector3f getVelocityVariance() const;
 
 private:
 	std::shared_ptr<Ekf> _ekf;

test/sensor_simulator/sensor_simulator.cpp

@@ -74,3 +74,15 @@ 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);
+}

test/sensor_simulator/sensor_simulator.h

@@ -83,6 +83,7 @@ public:
 	void stopRangeFinder(){ _rng.stop(); }
 
 	void setImuBias(Vector3f accel_bias, Vector3f gyro_bias);
+	void simulateOrientation(Quatf orientation);
 
 	Imu _imu;
 	Mag _mag;