%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%??Kalman濾波在船舶GPS導航定位系統中的應用
%??詳細原理介紹及中文注釋請參考：
%??《卡爾曼濾波原理及應用-MATLAB仿真》，電子工業出版社，黃小平著。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function?main
clc;clear;
T=1;
N=80/T;
X=zeros（4N）;
X（:1）=[-100220020];
Z=zeros（2N）;
Z（:1）=[X（11）X（31）];
delta_w=1e-2;
Q=delta_w*diag（[0.510.51]）?;
R=100*eye（2）;
F=[1T00;0100;001T;0001];
H=[1000;0010];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for?t=2:N
????X（:t）=F*X（:t-1）+sqrtm（Q）*randn（41）;
????Z（:t）=H*X（:t）+sqrtm（R）*randn（21）;??%?此處有誤，請修改為P54頁第二句一致即可運行
end
Xkf=zeros（4N）;
Xkf（:1）=X（:1）;
P0=eye（4）;
for?i=2:N
????Xn=F*Xkf（:i-1）;
????P1=F*P0*F‘+Q;
????K