线性二次调节器 Linear Quadratic Regulator (LQR)

[P, L, G] = icare(A_tilde, B, Q, R); % MATlAB 推荐使用 icare 取代 care
K = (R \ (B' * P));

[K, P, eigVals] = lqr(A_tilde, B, Q, R);

% Infinite-Horizon Continuous-Time LQR Control Simulation
clear; clc;

% --------------------------------------
% 1. System definition (continuous-time)
% --------------------------------------
A = [0 1; -2 -3];       % State matrix
B = [0; 1];             % Input matrix
Q = eye(2);             % State weighting
R = 1;                  % Input weighting

% --------------------------------------
% 2. Solve continuous-time LQR via CARE
% --------------------------------------
[K, P, eigVals] = lqr(A, B, Q, R);  % Compute optimal feedback gain

disp('LQR feedback gain K:');
disp(K);

% --------------------------------------
% 3. Simulation setup
% --------------------------------------
x0 = [1; 0];           % Initial state
t_span = [0 10];       % Simulation time interval

% --------------------------------------
% 4. Define closed-loop system dynamics
% dx/dt = (A - B*K)*x
% --------------------------------------
A_cl = A - B * K;      % Closed-loop system matrix

f = @(t, x) A_cl * x;  % Anonymous function for ODE solver

% --------------------------------------
% 5. Simulate using ODE45
% --------------------------------------
[t, X] = ode45(f, t_span, x0);  % Solve ODE

% --------------------------------------
% 6. Compute control input u(t) = -K * x(t)
% --------------------------------------
U = -X * K';           % Each row of X multiplied by -K

% --------------------------------------
% 7. Plot results
% --------------------------------------
figure;

subplot(2,1,1);
plot(t, X(:,1), 'r-', 'LineWidth', 2); hold on;
plot(t, X(:,2), 'b--', 'LineWidth', 2);
xlabel('Time t'); ylabel('State $x(t)$');
legend('$x_1$','$x_2$');
title('LQR Closed-Loop State Response');
grid on;

subplot(2,1,2);
plot(t, U, 'k-', 'LineWidth', 2);
xlabel('Time $t$'); ylabel('Control input $u(t)$');
title('LQR Control Input');
grid on;

% Finite-Horizon Continuous-Time LQR Simulation
clear; clc;

% --------------------------------------
% 1. System definition (continuous-time)
% --------------------------------------
A = [0 1; -2 -3];       % State matrix
B = [0; 1];             % Input matrix
Q = eye(2);             % State weighting
R = 1;                  % Input weighting
S = 10 * eye(2);        % Terminal state cost matrix

t0 = 0; tf = 10;        % Time interval
x0 = [1; 0];            % Initial state

% --------------------------------------
% 2. Solve Riccati Differential Equation backward in time
% --------------------------------------
% Convert matrix P to vector for ODE solver (column-stacked)
P_final = S;
p0 = reshape(P_final, [], 1);  % Vectorize P(t_f)

% Riccati ODE: dP/dt = -A'*P - P*A + P*B*inv(R)*B'*P - Q
riccati_ode = @(t, p) ...
    -reshape(A'*reshape(p,2,2) + reshape(p,2,2)*A ...
    - reshape(p,2,2)*B*(R\B')*reshape(p,2,2) + Q, [], 1);

% Integrate backward in time
[t_back, p_sol] = ode45(riccati_ode, linspace(tf, t0, 100), p0);

% Flip time to increasing order
t_vec = flipud(t_back);
P_t = flipud(p_sol);

% Interpolate K(t) at arbitrary time
K_func = @(t) ...
    (B' * reshape(interp1(t_vec, P_t, t, 'linear', 'extrap'), 2, 2)) / R;


% --------------------------------------
% 3. Define closed-loop dynamics with time-varying feedback
% dx/dt = (A - B*K(t)) * x
% --------------------------------------
f = @(t, x) (A - B * K_func(t)) * x;

% --------------------------------------
% 4. Simulate system dynamics
% --------------------------------------
[t_sim, X] = ode45(f, [t0 tf], x0);

% Compute u(t) = -K(t) * x(t)
U = zeros(length(t_sim),1);
for i = 1:length(t_sim)
    Kt = K_func(t_sim(i));
    U(i) = -Kt * X(i,:)';
end

% --------------------------------------
% 5. Plot results
% --------------------------------------
figure;

subplot(2,1,1);
plot(t_sim, X(:,1), 'r-', 'LineWidth', 2); hold on;
plot(t_sim, X(:,2), 'b--', 'LineWidth', 2);
xlabel('Time $t$'); ylabel('State $x(t)$');
legend('$x_1$','$x_2$');
title('Finite-Horizon LQR State Response');
grid on;

subplot(2,1,2);
plot(t_sim, U, 'k-', 'LineWidth', 2);
xlabel('Time $t$'); ylabel('Control input $u(t)$');
title('Finite-Horizon LQR Control Input');
grid on;

% Discrete-Time Infinite-Horizon LQR Simulation
clear; clc;

% --------------------------------------
% 1. System definition (discrete-time)
% --------------------------------------
A = [1 1; 0 1];       % State transition matrix
B = [0; 1];           % Input matrix

Q = eye(2);           % State weighting matrix
R = 1;                % Control weighting scalar

% --------------------------------------
% 2. Solve Discrete-Time Algebraic Riccati Equation (DARE)
% --------------------------------------
[K, P, eigVals] = dlqr(A, B, Q, R);   % Compute LQR gain

disp('LQR gain K:');
disp(K);

% --------------------------------------
% 3. Simulation setup
% --------------------------------------
x = [2; 0];           % Initial state
N = 20;               % Number of time steps

X = zeros(2, N+1);    % State trajectory storage
U = zeros(1, N);      % Control input storage
X(:,1) = x;           % Set initial state

% --------------------------------------
% 4. Closed-loop simulation
% x_{k+1} = (A - B*K)*x_k
% --------------------------------------
for k = 1:N
    u = -K * x;                      % Compute control input
    x = A * x + B * u;               % Apply closed-loop dynamics
    X(:,k+1) = x;                    % Store new state
    U(k) = u;                        % Store control input
end

% --------------------------------------
% 5. Plot results
% --------------------------------------
figure;

% Plot state trajectories
subplot(2,1,1);
plot(0:N, X(1,:), 'r-s', 0:N, X(2,:), 'b-*');
legend('$x_1$','$x_2$');
xlabel('Time step $k$');
ylabel('State $x_k$');
title('LQR Closed-Loop State Trajectory');
grid on;

% Plot control input
subplot(2,1,2);
stairs(0:N-1, U, 'k-', 'LineWidth', 2);
xlabel('Time step $k$');
ylabel('Control input $u_k$');
title('LQR Control Input');
grid on;

% Finite-Horizon Discrete-Time LQR Simulation
clear; clc;

% --------------------------------------
% 1. System definition
% --------------------------------------
A = [1 1; 0 1];       % State transition matrix
B = [0; 1];           % Input matrix

Q = eye(2);           % State cost
R = 1;                % Control cost
S = 10 * eye(2);      % Terminal state cost

N = 20;               % Time horizon
x0 = [2; 0];          % Initial state

% --------------------------------------
% 2. Backward Riccati recursion
% --------------------------------------
P = cell(N+1,1);      % Store P matrices
K = cell(N,1);        % Store feedback gains

P{N+1} = S;           % Terminal cost

for k = N:-1:1
    Pk1 = P{k+1};
    Kk = (R + B' * Pk1 * B) \ (B' * Pk1 * A);
    K{k} = Kk;
    P{k} = Q + A' * Pk1 * A - A' * Pk1 * B * Kk;
end

% --------------------------------------
% 3. Forward simulation with time-varying K_k
% --------------------------------------
X = zeros(2, N+1);
U = zeros(1, N);

x = x0;
X(:,1) = x;

for k = 1:N
    u = -K{k} * x;
    x = A * x + B * u;
    X(:,k+1) = x;
    U(k) = u;
end

% --------------------------------------
% 4. Plot results
% --------------------------------------
figure;

% State trajectories
subplot(2,1,1);
plot(0:N, X(1,:), 'r-s', 'LineWidth', 1); hold on;
plot(0:N, X(2,:), 'b-*', 'LineWidth', 1);
xlabel('Time step $k$'); ylabel('State');
legend('$x_1$','$x_2$');
title('Finite-Horizon LQR State Trajectory');
grid on;

% Control input
subplot(2,1,2);
stairs(0:N-1, U, 'k-', 'LineWidth', 2);
xlabel('Time step $k$'); ylabel('Control input $u_k$');
title('Finite-Horizon LQR Control Input');
grid on;