mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-07-22 06:01:23 +00:00
Resolve "Implement CI/CD"
This commit is contained in:
30
matlabHelpers/LQR.m
Normal file
30
matlabHelpers/LQR.m
Normal file
@@ -0,0 +1,30 @@
|
||||
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)
|
Reference in New Issue
Block a user