Kalman滤波在船舶GPS导航定位系统中的应用

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  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