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)
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
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)
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)
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
"""
ekf = ecl_EKF.Ekf()
time_usec = 1000
dt_usec = ecl_EKF.Ekf.FILTER_UPDATE_PERIOD_MS * 1000
# Run for a while
n_samples = 10000
# 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)