%?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;%初始轉(zhuǎn)速差
%?
%?long=20;
%?lati=10;%初始位置
%?head=0;%當(dāng)前航向
%?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）

%%%%%%%%%%%%%%%%%%%坐標(biāo)系中任意直線跟蹤%%%%%%%%%%%%%%%%
xx0=20;
yy0=40;%初始位置
xx1=5;
yy1=5;%位點(diǎn)1
xx2=60;
yy2=60;%位點(diǎn)2
head=0;%初始航向



theta2=acosd（dot（[xx2-xx1yy2-yy1][01]）/（norm（[xx2-xx1yy2-yy1]）*norm（[01]）））;%位點(diǎn)2與位點(diǎn)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