fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 17:54:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add Python wrapper to ecl and use it to test functionality

Browse Source 
* 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 2017-08-11 18:30:11 +02:00 committed by Lorenz Meier
parent 5988900044
commit bb5719a0da
10 changed files with 4399 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
EKF/CMakeLists.txt
View File

@ -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()

137
EKF/tests/pytest/ekf_test Executable file
View File

@ -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()

94
EKF/tests/pytest/lint Executable file
View File

@ -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)

91
EKF/tests/pytest/plot_utils.py Normal file
View File

@ -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)

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

@ -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)

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

@ -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)

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

@ -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

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

@ -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

266
swig/ecl.i Normal file
View File

@ -0,0 +1,266 @@
// SWIG Wrapper for the ecl's EKF
%module(directors="1") ecl
%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 "../EKF/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_mag_hdg; // 3 - true if the fusion of the magnetic heading 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_mag_hdg: " << bad_mag_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/matrix/Vector3.hpp"
%include "../matrix/matrix/Vector2.hpp"
%include "../matrix/matrix/Quaternion.hpp"
%include "../matrix/matrix/Dcm.hpp"
%include "../matrix/matrix/Euler.hpp"
%include "../matrix/matrix/SquareMatrix.hpp"
%include "../matrix/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_mag_hdg = result_union.flags.bad_mag_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]) {
return self->copy_quaternion(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
swig/numpy.i Normal file
View File

File diff suppressed because it is too large Load Diff