highspeedlogic算法仿真-卡尔曼滤波

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);