zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
953 Commits
14 Branches
0 Tags
306 MiB
 
 
 
 
 
 
Tree: af7004ef01
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'af7004ef01'
${ noResults }
px4_autopilot/EKF/tests/pytest/test_altitude.py

230 lines
8.9 KiB
Raw Blame History

###############################################################################
#
# 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-6
for i in range(n_samples)])
plot_data.accel_bias = np.zeros(n_samples)
plot_data.altitude = np.zeros(n_samples)
# Simulation
for i in range(n_samples):
update_sensors(ekf,
time_usec,
dt_usec,
accel=float_array([0.0,
0.0,
-ecl.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-6
for i in range(n_samples)])
plot_data.baro = np.zeros(n_samples)
plot_data.accel_z_bias = np.zeros(n_samples)
plot_data.altitude = np.zeros(n_samples)
# Simulate
current_state = namedtuple('State', ['alt', 'vel'])
current_state.alt = 0.0
current_state.vel = 0.0
for i in range(n_samples // 2):
update_sensors(ekf,
time_usec,
dt_usec,
accel=float_array([0,
0,
-ecl.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)