線性二次型調節器LQR/LQC算法解析及求解器代碼（matlab）

參考鏈接：http://120.52.51.14/stanford.edu/class/ee363/lectures/dlqr.pdf

本文參考講義中的第20頁PPT，根據Hamilton-Jacobi方法，推導得到黎卡提方程的數值迭代求解方法（可實時在線求解黎卡提方程），具體推導過程請參考PPT。本文列出最后的結論及對應的matlab代碼，其他編程語言也可參考貼出的代碼自行改編。

對應的matlab代碼如下：

%%%參考文獻dlqr
close all
A=[1 1;0 1];
B=[0;1];
C=[1 0];
Q=C'*C;
Qf=C'*C;
R=10*ones(1,1);
[p,L,k] = dare(A,B,Q,R)

x0=[100;10]*1e-2;
N=20;%每一時間步求解器迭代次數，一般20足夠，若不收斂，則適當增大該值

P=zeros(2,2,N+1);

P(:,:,N+1)=Qf;

for t=2:(N+1)
    tUsed=(N+3-t);
    P(:,:,tUsed-1)=Q+A'*P(:,:,tUsed)*A-A'*P(:,:,tUsed)*B/(R+B'*P(:,:,tUsed)*B)*B'*P(:,:,tUsed)*A;
end
s=P(:,:,1)

K=zeros(N,2);
U=zeros(N,1);
X=zeros(N+1,2);
X(1,:)=x0;
for t=1:N
    K(t,:)=-(R+B'*P(:,:,t+1)*B)\B'*P(:,:,t+1)*A;
    U(t,:)=K(t,:)*X(t,:)';
    X(t+1,:)=(A*X(t,:)'+B*U(t,:))';
end
k=K(1,:)

figure (30)
PlotX=1:N;
plot(PlotX,K(:,1),'o-b');
hold on
plot(PlotX,K(:,2),'o-r');
title('反饋系數K')

figure (40)
PlotX=1:N+1;
plot(PlotX,X(:,1),'o-b');
hold on
plot(PlotX,X(:,2),'o-r');
title('狀態X')

figure (50)
PlotX=1:N;
plot(PlotX,U(:,1),'o-b');
title('控制輸入U')