%---------------小車軌跡跟蹤全維狀態觀測器：采用四階龍格庫塔---------
%%?
clear?all;?clc;?close?all;
Aa?=?[-2?1;-2?0];?Bb?=?[0;1];E?=?[2;2];
A?=?[0?1;0?0];?B?=?[0;1];?C?=?[1?0];
%%?初始化參數
dx0?=?0;?dx1?=?0;?????%編隊隊形相對位置
dy0?=?0;?dy1?=?0;?
x0?=?0;??x1?=?1;????%機器人初始位置
y0?=?-1.5;?y1?=?2;??

h?=?0.125;?%時間間隔
t?=?0:h:50;?
kx?=?3;?ky?=?3;?
v1?=?0.1;?%機器人初始速度
theta1?=?pi;?%機器人初始航向角

%%?領航者坐標（xy）、航向角、速度、角速度---全過程
Agv0?=?0.5;??%領航者角速度
Theta0?=?Agv0*t;??%領航者角度
V0?=?0.5;??%領航者速度
r0?=?0;?%領航者速度導數
%%?從機1-5初始化階段----從機1-5

Theta1?=?[];?V1?=?[];?Agv1?=?[];?r1?=?[];

ux1?=?0;?ux2?=?0;?ux3?=?0;
uy1?=?0;?uy2?=?0;?uy3?=?0;?
zx1?=?0;?zx2?=?0;?zx3?=?0;?
zy1?=?0;?zy2?=?0;?zy3?=?0;?

X0?=?[];?Y0?=?[];?X0（1）?=?x0;?Y0（1）?=?y0;
X1?=?[];?Y1?=?[];?X1（1）?=?x1;?Y1（1）?=?y1;

Theta1（1）?=?theta1;?V1（1）?=?v1;?

i?=?1;
figure
grid?MINOR
axis?equal
xba?=?[X1（i）;V1（i）*cos（Theta1（i））];
yba?=?[Y1（i）;V1（i）*sin（Theta1（i））];
Vhat（1）?=?V1（i）;?
x?=?[1?0.1]‘;
for?i?=?1:1:length（t）-1
?
??????zx1?=?X1（i）?-?X0（i）?-?dx1?+?dx0;
??????zy1?=?Y1（i）?-?Y0（i）?-?dy1?+?dy0;
??????
??????ux1?=?-kx*（V1（i）*cos（Theta1（i））?-?V0*cos（Theta0（i））?-?phi_xy（zx1））;
??????uy1?=?-ky*（V1（i）*sin（Theta1（i））?-?V0*sin（Theta0（i））?-?phi_xy（zy1））;

????K1x?=?Aa*xba（:i）+Bb*ux1+E*X1（i）;????????????%%?觀測器:對x方向速度觀測?采用四階龍格庫塔
????K2x?=?Aa*（xba（:i）+K1x*h/2）+Bb*ux1+E*X1（i）;
????K3x?=?Aa*（xba（:i）+K2x*h/2）+Bb*ux1+E*X1（i）;
????K4x?=?Aa*（xba（:i）+K3x*h/2）+Bb*ux1+E*X1（i）;
????xba（:i+1）?=?xba（:i）?+?h*（K1x+2*K2x+2*K3x+K4x）/6;
????
????K1y?=?Aa*yba（:i）+Bb*uy1+E*Y1（i）;????????????%%?觀測器:對y方向速度觀測?采用四階龍格庫塔
????K2y?=?Aa*（yba（:i）+K1y*h/2）+Bb*uy1+E*Y1（i）;
????K3y?=?Aa*（yba（:i）+K2y*h/2）+Bb*uy1+E*Y1（i）;
????K4y?=?Aa*（yba（:i）+K3y*h/2）+Bb*uy1+E*Y1（i）;
????yba（:i+1）?=?yba（:i）?+?h*（K1y+2*K2y+2*K3y+K4y）/6;
???????
????Vhat（i+1）?=?sqrt（（yba（2i+1））^2+（xba（2i+1））^2）;??
??????r1（i）?=?cos（Theta1（i））*ux1?+?sin（Theta1（i））*uy1?+?r0;
??????Agv1（i）?=?（（-sin（Theta1（i））/V1（i））*ux1）?+?（（cos（Theta1（i））/V1（i））*uy1）?+?Agv0;
?????
??????
??????V1（i+1）?=?V1（i）?+?r1（i）*h;
??????
??????if?（V1（i+1）??????????V1（i+1）?=?-V1（i+1）;
??????end
??????Theta1（i+1）?=?Theta1（i）?+?Agv1（i）*h;

??????
??????X0（i+1）?=?X0（i）?+?V0*cos（Theta0（i+1））*h;
??????Y0（i+1）?=?Y0（i）?+?V0*sin（Theta0（i+1））*h;
??????X1（i+1）?=?X1（i）?+?V1（i+1）*cos（Theta1（i+1））*h;
??????Y1（i+1）?=?Y1（i）?+?V1（i+1）*sin（Theta1（i+1））*h;
????
???
%????hold?on
%?????plot（[X1（i）X1（i+1）][Y1（i）Y1（i+1）]‘r-‘‘LineWidth‘2）
%?????hold?on
%?????plot（[X0（i）X0（i+1）][Y0（i）Y0（i+1）]‘k-.‘‘LineWidth‘2）
%???
%?????pause（0.05）
end??

???

figure
plot（tV1（1:length（t））‘r‘）;
hold?on?
plot（tVhat（1:length（t））‘r--‘）;
hold?on
plot（tX1（1:length（t））‘b‘）;
hold?on?
plot（txba（11:length（t））‘b--‘）;

hold?on
plot（tY1（1:length（t））‘m‘）;
hold?on?
plot（tyba（11:length（t））‘m--‘）;

grid?minor
legend（‘真實速度‘‘估計速度‘‘真實x‘‘估計x‘‘真實y‘‘估計y‘）;
xlabel（‘t/s‘）
ylabel（‘v/（m/s）‘）







???

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2018-12-31?09:08??觀測器\
?????目錄???????????0??2018-12-30?11:20??觀測器\全維狀態觀測器\
?????文件????????3126??2018-12-29?16:07??觀測器\全維狀態觀測器\carobserer.m
?????文件????????1175??2018-12-29?11:21??觀測器\全維狀態觀測器\observer.m
?????文件??????????71??2018-04-03?22:51??觀測器\全維狀態觀測器\phi_xy.m
?????文件????????1029??2018-12-29?16:11??觀測器\全維狀態觀測器\two.m
?????目錄???????????0??2018-12-30?22:55??觀測器\降維狀態觀測器\
?????文件????????1372??2018-12-30?15:14??觀測器\降維狀態觀測器\Dimensionalstateobserver.m
?????文件????????1211??2018-12-30?15:17??觀測器\降維狀態觀測器\observer.m