mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 15:17:23 +00:00
24 lines
441 B
Mathematica
24 lines
441 B
Mathematica
clear all; close all; clc;
|
|
|
|
syms I11 I22 I33 I11dot I22dot I33dot
|
|
|
|
I = [I11 0 0; 0 I22 0; 0 0 I33];
|
|
Idot = [I11dot 0 0; 0 I22dot 0; 0 0 I33dot];
|
|
|
|
syms yaw pitch roll yawdot pitchdot rolldot yawddot pitchddot rollddot
|
|
|
|
w = [yawdot; pitchdot; rolldot];
|
|
|
|
Iw = I * w;
|
|
Idw = Idot * w;
|
|
|
|
C = cross(w, Iw);
|
|
|
|
syms momentX momentY momentZ
|
|
|
|
M = [momentX; momentY; momentZ];
|
|
A = M - Idw - C;
|
|
B = [A(1) A(2) A(3)];
|
|
|
|
X = B*inv(I);
|
|
E = [X(1); X(2); X(3)] |