mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-20 23:33:35 +08:00
Add Python wrapper to ecl and use it to test functionality
* Add SWIG interface definition (and external numpy interface) to ecl classes * Add section in CMakeLists.txt to build Python bindings and execute Python-based tests * Write (property-based) tests that show the basic functionality of the Python bindings and the EKF (using pytest and hypothesis libraries) * Write minimal benchmark for the EKF update (using benchmark plugin for pytest) * Add plotting utilities to analyze tests * Add lint script to keep the Python scripts clean
This commit is contained in:
Peter Dürr
@@ -92,3 +92,70 @@ if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
|
||||
endif()
|
||||
|
||||
add_library(ecl SHARED ${SRCS})
|
||||
|
||||
# Python bindings & tests
|
||||
# Use cmake -DPythonTests=1 ../EKF && make pytest
|
||||
if(PythonTests)
|
||||
# Use Python 3.5
|
||||
set(Python_ADDITIONAL_VERSIONS 3.5)
|
||||
find_package(PythonLibs 3 REQUIRED)
|
||||
find_package(PythonInterp 3 REQUIRED)
|
||||
|
||||
# Check for required python packages
|
||||
set(ECL_TEST_PYTHON_PACKAGES
|
||||
pytest
|
||||
hypothesis
|
||||
numpy
|
||||
)
|
||||
|
||||
foreach(package ${ECL_TEST_PYTHON_PACKAGES})
|
||||
execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "import ${package}" RESULT_VARIABLE PytestAvailable OUTPUT_QUIET ERROR_QUIET)
|
||||
if(${PytestAvailable})
|
||||
message(FATAL_ERROR "Python package missing:\n Please install ${package}, e.g., \"pip3 install --user ${package}\"")
|
||||
endif()
|
||||
endforeach(package)
|
||||
|
||||
# Need SWIG to wrap ecl
|
||||
find_package(SWIG REQUIRED)
|
||||
include(${SWIG_USE_FILE})
|
||||
|
||||
add_definitions(-DSWIG_TYPE_TABLE=ecl)
|
||||
set_source_files_properties(../swig/ecl.i PROPERTIES CPLUSPLUS ON)
|
||||
set(swigged_sources ekf.h)
|
||||
|
||||
include_directories(
|
||||
..
|
||||
${MATRIX_DIR}
|
||||
${EIGEN3_INCLUDE_DIR}
|
||||
${PYTHON_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
set(SWIG_MODULE_ecl_EXTRA_DEPS ${swigged_sources})
|
||||
swig_add_module(ecl python ../swig/ecl.i)
|
||||
swig_link_libraries(ecl ${PYTHON_LIBRARIES} ecl)
|
||||
|
||||
add_custom_target(pytest
|
||||
env PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_SOURCE_DIR}/tests/pytest/ekf_test --verbose
|
||||
)
|
||||
add_dependencies(pytest ${SWIG_MODULE_ecl_REAL_NAME})
|
||||
|
||||
add_custom_target(pytest-quick
|
||||
env PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_SOURCE_DIR}/tests/pytest/ekf_test --quick --verbose
|
||||
)
|
||||
add_dependencies(pytest-quick ${SWIG_MODULE_ecl_REAL_NAME})
|
||||
|
||||
add_custom_target(pytest-benchmark
|
||||
env PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_SOURCE_DIR}/tests/pytest/ekf_test --benchmark
|
||||
)
|
||||
add_dependencies(pytest-benchmark ${SWIG_MODULE_ecl_REAL_NAME})
|
||||
|
||||
add_custom_target(pytest-plots
|
||||
env PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_SOURCE_DIR}/tests/pytest/ekf_test --plots
|
||||
)
|
||||
add_dependencies(pytest-plots ${SWIG_MODULE_ecl_REAL_NAME})
|
||||
|
||||
add_custom_target(pytest-lint
|
||||
COMMAND cd ${CMAKE_SOURCE_DIR}/tests/pytest/ && ${CMAKE_SOURCE_DIR}/tests/pytest/lint
|
||||
)
|
||||
add_dependencies(pytest-lint ${SWIG_MODULE_ecl_REAL_NAME})
|
||||
endif()
|
||||
|
||||
Executable
+137
@@ -0,0 +1,137 @@
|
||||
#!/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 -DPythonTests=1 ../EKF && make 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()
|
||||
Executable
+94
@@ -0,0 +1,94 @@
|
||||
#!/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'],
|
||||
['pep8', './lint'],
|
||||
['pylint', './lint', '--reports=n', '--score=n'],
|
||||
['pylint', './ekf_test', '--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)
|
||||
@@ -0,0 +1,91 @@
|
||||
###############################################################################
|
||||
#
|
||||
# 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:
|
||||
from matplotlib import pyplot as plt
|
||||
import seaborn as sns
|
||||
except ImportError as err:
|
||||
print("Cannot import plotting libraries, "
|
||||
"please install matplotlib and seaborn.")
|
||||
raise err
|
||||
|
||||
# Nice plot defaults
|
||||
sns.set_style('darkgrid')
|
||||
sns.set_palette('colorblind',
|
||||
desat=0.6)
|
||||
|
||||
|
||||
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)
|
||||
@@ -0,0 +1,230 @@
|
||||
###############################################################################
|
||||
#
|
||||
# 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
|
||||
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()
|
||||
|
||||
time_usec = 1000
|
||||
dt_usec = ecl.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-3
|
||||
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.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
|
||||
"""
|
||||
# Due to hypothesis not interacting with pytest, cannot use fixture here
|
||||
ekf, time_usec = initialized_ekf()
|
||||
|
||||
dt_usec = ecl.Ekf.FILTER_UPDATE_PERIOD_MS * 1000
|
||||
|
||||
# No samples, half are used for ramping up / down to the altitude
|
||||
n_samples = 200
|
||||
|
||||
# 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-3
|
||||
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.one_g - rampup_accel
|
||||
if i < n_samples // 4
|
||||
else -ecl.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)
|
||||
@@ -0,0 +1,115 @@
|
||||
###############################################################################
|
||||
#
|
||||
# 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
|
||||
|
||||
# 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.update()
|
||||
|
||||
|
||||
@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()
|
||||
|
||||
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)
|
||||
@@ -0,0 +1,135 @@
|
||||
###############################################################################
|
||||
#
|
||||
# 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
|
||||
from test_utils import float_array
|
||||
|
||||
|
||||
@pytest.mark.parametrize("dt_usec, downsampling_factor", [
|
||||
(ecl.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 // 3, 3),
|
||||
(ecl.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 // 2, 2),
|
||||
(ecl.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()
|
||||
# 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.FILTER_UPDATE_PERIOD_MS * 1000 // 3,
|
||||
ecl.Ekf.FILTER_UPDATE_PERIOD_MS * 1000),
|
||||
(ecl.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 // 2,
|
||||
ecl.Ekf.FILTER_UPDATE_PERIOD_MS * 1000),
|
||||
(ecl.Ekf.FILTER_UPDATE_PERIOD_MS * 1000,
|
||||
ecl.Ekf.FILTER_UPDATE_PERIOD_MS * 1000),
|
||||
(ecl.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 * 2,
|
||||
ecl.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()
|
||||
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-6)
|
||||
assert imu_sample.delta_vel_dt == pytest.approx(
|
||||
expected_dt_usec / 1e6, abs=1e-6)
|
||||
# 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
|
||||
@@ -0,0 +1,98 @@
|
||||
###############################################################################
|
||||
#
|
||||
# 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 # 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.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()
|
||||
|
||||
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
|
||||
Reference in New Issue
Block a user