clear?all;
close?all;

Ts?=?0.01;%采樣時(shí)間
Te?=?1;%預(yù)估時(shí)間

g?=?0.6;
a?=?0.06;
d?=?0.36;

F?=?[1?Ts;?-d*Ts??1-a*Ts];%狀態(tài)轉(zhuǎn)移矩陣
G?=?[g*Ts?-a*g*Ts]‘;%噪聲系數(shù)矩陣
H?=?[1??0];%觀測(cè)系數(shù)矩陣
F1?=?[1?Te;?-d*Te??1-a*Te];%預(yù)估矩陣

t=0:Ts:35-Ts;
length=size（t2）;

Q=1.0;?????????????????%系統(tǒng)噪聲協(xié)方差
R=1;???????????????????%測(cè)量噪聲協(xié)方差
P?=?[1?0;?0?0.25];?????%狀態(tài)估計(jì)協(xié)方差
X（:1）=[0.5?0.5]‘;?????%狀態(tài)估計(jì)初始值設(shè)定

Z?=?zeros（1length）;??%甲板運(yùn)動(dòng)初始值
Z（:）?=5*sin（0.6*t）?+?sqrt（R）*randn（1length）;%甲板運(yùn)動(dòng)模型
Xe?=