function data_out2 = func_kalman(data);
T             = 0.7;
LL            = length(data);
data_out      = zeros(2,LL);  %产生2*LL的全零矩阵
Y0            = [0;1];
data_out(:,1) = Y0;           %Y的第一列等于Y0
A             = [1 T
                 0 1];   
      
B      = [1/2*(T)^2 T]';
H      = [1 0];
P0     = [0 0
          0 1];
      
P      = [P0 zeros(2,2*(LL-1))];
Q      = (0.1)^2; 
R      = (0.1)^2; 
X      = zeros(1,LL);
% kalman
for n               = 1:LL
    i               = (n-1)*2+1;
    K               = P(:,i:i+1)*H'*inv(H*P(:,i:i+1)*H'+R);%滤波增益
    data_out(:,n)   = data_out(:,n)+K*(data(:,n)-H*data_out(:,n));  %估计
    data_out(:,n+1) = A*data_out(:,n);                     %预测
    P(:,i:i+1)      = (eye(2,2)-K*H)*P(:,i:i+1);  %误差
    P(:,i+2:i+3)    = A*P(:,i:i+1)*A'+B*Q*B';   %kalman滤波
end
data_out2 = data_out(1,2:end);