clc; clear; close all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N = 10000;
r = 4;
t = 1:1:N;
T = 1;
x1 = zeros(1,N);
x2 = zeros(1,N);
x3 = zeros(1,N);
X = zeros(1,N);
Y = zeros(1,N);
yy(1)=0;
for n = 1:N
% x1(n) =cos(0.001*n)^2;
% x2(n) =sin(0.001*n)^2;
% x3(n) =n^0.5/100;
x1(n) =n*sin(n);
x2(n) =2*n;
x3(n) =n^0.5;
xx(n)=5*x1(n)+4*x2(n)+3*x3(n);
yy(n+1)=yy(n)+xx(n);
end
figure(1)
plot(xx)
hold on
plot(yy)
figure(2)
plot(x1)
hold on
plot(x2)
hold on
plot(x3)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%----------生成数据
% 卡尔曼滤波部分，继承之前初始化变量
% A：转移矩阵
% H：量测矩阵
% Qk：系统噪声矩阵
% Rk：量测噪声矩阵
% P0：均方误差矩阵初始值
% Y：火箭的状态矩阵，由k_s，k_v,k_a组成
% Y0：状态矩阵的初始值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
X =zeros(N,r);
X(1,:) =[0 1 1 1];
H = [1 0 0 0];
Qk = [0.01 0 0 0;0 0.01 0 0 ;0 0 0.01 0;0 0 0 0.01 ];
Rk = 0.01;
P0 = [0.01 0 0 0;0 0.01 0 0 ;0 0 0.01 0;0 0 0 0.01 ];
P1 = P0;
P2 = zeros(r,r);
for k = 1:N-1
A=[1 x1(k) x2(k) x3(k);0 1 0 0;0 0 1 0;0 0 0 1];
P2 = A*P1*A'+Qk;
Kk = P2*H'*inv(H*P2*H'+Rk);
P1 = (eye(r,r)-Kk*H)*P2;
X(k+1,:) = X(k,:)+Kk'*(yy(k)-H*X(k+1,:)');
end
figure(3)
plot(X(:,2),'-');
hold on
plot(X(:,3),':');
hold on
plot(X(:,4),'k');
