MPU6050姿态解算2-欧拉角&旋转矩阵

% 旋转顺序：Z,Y,X（从大地坐标系到IMU坐标系）



% 定义一些符号 r=row p=pitch y=yaw

syms r p y



% 3个旋转矩阵（坐标系旋转,注意负号的位置！）

M_x = [  1,       0,      0;

         0,  cos(r), sin(r); 

         0, -sin(r), cos(r)];

  

M_y = [cos(p),   0, -sin(p);

            0,   1,       0;

       sin(p),   0,  cos(p)]; 



M_z = [ cos(y),  sin(y),  0;

       -sin(y),  cos(y),  0;

            0,       0,   1]; 

%% 推导陀螺仪的变换矩阵



%定义一些符号 drdt dpdt dydt 指的是分别对 roll pitch yaw求导，也就是角速度

syms drdt dpdt dydt



% 绕x轴转动 row 的角速度

dr_t = [drdt;

           0;

           0]; 

       

% 绕y轴转动 pitch 的角速度

dp_t = [   0;

        dpdt;

           0];

       

% 绕z轴转动 yaw 的角速度

dy_t = [   0;

           0;

        dydt]; 



% [矩阵X*矩阵Y*yaw角速度(绕Z)] + [矩阵X*pitch角速度(绕Y)] + [roll角速度(绕X)]

% IMU_gyro实际就是IMU测得的3个陀螺仪数据

IMU_gyro = M_x*M_y*dy_t + M_x*dp_t + dr_t;

fprintf('M_x*M_y*dy_t + M_x*dp_t + dr_t=\r\n')

disp(IMU_gyro)

% roll pitch yaw角速度组成的列向量，这个实际是要求的大地坐标系的3个角速度

rpy_t = [drdt;

         dpdt;

         dydt]; 



%手动分解IMU_gyro为矩阵M_gyro与列向量rpy_t相乘的形式

%根据IMU_gyro写出M_gyro，该矩阵将大地坐标系的角速度转换为IMU坐标系

M_gyro = [ 1,     0,       -sin(p);

           0, cos(r),cos(p)*sin(r);

           0,-sin(r),cos(p)*cos(r)];

%验证一下

if M_gyro * rpy_t==IMU_gyro

    fprintf('M_gyro is true\r\n');

else

    fprintf('M_gyro is false\r\n');

end

fprintf('M_gyro=\r\n')

disp(M_gyro)



% 对M_gyro求逆矩阵，用于将IMU坐标系的陀螺仪角速度值转换到大地坐标系

M_gyro_inv = inv(M_gyro);

fprintf('M_gyro_inv=\r\n')

disp(M_gyro_inv)

 

 % matlab求解的逆矩阵需要在再手工化简

M_gyro_inv_ =[ 1, (sin(p)*sin(r))/cos(p), (cos(r)*sin(p))/cos(p);

               0,                 cos(r),                -sin(r);

               0,          sin(r)/cos(p),          cos(r)/cos(p)];

 % 验证一下，M_gyro_inv_ * M_gyro_inv应该是单位矩阵

fprintf('M_gyro_inv_ * M_gyro=\r\n')

disp(M_gyro_inv_ * M_gyro)

fprintf('M_gyro_inv_ =\r\n')

disp(M_gyro_inv_)

M_x*M_y*dy_t + M_x*dp_t + dr_t=



               drdt - dydt*sin(p)

 dpdt*cos(r) + dydt*cos(p)*sin(r)

 dydt*cos(p)*cos(r) - dpdt*sin(r)

 

M_gyro is true



M_gyro=



[ 1,       0,       -sin(p)]

[ 0,  cos(r), cos(p)*sin(r)]

[ 0, -sin(r), cos(p)*cos(r)]

 

M_gyro_inv=



[ 1, (sin(p)*sin(r))/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2), (cos(r)*sin(p))/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2)]

[ 0,                        cos(r)/(cos(r)^2 + sin(r)^2),                       -sin(r)/(cos(r)^2 + sin(r)^2)]

[ 0,          sin(r)/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2),          cos(r)/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2)]

 

M_gyro_inv_ * M_gyro=



[ 1,                   0, cos(r)^2*sin(p) - sin(p) + sin(p)*sin(r)^2]

[ 0, cos(r)^2 + sin(r)^2,                                          0]

[ 0,                   0,                        cos(r)^2 + sin(r)^2]

 

M_gyro_inv_ =



[ 1, (sin(p)*sin(r))/cos(p), (cos(r)*sin(p))/cos(p)]

[ 0,                 cos(r),                -sin(r)]

[ 0,          sin(r)/cos(p),          cos(r)/cos(p)]

%% 推导加速度计的变换矩阵



M_acc=M_x*M_y*M_z;

fprintf('M_acc=\r\n')

disp(M_acc)



%重力向量

syms g

acc = [0;

       0;

       g]; 

IMU_acc=M_acc*acc;

fprintf('IMU_acc=\r\n')

disp(IMU_acc)

M_acc=



[                        cos(p)*cos(y),                        cos(p)*sin(y),       -sin(p)]

[ cos(y)*sin(p)*sin(r) - cos(r)*sin(y), cos(r)*cos(y) + sin(p)*sin(r)*sin(y), cos(p)*sin(r)]

[ sin(r)*sin(y) + cos(r)*cos(y)*sin(p), cos(r)*sin(p)*sin(y) - cos(y)*sin(r), cos(p)*cos(r)]

 

IMU_acc=



       -g*sin(p)

 g*cos(p)*sin(r)

 g*cos(p)*cos(r)

[em:24:]