diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index d3a8a9acfe..e172a2e403 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -43,6 +43,7 @@ set(SRCS test_AlphaFilter.cpp test_EKF_fusionLogic.cpp test_EKF_initialization.cpp + test_EKF_externalVision.cpp ) add_executable(ECL_GTESTS ${SRCS}) diff --git a/test/sensor_simulator/CMakeLists.txt b/test/sensor_simulator/CMakeLists.txt index 9c7dc79f44..88730c87cf 100644 --- a/test/sensor_simulator/CMakeLists.txt +++ b/test/sensor_simulator/CMakeLists.txt @@ -42,6 +42,7 @@ set(SRCS gps.cpp flow.cpp range_finder.cpp + vio.cpp ) add_library(ecl_sensor_sim ${SRCS}) diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp index 8e474bacaa..840dfadb3e 100644 --- a/test/sensor_simulator/ekf_wrapper.cpp +++ b/test/sensor_simulator/ekf_wrapper.cpp @@ -44,6 +44,67 @@ bool EkfWrapper::isIntendingFlowFusion() const return control_status.flags.opt_flow; } +void EkfWrapper::enableExternalVisionPositionFusion() +{ + _ekf_params->fusion_mode |= MASK_USE_EVPOS; +} + +void EkfWrapper::disableExternalVisionPositionFusion() +{ + _ekf_params->fusion_mode &= ~MASK_USE_EVPOS; +} + +bool EkfWrapper::isIntendingExternalVisionPositionFusion() const +{ + filter_control_status_u control_status; + _ekf->get_control_mode(&control_status.value); + return 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 +{ + filter_control_status_u control_status; + _ekf->get_control_mode(&control_status.value); + return 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 +{ + filter_control_status_u control_status; + _ekf->get_control_mode(&control_status.value); + return 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; +} + Vector3f EkfWrapper::getPosition() const { float temp[3]; @@ -101,3 +162,10 @@ Vector3f EkfWrapper::getVelocityVariance() const { return Vector3f(_ekf->velocity_covariances().diag()); } + +Quatf EkfWrapper::getVisionAlignmentQuaternion() const +{ + float temp[4]; + _ekf->get_ev2ekf_quaternion(temp); + return Quatf(temp); +} diff --git a/test/sensor_simulator/ekf_wrapper.h b/test/sensor_simulator/ekf_wrapper.h index 522a1761dd..e6930accbf 100644 --- a/test/sensor_simulator/ekf_wrapper.h +++ b/test/sensor_simulator/ekf_wrapper.h @@ -48,17 +48,28 @@ public: ~EkfWrapper(); void enableGpsFusion(); - void disableGpsFusion(); - bool isIntendingGpsFusion() const; void enableFlowFusion(); - void disableFlowFusion(); - bool isIntendingFlowFusion() const; + void enableExternalVisionPositionFusion(); + void disableExternalVisionPositionFusion(); + bool isIntendingExternalVisionPositionFusion() const; + + void enableExternalVisionVelocityFusion(); + void disableExternalVisionVelocityFusion(); + bool isIntendingExternalVisionVelocityFusion() const; + + void enableExternalVisionHeadingFusion(); + void disableExternalVisionHeadingFusion(); + bool isIntendingExternalVisionHeadingFusion() const; + + void enableExternalVisionAlignment(); + void disableExternalVisionAlignment(); + Vector3f getPosition() const; Vector3f getVelocity() const; Vector3f getAccelBias() const; @@ -70,6 +81,8 @@ public: Vector3f getPositionVariance() const; Vector3f getVelocityVariance() const; + Quatf getVisionAlignmentQuaternion() const; + private: std::shared_ptr _ekf; diff --git a/test/sensor_simulator/sensor_simulator.cpp b/test/sensor_simulator/sensor_simulator.cpp index 77ad62a3de..6b5ff1b906 100644 --- a/test/sensor_simulator/sensor_simulator.cpp +++ b/test/sensor_simulator/sensor_simulator.cpp @@ -8,7 +8,8 @@ _mag(ekf), _baro(ekf), _gps(ekf), _flow(ekf), -_rng(ekf) +_rng(ekf), +_vio(ekf) { setSensorDataToDefault(); setSensorRateToDefault(); @@ -28,6 +29,7 @@ void SensorSimulator::setSensorDataToDefault() _gps.setRateHz(5); _flow.setRateHz(50); _rng.setRateHz(30); + _vio.setRateHz(30); } void SensorSimulator::setSensorRateToDefault() { @@ -38,6 +40,7 @@ void SensorSimulator::setSensorRateToDefault() _gps.setData(_gps.getDefaultGpsData()); _flow.setData(_flow.dataAtRest()); _rng.setData(0.2f, 100); + _vio.setData(_vio.dataAtRest()); } void SensorSimulator::startBasicSensor() { @@ -64,6 +67,7 @@ void SensorSimulator::runMicroseconds(uint32_t duration) _gps.update(_time); _flow.update(_time); _rng.update(_time); + _vio.update(_time); _ekf->update(); } diff --git a/test/sensor_simulator/sensor_simulator.h b/test/sensor_simulator/sensor_simulator.h index 0242e9c3ea..51169eb6c9 100644 --- a/test/sensor_simulator/sensor_simulator.h +++ b/test/sensor_simulator/sensor_simulator.h @@ -50,6 +50,7 @@ #include "gps.h" #include "flow.h" #include "range_finder.h" +#include "vio.h" #include "EKF/ekf.h" using namespace sensor_simulator::sensor; @@ -82,6 +83,9 @@ public: void startRangeFinder(){ _rng.start(); } void stopRangeFinder(){ _rng.stop(); } + void startExternalVision(){ _vio.start(); } + void stopExternalVision(){ _vio.stop(); } + void setImuBias(Vector3f accel_bias, Vector3f gyro_bias); void simulateOrientation(Quatf orientation); @@ -91,5 +95,5 @@ public: Gps _gps; Flow _flow; RangeFinder _rng; - + Vio _vio; }; diff --git a/test/sensor_simulator/vio.cpp b/test/sensor_simulator/vio.cpp new file mode 100644 index 0000000000..856047e207 --- /dev/null +++ b/test/sensor_simulator/vio.cpp @@ -0,0 +1,69 @@ +#include "vio.h" + +namespace sensor_simulator +{ +namespace sensor +{ + +Vio::Vio(std::shared_ptr ekf):Sensor(ekf) +{ +} + +Vio::~Vio() +{ +} + +void Vio::send(uint32_t time) +{ + _ekf->setExtVisionData(time, &_vio_data); +} + +void Vio::setData(const ext_vision_message& vio_data) +{ + _vio_data = vio_data; +} + +void Vio::setVelocityVariance(const Vector3f& velVar) +{ + _vio_data.velVar = velVar; +} + +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; +} + +ext_vision_message Vio::dataAtRest() +{ + ext_vision_message 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.velVar = Vector3f{0.1f, 0.1f, 0.1f}; + vio_data.angVar = 0.05f; + return vio_data; +} + +} // namespace sensor +} // namespace sensor_simulator diff --git a/test/sensor_simulator/vio.h b/test/sensor_simulator/vio.h new file mode 100644 index 0000000000..a53f0238bb --- /dev/null +++ b/test/sensor_simulator/vio.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 + */ +#pragma once + +#include "sensor.h" + +namespace sensor_simulator +{ +namespace sensor +{ + +class Vio: public Sensor +{ +public: + Vio(std::shared_ptr ekf); + ~Vio(); + + void setData(const ext_vision_message& vio_data); + void setVelocityVariance(const Vector3f& velVar); + void setPositionVariance(const Vector3f& posVar); + void setAngularVariance(float angVar); + void setVelocity(const Vector3f& vel); + void setPosition(const Vector3f& pos); + void setOrientation(const Quatf& quat); + + ext_vision_message dataAtRest(); + +private: + ext_vision_message _vio_data; + + void send(uint32_t time) override; + +}; + +} // namespace sensor +} // namespace sensor_simulator diff --git a/test/test_EKF_externalVision.cpp b/test/test_EKF_externalVision.cpp new file mode 100644 index 0000000000..de2ebf6b0f --- /dev/null +++ b/test/test_EKF_externalVision.cpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Test the external vision functionality + * @author Kamil Ritz + */ + +#include +#include "EKF/ekf.h" +#include "sensor_simulator/sensor_simulator.h" +#include "sensor_simulator/ekf_wrapper.h" + + +class EkfExternalVisionTest : public ::testing::Test { + public: + + EkfExternalVisionTest(): ::testing::Test(), + _ekf{std::make_shared()}, + _sensor_simulator(_ekf), + _ekf_wrapper(_ekf) {}; + + std::shared_ptr _ekf; + SensorSimulator _sensor_simulator; + EkfWrapper _ekf_wrapper; + + // Setup the Ekf with synthetic measurements + void SetUp() override + { + _ekf->init(0); + _sensor_simulator.runSeconds(2); + } + + // Use this method to clean up any memory, network etc. after each test + void TearDown() override + { + } +}; + +TEST_F(EkfExternalVisionTest, checkVisionFusionLogic) +{ + _ekf_wrapper.enableExternalVisionPositionFusion(); + _sensor_simulator.startExternalVision(); + _sensor_simulator.runSeconds(2); + + EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); + + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + _ekf_wrapper.enableExternalVisionVelocityFusion(); + _sensor_simulator.runSeconds(2); + + EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); + + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + _ekf_wrapper.enableExternalVisionHeadingFusion(); + _sensor_simulator.runSeconds(2); + + EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); + + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); +} + +TEST_F(EkfExternalVisionTest, visionVarianceCheck) +{ + const Vector3f velVar_init = _ekf_wrapper.getVelocityVariance(); + EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001); + + _sensor_simulator._vio.setVelocityVariance(Vector3f{2.0f,0.01f,0.01f}); + _ekf_wrapper.enableExternalVisionVelocityFusion(); + _sensor_simulator.startExternalVision(); + _sensor_simulator.runSeconds(4); + + const Vector3f velVar_new = _ekf_wrapper.getVelocityVariance(); + EXPECT_TRUE(velVar_new(0) > velVar_new(1)); +} + +TEST_F(EkfExternalVisionTest, visionAlignment) +{ + // GIVEN: Drone is pointing north, and we use mag (ROTATE_EV) + // Heading of drone in EKF frame is 0° + + // WHEN: Vision frame is rotate +90°. The reported heading is -90° + Quatf externalVisionFrameOffset(Eulerf(0.0f,0.0f,math::radians(90.0f))); + _sensor_simulator._vio.setOrientation(externalVisionFrameOffset.inversed()); + _ekf_wrapper.enableExternalVisionAlignment(); + + // Simulate high uncertainty on vision x axis which is in this case + // the y EKF frame axis + _sensor_simulator._vio.setVelocityVariance(Vector3f{2.0f,0.01f,0.01f}); + _ekf_wrapper.enableExternalVisionVelocityFusion(); + _sensor_simulator.startExternalVision(); + + const Vector3f velVar_init = _ekf_wrapper.getVelocityVariance(); + EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001); + + _sensor_simulator.runSeconds(4); + + // THEN: velocity uncertainty in y should be bigger + const Vector3f velVar_new = _ekf_wrapper.getVelocityVariance(); + EXPECT_TRUE(velVar_new(1) > velVar_new(0)); + + // THEN: the frame offset should be estimated correctly + Quatf estimatedExternalVisionFrameOffset = _ekf_wrapper.getVisionAlignmentQuaternion(); + EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(), + estimatedExternalVisionFrameOffset.canonical())); +} diff --git a/test/test_EKF_fusionLogic.cpp b/test/test_EKF_fusionLogic.cpp index 567277e27b..d3a5cd1bd9 100644 --- a/test/test_EKF_fusionLogic.cpp +++ b/test/test_EKF_fusionLogic.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * Feeds Ekf with Imu data + * Test the fusion start and stop logic * @author Kamil Ritz */ @@ -58,7 +58,7 @@ class EkfFusionLogicTest : public ::testing::Test { void SetUp() override { _ekf->init(0); - _sensor_simulator.runSeconds(5); + _sensor_simulator.runSeconds(2); } // Use this method to clean up any memory, network etc. after each test @@ -74,7 +74,7 @@ TEST_F(EkfFusionLogicTest, doGpsFusion) // WHEN: we enable GPS fusion and we send good quality gps data for 11s _ekf_wrapper.enableGpsFusion(); _sensor_simulator.startGps(); - _sensor_simulator.runSeconds(15); + _sensor_simulator.runSeconds(11); // THEN: EKF should intend to fuse GPS EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); @@ -151,7 +151,7 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) // WHEN: sending flow data without having the flow fusion enabled // flow measurement fusion should not be intended. _sensor_simulator.startFlow(); - _sensor_simulator.runSeconds(3); + _sensor_simulator.runSeconds(4); // THEN: EKF should intend to fuse flow measurements EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion());