dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Remove swig and python test related things

This commit is contained in:
kamilritz 2019-10-17 17:28:10 +02:00 committed by Mathieu Bresciani
parent d88e242a60
commit d79199c863
25 changed files with 7 additions and 4837 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -2,6 +2,4 @@
*.gcov
*~
.cache/
.pytest_cache/
build/
EKF/tests/pytest/__pycache__/

12
CMakeLists.txt
View File

@ -92,7 +92,7 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
-pedantic
-Wall
#-Wextra # This was causing an issue in the swig stuff
-Wextra
-Werror
-Wno-missing-field-initializers # ignore for GCC 4.8 support
@ -110,15 +110,6 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
DEPENDS ecl_EKF
USES_TERMINAL
)
option(EKF_PYTHON_TESTS "Build the EKF python tests" OFF)
if (EKF_PYTHON_TESTS)
# swig requires -fPIC
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
endif()
endif()
# fetch latest matrix from github
@ -165,6 +156,7 @@ add_subdirectory(geo)
add_subdirectory(geo_lookup)
add_subdirectory(l1)
add_subdirectory(tecs)
if(BUILD_TESTING)
add_subdirectory(validation)
add_subdirectory(test)

8
EKF/CMakeLists.txt
View File

@ -57,11 +57,3 @@ target_link_libraries(ecl_EKF PRIVATE ecl_geo ecl_geo_lookup mathlib)
set_target_properties(ecl_EKF PROPERTIES PUBLIC_HEADER "ekf.h")
target_compile_options(ecl_EKF PRIVATE -fno-associative-math)
if(EKF_PYTHON_TESTS)
add_subdirectory(swig)
endif()
if(BUILD_TESTING)
add_subdirectory(tests)
endif()

42
EKF/swig/CMakeLists.txt
View File

@ -1,42 +0,0 @@
############################################################################
#
# Copyright (c) 2015-2018 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.
#
############################################################################
message(STATUS "adding EKF python tests")
include_directories(${ECL_SOURCE_DIR})
# Need SWIG to wrap ecl
find_package(SWIG REQUIRED)
include(${SWIG_USE_FILE})
add_subdirectory(python)

265
EKF/swig/ecl_EKF.i
View File

@ -1,265 +0,0 @@
// SWIG Wrapper for the ecl's EKF
%module(directors="1") ecl_EKF
%feature("autodoc", "3");
%include "inttypes.i"
%include "std_vector.i"
%include "std_string.i"
%include "typemaps.i"
// Include headers in the SWIG generated C++ file
%{
#define SWIG_FILE_WITH_INIT
#include <iostream>
#include <EKF/ekf.h>
#include <geo/geo.h>
%}
%include "numpy.i"
%init %{
import_array();
%}
%apply (float ARGOUT_ARRAY1[ANY]) {(float out[3])};
%apply (float ARGOUT_ARRAY1[ANY]) {(float bias[3])};
%apply (float ARGOUT_ARRAY1[ANY]) {(float out[24])};
%apply (float IN_ARRAY1[ANY]) {(float delta_ang[3]), (float delta_vel[3])};
%apply (float IN_ARRAY1[ANY]) {(float mag_data[3])};
%inline {
struct ekf_control_mode_flags_t {
bool tilt_align; // 0 - true if the filter tilt alignment is complete
bool yaw_align; // 1 - true if the filter yaw alignment is complete
bool gps; // 2 - true if GPS measurements are being fused
bool opt_flow; // 3 - true if optical flow measurements are being fused
bool mag_hdg; // 4 - true if a simple magnetic yaw heading is being fused
bool mag_3D; // 5 - true if 3-axis magnetometer measurement are being fused
bool mag_dec; // 6 - true if synthetic magnetic declination measurements are being fused
bool in_air; // 7 - true when the vehicle is airborne
bool wind; // 8 - true when wind velocity is being estimated
bool baro_hgt; // 9 - true when baro height is being fused as a primary height reference
bool rng_hgt; // 10 - true when range finder height is being fused as a primary height reference
bool gps_hgt; // 11 - true when GPS height is being fused as a primary height reference
bool ev_pos; // 12 - true when local position data from external vision is being fused
bool ev_yaw; // 13 - true when yaw data from external vision measurements is being fused
bool ev_hgt; // 14 - true when height data from external vision measurements is being fused
bool fuse_beta; // 15 - true when synthetic sideslip measurements are being fused
bool update_mag_states_only; // 16 - true when only the magnetometer states are updated by the magnetometer
bool fixed_wing; // 17 - true when the vehicle is operating as a fixed wing vehicle
std::string __repr__() {
std::stringstream ss;
ss << "[tilt_align: " << tilt_align << "\n";
ss << " yaw_align: " << yaw_align << "\n";
ss << " gps: " << gps << "\n";
ss << " opt_flow: " << opt_flow << "\n";
ss << " mag_hdg: " << mag_hdg << "\n";
ss << " mag_3D: " << mag_3D << "\n";
ss << " mag_dec: " << mag_dec << "\n";
ss << " in_air: " << in_air << "\n";
ss << " wind: " << wind << "\n";
ss << " baro_hgt: " << baro_hgt << "\n";
ss << " rng_hgt: " << rng_hgt << "\n";
ss << " gps_hgt: " << gps_hgt << "\n";
ss << " ev_pos: " << ev_pos << "\n";
ss << " ev_yaw: " << ev_yaw << "\n";
ss << " ev_hgt: " << ev_hgt << "\n";
ss << " fuse_beta: " << fuse_beta << "\n";
ss << " update_mag_states_only: " << update_mag_states_only << "\n";
ss << " fixed_wing: " << fixed_wing << "]\n";
return std::string(ss.str());
}
};
struct ekf_fault_status_flags_t {
bool bad_mag_x; // 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error
bool bad_mag_y; // 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error
bool bad_mag_z; // 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error
bool bad_hdg; // 3 - true if the fusion of the heading angle has encountered a numerical error
bool bad_mag_decl; // 4 - true if the fusion of the magnetic declination has encountered a numerical error
bool bad_airspeed; // 5 - true if fusion of the airspeed has encountered a numerical error
bool bad_sideslip; // 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
bool bad_optflow_X; // 7 - true if fusion of the optical flow X axis has encountered a numerical error
bool bad_optflow_Y; // 8 - true if fusion of the optical flow Y axis has encountered a numerical error
bool bad_vel_N; // 9 - true if fusion of the North velocity has encountered a numerical error
bool bad_vel_E; // 10 - true if fusion of the East velocity has encountered a numerical error
bool bad_vel_D; // 11 - true if fusion of the Down velocity has encountered a numerical error
bool bad_pos_N; // 12 - true if fusion of the North position has encountered a numerical error
bool bad_pos_E; // 13 - true if fusion of the East position has encountered a numerical error
bool bad_pos_D; // 14 - true if fusion of the Down position has encountered a numerical error
bool bad_acc_bias; // 15 - true if bad delta velocity bias estimates have been detected
std::string __repr__() {
std::stringstream ss;
ss << "[bad_mag_x: " << bad_mag_x << "\n";
ss << " bad_mag_y: " << bad_mag_y << "\n";
ss << " bad_mag_z: " << bad_mag_z << "\n";
ss << " bad_hdg: " << bad_hdg << "\n";
ss << " bad_mag_decl: " << bad_mag_decl << "\n";
ss << " bad_airspeed: " << bad_airspeed << "\n";
ss << " bad_sideslip: " << bad_sideslip << "\n";
ss << " bad_optflow_X: " << bad_optflow_X << "\n";
ss << " bad_optflow_Y: " << bad_optflow_Y << "\n";
ss << " bad_vel_N: " << bad_vel_N << "\n";
ss << " bad_vel_E: " << bad_vel_E << "\n";
ss << " bad_vel_D: " << bad_vel_D << "\n";
ss << " bad_pos_N: " << bad_pos_N << "\n";
ss << " bad_pos_E: " << bad_pos_E << "\n";
ss << " bad_pos_D: " << bad_pos_D << "\n";
ss << " bad_acc_bias: " << bad_acc_bias << "]\n";
return std::string(ss.str());
}
};
struct ekf_imu_sample_t {
float delta_ang_x; // delta angle in body frame (integrated gyro measurements)
float delta_ang_y; // delta angle in body frame (integrated gyro measurements)
float delta_ang_z; // delta angle in body frame (integrated gyro measurements)
float delta_vel_x; // delta velocity in body frame (integrated accelerometer measurements)
float delta_vel_y; // delta velocity in body frame (integrated accelerometer measurements)
float delta_vel_z; // delta velocity in body frame (integrated accelerometer measurements)
float delta_ang_dt; // delta angle integration period in seconds
float delta_vel_dt; // delta velocity integration period in seconds
uint64_t time_us; // timestamp in microseconds
std::string __repr__() {
std::stringstream ss;
ss << "[delta_ang_x: " << delta_ang_x << "\n";
ss << " delta_ang_y: " << delta_ang_y << "\n";
ss << " delta_ang_z: " << delta_ang_z << "\n";
ss << " delta_vel_x: " << delta_vel_x << "\n";
ss << " delta_vel_y: " << delta_vel_y << "\n";
ss << " delta_vel_z: " << delta_vel_z << "\n";
ss << " delta_ang_dt: " << delta_ang_dt << "\n";
ss << " delta_vel_dt: " << delta_vel_dt << "\n";
ss << " time_us: " << time_us << "]\n";
return std::string(ss.str());
}
};
static float last_mag_data[3];
static float last_imu_delta_ang[3];
static float last_imu_delta_vel[3];
const float one_g = CONSTANTS_ONE_G;
}
// Tell swig to wrap ecl classes
%include <matrix/Vector3.hpp>
%include <matrix/Vector2.hpp>
%include <matrix/Quaternion.hpp>
%include <matrix/Dcm.hpp>
%include <matrix/Euler.hpp>
%include <matrix/helper_functions.hpp>
%include <EKF/common.h>
%include <EKF/estimator_interface.h>
%include <EKF/ekf.h>
%extend Ekf {
void set_imu_data(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float delta_ang[3], float delta_vel[3]) {
for (int i = 0; i < 3; ++i) {
last_imu_delta_ang[i] = delta_ang[i];
last_imu_delta_vel[i] = delta_vel[i];
}
self->setIMUData(time_usec, delta_ang_dt, delta_vel_dt, last_imu_delta_ang, last_imu_delta_vel);
}
void set_mag_data(uint64_t time_usec, float mag_data[3]) {
for (int i = 0; i < 3; ++i) {
last_mag_data[i] = mag_data[i];
}
self->setMagData(time_usec, last_mag_data);
}
void set_baro_data(uint64_t time_usec, float baro_data) {
self->setBaroData(time_usec, baro_data);
}
%rename (get_control_mode) get_control_mode_;
ekf_control_mode_flags_t get_control_mode_() {
filter_control_status_u result_union;
self->get_control_mode(&result_union.value);
ekf_control_mode_flags_t result;
result.tilt_align = result_union.flags.tilt_align; // 0 - true if the filter tilt alignment is complete
result.yaw_align = result_union.flags.yaw_align; // 1 - true if the filter yaw alignment is complete
result.gps = result_union.flags.gps; // 2 - true if GPS measurements are being fused
result.opt_flow = result_union.flags.opt_flow; // 3 - true if optical flow measurements are being fused
result.mag_hdg = result_union.flags.mag_hdg; // 4 - true if a simple magnetic yaw heading is being fused
result.mag_3D = result_union.flags.mag_3D; // 5 - true if 3-axis magnetometer measurement are being fused
result.mag_dec = result_union.flags.mag_dec; // 6 - true if synthetic magnetic declination measurements are being fused
result.in_air = result_union.flags.in_air; // 7 - true when the vehicle is airborne
result.wind = result_union.flags.wind; // 8 - true when wind velocity is being estimated
result.baro_hgt = result_union.flags.baro_hgt; // 9 - true when baro height is being fused as a primary height reference
result.rng_hgt = result_union.flags.rng_hgt; // 10 - true when range finder height is being fused as a primary height reference
result.gps_hgt = result_union.flags.gps_hgt; // 11 - true when GPS height is being fused as a primary height reference
result.ev_pos = result_union.flags.ev_pos; // 12 - true when local position data from external vision is being fused
result.ev_yaw = result_union.flags.ev_yaw; // 13 - true when yaw data from external vision measurements is being fused
result.ev_hgt = result_union.flags.ev_hgt; // 14 - true when height data from external vision measurements is being fused
result.fuse_beta = result_union.flags.fuse_beta; // 15 - true when synthetic sideslip measurements are being fused
result.update_mag_states_only = result_union.flags.update_mag_states_only; // 16 - true when only the magnetometer states are updated by the magnetometer
result.fixed_wing = result_union.flags.fixed_wing; // 17 - true when the vehicle is operating as a fixed wing vehicle
return result;
}
%rename (get_filter_fault_status) get_filter_fault_status_;
ekf_fault_status_flags_t get_filter_fault_status_() {
fault_status_u result_union;
self->get_filter_fault_status(&result_union.value);
ekf_fault_status_flags_t result;
result.bad_mag_x = result_union.flags.bad_mag_x;
result.bad_mag_y = result_union.flags.bad_mag_y;
result.bad_mag_z = result_union.flags.bad_mag_z;
result.bad_hdg = result_union.flags.bad_hdg;
result.bad_mag_decl = result_union.flags.bad_mag_decl;
result.bad_airspeed = result_union.flags.bad_airspeed;
result.bad_sideslip = result_union.flags.bad_sideslip;
result.bad_optflow_X = result_union.flags.bad_optflow_X;
result.bad_optflow_Y = result_union.flags.bad_optflow_Y;
result.bad_vel_N = result_union.flags.bad_vel_N;
result.bad_vel_E = result_union.flags.bad_vel_E;
result.bad_vel_D = result_union.flags.bad_vel_D;
result.bad_pos_N = result_union.flags.bad_pos_N;
result.bad_pos_E = result_union.flags.bad_pos_E;
result.bad_pos_D = result_union.flags.bad_pos_D;
result.bad_acc_bias = result_union.flags.bad_acc_bias;
return result;
}
%rename (get_imu_sample_delayed) get_imu_sample_delayed_;
ekf_imu_sample_t get_imu_sample_delayed_() {
imuSample result_sample = self->get_imu_sample_delayed();
ekf_imu_sample_t result;
result.delta_ang_x = result_sample.delta_ang(0);
result.delta_ang_y = result_sample.delta_ang(1);
result.delta_ang_z = result_sample.delta_ang(2);
result.delta_vel_x = result_sample.delta_vel(0);
result.delta_vel_y = result_sample.delta_vel(1);
result.delta_vel_z = result_sample.delta_vel(2);
result.delta_ang_dt = result_sample.delta_ang_dt;
result.delta_vel_dt = result_sample.delta_vel_dt;
result.time_us = result_sample.time_us;
return result;
}
%rename (get_position) get_position_;
void get_position_(float out[3]) {
return self->get_position(out);
};
%rename (get_velocity) get_velocity_;
void get_velocity_(float out[3]) {
return self->get_velocity(out);
};
%rename (get_state_delayed) get_state_delayed_;
void get_state_delayed_(float out[24]) {
return self->get_state_delayed(out);
}
void get_quaternion(float out[4]) {
self->get_quaternion().copyTo(out);
}
}
// Let SWIG instantiate vector templates
%template(vector_int) std::vector<int>;
%template(vector_double) std::vector<double>;
%template(vector_float) std::vector<float>;

3166
EKF/swig/numpy.i
View File

File diff suppressed because it is too large Load Diff

43
EKF/swig/python/CMakeLists.txt
View File

@ -1,43 +0,0 @@
# Use Python 3.5
set(Python_ADDITIONAL_VERSIONS 3.5)
find_package(PythonLibs 3 REQUIRED)
find_package(PythonInterp 3 REQUIRED)
include_directories(${PYTHON_INCLUDE_PATH})
# Find numpy include
execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "import numpy; print(numpy.get_include())" OUTPUT_VARIABLE NUMPY_INCLUDE_DIRS ERROR_QUIET)
include_directories(${NUMPY_INCLUDE_DIRS})
set(CMAKE_SWIG_FLAGS "")
set_source_files_properties(../ecl_EKF.i PROPERTIES CPLUSPLUS ON)
include_directories(../..)
# Add swig module
swig_add_library(ecl_EKF
LANGUAGE python
SOURCES ../ecl_EKF.i
)
swig_link_libraries(ecl_EKF ecl_EKF ${PYTHON_LIBRARIES})
# Files to install with Python
set(PYTHON_INSTALL_FILES
${CMAKE_CURRENT_BINARY_DIR}/ecl_EKF.py
${CMAKE_CURRENT_BINARY_DIR}/_ecl_EKF.so
)
# Configure setup.py and copy to output directory
set(SETUP_PY_IN ${CMAKE_CURRENT_SOURCE_DIR}/setup.py.in)
set(SETUP_PY_OUT ${CMAKE_CURRENT_BINARY_DIR}/setup.py)
configure_file(${SETUP_PY_IN} ${SETUP_PY_OUT})
# Declare install target for python
#install(TARGETS swig_example
# COMMAND "${PYTHON_EXECUTABLE} setup.py"
# COMPONENT swig-python)
# Install target to call setup.py
add_custom_target(install-python
DEPENDS _ecl_EKF
COMMAND python ${SETUP_PY_OUT} install
)

35
EKF/swig/python/setup.py.in
View File

@ -1,35 +0,0 @@
import setuptools.command.install
import shutil
from distutils.sysconfig import get_python_lib
class CompiledLibInstall(setuptools.command.install.install):
"""
Specialized install to install to python libs
"""
def run(self):
"""
Run method called by setup
:return:
"""
# Get filenames from CMake variable
filenames = '${PYTHON_INSTALL_FILES}'.split(';')
# Directory to install to
install_dir = get_python_lib()
# Install files
[shutil.copy(filename, install_dir) for filename in filenames]
if __name__ == '__main__':
setuptools.setup(
name='ecl_EKF',
version='1.0.0-dev',
packages=['ecl_EKF'],
license='BSD 3.0',
author='PX4',
author_email='px4@px4.io',
cmdclass={'install': CompiledLibInstall}
)

3
EKF/swig/python/test.py
View File

@ -1,3 +0,0 @@
import swig_example
swig_example.swig_example_hello()
swig_example.link_liba_hello()

44
EKF/tests/CMakeLists.txt
View File

@ -1,44 +0,0 @@
############################################################################
#
# Copyright (c) 2015-2018 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.
#
############################################################################
if(BUILD_TESTING AND ECL_STANDALONE)
add_definitions(-UNDEBUG) # keep assert
add_subdirectory(base)
if(EKF_PYTHON_TESTS)
add_subdirectory(pytest)
endif()
endif()

