zoukankan      html  css  js  c++  java
  • matlab练习程序(旋转矩阵、欧拉角、四元数互转)

    欧拉角转旋转矩阵公式:

    旋转矩阵转欧拉角公式:

    旋转矩阵转四元数公式,其中1+r11+r22+r33>0:

    四元数转旋转矩阵公式,q0^2+q1^2+q2^2+q3^2=1:

    欧拉角转四元数公式:

    四元数转欧拉角公式:

    matlab代码如下:

    clear all;
    close all;
    clc;
    
    %欧拉角
    x = 0.5;
    y = 0.6;
    z = 0.7;
    Ang1 = [x y z];
    
    %欧拉角转旋转矩阵
    Rx = [1      0      0;
        0 cos(x) -sin(x);
        0 sin(x) cos(x)];
    Ry = [cos(y)  0 sin(y);
        0       1      0;
        -sin(y) 0 cos(y)];
    Rz = [cos(z) -sin(z) 0;
        sin(z) cos(z)  0;
        0      0       1];
    R = Rz*Ry*Rx;
    R1 = R;
    
    %旋转矩阵转欧拉角
    x = atan2(R(3,2),R(3,3));
    y = atan2(-R(3,1),sqrt(R(3,2)^2+R(3,3)^2));
    z = atan2(R(2,1),R(1,1));
    Ang2 = [x y z];
    
    %旋转矩阵转四元数
    t=sqrt(1+R(1,1)+R(2,2)+R(3,3))/2;
    q=[t (R(3,2)-R(2,3))/(4*t) (R(1,3)-R(3,1))/(4*t) (R(2,1)-R(1,2))/(4*t)];
    Q1 = q;
    
    %四元数转旋转矩阵
    R=[ 2*q(1).^2-1+2*q(2)^2    2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3));
        2*(q(2)*q(3)+q(1)*q(4)) 2*q(1)^2-1+2*q(3)^2     2*(q(3)*q(4)-q(1)*q(2));
        2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) 2*q(1)^2-1+2*q(4)^2];
    R2 = R;
    
    %欧拉角转四元数
    q = [cos(x/2)*cos(y/2)*cos(z/2) + sin(x/2)*sin(y/2)*sin(z/2) ...
        sin(x/2)*cos(y/2)*cos(z/2) - cos(x/2)*sin(y/2)*sin(z/2) ...
        cos(x/2)*sin(y/2)*cos(z/2) + sin(x/2)*cos(y/2)*sin(z/2) ...
        cos(x/2)*cos(y/2)*sin(z/2) - sin(x/2)*sin(y/2)*cos(z/2)];
    Q2 = q;
    
    %四元数转欧拉角
    x = atan2(2*(q(1)*q(2)+q(3)*q(4)),1 - 2*(q(2)^2+q(3)^2));
    y = asin(2*(q(1)*q(3) - q(2)*q(4)));
    z = atan2(2*(q(1)*q(4)+q(2)*q(3)),1 - 2*(q(3)^2+q(4)^2));
    Ang3 = [x y z];
    
    Ang1
    Ang2
    Ang3
    
    R1
    R2
    
    Q1
    Q2

    参考:

    https://blog.csdn.net/lql0716/article/details/72597719

    https://www.cnblogs.com/21207-iHome/p/6894128.html

  • 相关阅读:
    配置别名
    Git永久删除commit--[非教程]
    忽略特殊文件
    自定义Git
    使用GitHub
    模块化开发的几种思想AMD,CMD,commonJS,es6
    angualr设置select默认值
    在vue-cli中引入jquery的坑
    chromedriver@2.33.2 install: `node install.js` vue脚手架安装报错
    vue事件绑定demo
  • 原文地址:https://www.cnblogs.com/tiandsp/p/10733607.html
Copyright © 2011-2022 走看看