def show_betas(betas, beta_optimum, Q_base):
""" Plots Filter yaw and quaternion error at different Beta values """
betas = np.round(betas, decimals=2)
betas = np.sort(betas)
# Set up figure
yaw_base = ot.q_to_aero_yaw(Q_base)
fig = plt.figure(figsize=(14, 7), facecolor="w")
fig.suptitle("Different Beta values for C filter", fontsize=16)
gs = gridspec.GridSpec(ncols=1, nrows=2, height_ratios=[2, 1])
# Set up colourmap
cmap = plt.get_cmap('winter')
norm = matplotlib.colors.Normalize(
vmin=0, vmax=len(betas)) # normalize item number values to colormap
ax0 = plt.subplot(gs[0])
ax1 = plt.subplot(gs[1])
# Plot all the different beta curves
mad_sqrt = cpp.MadgwickOriginalSqrt(freq=freq)
for idx, beta in enumerate(betas):
sqrt_Q = mad_sqrt.update(acc, gyr, mag, beta=beta, q=q0)
sqrt_yaw = ot.q_to_aero_yaw(sqrt_Q)
diff = ot.q_angle_diff_safe(sqrt_Q, Q_base)
# Highlight the optimimum beta value in the legend
label = rf'$\beta={beta:.2f}$'
if abs(beta - beta_optimum) < 0.01:
label += " (o)"
rgba_colour = cmap(norm(idx))
ax0.plot(times, sqrt_yaw, c=rgba_colour, label=label, alpha=0.7)
ax1.plot(times, diff, c=rgba_colour, label=label, alpha=0.7)
ax0.plot(times,
yaw_base,
ls='--',
lw=2,
c='orangered',
label='Py: ahrs',
alpha=0.7)
ax0.set(ylabel='yaw (deg)', title="C filter yaw over time")
ax0.grid()
ax0.text(16,
15,
rf'$f={freq:.1f} \mathrm{{Hz}},$ Py:ahrs $\beta={beta_py:0.2f}$',
fontsize=12,
bbox=dict(boxstyle='round', fc='white', ec='lightgray'))
ax0.legend()
ax0.set_yticks([-30, 0, 45, 90, 135])
ax1.set(xlabel='time (s)',
ylabel='difference (deg)',
title="Difference in angle between quaternions")
ax1.grid()
bbox=dict(boxstyle='round', fc='white', ec='lightgray'))
ax1 = plt.subplot(spec[1])
ax1.set(xlabel='time (s)',
ylabel='difference (deg)',
title="Difference in angle between quaternions")
ax1.grid()
num_plots = 5
curr_plot = 0
cmap = plt.get_cmap('gist_rainbow')
norm = matplotlib.colors.Normalize(vmin=0, vmax=num_plots - 1)
# Through the code without fast_inv_sqrt()
mad_sqrt = cpp.MadgwickOriginalSqrt(beta=beta, freq=freq, q0=q0)
Q_base = mad_sqrt.update(acc, gyr, mag)
def run_and_plot_filter(ax0, ax1, Q_base, madg, label):
global curr_plot
Q = madg.update(acc, gyr, mag)
yaw = ot.q_to_aero_yaw(Q)
diff = ot.q_angle_diff_safe(Q_base, Q)
rgba = cmap(norm(curr_plot))
curr_plot += 1
ax0.plot(times, yaw, ls='--', c=rgba, label=label, alpha=0.7)
ax1.plot(times, diff, ls='--', c=rgba, label=label, alpha=0.7)
def show_beta_optimisation(acc, gyr, mag, times, Q_comp):
""" Chooses a value of beta so that the c based filter most closely
tracks the python filter. """
# Optimisation part
mad_filter = cpp.MadgwickOriginalSqrt(freq=freq)
def quat_diff(beta):
"""Calculate quaternions and average difference"""
sqrt_Q = mad_filter.update(acc, gyr, mag, beta=beta, q=q0)
diff = ot.q_angle_diff_safe(Q_comp, sqrt_Q)
return np.average(diff)
result = scipy.optimize.minimize(fun=quat_diff,
x0=beta_py,
method='Nelder-Mead',
options=dict(maxiter=100))
beta_optimum = result.x[0]
diff_optimum = result.fun
# Generate plot to show the result of optmisation.
fig = plt.figure(figsize=(14, 7), facecolor="w")
fig.suptitle(fr"Finding optimum Beta to match $\beta={beta_py:.2f}$",
fontsize=16)
gs = gridspec.GridSpec(2,
2,
height_ratios=[2.5, 1],
figure=fig,
hspace=0.3)
ax0 = fig.add_subplot(gs[0, 0])
ax1 = fig.add_subplot(gs[0, 1])
ax2 = fig.add_subplot(gs[1, :])
# Plot 0 shows the yaw output of the C and Py filter. This is the most similar that was found.
yaw_comp = ot.q_to_aero_yaw(Q_comp)
sqrt_Q = mad_filter.update(acc, gyr, mag, beta=beta_optimum, q=q0)
yaw_sqrt = ot.q_to_aero_yaw(sqrt_Q)
ax0.set(ylabel='yaw (deg)',
xlabel='time (s)',
title="Plot 1: Filter yaw value over time")
ax0.grid()
ax0.plot(times,
yaw_comp,
'--r',
label=fr"Py: $\beta={beta_py:.2f}$",
alpha=0.7)
ax0.plot(times,
yaw_sqrt,
'--g',
label=fr"C: $\beta={beta_optimum:.2f}$",
alpha=0.7)
ax0.legend()
# Plot 1 shows the optimisation
betas = np.linspace(0.01, 6, 50)
vquat_diff = np.vectorize(quat_diff)
y_values = vquat_diff(betas)
ax1.plot(betas, y_values, label='Quat Difference')
ax1.axvline(beta_optimum,
label=fr"$\beta={beta_optimum:.2f}$",
color='g',
ls=':')
ax1.axhline(diff_optimum,
label=fr"Diff = ${diff_optimum:.2f}°$",
color='r',
ls=':')
ax1.set(xlabel='beta',
ylabel='average difference (deg)',
title='Plot 2: Average difference')
ax1.legend()
# Plot 2 shows the difference in quaternions
diff = ot.q_angle_diff_safe(sqrt_Q, Q_comp)
diff_avg = np.average(diff)
ax2.plot(times, diff, c='#3355ff', label='Diff', alpha=0.7)
ax2.axhline(diff_avg,
label=fr"Avg Diff = ${diff_avg:.2f}°$",
color='r',
ls=':')
ax2.set(xlabel='time (s)',
ylabel='difference (deg)',
title="Plot 3: Difference in angle between quaternions")
ax2.legend()
return beta_optimum
options=dict(maxiter=100))
beta_optimum = result.x[0]
return beta_optimum
def find_optimum_betas(base_filter, free_filter):
betas = np.linspace(0.001, 1, 10)
vbeta_diff = np.vectorize(find_optimum_beta)
y_values = vbeta_diff(betas, base_filter, free_filter)
return betas, y_values
mad_filter = cpp.MadgwickOriginalSqrt(freq=freq)
betas, y_values = find_optimum_betas(base_filter=ahrs_filter,
free_filter=mad_filter)
fig = plt.figure(figsize=(14, 9), facecolor="w")
fig.suptitle('Beta Conversions', fontsize=16)
ax0 = fig.add_subplot(211)
ax0.plot(betas, y_values / betas, label='Py AHRS vs original C')
ax0.set(xlabel='Py Ahrs beta',
ylabel='beta ratio: C / Py Ahrs',
title="Ratio of beta values")
ax0.grid()
ax0.legend()
def plot_filter_output(ax0, ax1, Q_base, Q, label):
global curr_plot
yaw = ot.q_to_aero_yaw(Q)
diff = ot.q_angle_diff_safe(Q_base, Q)
rgba = cmap(norm(curr_plot))
curr_plot += 1
ax0.plot(times, yaw, ls='--', c=rgba, label=label, alpha=0.7)
ax1.plot(times, diff, ls='--', c=rgba, label=label, alpha=0.7)
plot_filter_output(ax0, ax1, Q_base, Q_base, "Matlab")
# Through the code without fast_inv_sqrt()
Q = cpp.MadgwickOriginalSqrt(beta=beta, freq=freq, q0=q0).update(acc, gyr, mag)
plot_filter_output(ax0, ax1, Q_base, Q, "C + 1/sqrt()")
# Through the code with fix
Q = cpp.MadgwickFixed(beta=beta, freq=freq, q0=q0).update(acc, gyr, mag)
plot_filter_output(ax0, ax1, Q_base, Q, "C fix")
# Through the code from the paper
Q = cpp.MadgwickPaper(beta=beta, freq=freq, q0=q0).update(acc, gyr, mag)
plot_filter_output(ax0, ax1, Q_base, Q, "paper org")
# Through the code from the paper but without compensation
Q = cpp.MadgwickPaperFixed(beta=beta, freq=freq, q0=q0).update(acc, gyr, mag)
plot_filter_output(ax0, ax1, Q_base, Q, "paper fix")