39
EKF/tests/base/CMakeLists.txt
View File

@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2015-2018 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.
#
############################################################################
add_executable(ecl_EKF_tests_base base.cpp)
target_link_libraries(ecl_EKF_tests_base ecl_EKF)
add_test(NAME ecl_EKF_tests_base
COMMAND ecl_EKF_tests_base
)

152
EKF/tests/base/base.cpp
View File

@ -1,152 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 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.
*
****************************************************************************/
/**
* @file base.cpp
*
* Tests for the estimator base class
*/
#include <EKF/ekf.h>
#include <cstdio>
#include <random>
int main(int argc, char *argv[])
{
(void)argc; // unused
(void)argv; // unused
Ekf *base = new Ekf();
// Test1: feed in fake imu data and check if delta angles are summed correctly
float delta_vel[3] = { 0.002f, 0.002f, 0.002f};
float delta_ang[3] = { -0.1f, 0.2f, 0.3f};
uint32_t time_usec = 1000;
// simulate 400 Hz imu rate, filter should downsample to 100Hz
// feed in 2 seconds of data
for (int i = 0; i < 800; i++) {
base->setIMUData(time_usec, 2500, 2500, delta_ang, delta_vel);
time_usec += 2500;
}
//base->printStoredIMU();
// Test2: feed fake imu data and check average imu delta t
// simulate 400 Hz imu rate, filter should downsample to 100Hz
// feed in 2 seconds of data
for (int i = 0; i < 800; i++) {
base->setIMUData(time_usec, 2500, 2500, delta_ang, delta_vel);
//base->print_imu_avg_time();
time_usec += 2500;
}
// Test3: feed in slow imu data, filter should now take every sample
for (int i = 0; i < 800; i++) {
base->setIMUData(time_usec, 2500, 2500, delta_ang, delta_vel);
time_usec += 30000;
}
//base->printStoredIMU();
// Test4: Feed in mag data at 50 Hz (too fast), check if filter samples correctly
float mag[3] = {0.2f, 0.0f, 0.4f};
for (int i = 0; i < 100; i++) {
base->setMagData(time_usec, mag);
time_usec += 20000;
}
//base->printStoredMag();
// Test5: Feed in baro data at 50 Hz (too fast), check if filter samples correctly
float baro = 120.22f;
time_usec = 100000;
for (int i = 0; i < 100; i++) {
base->setBaroData(time_usec, baro);
baro += 10.0f;
time_usec += 20000;
}
//base->printStoredBaro();
// Test 5: Run everything rogether in one run
std::default_random_engine generator;
std::uniform_int_distribution<int> distribution(-200, 200);
int imu_sample_period = 2500;
uint64_t timer = 2000; // simulation start time
uint64_t timer_last = timer;
float airspeed = 0.0f;
struct gps_message gps = {};
gps.lat = 40 * 1e7; // Latitude in 1E-7 degrees
gps.lon = 5 * 1e7; // Longitude in 1E-7 degrees
gps.alt = 200 * 1e3; // Altitude in 1E-3 meters (millimeters) above MSL
gps.fix_type = 4; // 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time
gps.eph = 5.0f; // GPS HDOP horizontal dilution of position in m
gps.epv = 5.0f; // GPS VDOP horizontal dilution of position in m
gps.vel_ned_valid = true; // GPS ground speed is valid
// simulate two seconds
for (int i = 0; i < 800; i++) {
timer += (imu_sample_period + distribution(generator));
if ((timer - timer_last) > 70000) {
base->setAirspeedData(timer, airspeed, 1.0f);
}
gps.time_usec = timer;
base->setIMUData(timer, timer - timer_last, timer - timer_last, delta_ang, delta_vel);
base->setMagData(timer, mag);
base->setBaroData(timer, baro);
base->setGpsData(timer, gps);
//base->print_imu_avg_time();
timer_last = timer;
}
//base->printStoredIMU();
//base->printStoredBaro();
//base->printStoredMag();
//base->printStoredGps();
delete base;
return 0;
}

79
EKF/tests/pytest/CMakeLists.txt
View File

@ -1,79 +0,0 @@
############################################################################
#
# Copyright (c) 2015-2018 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.
#
############################################################################
if(EKF_PYTHON_TESTS)
# pip3 install -r requirements.txt
add_custom_target(ecl_EKF_pytest
COMMAND ${CMAKE_COMMAND} -E env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/ekf_test.py --verbose --plots
DEPENDS ecl_EKF _ecl_EKF
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
USES_TERMINAL
)
add_custom_target(ecl_EKF_pytest-quick
COMMAND ${CMAKE_COMMAND} -E env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/ekf_test.py --quick --verbose --plots
DEPENDS ecl_EKF _ecl_EKF
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
USES_TERMINAL
)
add_custom_target(ecl_EKF_pytest-benchmark
COMMAND ${CMAKE_COMMAND} -E env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/ekf_test.py --benchmark
DEPENDS ecl_EKF _ecl_EKF
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
USES_TERMINAL
)
# requires python3-tk
add_custom_target(ecl_EKF_pytest-plots
COMMAND ${CMAKE_COMMAND} -E env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/ekf_test.py --plots
DEPENDS ecl_EKF _ecl_EKF
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
USES_TERMINAL
)
add_custom_target(ecl_EKF_pytest-lint
COMMAND ${CMAKE_COMMAND} -E env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/lint.py
DEPENDS ecl_EKF _ecl_EKF
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
USES_TERMINAL
)
# ctest entry
add_test(NAME ecl_EKF_pytest_run
COMMAND ${CMAKE_COMMAND} --build ${CMAKE_BINARY_DIR} --target ecl_EKF_pytest-quick
)
endif()

137
EKF/tests/pytest/ekf_test.py
View File

