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.
135 lines
5.4 KiB
135 lines
5.4 KiB
############################################################################### |
|
# |
|
# Copyright (c) 2017 Estimation and Control Library (ECL). All rights reserved. |
|
# |
|
# Redistribution and use in source and binary forms, with or without |
|
# modification, are permitted provided that the following conditions |
|
# are met: |
|
# |
|
# 1. Redistributions of source code must retain the above copyright |
|
# notice, this list of conditions and the following disclaimer. |
|
# 2. Redistributions in binary form must reproduce the above copyright |
|
# notice, this list of conditions and the following disclaimer in |
|
# the documentation and/or other materials provided with the |
|
# distribution. |
|
# 3. Neither the name ECL nor the names of its contributors may be |
|
# used to endorse or promote products derived from this software |
|
# without specific prior written permission. |
|
# |
|
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
|
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
|
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
|
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
|
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
|
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
|
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
|
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
|
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
|
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
|
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
|
# POSSIBILITY OF SUCH DAMAGE. |
|
# |
|
############################################################################### |
|
|
|
"""Test the sensor data sampling of the ecl's EKF |
|
|
|
@author: Peter Dürr <Peter.Duerr@sony.com> |
|
""" |
|
from __future__ import print_function |
|
from __future__ import unicode_literals |
|
from __future__ import division |
|
from __future__ import absolute_import |
|
|
|
import pytest |
|
from hypothesis import given |
|
from hypothesis import strategies as st |
|
|
|
from test_utils import ecl_EKF |
|
from test_utils import float_array |
|
|
|
|
|
@pytest.mark.parametrize("dt_usec, downsampling_factor", [ |
|
(ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 // 3, 3), |
|
(ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 // 2, 2), |
|
(ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000, 1), |
|
]) |
|
@given(accel_x=st.floats(-5, 5), |
|
accel_y=st.floats(-5, 5), |
|
accel_z=st.floats(-5, 5)) |
|
def test_imu_input(dt_usec, downsampling_factor, accel_x, accel_y, accel_z): |
|
"""Make sure the acceleration is updated correctly when there is no angular |
|
velocity (test with and without downsampling) |
|
|
|
Tests random accelerations in x, y, z directions (using the hypothesis |
|
decorator) with different update frequencies (using pytest's parametrize |
|
decorator) |
|
|
|
""" |
|
time_usec = 100 |
|
delta_ang = float_array([0, 0, 0]) |
|
delta_vel = float_array([accel_x, |
|
accel_y, |
|
accel_z]) * dt_usec / 1e6 |
|
|
|
ekf = ecl_EKF.Ekf() |
|
# Run to accumulate buffer (choose sample after downsampling) |
|
for _ in range(20 * downsampling_factor): |
|
time_usec += dt_usec |
|
ekf.set_imu_data(time_usec, |
|
dt_usec, |
|
dt_usec, |
|
delta_ang, |
|
delta_vel) |
|
|
|
imu_sample = ekf.get_imu_sample_delayed() |
|
assert imu_sample.delta_ang_x == pytest.approx(0.0, abs=1e-3) |
|
assert imu_sample.delta_ang_y == pytest.approx(0.0, abs=1e-3) |
|
assert imu_sample.delta_ang_z == pytest.approx(0.0, abs=1e-3) |
|
assert imu_sample.delta_vel_x == pytest.approx( |
|
accel_x * dt_usec * downsampling_factor / 1e6, abs=1e-3) |
|
assert imu_sample.delta_vel_y == pytest.approx( |
|
accel_y * dt_usec * downsampling_factor / 1e6, abs=1e-3) |
|
assert imu_sample.delta_vel_z == pytest.approx( |
|
accel_z * dt_usec * downsampling_factor / 1e6, abs=1e-3) |
|
|
|
|
|
@pytest.mark.parametrize("dt_usec, expected_dt_usec", [ |
|
(ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 // 3, |
|
ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000), |
|
(ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 // 2, |
|
ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000), |
|
(ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000, |
|
ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000), |
|
(ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 * 2, |
|
ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000 * 2), |
|
(500 * 1000, |
|
500 * 1000) |
|
]) |
|
def test_imu_sampling(dt_usec, expected_dt_usec): |
|
"""Make sure the timing is updated correctly |
|
|
|
If the imu update is faster than the filter period, it should be |
|
downsampled, otherwise used as is. |
|
|
|
""" |
|
time_usec = 0 |
|
delta_ang = float_array([0, 0, 0]) |
|
delta_vel = float_array([0, 0, 0]) |
|
ekf = ecl_EKF.Ekf() |
|
for _ in range(100): |
|
time_usec += dt_usec |
|
ekf.set_imu_data(time_usec, |
|
dt_usec, |
|
dt_usec, |
|
delta_ang, |
|
delta_vel) |
|
|
|
imu_sample = ekf.get_imu_sample_delayed() |
|
assert imu_sample.delta_ang_dt == pytest.approx( |
|
expected_dt_usec / 1e6, abs=1e-5) |
|
assert imu_sample.delta_vel_dt == pytest.approx( |
|
expected_dt_usec / 1e6, abs=1e-5) |
|
# Make sure the timestamp of the last sample is a small positive multiple |
|
# of the period away from now |
|
assert (time_usec - imu_sample.time_us) >= 0 |
|
assert (time_usec - imu_sample.time_us) / expected_dt_usec < 20
|
|
|