Quaternions and gyroscope
Introduction
A gyroscope measures the angular acceleration around the three axis \( X \), \( Y \) and \( Z \) of the sensor frame named \( \omega_X \), \( \omega_Y \) and \( \omega_Z \) respectively. By integrating twice these angular accelerations, it is theoretically possible to estimate the orientation over time. Note that it don't work properly in practice due to the cumulated errors, this phenomena is called "drift".
Explanation
Let's define the vector \( S_{\omega} \) which contains the angular accelerations (or velocities):
$$ S_{\omega} = \left[ \begin{matrix} 0 && \omega_x && \omega_y && \omega_z \end{matrix} \right] $$
Where \( \omega_X \), \( \omega_Y \) and \( \omega_Z \) are expressed in \( rad.s^{-2} \) or \( rad.s^{-1} \).
Let's now consider the quaternion derivative that describes the rate of change of orientation:
$$ \frac{dQ_k}{dt} = \frac{1}{2}.\hat{Q}_{k-1} \otimes S_{\omega} $$
Where :
- \( \frac{dQ_k}{dt} \) is the derivative at time step \( k \) expressed in the quaternion space.
- \( \hat{Q}_{k} \) is the estimated orientation at time step \( k \).
- \( \otimes \) is the quaternion product operator.
By integrating the quaternion derivative it becomes possible to estimate the orientation over time:
$$ \hat{Q}_k = \hat{Q}_{k-1}+ \Delta_t.\frac{dQ_k}{dt} $$
\( \Delta_t \) is the step time. Note that for some applications, the quaternion must be normalized after integration:
$$ Q_k^n=\frac{\hat{Q}_k}{| \hat{Q}_k | } $$
Example
This example simulates the evolution of a gyroscope during 50 seconds. The rotation is along the X and Y axis during respectively the 25 first seconds and during the 25 last seconds. To avoid aving a double integration, we assume in this exemple that the gyroscope produces angular velocities instead of angular accelerations.
Here is the Matlab source code of this example.
close all;
clear all;
clc;
%% Initial parameters
% Initial quaternion
Q(1,:)=[1 0 0 0];
% Step time
dt=0.5;
% Initial coordinates of the point
P(1,:)=[1,2,3];
PX(1,:)=[2,2,3];
PY(1,:)=[1,3,3];
PZ(1,:)=[1,2,4];
%% Main loop
for i=2:100
%% Simulate gyroscope value
% pi/8 rad/s along X axis for the 50 first steps
% pi/16 rad/s along Y axis for the 50 last steps
if (i<50) Sw=[0 pi/8 0 0]; else Sw=[0 0 pi/16 0]; end;
%% Update orientation
% Compute the quaternion derivative
Qdot=quaternProd(0.5*Q(i-1,:),Sw);
% Update the estimated position
Q(i,:)=Q(i-1,:)+Qdot*dt;
% Normalize quaternion
Q(i,:)=Q(i,:)/norm(Q(i,:));
%% Update point coordinates
% Compute the associated transformation marix
M=quatern2rotMat(Q(i,:));
% Calculate the coordinate of the new point
P(i,:)=(M*P(1,:)')';
PX(i,:)=(M*PX(1,:)')';
PY(i,:)=(M*PY(1,:)')';
PZ(i,:)=(M*PZ(1,:)')';
% Display the new point
plot3 (P(i,1),P(i,2),P(i,3),'k.');
plot3 (PX(i,1),PX(i,2),PX(i,3),'r.');
plot3 (PY(i,1),PY(i,2),PY(i,3),'g.');
plot3 (PZ(i,1),PZ(i,2),PZ(i,3),'b.');
line ( [ P(i,1) , PX(i,1) ] , [ P(i,2) , PX(i,2) ], [ P(i,3) , PX(i,3) ] ,'Color','r');
line ( [ P(i,1) , PY(i,1) ] , [ P(i,2) , PY(i,2) ], [ P(i,3) , PY(i,3) ] ,'Color','g');
line ( [ P(i,1) , PZ(i,1) ] , [ P(i,2) , PZ(i,2) ], [ P(i,3) , PZ(i,3) ] ,'Color','b');
hold on;
axis square equal;
grid on;
drawnow;
end
%% Display point trajectory
plot3 (P(:,1),P(:,2),P(:,3),'r.');
axis square equal;
grid on;
xlabel ('x');
ylabel ('y');
zlabel ('z');Download
Files for the previous example can be downloaded here:
Credit: based on the Madgwick's quaternion library for Matlab.
See also
