eng-lab/labs/el01_hover/tests/test_kinematics.py

"""EL01 lab tests for hwe/core/kinematics.py (step 8): integrating accelerometer data."""
import math
import unittest

import numpy as np

from _impl import load
from hwe.units import Q_

km = load("kinematics")
G = 9.80665


def smooth_lift(height=0.30, start=1.5, duration=1.2, total=4.0, rate=100.0, bias=0.07, noise=0.0, seed=1):
    """A phone lying screen-up, lifted by `height` metres along a smooth S-curve.

    The lift follows a cycloid, h(s) = H (s/T - sin(2 pi s/T) / (2 pi)) for
    s = t - start in [0, T], so the true acceleration, 2 pi H / T^2 sin(2 pi s/T),
    starts and ends at exactly zero: no jump for an integrator to trip over. The
    phone reads that plus g plus a constant bias, plus optional random noise.
    """
    t = np.arange(0.0, total, 1.0 / rate)
    s = np.clip(t - start, 0.0, duration)
    moving = (t >= start) & (t <= start + duration)
    accel = np.where(moving, 2 * math.pi * height / duration ** 2 * np.sin(2 * math.pi * s / duration), 0.0)
    rng = np.random.default_rng(seed)
    reading = accel + G + bias + noise * rng.standard_normal(t.size)
    return Q_(t, "s"), Q_(reading, "m/s^2")


class Step8Phone(unittest.TestCase):
    def test_trapezoid_integrates_a_straight_line_exactly(self):
        # y = t at 0.5 s steps: the running area is t^2/2 exactly -> 0, 0.125, 0.5, 1.125, 2.0
        t = Q_(np.array([0.0, 0.5, 1.0, 1.5, 2.0]), "s")
        y = Q_(np.array([0.0, 0.5, 1.0, 1.5, 2.0]), "m/s^2")
        out = km.cumulative_trapezoid(t, y)
        np.testing.assert_allclose(out.to("m/s").m, [0.0, 0.125, 0.5, 1.125, 2.0], atol=1e-12)

    def test_trapezoid_with_uneven_steps(self):
        # y = 3 m/s everywhere: the running area is 3 t, whatever the spacing
        t = Q_(np.array([0.0, 0.1, 0.4, 1.0, 1.05]), "s")
        y = Q_(np.full(5, 3.0), "m/s")
        np.testing.assert_allclose(km.cumulative_trapezoid(t, y).to("m").m, 3 * t.m, atol=1e-12)

    def test_trapezoid_units_multiply(self):
        t = Q_(np.array([0.0, 1.0, 2.0]), "s")
        y = Q_(np.array([1.0, 1.0, 1.0]), "m/s^2")
        out = km.cumulative_trapezoid(t, y)
        self.assertEqual(out.dimensionality, Q_(1, "m/s").dimensionality)
        self.assertEqual(len(out), 3)

    def test_trapezoid_rejects_bad_input(self):
        with self.assertRaises(ValueError):
            km.cumulative_trapezoid(Q_(np.array([0.0, 1.0]), "s"), Q_(np.array([1.0, 2.0, 3.0]), "m"))
        with self.assertRaises(ValueError):
            km.cumulative_trapezoid(Q_(np.array([0.0]), "s"), Q_(np.array([1.0]), "m"))

    def test_remove_bias_leaves_the_true_acceleration(self):
        t, reading = smooth_lift()
        true_accel = reading.m - G - 0.07
        np.testing.assert_allclose(km.remove_bias(t, reading, Q_(1.0, "s")).to("m/s^2").m, true_accel, atol=1e-9)

    def test_bias_is_the_average_of_the_whole_still_window(self):
        # One glitchy first sample must not decide the bias: the average of the window does.
        t = Q_(np.arange(0.0, 2.0, 0.01), "s")
        raw = np.full(t.m.size, G + 0.05)
        raw[0] += 1.0                                   # a single outlier at the very start
        window = raw[t.m < 1.0]
        out = km.remove_bias(t, Q_(raw, "m/s^2"), Q_(1.0, "s")).to("m/s^2").m
        np.testing.assert_allclose(out, raw - window.mean(), atol=1e-12)

    def test_remove_bias_needs_a_still_window(self):
        t, reading = smooth_lift()
        with self.assertRaises(ValueError):
            km.remove_bias(t, reading, Q_(0.0, "s"))

    def test_lift_height_without_noise(self):
        t, reading = smooth_lift()
        v, h = km.velocity_and_height(t, reading, Q_(1.0, "s"))
        self.assertAlmostEqual(h[-1].to("m").m, 0.30, delta=0.002)
        self.assertAlmostEqual(v[-1].to("m/s").m, 0.0, delta=0.002)

    def test_lift_height_with_noise(self):
        t, reading = smooth_lift(noise=0.02)
        v, h = km.velocity_and_height(t, reading, Q_(1.0, "s"))
        self.assertAlmostEqual(h[-1].to("m").m, 0.30, delta=0.02)

    def test_bias_must_come_from_the_still_window(self):
        # A record that ends while still speeding up: the average of the WHOLE record is not g + bias.
        # From rest, a = 0.5 m/s^2 for the last 2 s: v_end = 1.0 m/s, h_end = 0.5 x 0.5 x 2^2 = 1.0 m
        t = np.arange(0.0, 3.0, 0.01)
        accel = np.where(t >= 1.0, 0.5, 0.0)
        reading = Q_(accel + G + 0.05, "m/s^2")
        v, h = km.velocity_and_height(Q_(t, "s"), reading, Q_(1.0, "s"))
        self.assertAlmostEqual(v[-1].to("m/s").m, 0.5 * (t[-1] - 1.0), delta=0.01)
        self.assertAlmostEqual(h[-1].to("m").m, 0.25 * (t[-1] - 1.0) ** 2, delta=0.02)

    def test_results_carry_units(self):
        t, reading = smooth_lift()
        v, h = km.velocity_and_height(t, reading, Q_(1.0, "s"))
        self.assertEqual(v.dimensionality, Q_(1, "m/s").dimensionality)
        self.assertEqual(h.dimensionality, Q_(1, "m").dimensionality)
        self.assertEqual(len(h), len(t.m))


if __name__ == "__main__":
    unittest.main()