matlab求解黎卡提代数方程_LQR

三种方法求解：

方法一：迭代求解代码——保存文件名“mylqr”。

%% 利用MATLAB求解黎卡提代数方程
% 方法一：简单代数迭代法
% 令fai_0=0,则可以写出如下的迭代公式。
% fai_(i+1)=E'*fai_i*E-(E'*fai_i*G+W)*(G'*fai_i*G+H)^(-1)*(E'*fai_i*G+W+Q);
% E=(I-A)^(-1)*(I+A);
% G=2*(I-A)^(-1)*B;
% H=R+B'*(I-A')^(-1)*Q*(I-A)^(-1)*B;
% W=Q*(I-A)^(-1)*B; 
% 如果fai_(i+1)收敛于一个常数矩阵，即||fai_(i+1)-fai_i||<kesai，则可得黎卡提代数方程解为：
% P=2*(I-A')^(-1)*fai_(i+1)*(I-A)^(-1);
%% MATLAB代码实现
%%%%%%%%%%%%%matlab程序%%%%%%%%%%%%%%
I=eye(size(A));
iA=inv(I-A);%iA=(I-A)^(-1);
E=iA*(I+A);
G=2*iA^2*B;
H=R+B'*iA'*Q*iA*B;
W=Q*iA*B;
P0=zeros(size(A));
i=0;
while(1)
    i=i+1;
    P=E'*P0*E-(E'*P0*G+W)*inv(G'*P0*G+H)*(E'*P0*G+W)'+Q;
    if (norm(P-P0)<eps)
        break;
    else
        P0=P;
    end
end
P=2*iA'*P*iA;
%%%%%%*************************

方法二：matlab工具箱

[K,P,E]=lqr(A,B,Q,R)

方法三：

[P,E,K,RR]=care(A,B,Q,R,zeros(size(B)),eye(size(A)))
% % 如果设置了P的终值条件，则只能使用方法三care（），
% % 假设终值【0.2；0.2】，则：
% [P,E,K,RR]=care(A,B,Q,R,[0.2;0.2],eye(size(A)))

测试验证：——新建.m文件。

clear,clc;
A=[0 1;-5 -3];
B=[0;1];
Q=[500 200;200 100];
R=1.6667;
% %% 方法一：
mylqr
K=inv(R)*B'*P
P
E
% %% 方法二
% [K,P,E]=lqr(A,B,Q,R)
% %% 方法三
% [P,E,K,RR]=care(A,B,Q,R,zeros(size(B)),eye(size(A)))
% % 如果设置了P的终值条件，则只能使用方法三care（），
% % 假设终值【0.2；0.2】，则：
% [P,E,K,RR]=care(A,B,Q,R,[0.2;0.2],eye(size(A)))

改进后（XXXX）：

clear；clc;
A=[0 1;-5 -3];
B=[0;1];
Q=[500 200;200 100];
R=1.6667;
% %% 方法一：
% mylqr
P = diedai(A,B,Q,R);
K=R^(-1)*B'*P
P
% %% 方法二
% [K,P,E]=lqr(A,B,Q,R)
% %% 方法三
% [P,E,K,RR]=care(A,B,Q,R,zeros(size(B)),eye(size(A)))
% % 如果设置了P的终值条件，则只能使用方法三care（），
% % 假设终值【0.2；0.2】，则：
% [P,E,K,RR]=care(A,B,Q,R,[0.2;0.2],eye(size(A)))

%% 方法一，迭代求解
function P = diedai(A,B,Q,R)
%% MATLAB代码实现
%%%%%%%%%%%%%matlab程序%%%%%%%%%%%%%%
epslion = 1e-10;%或者epslion = eps;
I=eye(size(A));
%iA=inv(I-A);
iA=(I-A)^(-1);  %inv(X)矩阵的逆矩阵
E=iA*(I+A);
G=2*iA*B;
H=R+B'*iA'*Q*iA*B;
W=Q*iA*B;
P0=zeros(size(A));%预先产生空矩阵
while(1)
    P=E'*P0*E-(E'*P0*G+W)*(G'*P0*G+H)^(-1)*(E'*P0*G+W)'+Q;
    if (norm(P-P0) < epslion)
        break;
    else
        P0=P;
    end
end
P=2*iA'*P*iA;

%%%%%%%%%%*************************
%% 利用MATLAB求解黎卡提代数方程
% 方法一：简单代数迭代法
% 令fai_0=0,则可以写出如下的迭代公式。
% fai_(i+1)=E'*fai_i*E-(E'*fai_i*G+W)*(G'*fai_i*G+H)^(-1)*(E'*fai_i*G+W+Q);
% E=(I-A)^(-1)*(I+A);
% G=2*(I-A)^(-1)*B;
% H=R+B'*(I-A')^(-1)*Q*(I-A)^(-1)*B;
% W=Q*(I-A)^(-1)*B; 
% 如果fai_(i+1)收敛于一个常数矩阵，即||fai_(i+1)-fai_i||<kesai，则可得黎卡提代数方程解为：
% P=2*(I-A')^(-1)*fai_(i+1)*(I-A)^(-1);
end