三轴陀螺仪数据转换姿态角，高动态作业matlab

数据源为.mat文件，点击这里免费下载

% 基于等效旋转矢量姿态更新算法 解算三轴陀螺仪的固连载体位姿，画出得到的三个姿态角

delta0 = [0, 0, 0]; %上一周期角增量
delta = [0, 0, 0];  %当前周期角增量
Q = [1, 0, 0, 0];   % 姿态四元数初始值
q = [0, 0, 0, 0];   % 姿态变化四元数初始值
eular = [0, 0, 0];  %用来保存Q对应的姿态角
load('gyro_data.mat');

% 求出欧拉角随时间的变化（固定轴欧拉角）
for i = 1:6950          % 数组索引必须为正整数或逻辑值,所以i从1开始
    t(i) = i/1000;      % 采样频率1kHz, 画图用

    % 当前时刻角增量delta
    delta_roll  = gyro_data(i,1) * pi /180;	% x翻滚角 弧度
    delta_yaw   = gyro_data(i,2) * pi /180;	% y偏航角 弧度
    delta_pitch = gyro_data(i,3) * pi /180;	% z俯仰角 弧度
    delta = [delta_roll, delta_yaw, delta_pitch];

    % 旋转矢量fai (单子样+前一周期字样)
    Fai = delta + cross(delta0, delta) / 12;     % 3维旋转向量fai

    % 旋转矢量fai的模，也就是旋转角度theta大小
    theta = sqrt( sum(Fai.*Fai) );  %也可以直接用二范数 norm(Fai,2)

    % 由旋转矢量 计算姿态变化四元数
%    q_w = cos(theta/2);    //对计算公式理解不对
%    q_i = cos(theta/2) + Fai(1) / theta * sin(theta/2);
%    q_j = cos(theta/2) + Fai(2) / theta * sin(theta/2);
%    q_k = cos(theta/2) + Fai(3) / theta * sin(theta/2);
    q_w = cos(theta/2);
    q_i = Fai(1) / theta * sin(theta/2);
    q_j = Fai(2) / theta * sin(theta/2);
    q_k = Fai(3) / theta * sin(theta/2);
    q = [q_w, q_i, q_j, q_k];	%构建旋转四元数

    % 四元数乘法实现方法1
    Q = quatmultiply(Q, q);
    % 四元数乘法实现方法2 -自定义函数
%     Q = q1oq2(Q, q);
    % 四元数乘法实现方法3 -反对称矩阵+矩阵乘法
%     AntisA = quat2Antisymmetry(Q(1), Q(2), Q(3), Q(4))    % Q的反对称矩阵
%     Q = transpose (AntisA * transpose(q));

    %四元数转为欧拉角,绘制姿态角
    eular = quat2eul(Q);

    % 记录载体历史姿态
    eul(i,1) = eular(1) *180 /pi;  % x-roll 累积角度随时间变化曲线
    eul(i,2) = eular(2) *180 /pi;  % y-yaw  累积角度随时间变化曲线
    eul(i,3) = eular(3) *180 /pi;  % z-pitch累积角度随时间变化曲线
    angle(i,1) = Fai(1) *180 /pi;  % x-roll增量随时间变化曲线
    angle(i,2) = Fai(2) *180 /pi;  % y-yaw 增量随时间变化曲线
    angle(i,3) = Fai(3) *180 /pi;  % z-pitch增量随时间变化曲线

    % 上一周期变量
    delta_roll0 = delta_roll;
    delta_yaw0  = delta_yaw;
    delta_pitch0= delta_pitch;
end

t = t - 0.001;	% i从1开始，初始时刻是从0，要减1
figure(1);
subplot(6,1,1); plot(t, eul(:,1), 'r'); title('x-roll /°'); grid on;
subplot(6,1,2); plot(t, eul(:,2), 'g'); title('y-yaw /°'); grid on;
subplot(6,1,3); plot(t, eul(:,3), 'b'); title('z-pitch /°'); grid on;
subplot(6,1,4); plot(t, angle(:,1), 'r'); title('x-roll 增量随时间变化曲线 /°'); grid on;
subplot(6,1,5); plot(t, angle(:,2), 'g'); title('y-yaw 增量随时间变化曲线 /°'); grid on;
subplot(6,1,6); plot(t, angle(:,3), 'b'); title('z-pitch 增量随时间变化曲线 /°'); grid on;

% 四元数乘法实现 方法2-直接返回结果
function Q_ = q1oq2(q1, q2)
    a1 = q1(1)*q2(1) - q1(2)*q2(2) - q1(3)*q2(3) - q1(4)*q2(4);
    b1 = q1(1)*q2(2) + q1(2)*q2(1) + q1(3)*q2(4) - q1(4)*q2(3);
    c1 = q1(1)*q2(3) + q1(3)*q2(1) + q1(4)*q2(2) - q1(2)*q2(4);
    d1 = q1(1)*q2(4) + q1(4)*q2(1) + q1(2)*q2(3) - q1(3)*q2(2);
    Q_ = [a1, b1, c1, d1];
end

% 四元数乘法实现 方法3-求向量q的反对称矩阵，再借助矩阵乘法计算，调用见主函数
function Aq_ = quat2A(qw, qi, qj, qk)
Aq_ = [ qw -qi -qj -qk;
       qi  qw -qk  qj;
       qj  qk  qw -qi;
       qk -qj  qi  qw ];
end