基于卡尔曼滤波理论设计的航母甲板纵向运动预估器

clear all;
close all;

Ts = 0.01;%采样时间
Te = 1;%预估时间

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

F = [1 Ts; -d*Ts  1-a*Ts];%状态转移矩阵
G = [g*Ts -a*g*Ts]‘;%噪声系数矩阵
H = [1  0];%观测系数矩阵
F1 = [1 Te; -d*Te  1-a*Te];%预估矩阵

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

Q=1.0;                 %系统噪声协方差
R=1;                   %测量噪声协方差
P = [1 0; 0 0.25];     %状态估计协方差
X（:1）=[0.5 0.5]‘;     %状态估计初始值设定

Z = zeros（1length）;  %甲板运动初始值
Z（:） =5*sin（0.6*t） + sqrt（R）*randn（1length）;%甲板运动模型
Xe =