px4_autopilot/EKF/tests/pytest/test_altitude.py

###############################################################################
#
# 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_EKF
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.Ekf()

    time_usec = 1000
    dt_usec = ecl_EKF.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_EKF.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.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_EKF.one_g - rampup_accel
                                          if i < n_samples // 4
                                          else -ecl_EKF.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)