mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 23:26:43 +00:00
30 lines
455 B
Mathematica
30 lines
455 B
Mathematica
clear all; clc;
|
|
|
|
syms R11 R22 R33
|
|
R = [R11 0 0;
|
|
0 R22 0;
|
|
0 0 R33];
|
|
|
|
syms I33
|
|
Q = eye(6) * I33;
|
|
|
|
syms F11 F22 F33
|
|
F = [F11 0 0;
|
|
0 F22 0
|
|
0 0 F33];
|
|
|
|
syms G13 G31
|
|
G = [0 0 G13;
|
|
0 0 0;
|
|
G31 0 0];
|
|
|
|
syms I11 I22 d
|
|
I = [I11 0 0;
|
|
0 I22 0;
|
|
0 0 I33];
|
|
|
|
A = [zeros(3,3), d * eye(3); F, G];
|
|
B = [zeros(3,3); inv(I)];
|
|
P = [-Q, -A'] * pinv([A, -B*inv(R)*B']);
|
|
|
|
K = simplify((R^-1) * B' * P) |