lqr轨迹跟踪的matlab编程

% long=20
% lati=50
% 
% A=[-1.4934 0; 0 -0.3038];
% B=[0;0.0012];
% C=[0 1];
% D=[0];
% E=[00.8];
% N=100;
% Ahat=[A zeros（21） zeros（21）;-C 0 0;-E 0 0];
% Bhat=[B;0;0];
% Q=[100 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1 ];
% R=[0.01];
% KK=lqr（AhatBhatQR）
% 
% K1=[KK（1）KK（2）];
% K2=-KK（3）;
% K3=-KK（4）;
% for i=1:N;
%     du=-0.7383*x1（i）+0.5565;
%     x1（i+1）=x1（i）+du;
%     e（i）=exhead-head（i）;
%     n（i）=15*e（i）-K1*[x2（i）;x3（i）];
%    
%     d=A*[x2（i）;x3（i）]+B*n（i）;
%     x2（i+1）=x2（i）+d（1）;
%     x3（i+1）=x3（i）+d（2）;
%     head（i+1）=head（i）+x3（i+1）*1;
%     u（i+1）=x1（i）*cos（head（i））-x2（i）*sin（head（i））;
%     v（i+1）=x1（i）*sin（head（i））+x2（i）*cos（head（i））;
%     long（i+1）=long（i）+v（i+1）*1;
%     lati（i+1）=lati（i）+u（i+1）*1;
% end
% 
% figure
% plot（longlati）
% figure
% plot（x1）
% figure
% plot（x2）
% figure
% plot（x3）
% figure
% plot（head）
% figure
% plot（n）


% tic
% 2*6/8*4*6*1/5*7*2^5/3^3
% toc 
%%%%%%%%%%%%%%%%%%%90°直线跟踪%%%%%%%%%%%%%%%%
% x1=0;
% x2=0;
% x3=0;
% 
% 
% n=0;%初始转速差
% 
% long=20;
% lati=10;%初始位置
% head=0;%当前航向
% exhead=pi/2;%期望航向
% 
% K1=[0 31.1923];
% K2=10;
% K3=0.4;
% 
% for i=1:300;
%     du=-0.7383*x1（i）+0.5565;
%     x1（i+1）=x1（i）+du;
%     e（i）=exhead-head（i）;
%     n（i）=K3*（lati（i））+K2*e（i）-K1*[x2（i）;x3（i）];
%    
%     d=A*[x2（i）;x3（i）]+B*n（i）;
%     x2（i+1）=x2（i）+d（1）;
%     x3（i+1）=x3（i）+d（2）;
%     head（i+1）=head（i）+x3（i+1）*1;
%     u（i+1）=x1（i）*cos（head（i））-x2（i）*sin（head（i））;
%     v（i+1）=x1（i）*sin（head（i））+x2（i）*cos（head（i））;
%     long（i+1）=long（i）+v（i+1）*1;
%     lati（i+1）=lati（i）+u（i+1）*1;
% end
% 
% figure
% plot（longlati）
% figure
% plot（n）

%%%%%%%%%%%%%%%%%%%坐标系中任意直线跟踪%%%%%%%%%%%%%%%%
xx0=20;
yy0=40;%初始位置
xx1=5;
yy1=5;%位点1
xx2=60;
yy2=60;%位点2
head=0;%初始航向



theta2=acosd（dot（[xx2-xx1yy2-yy1][01]）/（norm（[xx2-xx1yy2-yy1]）*norm（[01]）））;%位点2与位点1连线与y轴夹角
exhead=theta2*pi/180;
x1=0;
x2=0;
x3=0;
n=0;
% KK=[5.9583 2.6656 -0.0293];%Q=[1 0 0;0 1 0;0 0 0.01]R=0.1
KK=[6.1597 2.5177 -0.2769];%Q=[1 0 0;0 1 0;0 0 0.1]R=0.1
% KK=[6.0497 2.5970 -0.1428];%Q=[1 0 0;0 1 0;0 0 0.05]R=0.1

K1=KK（1）;
K2=KK（2）;
K3=KK（3）;

% A=[-1.4934 0; 0 -0.3038];
% B=[0;0.0012];
A=[0.7037 0 0;0.8432 1 0;-0.9