mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-16 11:55:39 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

a/math: Use angle as magnitude of change in m_filter_euro_quat

Browse source 
Previously this was using math_quat_len which was always 1 for
these unit quaternions. This commit assumes math_quat_ln works properly which is
not exactly true at the moment and the returned angle will be twice as large.
This commit is contained in:
Mateo de Mayo 2022-06-13 16:59:50 -03:00
parent d773ab4cf2
commit 326402da2a
1 changed files with 6 additions and 2 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
src/xrt/auxiliary/math/m_filter_one_euro.c
View file

@ -282,8 +282,12 @@ m_filter_euro_quat_run(struct m_filter_euro_quat *f, uint64_t ts, const struct x
f->prev_dy = exp_smooth_quat(alpha_d, dy, f->prev_dy);
double dy_mag = math_quat_len(&f->prev_dy);
double alpha = filter_one_euro_compute_alpha(&f->base, dt, dy_mag);
// The magnitud of the smoothed dy (f->prev_dy) is its rotation angle in radians
struct xrt_vec3 smooth_dy_aa;
math_quat_ln(&f->prev_dy, &smooth_dy_aa);
double smooth_dy_mag = m_vec3_len(smooth_dy_aa);
double alpha = filter_one_euro_compute_alpha(&f->base, dt, smooth_dy_mag);
/* Smooth the dy values and use them to calculate the frequency cutoff for the main filter */
f->prev_y = exp_smooth_quat(alpha, *in_y, f->prev_y);