mirror of
https://github.com/ME-561-W20-Quadcopter-Project/Quadcopter-Control.git
synced 2025-09-03 22:13:15 +00:00
Merge branch 'nonlinear' into infinite_LQR
This commit is contained in:
Sravan Balaji
BIN
src/LQRSim_Goal_1.slx
Normal file
BIN
src/LQRSim_Goal_1.slx
Normal file
Binary file not shown.
156
src/LQR_Goal_1.m
Normal file
156
src/LQR_Goal_1.m
Normal file
@@ -0,0 +1,156 @@
|
|||||||
|
% Clear workspace
|
||||||
|
clear all; close all; clc;
|
||||||
|
|
||||||
|
% Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf
|
||||||
|
g = 9.81; m = 0.468; Ix = 4.856*10^-3;
|
||||||
|
Iy = 4.856*10^-3; Iz = 8.801*10^-3;
|
||||||
|
|
||||||
|
% States:
|
||||||
|
% X1: x X4: x'
|
||||||
|
% X2: y X5: y'
|
||||||
|
% X3: z X6: z'
|
||||||
|
% X7: Pitch angle (x-axis) X10: Pitch rate (x-axis)
|
||||||
|
% X8: Roll angle (y-axis) X11: Roll rate (y-axis)
|
||||||
|
% X9: Yaw angle (z-axis) X12: Yaw rate (z-axis)
|
||||||
|
|
||||||
|
% Inputs: Outputs:
|
||||||
|
% U1: Total Upward Force (along z-axis) Y1: Position along x axis
|
||||||
|
% U2: Pitch Torque (about x-axis) Y2: Position along y axis
|
||||||
|
% U3: Roll Torque (about y-axis) Y3: Position along z axis
|
||||||
|
% U4: Yaw Torque (about z-axis) Y4: Pitch (about x-axis)
|
||||||
|
% Y5: Roll (about y-axis)
|
||||||
|
% Y6: Yaw (about z-axis)
|
||||||
|
|
||||||
|
% State Space Source: https://arxiv.org/ftp/arxiv/papers/1908/1908.07401.pdf
|
||||||
|
% X' = Ax + Bu
|
||||||
|
% Y = Cx
|
||||||
|
|
||||||
|
nStates = 12;
|
||||||
|
nInputs = 4;
|
||||||
|
nOutputs = 6;
|
||||||
|
|
||||||
|
A = [0 0 0 1 0 0 0 0 0 0 0 0;
|
||||||
|
0 0 0 0 1 0 0 0 0 0 0 0;
|
||||||
|
0 0 0 0 0 1 0 0 0 0 0 0;
|
||||||
|
0 0 0 0 0 0 0 -g 0 0 0 0;
|
||||||
|
0 0 0 0 0 0 g 0 0 0 0 0;
|
||||||
|
0 0 0 0 0 0 0 0 0 0 0 0;
|
||||||
|
0 0 0 0 0 0 0 0 0 1 0 0;
|
||||||
|
0 0 0 0 0 0 0 0 0 0 1 0;
|
||||||
|
0 0 0 0 0 0 0 0 0 0 0 1;
|
||||||
|
0 0 0 0 0 0 0 0 0 0 0 0;
|
||||||
|
0 0 0 0 0 0 0 0 0 0 0 0;
|
||||||
|
0 0 0 0 0 0 0 0 0 0 0 0];
|
||||||
|
|
||||||
|
% Note: In paper, 1/m is in wrong spot
|
||||||
|
B = [0 0 0 0;
|
||||||
|
0 0 0 0;
|
||||||
|
0 0 0 0;
|
||||||
|
0 0 0 0;
|
||||||
|
0 0 0 0;
|
||||||
|
1/m 0 0 0;
|
||||||
|
0 0 0 0;
|
||||||
|
0 0 0 0;
|
||||||
|
0 0 0 0;
|
||||||
|
0 1/Ix 0 0;
|
||||||
|
0 0 1/Iy 0;
|
||||||
|
0 0 0 1/Iz];
|
||||||
|
|
||||||
|
C = [1 0 0 0 0 0 0 0 0 0 0 0;
|
||||||
|
0 1 0 0 0 0 0 0 0 0 0 0;
|
||||||
|
0 0 1 0 0 0 0 0 0 0 0 0;
|
||||||
|
0 0 0 0 0 0 1 0 0 0 0 0;
|
||||||
|
0 0 0 0 0 0 0 1 0 0 0 0;
|
||||||
|
0 0 0 0 0 0 0 0 1 0 0 0];
|
||||||
|
|
||||||
|
D = zeros(6,4);
|
||||||
|
|
||||||
|
continuous = ss(A, B, C, D);
|
||||||
|
T_s = 0.05;
|
||||||
|
discrete = c2d(continuous, T_s);
|
||||||
|
|
||||||
|
%Check if this works
|
||||||
|
impulse(discrete, 0:T_s:1);
|
||||||
|
|
||||||
|
%We should see that U1 gets us only translation in z, U2 couples Y2 and Y4,
|
||||||
|
%U3 couples Y1 and Y5, and U4 gets us Y6
|
||||||
|
|
||||||
|
%% Define goals
|
||||||
|
% Desired position
|
||||||
|
x_d = 0;
|
||||||
|
y_d = 0;
|
||||||
|
z_d = 0;
|
||||||
|
|
||||||
|
%Goal 1: settle at 1m height <2s
|
||||||
|
x_0_up = [0, 0, -1, ...
|
||||||
|
0, 0, 0, ...
|
||||||
|
0, 0, 0, ...
|
||||||
|
0, 0, 0]'; %Redefine origin!
|
||||||
|
|
||||||
|
%Goal 2: Stabilize from a 10-degree roll and pitch with <3deg overshoot
|
||||||
|
x_0_pitch = [0, 0, 0, ...
|
||||||
|
0, 0, 0, ...
|
||||||
|
10, 0, 0, ...
|
||||||
|
0, 0, 0]'; %Pitch of 10 degrees
|
||||||
|
|
||||||
|
x_0_roll = [0, 0, 0, ...
|
||||||
|
0, 0, 0, ...
|
||||||
|
0, 10, 0, ...
|
||||||
|
0, 0, 0]'; %Roll of 10 degrees
|
||||||
|
|
||||||
|
%Goal 3: Move from position (0,0,0) to within 5 cm of (1,1,1) within 5 seconds.
|
||||||
|
x_0_trans = [-1, -1, -1, ...
|
||||||
|
0, 0, 0, ...
|
||||||
|
0, 0, 0, ...
|
||||||
|
0, 0, 0]'; %Redefine origin!
|
||||||
|
|
||||||
|
%Define Q and R for the cost function. Begin with nominal ones for all.
|
||||||
|
Q = diag([1000, 1000, 1000, ... % x, y, z
|
||||||
|
1, 1, 100, ... % x', y', z'
|
||||||
|
100, 100, 1, ... % roll, pitch, yaw
|
||||||
|
1, 1, 1]); % roll', pitch', yaw'
|
||||||
|
|
||||||
|
R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque
|
||||||
|
%% Finite-Time Horizon LQR for Goal 1
|
||||||
|
|
||||||
|
%Calculate number of timesteps.
|
||||||
|
tSpan = 0:T_s:2;
|
||||||
|
nSteps = length(tSpan);
|
||||||
|
|
||||||
|
%Determine gains
|
||||||
|
[K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps);
|
||||||
|
|
||||||
|
%Set up for propagation
|
||||||
|
ulqr = zeros(nInputs, nSteps);
|
||||||
|
xlqr = zeros(nStates, nSteps);
|
||||||
|
xlqr(:, 1) = x_0_up;
|
||||||
|
|
||||||
|
for i = 1:(nSteps - 1)
|
||||||
|
ulqr(:,i) = K(:,:,i) * xlqr(:,i);
|
||||||
|
xlqr(:,i+1) = (A*xlqr(:, i) - B*ulqr(:, i));
|
||||||
|
end
|
||||||
|
|
||||||
|
figure(10);
|
||||||
|
hold on;
|
||||||
|
plot(tSpan, ulqr(1,:), 'b-');
|
||||||
|
plot(tSpan, ulqr(2,:), 'r-');
|
||||||
|
plot(tSpan, ulqr(3,:), 'g-');
|
||||||
|
plot(tSpan, ulqr(4,:), 'y-');
|
||||||
|
|
||||||
|
%% Helper Functions
|
||||||
|
|
||||||
|
function [K, P] = LQR_LTI(A, B, Q, R, nSteps)
|
||||||
|
%Set P up
|
||||||
|
P = zeros(size(Q, 1), size(Q, 2), nSteps);
|
||||||
|
%Initial value of P
|
||||||
|
P(:, :, nSteps) = 1/2 * Q;
|
||||||
|
%Set K up, initial K is 0, so this is fine.
|
||||||
|
K = zeros(length(R), length(Q), nSteps);
|
||||||
|
|
||||||
|
for i = nSteps-1:-1:1
|
||||||
|
P_ = P(:,:, i+1);
|
||||||
|
|
||||||
|
K(:, :, i) = ( 1/2 * R + B' * P_ * B )^(-1) * B' * P_ * A;
|
||||||
|
P(:, :, i) = A' * P_ * ( A - B * K(:, :, i) ) + Q * 1/2;
|
||||||
|
end
|
||||||
|
end
|
||||||
Reference in New Issue
Block a user