@ -1,137 +0,0 @@
#!/usr/bin/env python3
###############################################################################
#
# Copyright (c) 2017 Estimation and Control Library (ECL). 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.
#
###############################################################################
"""Tests for the ecl library (using it's Python bindings via SWIG)
In order to run the tests, make sure to compile the ecl library including the
SWIG bindings, e.g. by running
cmake -DEKF_PYTHON_TESTS=ON .. && make ecl_EKF_pytest-quick
from your build directory.
This script can also be run directly (e.g., by using `make pytest`), or you can
run pytest on the test folder.
Running the script directly provides some useful flags, in particular one can
enable plots (using `-p`) to visualize the test results. For example, the
following command will run all test that include the string "baro" in the test
name and plot the results:
PYTHONPATH='/path/to/ecl' ekf_test -p "baro"
@author: Peter Dürr <Peter.Duerr@sony.com>
"""
from __future__ import print_function
from __future__ import unicode_literals
from __future__ import division
from __future__ import absolute_import
import os
from datetime import datetime
import argparse
import pytest
from hypothesis import settings
START_TIME = datetime.now()
"""Record when the script was loaded
"""
# Register hypothesis profiles
settings.register_profile("plots", settings(max_examples=0))
settings.register_profile("quick", settings(max_examples=20))
def main():
"""Called when the script is run standalone
"""
parser = argparse.ArgumentParser(description="Tests ecl's EKF",
usage=(__file__ +
" [OPTIONS] [EXPRESSION]"))
parser.add_argument('-q', '--quick',
action='store_true',
help='Run only the quick tests')
parser.add_argument('-v', '--verbose',
action='store_true',
help='Print verbose output')
parser.add_argument('-s', '--stdout',
action='store_true',
help='Print output to stdout during tests')
parser.add_argument('-b', '--benchmark',
action='store_true',
help='Run the benchmark tests')
parser.add_argument('-p', '--plots',
action='store_true',
help='Create plots during the tests '
'(requires matplotlib and seaborn)')
parser.add_argument('tests', type=str, nargs='?',
help='Run only tests which match '
'the given substring expression')
args = parser.parse_args()
pytest_arguments = [os.path.dirname(os.path.realpath(__file__))]
pytest_plugins = []
if args.quick:
pytest_arguments.append('--hypothesis-profile=plots')
if args.verbose:
pytest_arguments.append('-v')
pytest_arguments.append('-l')
pytest_arguments.append('--hypothesis-show-statistics')
if args.stdout:
pytest_arguments.append('-s')
if args.benchmark:
pytest_arguments.append('-m benchmark')
pytest_arguments.append('--benchmark-verbose')
else:
pytest_arguments.append('--benchmark-disable')
if args.tests:
pytest_arguments.append('-k ' + args.tests)
# To control plotting, monkeypatch a flag onto pytest (cannot just use a
# global variable due to pytest's discovery)
pytest.ENABLE_PLOTTING = args.plots
if args.plots:
pytest_arguments.append('--hypothesis-profile=plots')
# Run pytest
result = pytest.main(pytest_arguments, plugins=pytest_plugins)
exit(result)
if __name__ == '__main__':
main()

94
EKF/tests/pytest/lint.py
View File

@ -1,94 +0,0 @@
#!/usr/bin/env python3
###############################################################################
#
# Copyright (c) 2017 Estimation and Control Library (ECL). 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.
#
###############################################################################
"""Runs code checkers on ecl pytest code
@author: Peter Dürr <Peter.Duerr@sony.com>
"""
from __future__ import print_function
from __future__ import unicode_literals
from __future__ import division
from __future__ import absolute_import
from subprocess import Popen, PIPE
import sys
from datetime import datetime
CHECKERS = [
['pep8', '.'],
['pep8', './ekf_test.py'],
['pep8', './lint.py'],
['pylint', './lint.py', '--reports=n', '--score=n'],
['pylint', './ekf_test.py', '--reports=n', '--score=n'],
['pylint', './plot_utils.py', '--reports=n', '--score=n'],
['pylint', './test_altitude.py', '--reports=n', '--score=n'],
['pylint', './test_basics.py', '--reports=n', '--score=n'],
['pylint', './test_sampling.py', '--reports=n', '--score=n'],
['pylint', './test_utils.py', '--reports=n', '--score=n'],
]
"""List of checkers to run and their arguments
"""
if __name__ == "__main__":
try:
TIC = datetime.now()
CHECKERS_RUN = 0
ERROR = False
for checker in CHECKERS:
process = Popen(checker, stdout=PIPE)
sys.stdout.write('.')
sys.stdout.flush()
CHECKERS_RUN += 1
header = "Output from %s" % " ".join(checker)
output = process.communicate()
message = ""
for elem in output:
if elem is not None and elem:
message += elem.decode('utf-8')
ERROR = True
if not message == "":
print(header)
print("=" * len(header))
print(message)
print("=" * len(header))
TOC = datetime.now()
print("\n" + ''.join(['-']*70))
print("Ran %i checkers in %fs\n" % (CHECKERS_RUN,
(TOC - TIC).total_seconds()))
if not ERROR:
exit(0)
else:
exit(1)
except KeyboardInterrupt:
exit(2)

100
EKF/tests/pytest/plot_utils.py
View File

@ -1,100 +0,0 @@
###############################################################################
#
# Copyright (c) 2017 Estimation and Control Library (ECL). 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.
#
###############################################################################
"""Plotting utilities for the Python-based tests
@author: Peter Dürr <Peter.Duerr@sony.com>
"""
from __future__ import print_function
from __future__ import unicode_literals
from __future__ import division
from __future__ import absolute_import
import inspect
from contextlib import contextmanager
try:
# matplotlib don't use Xwindows backend (must be before pyplot import)
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib.backends.backend_pdf import PdfPages
#from matplotlib import pyplot as plt
#from matplotlib.backends.backend_pdf import PdfPages
#import seaborn as sns
except ImportError as err:
print("Cannot import plotting libraries, "
"please install matplotlib.")
raise err
# Nice plot defaults
#sns.set_style('darkgrid')
#sns.set_palette('colorblind', desat=0.6)
pp = PdfPages("ecl_EKF_test.pdf")
def quit_figure_on_key(key, fig=None):
"""Add handler to figure (defaults to current figure) that closes it
on a key press event.
"""
def quit_on_keypress(event):
"""Quit the figure on key press
"""
if event.key == key:
plt.close(event.canvas.figure)
if fig is None:
fig = plt.gcf()
fig.canvas.mpl_connect('key_press_event', quit_on_keypress)
@contextmanager
def figure(name=None, params=None, figsize=None, subplots=None):
"""Setup a figure that can be closed by pressing 'q'
"""
# As a default, use the name of the calling function as the title of the
# window
if name is None:
# Get name of function calling the context from the stack
name = inspect.stack()[2][3]
fig, axes = plt.subplots(*subplots, figsize=figsize)
#fig.canvas.set_window_title(name)
#quit_figure_on_key('q', fig)
yield fig, axes
if params is not None:
name += "\n" + repr(params)
axes[0].set_title(name)
#plt.show(True)
pp.savefig()

