mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-12 17:07:35 +08:00
Daniel Agar
cebdc3d829
* EKF collect_imu take const imu sample and populate buffer * EKF calculateOutputStates cleanup * EKF add calculate_quaternion output predictor method * EKF: update documentation * EKF: remove unnecessary getter function * EKF calculateOutputStates only apply dt correction to bias * EKF pytest assert attitude validity, not update() return * EKF: correct documentation * EKF: Do not make attitude validity dependent on yaw alignment status Yaw alignment could fail in flight due to temporary loss of data and yet the quaternions would still usable for stabilisation even though the absolute earth yaw angle wrt true north was uncertain.
116 lines
4.2 KiB
Python
116 lines
4.2 KiB
Python
###############################################################################
|
|
#
|
|
# 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_EKF
|
|
|
|
# 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.attitude_valid()
|
|
|
|
|
|
@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.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)
|