mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-07 03:51:36 +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
@@ -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)
|
||||
Reference in New Issue
Block a user