function?ekf_for_track_9
clc;
clear;
n=9;
T=0.5;????????%?雷達掃描周期
N=50;?????????%?總的采樣次數

F=[100T00T^2/200;
???0100T00T^2/20;
???00100T00T^2/2;
???000100T00;
???0000100T0;
???00000100T;
???000000100;
???000000010;
???000000001];?%?狀態轉移矩陣

Q=[1?0?0?0?0?0?0?0?0;????%?過程噪聲協方差矩陣
????0?1?0?0?0?0?0?0?0;
????0?0?1?0?0?0?0?0?0;
????0?0?0?0.01?0?0?0?0?0;
????0?0?0?0?0.01?0?0?0?0;
????0?0?0?0?0?0.01?0?0?0;
????0?0?0?0?0?0?0.0001?0?0;
????0?0?0?0?0?0?0?0.0001?0;
????0?0?0?0?0?0?0?0?0.0001];

%?R=0.1*pi/180;???%?觀測噪聲方差（因為只有一個觀測，所以是一個值，不是協方差矩陣）
R?=?[100?0?0;?????????????%?觀測噪聲協方差矩陣??
????0?0.001^2?0
????0?0?0.001^2];

X=zeros（9N）;???%?目標真實位置、速度
X（:1）=[1000;5000;200;10;50;10;2;-4;2]+sqrtm（Q）*randn（n1）;

Z=zeros（3N）;???%?傳感器對位置的觀測

x0=0;???????????
y0=0;?
z0=0;
Xstation=[x0;y0;z0];???%?觀測站的位置
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for?t=2:N
????X（:t）=F*X（:t-1）+sqrtm（Q）*randn（91）;??%?目標真實軌跡
end

for?t=1:N
????%?Z（t）=hfun（X（:t）Xstation）+w（t）;??????????%?對目標的觀測
????[ddalphabeta]=funh（X（:t）Xstation）;
????Z（:t）?=?[ddalphabeta]‘?+?sqrtm（R）*randn（31）;
end

%?EKF濾波
Xekf=zeros（9N）;???%?EKF濾波值
Xekf（:1）=X（:1）;??%?EKF濾波初始化
P0?=[100?0?0?0?0?0?0?0?0;?????????????%?協方差初始化
?????0?100?0?0?0?0?0?0?0;
?????0?0?100?0?0?0?0?0?0;
?????0?0?0?1?0?0?0?0?0;
?????0?0?0?0?1?0?0?0?0;
?????0?0?0?0?0?1?0?0?0;
?????0?0?0?0?0?0?0.1?0?0;
?????0?0?0?0?0?0?0?0.1?0
?????0?0?0?0?0?0?0?0?0.1];
?
for?i=2:N
????Xn=F*Xekf（:i-1）;????????????%?預測
????P1=F*P0*F‘+?Q;???????????????%?預測誤差協方差???沒有G就不寫
????
????[ddalphabeta]=funh（XnXstation）;???%?觀測預測
????%?求Jacobian矩陣H，H為3行9列的矩陣
????D?=?Dist（XnXstation）;
????DD?=?Dist3（XnXstation）;
????H?=?[（Xn（11）-x0）/DD（Xn（21）-y0）/DD（Xn（31）-z0）/DD000000;
??????????-（Xn（21）-y0）/D^2（Xn（11）-x0）/D^20000000;
??????????（1/（1+（（Xn（31）-z0）/D）^2））.*（-2*（Xn（11）-x0）/D^4）（1/（1+（（Xn（31）-z0）/D）^2））.*（-2*（Xn（21）-y0）/D^4）（1/D）/（1/（1+（（Xn（31）-z0）/D）^2））000000];
??
????K=P1*H‘*inv（H*P1*H‘+R）;????????????????????%?kalman增益
????Xekf（:i）=Xn+K*（Z（:i）-[ddalphabeta]‘）;??%?狀態更新
????P0=（eye（9）-K*H）*P1;????????????????????????%?協方差更新
end

Err_KalmanFilter?=?zeros（1N）;
for?i=1:N
??Err_KalmanFilter（i）=?（Dist3（X（:i）Xekf（:i）））;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%?畫圖，軌跡圖
figure
t=1:N;
hold?on;
box?on;
grid?on;
plot3（X（1t）X（2t）X（3t）‘k-‘）;
plot3（Z（1t）.*cos（Z（3t））.*cos（Z（2t））Z（1t）.*cos（Z（3t））.*sin（Z（2t））Z（1t）.*sin（Z（3t））‘-b.‘）;
plot3（Xekf（1t）Xekf（2t）Xekf（3t）‘-r.‘）;
legend（‘軌跡‘‘估計點‘‘實際點‘）;
xlabel（‘x方向位置/米‘）
ylabel（‘y方向位置/米‘）
zlabel（‘z方向位置/米‘）
view（3）


figure
hold?on;
box?on;
plot（Err_KalmanFilter‘-ks‘‘MarkerFace‘‘r‘）;
legend（‘跟蹤誤差‘）;

%figure?
%hold?on;
%box?on;
%plot（Z/pi*180‘-r.‘‘MarkerFace‘‘r‘）;
%plot（Z/pi*180+w/pi*180‘-ko‘‘MarkerFace‘‘g‘）;

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2020-09-08?15:07??卡爾曼定位程序\
?????目錄???????????0??2018-01-02?21:15??卡爾曼定位程序\SI\
?????文件?????????178??2017-11-06?11:06??卡爾曼定位程序\SI\RMS.m
?????文件????????1107??2017-11-19?11:50??卡爾曼定位程序\SI\SI.m
?????文件????????1727??2017-11-17?19:42??卡爾曼定位程序\SI\main.m
?????文件?????????593??2020-09-08?14:07??卡爾曼定位程序\TOALLOP.m
?????目錄???????????0??2018-01-02?21:15??卡爾曼定位程序\Taylor\
?????文件?????????178??2017-11-06?11:06??卡爾曼定位程序\Taylor\RMS.m
?????文件????????1342??2017-11-19?12:07??卡爾曼定位程序\Taylor\Taylor.m
?????文件????????1730??2017-11-17?19:57??卡爾曼定位程序\Taylor\main.m
?????目錄???????????0??2018-01-02?21:15??卡爾曼定位程序\chan\
?????文件?????????178??2017-11-06?11:06??卡爾曼定位程序\chan\RMS.m
?????文件????????1690??2017-11-19?10:34??卡爾曼定位程序\chan\main.m
?????文件????????2522??2017-11-19?11:04??卡爾曼定位程序\chan\zz.m
?????文件????????4680??2020-09-11?20:11??卡爾曼定位程序\ekf_for_track_9.m
?????目錄???????????0??2018-01-02?21:15??卡爾曼定位程序\fang\
?????文件????????1109??2017-11-19?11:30??卡爾曼定位程序\fang\Fang.m
?????文件?????????178??2017-11-06?11:06??卡爾曼定位程序\fang\RMS.m
?????文件????????1723??2017-11-17?19:23??卡爾曼定位程序\fang\main.m
?????文件????????1996??2020-09-08?14:08??卡爾曼定位程序\main.m