Is the "kinematicTrajectory" function correct for calculating angular velocity?

Hello there,

I noticed that the “kinematicTrajectory” function calculates angular velocity from the body frame to the earth frame using the “rotatepoint” function. The “rotatepoint” function can be formulated with the Rzyx rotation matrix.

where ϕis roll, θ is pitch, ψis yaw.

However, some papers [1] and references [2] use the transfer matrix T.

Does anyone know which is correct?

Or perhaps I misunderstood something. Could someone correct any misunderstandings I have?

[1] Emran, Bara J., and Homayoun Najjaran. "A review of quadrotor: An underactuated mechanical system." Annual Reviews in Control 46 (2018): 165-180.

[2] KINEMATICS OF MOVING FRAMES. [Online]. Available: https://ocw.mit.edu/courses/2-017j-design-of-electromechanical-robotic-systems-fall-2009/0da9fb3965410fd50979bb179b56805a_MIT2_017JF09_ch09.pdf.

Here is the sample code.

clear; clc; close all;

%% declare
% sim
Fs = 400;
dt = 1/Fs;
t = 0:dt:5;
lenT = length(t);

% trajectory
gyro_body_roll = deg2rad(0) * ones(lenT,1);
gyro_body_pitch = deg2rad(0) * ones(lenT,1);
gyro_body_yaw = deg2rad(45) * square(2*pi*1*t)';
ori_IC = deg2rad([0 30 0]);

%% sim IMU trajectory
acc_body = zeros(lenT,3);
gyro_body = [gyro_body_roll gyro_body_pitch gyro_body_yaw];
quat_IC = quaternion( eul2quat(ori_IC,"ZYX") );
traj = kinematicTrajectory('SampleRate',Fs,'Orientation',quat_IC);
[~,quat_earth,~,~,gyro_earth] = traj(acc_body,gyro_body);

%% 
for i = 1:lenT
    ori_earth = quat2eul(quat_earth(i),'ZYX');
    yaw = ori_earth(1);
    pitch = ori_earth(2);
    roll = ori_earth(3);

    T = [1 sin(roll)*tan(pitch) cos(roll)*tan(pitch);
         0 cos(roll) -sin(roll);
         0 sin(roll)/cos(pitch) cos(roll)/cos(pitch)];

    rotm = eul2rotm(ori_earth,"ZYX");

    gyro_earth_T(i,:) = T * gyro_body(i,:)';
    gyro_earth_Rzyx(i,:) = rotm * gyro_body(i,:)';
end


%% plot angular vel from kinematicTrajectory
figure;
plot(t,rad2deg(gyro_earth))
title('Angular vel in earth frame from "kinematicTrajectory"')
ylabel('deg/s')
legend({'roll','pitch','yaw'})
xlabel('time (sec)')
grid('on')

%% plot angular vel from T matrix
figure;
plot(t,rad2deg(gyro_earth_T))
title('Angular vel in earth frame from "T matrix"')
ylabel('deg/s')
legend({'roll','pitch','yaw'})
xlabel('time (sec)')
grid('on')

Answer 1

[Paul]

Hi Erik,

The code is comparing two different quantities:

Here, T is the matrix that mutliplies the angular velocity vector resolved in body coordinates and compute a vector of Euler angle rates. The vector of Euler angle rates, or what is called gyro_earth, is defined as

gyro_earth_T = [roll_dot; pitch_dot; yaw_dot]

    yaw = ori_earth(1);
    pitch = ori_earth(2);
    roll = ori_earth(3);
  
    T = [1 sin(roll)*tan(pitch) cos(roll)*tan(pitch);
         0 cos(roll) -sin(roll);
         0 sin(roll)/cos(pitch) cos(roll)/cos(pitch)];
    rotm = eul2rotm(ori_earth,"ZYX");
    gyro_earth_T(i,:) = T * gyro_body(i,:)';

This line takes the angular velocity vector resolved in body coordinates (gyro_body) and computes the components of the angular velocity vector in earth coordinates (gyro_earth_Rzyx).

    gyro_earth_Rzyx(i,:) = rotm * gyro_body(i,:)';

In short, we have three different sets of numbers: Euler angle rates, angular velocity components resolved in body, and angular velocity components resolved in earth. In general, none of these are the same as the others.

[Umar]

Thanks Paul, great teamwork.

[Eries erik]

Thank you very much, Paul, for pointing out that the Euler angle rates and the angular velocity in the earth frame are different things in my example code. I had indeed confused the two.

To obtain the angular velocity in the earth frame, one needs to use the rotation matrix Rzyx for rotation.

However, if the goal is to calculate the Euler angles, one should use the Euler angle rates.

Answer 2

[Umar]

In Matlab, the choice between using the Rzyx rotation matrix and the transfer matrix T for calculating angular velocity in the "kinematicTrajectory" function depends on the specific application and the conventions followed. Both methods are valid but may be used in different contexts. It is essential to refer to the specific implementation details provided in the papers [1] and references [2] to determine which approach aligns with the requirements of your project. Understanding the underlying principles of each method will help clarify any potential misunderstandings and ensure the accurate computation of angular velocity in your application.