5
EKF/tests/pytest/requirements.txt
View File

@ -1,5 +0,0 @@
pytest>=3.2.1
hypothesis>=3.17.0
numpy>=1.12.1
matplotlib>=2.0.0
pytest-benchmark>=3.1.1

230
EKF/tests/pytest/test_altitude.py
View File

@ -1,230 +0,0 @@
###############################################################################
#
# Copyright (c) 2017 Estimation and Control Library (ECL). 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.
#
###############################################################################
"""Test the altitude estimation of the ecl's EKF
@author: Peter Dürr <Peter.Duerr@sony.com>
"""
from __future__ import print_function
from __future__ import unicode_literals
from __future__ import division
from __future__ import absolute_import
from collections import namedtuple
import numpy as np
import pytest
from hypothesis import given
from hypothesis import example
from hypothesis import strategies as st
from test_utils import ecl_EKF
from test_utils import update_sensors
from test_utils import float_array
from test_utils import initialized_ekf
@given(z_bias=st.floats(-0.2, 0.2))
@example(0.1)
@example(0.2)
def test_accel_z_bias_converges(z_bias):
"""Make sure the accelerometer bias in z-direction is estimated correctly
"""
ekf = ecl_EKF.Ekf()
time_usec = 1000
dt_usec = ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000
# Run for a while
n_samples = 10000
# Prepare data collection for plotting
if pytest.ENABLE_PLOTTING: # pylint: disable=no-member
plot_data = namedtuple('PlotData', ['time', 'accel_bias', 'altitude'])
plot_data.time = np.array([i * dt_usec * 1e-6
for i in range(n_samples)])
plot_data.accel_bias = np.zeros(n_samples)
plot_data.altitude = np.zeros(n_samples)
# Simulation
for i in range(n_samples):
update_sensors(ekf,
time_usec,
dt_usec,
accel=float_array([0.0,
0.0,
-ecl_EKF.one_g + z_bias]))
ekf.update()
time_usec += dt_usec
if pytest.ENABLE_PLOTTING: # pylint: disable=no-member
plot_data.altitude[i] = ekf.get_position()[2]
plot_data.accel_bias[i] = ekf.get_accel_bias()[2]
# Plot if necessary
if pytest.ENABLE_PLOTTING: # pylint: disable=no-member
from plot_utils import figure
with figure(params={'z_bias': z_bias},
subplots=(2, 1)) as (_, (ax1, ax2)):
ax1.plot(plot_data.time,
plot_data.altitude,
label='Altitude Estimate')
ax1.legend(loc='best')
ax1.set_ylabel('Altitude $[m]$')
ax2.plot(plot_data.time,
plot_data.accel_bias,
label='Accel Z Bias Estimate')
ax2.axhline(z_bias, color='k', label='Accel Bias Value')
ax2.legend(loc='best')
ax2.set_ylabel('Bias $[m / s^2]$')
ax2.set_xlabel('Time $[s]$')
# Check assumptions
converged_pos = ekf.get_position()
converged_vel = ekf.get_velocity()
converged_accel_bias = ekf.get_accel_bias()
for i in range(3):
assert converged_pos[i] == pytest.approx(0.0, abs=1e-2)
assert converged_vel[i] == pytest.approx(0.0, abs=1e-2)
for i in range(2):
assert converged_accel_bias[i] == pytest.approx(0.0, abs=1e-3)
assert converged_accel_bias[2] == pytest.approx(z_bias, abs=1e-2)
@given(altitude=st.floats(0.0, 10.0))
@example(0.1)
@example(1.0)
@example(5.0)
@example(10.0)
def test_converges_to_baro_altitude(altitude):
"""Make sure that the ekf converges to (arbitrary) baro altitudes
Increase the altitude with a bang-bang acceleration profile to target
altitude, then wait there for a while and make sure it converges
"""
ekf = ecl_EKF.Ekf()
time_usec = 1000
dt_usec = ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000
# Run for a while
n_samples = 10000
# Compute smooth acceleration profile
rampup_accel = altitude / (((n_samples // 2 // 2) * (dt_usec / 1e6))**2)
# Prepare data collection for plotting
if pytest.ENABLE_PLOTTING: # pylint: disable=no-member
plot_data = namedtuple('PlotData', ['time',
'baro',
'altitude',
'accel_z_bias'])
plot_data.time = np.array([i * dt_usec * 1e-6
for i in range(n_samples)])
plot_data.baro = np.zeros(n_samples)
plot_data.accel_z_bias = np.zeros(n_samples)
plot_data.altitude = np.zeros(n_samples)
# Simulate
current_state = namedtuple('State', ['alt', 'vel'])
current_state.alt = 0.0
current_state.vel = 0.0
for i in range(n_samples // 2):
update_sensors(ekf,
time_usec,
dt_usec,
accel=float_array([0,
0,
-ecl_EKF.one_g - rampup_accel
if i < n_samples // 4
else -ecl_EKF.one_g + rampup_accel]),
baro_data=current_state.alt)
ekf.update()
if pytest.ENABLE_PLOTTING: # pylint: disable=no-member
plot_data.baro[i] = current_state.alt
plot_data.altitude[i] = -ekf.get_position()[2]
plot_data.accel_z_bias[i] = ekf.get_accel_bias()[2]
# Euler step
current_state.vel += (dt_usec / 1e6) * (rampup_accel
if i < n_samples // 4
else -rampup_accel)
current_state.alt += current_state.vel * dt_usec / 1e6
time_usec += dt_usec
# Stay at altitude
for i in range(n_samples // 2):
update_sensors(ekf, time_usec, dt_usec, baro_data=altitude)
ekf.update()
time_usec += dt_usec
if pytest.ENABLE_PLOTTING: # pylint: disable=no-member
plot_data.baro[n_samples // 2 + i] = altitude
plot_data.altitude[n_samples // 2 + i] = -ekf.get_position()[2]
plot_data.accel_z_bias[
n_samples // 2 + i] = ekf.get_accel_bias()[2]
# Plot if necessary
if pytest.ENABLE_PLOTTING: # pylint: disable=no-member
from plot_utils import figure
with figure(params={'altitude': altitude},
subplots=(2, 1)) as (_, (ax1, ax2)):
ax1.plot(plot_data.time,
plot_data.altitude,
label='Altitude Estimate')
ax1.plot(plot_data.time,
plot_data.altitude,
label='Baro Input')
ax1.legend(loc='best')
ax1.set_ylabel('Altitude $[m]$')
ax2.plot(plot_data.time,
plot_data.accel_z_bias,
label='Accel Z Bias Estimate')
ax2.legend(loc='best')
ax2.set_ylabel('Bias $[m / s^2]$')
ax2.set_xlabel('Time $[s]$')
# plt.plot(np.array(baro_vs_pos))
# plt.show()
# print(ekf.get_accel_bias())
converged_pos = ekf.get_position()
converged_vel = ekf.get_velocity()
assert converged_pos[0] == pytest.approx(0.0, abs=1e-6)
assert converged_pos[1] == pytest.approx(0.0, abs=1e-6)
# Check for 10cm level accuracy
assert converged_pos[2] == pytest.approx(-altitude, abs=1e-1)
assert converged_vel[0] == pytest.approx(0.0, abs=1e-3)
assert converged_vel[1] == pytest.approx(0.0, abs=1e-3)
assert converged_vel[2] == pytest.approx(0.0, abs=1e-1)

115
EKF/tests/pytest/test_basics.py
View File

@ -1,115 +0,0 @@
###############################################################################
#
# Copyright (c) 2017 Estimation and Control Library (ECL). 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.
#
###############################################################################
"""Basic tests and benchmarks testing the ecl's EKF
@author: Peter Dürr <Peter.Duerr@sony.com>
"""
from __future__ import print_function
from __future__ import unicode_literals
from __future__ import division
from __future__ import absolute_import
import pytest
from test_utils import ecl_EKF
# Pylint does not like pytest fixtures, disable the warning
# pylint: disable=redefined-outer-name
from test_utils import update_sensors
from test_utils import initialized_ekf # pylint: disable=unused-import
def test_filter_initialized(initialized_ekf):
"""Make sure the EKF updates after a few IMU, Mag and Baro updates
"""
ekf, _ = initialized_ekf
assert ekf.attitude_valid()
@pytest.mark.benchmark
def test_ekf_imu_update_benchmark(initialized_ekf, benchmark):
"""Benchmark the time for an ekf update using just IMU, mag and baro data
"""
ekf, _ = initialized_ekf
benchmark(ekf.update)
def test_status_on_imu_mag_baro(initialized_ekf):
"""Make sure the EKF with zero inputs has reasonable status
"""
ekf, _ = initialized_ekf
control_mode = ekf.get_control_mode()
assert control_mode.yaw_align
assert control_mode.gps is False
assert control_mode.opt_flow is False
assert control_mode.mag_hdg
assert control_mode.mag_3D is False
assert control_mode.mag_dec is False
assert control_mode.in_air is False
assert control_mode.wind is False
assert control_mode.baro_hgt
assert control_mode.rng_hgt is False
assert control_mode.gps_hgt is False
assert control_mode.ev_pos is False
assert control_mode.ev_yaw is False
assert control_mode.ev_hgt is False
assert control_mode.fuse_beta is False
assert control_mode.update_mag_states_only is False
assert control_mode.fixed_wing is False
def test_converges_to_zero():
"""Make sure the EKF with zero inputs converges to / stays at zero
"""
ekf = ecl_EKF.Ekf()
time_usec = 1000
dt_usec = 5000
# Provide a few samples
for _ in range(10000):
update_sensors(ekf, time_usec, dt_usec)
ekf.update()
time_usec += dt_usec
converged_pos = ekf.get_position()
converged_vel = ekf.get_velocity()
converged_accel_bias = ekf.get_accel_bias()
converged_gyro_bias = ekf.get_gyro_bias()
for i in range(3):
assert converged_pos[i] == pytest.approx(0.0, abs=1e-6)
assert converged_vel[i] == pytest.approx(0.0, abs=1e-6)
assert converged_accel_bias[i] == pytest.approx(0.0, abs=1e-5)
assert converged_gyro_bias[i] == pytest.approx(0.0, abs=1e-5)

135
EKF/tests/pytest/test_sampling.py
View File

@ -1,135 +0,0 @@
###############################################################################
#
# Copyright (c) 2017 Estimation and Control Library (ECL). 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.
#
###############################################################################
"""Test the sensor data sampling of the ecl's EKF
@author: Peter Dürr <Peter.Duerr@sony.com>
"""
from __future__ import print_function
from __future__ import unicode_literals
from __future__ import division
from __future__ import absolute_import
import pytest
from hypothesis import given
from hypothesis import strategies as st
from test_utils import ecl_EKF
from test_utils import float_array
@pytest.mark.parametrize("dt_usec, downsampling_factor", [
(ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 // 3, 3),
(ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 // 2, 2),
(ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000, 1),
])
@given(accel_x=st.floats(-5, 5),
accel_y=st.floats(-5, 5),
accel_z=st.floats(-5, 5))
def test_imu_input(dt_usec, downsampling_factor, accel_x, accel_y, accel_z):
"""Make sure the acceleration is updated correctly when there is no angular
velocity (test with and without downsampling)
Tests random accelerations in x, y, z directions (using the hypothesis
decorator) with different update frequencies (using pytest's parametrize
decorator)
"""
time_usec = 100
delta_ang = float_array([0, 0, 0])
delta_vel = float_array([accel_x,
accel_y,
accel_z]) * dt_usec / 1e6
ekf = ecl_EKF.Ekf()
# Run to accumulate buffer (choose sample after downsampling)
for _ in range(20 * downsampling_factor):
time_usec += dt_usec
ekf.set_imu_data(time_usec,
dt_usec,
dt_usec,
delta_ang,
delta_vel)
imu_sample = ekf.get_imu_sample_delayed()
assert imu_sample.delta_ang_x == pytest.approx(0.0, abs=1e-3)
assert imu_sample.delta_ang_y == pytest.approx(0.0, abs=1e-3)
assert imu_sample.delta_ang_z == pytest.approx(0.0, abs=1e-3)
assert imu_sample.delta_vel_x == pytest.approx(
accel_x * dt_usec * downsampling_factor / 1e6, abs=1e-3)
assert imu_sample.delta_vel_y == pytest.approx(
accel_y * dt_usec * downsampling_factor / 1e6, abs=1e-3)
assert imu_sample.delta_vel_z == pytest.approx(
accel_z * dt_usec * downsampling_factor / 1e6, abs=1e-3)
@pytest.mark.parametrize("dt_usec, expected_dt_usec", [
(ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 // 3,
ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000),
(ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 // 2,
ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000),
(ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000,
ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000),
(ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 * 2,
ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 * 2),
(500 * 1000,
500 * 1000)
])
def test_imu_sampling(dt_usec, expected_dt_usec):
"""Make sure the timing is updated correctly
If the imu update is faster than the filter period, it should be
downsampled, otherwise used as is.
"""
time_usec = 0
delta_ang = float_array([0, 0, 0])
delta_vel = float_array([0, 0, 0])
ekf = ecl_EKF.Ekf()
for _ in range(100):
time_usec += dt_usec
ekf.set_imu_data(time_usec,
dt_usec,
dt_usec,
delta_ang,
delta_vel)
imu_sample = ekf.get_imu_sample_delayed()
assert imu_sample.delta_ang_dt == pytest.approx(
expected_dt_usec / 1e6, abs=1e-5)
assert imu_sample.delta_vel_dt == pytest.approx(
expected_dt_usec / 1e6, abs=1e-5)
# Make sure the timestamp of the last sample is a small positive multiple
# of the period away from now
assert (time_usec - imu_sample.time_us) >= 0
assert (time_usec - imu_sample.time_us) / expected_dt_usec < 20

98
EKF/tests/pytest/test_utils.py
View File

@ -1,98 +0,0 @@
###############################################################################
#
# Copyright (c) 2017 Estimation and Control Library (ECL). 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.
#
###############################################################################
"""Utils for the Python-based tests for the ecl library
@author: Peter Dürr <Peter.Duerr@sony.com>
"""
from __future__ import print_function
from __future__ import unicode_literals
from __future__ import division
from __future__ import absolute_import
import numpy as np
import pytest
try:
import ecl_EKF # pylint: disable=import-error
except ImportError:
print("ImportError: ecl library cannot be found."
" Make sure to compile ecl with Python bindings "
"(add -DPythonTests=1 to cmake invocation), "
"and set the PYTHONPATH to your build directory.")
exit(1)
def float_array(inp):
"""Convert input to an array of 32 bit floats
"""
return np.array(inp, dtype=np.float32)
def update_sensors(ekf, # pylint: disable=too-many-arguments
time_usec,
dt_usec,
accel=float_array([0.0, 0.0, -ecl_EKF.one_g]),
ang_vel=float_array([0.0, 0.0, 0.0]),
mag_data=float_array([1.0, 0.0, 0.0]),
baro_data=0.0):
"""Updates the sensors with inputs
"""
ekf.set_imu_data(time_usec,
dt_usec,
dt_usec,
ang_vel * dt_usec / 1e6,
accel * dt_usec / 1e6)
ekf.set_mag_data(time_usec,
mag_data)
ekf.set_baro_data(time_usec,
baro_data)
@pytest.fixture
def initialized_ekf():
"""Provides an initialized ekf, ready to go
"""
ekf = ecl_EKF.Ekf()
time_usec = 1000
dt_usec = 5000
# Provide a few samples
for _ in range(1000):
update_sensors(ekf, time_usec, dt_usec)
ekf.update()
time_usec += dt_usec
# Make sure the ekf updates as expected
return ekf, time_usec

18
Jenkinsfile vendored
View File

@ -102,24 +102,6 @@ pipeline {
}
}
stage('EKF pytest') {
agent {
docker {
image 'px4io/px4-dev-ecl:2019-01-31'
args '-v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
}
}
steps {
sh 'export'
sh 'ccache -z'
sh 'make distclean'
sh 'make test_EKF'
sh 'ccache -s'
archiveArtifacts 'build/test_build/*.pdf'
sh 'make distclean'
}
}
stage('test') {
agent {
docker {

10
Makefile
View File

@ -98,17 +98,11 @@ doxygen:
.PHONY: test_build test test_EKF
test_build:
@$(call cmake-build,$@,$(SRC_DIR), "-DEKF_PYTHON_TESTS=ON", "-DBUILD_TESTING=ON")
@$(call cmake-build,$@,$(SRC_DIR), "-DBUILD_TESTING=ON")
test: test_build
@cmake --build $(SRC_DIR)/build/test_build --target check
test_EKF: test_build
@cmake --build $(SRC_DIR)/build/test_build --target ecl_EKF_pytest-quick
test_EKF_plots: test_build
@cmake --build $(SRC_DIR)/build/test_build --target ecl_EKF_pytest-plots
test_build_asan:
@$(call cmake-build,$@,$(SRC_DIR), "-DECL_ASAN=ON", "-DBUILD_TESTING=ON")
@ -119,7 +113,7 @@ test_asan: test_build_asan
# --------------------------------------------------------------------
coverage_build:
@$(call cmake-build,$@,$(SRC_DIR), "-DCMAKE_BUILD_TYPE=Coverage", "-DEKF_PYTHON_TESTS=ON", "-DBUILD_TESTING=ON")
@$(call cmake-build,$@,$(SRC_DIR), "-DCMAKE_BUILD_TYPE=Coverage", "-DBUILD_TESTING=ON")
coverage: coverage_build
@cmake --build $(SRC_DIR)/build/coverage_build --target coverage

6
test/main.cpp
View File

@ -1,7 +1,7 @@
#include <gtest/gtest.h>
int main(int argc, char **argv) {
testing::InitGoogleTest(&argc, argv);
std::cout << "Run ECL gtests" << std::endl;
return RUN_ALL_TESTS();
testing::InitGoogleTest(&argc, argv);
std::cout << "Run ECL gtests" << std::endl;
return RUN_ALL_TESTS();
}

6
test/test_EKF_imuSampling.cpp
View File

@ -105,9 +105,3 @@ INSTANTIATE_TEST_CASE_P(imuSamplingAtMultipleRates,
std::make_tuple<float,float>(1.6f,1.6f),
std::make_tuple<float,float>(0.333f,1.0f)
));
// TODO:
// """Make sure the acceleration is updated correctly when there is no angular velocity (test with and without downsampling) Tests random accelerations in x, y, z directions (using the hypothesis decorator) with different update frequencies (using pytest's parametrize decorator)
// """Make sure the timing is updated correctly If the imu update is faster than the filter period, it should be downsampled, otherwise used as is.
// Feed in baro data at 50 Hz (too fast), check if filter samples correctly
// Feed in mag data at 50 Hz (too fast), check if filter samples correctly