lowpass filter orientation using quaternion slerp -凯发k8网页登录

this example shows how to use spherical linear interpolation (slerp) to create sequences of quaternions and lowpass filter noisy trajectories. slerp is a commonly used computer graphics technique for creating animations of a rotating object.

slerp overview

consider a pair of quaternions $q_0$ and $q_1$ . spherical linear interpolation allows you to create a sequence of quaternions that vary smoothly between $q_0$ and $q_1$ with a constant angular velocity. slerp uses an interpolation parameter h that can vary between 0 and 1 and determines how close the output quaternion is to either $q_0$ or $q_1$ .

the original formulation of quaternion slerp was given by ken shoemake [ 1] as:

$slerp(q_0,q_1,h) = q_1(q_1^{-1} q_2)^h$

an alternate formulation with sinusoids (used in the slerp function implementation) is:

$slerp(q_0,q_1,h) = \frac{\sin((1-h)\theta)}{\sin\theta}q_0 \frac{\sin(h\theta)}{\sin\theta}q_1$

where $\theta$ is the dot product of the quaternion parts. note that $\theta = dist(q_0, q_1)/2$ .

slerp vs linear interpolation of quaternion parts

consider the following example. build two quaternions from euler angles.

q0 = quaternion([-80 10 0], 'eulerd', 'zyx', 'frame');
q1 = quaternion([80 70 70], 'eulerd', 'zyx', 'frame');

to find a quaternion 30 percent of the way from q0 to q1, specify the slerp parameter as 0.3.

p30 = slerp(q0, q1, 0.3);

to view the interpolated quaternion's euler angle representation, use the eulerd function.

eulerd(p30, 'zyx', 'frame')

ans =
  -56.6792   33.2464   -9.6740

to create a smooth trajectory between q0 and q1, specify the slerp interpolation parameter as a vector of evenly spaced numbers between 0 and 1.

dt = 0.01;
h = (0:dt:1).';
trajslerped = slerp(q0, q1, h);

compare the results of the slerp algorithm with a trajectory between q0 and q1, using simple linear interpolation (lerp) of each quaternion part.

partslininterp = interp1( [0;1], compact([q0;q1]), h, 'linear');

note that linear interpolation does not give unit quaternions, so they must be normalized.

trajlerped = normalize(quaternion(partslininterp));

compute the angular velocities from each approach.

avslerp = helperquat2av(trajslerped, dt);
avlerp = helperquat2av(trajlerped, dt);

plot both sets of angular velocities. notice that the angular velocity for slerp is constant, but it varies for linear interpolation.

sp = helperslerpplotting;
sp.plotangularvelocities(avslerp, avlerp);

slerp produces a smooth rotation at a constant rate.

lowpass filtering with slerp

slerp can also be used to make more complex functions. here, slerp is used to lowpass filter a noisy trajectory.

rotational noise can be constructed by forming a quaternion from a noisy rotation vector.

rcurr = rng(1);
sigma = 1e-1;
noiserv = sigma .* ( rand(numel(h), 3) - 0.5);
qnoise = quaternion(noiserv, 'rotvec');
rng(rcurr);

to corrupt the trajectory trajslerped with noise, incrementally rotate the trajectory with the noise vector qnoise.

trajnoisy = trajslerped .* qnoise;

you can smooth real-valued signals using a single pole filter of the form:

$y_k = y_{k-1} \alpha(x_k - y_{k-1})$

this formula essentially says that the new filter state $y_k$ should be moved toward the current input $x_k$ by a step size that is proportional to the distance between the current input and the current filter state $y_{k-1}$ .

the spirit of this approach informs how a quaternion sequence can be lowpass filtered. to do this, both the dist and slerp functions are used.

the dist function returns a measurement in radians of the difference in rotation applied by two quaternions. the range of the dist function is the half-open interval [0, pi).

the slerp function is used to steer the filter state towards the current input. it is steered more towards the input when the difference between the input and current filter state has a large dist, and less toward the input when dist gives a small value. the interpolation parameter to slerp is in the closed-interval [0,1], so the output of dist must be re-normalized to this range. however, the full range of [0,1] for the interpolation parameter gives poor performance, so it is limited to a smaller range hrange centered at hbias.

hrange = 0.4;
hbias = 0.4;

limit low and high to the interval [0, 1].

low  = max(min(hbias - (hrange./2), 1), 0);
high = max(min(hbias   (hrange./2), 1), 0);
hrangelimited = high - low;

initialize the filter and preallocate outputs.

y = trajnoisy(1); % initial filter state
qout = zeros(size(y), 'like', y); % preallocate filter output
qout(1) = y;

filter the noisy trajectory, sample-by-sample.

for ii=2:numel(trajnoisy)
    x = trajnoisy(ii);
    d = dist(y, x);
    % renormalize dist output to the range [low, high]
    hlpf = (d./pi).*hrangelimited   low;
    y = slerp(y,x,hlpf);
    qout(ii) = y;
end
f = figure;
sp.ploteulerd(f, trajnoisy, 'o');
sp.ploteulerd(f, trajslerped, 'k-.', 'linewidth', 2);
sp.ploteulerd(f, qout, '-', 'linewidth', 2);
sp.addannotations(f, hrange, hbias);

conclusion

slerp can be used for creating both short trajectories between two orientations and for smoothing or lowpass filtering. it has found widespread use in a variety of industries.

references

shoemake, ken. "animating rotation with quaternion curves." acm sigraph computer graphics 19, no 3 (1985):245-54, doi:10.1145/325165.325242