function?EKF_angle
clc;clear;
T=1;
N=40/T;
X=zeros（4N）;
X（:1）=[021400-10];
Z=zeros（1N）;???%?此處有誤，請修改為P91頁一致即可運行
delta_w=1e-4;
Q=delta_w*diag（[11]）?;
G=[T^2/20;T0;0T^2/2;0T];
R=0.1*pi/180;
F=[1T00;0100;001T;0001];
x0=0;
y0=1000;?
Xstation=[x0;y0];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
w=sqrtm（R）*randn（1N）;
for?t=2:N
????X（:t）=F*X（:t-1）+G*sqrtm（Q）*randn（21）;
end
for?t=1:N
????Z（t）=hfun（X（:t）Xstation）+w（t）;
end
Xekf=zeros（4N）;
Xekf（:1）=X（:1）;
P0=eye（4）;
for?i=2:N
????Xn=F*Xekf（:i-1）;
????P1=F*P0*F‘+G*Q*G‘;
????dd=hfun（XnXstation）;
????D=Dist（XnXstation）;
????H=[-（Xn（31）-y0）/D0（Xn（11）-x0）/D0];
????K=P1*H‘*inv（H*P1*H‘+R）;
????Xekf（:i）=Xn+K*（Z（:i）-dd）;
????P0=（eye（4）-K*H）*P1;
end

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????2122??2018-01-11?14:35??4.3.5\EKF_angle.m
?????目錄???????????0??2018-03-14?21:20??4.3.5\