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_converges_to_zero():
"""Make sure the EKF with zero inputs converges to / stays at zero
"""
ekf = ecl.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_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
"""
# 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)
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_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)
