matcen.m

基于kalman滤波器的信息融合 用MATLAB应用举例实现

function [x, Ptra, time] = matcen(xfilt, PreP)

time = 0;

[ss,T,L] = size(xfilt);


x = zeros(ss,T);
P = zeros(ss*L);
I = eye(ss);
e = eye(ss);
for i = 1:(L-1)
    e = [e;I];
end


for t = 1:T
    tt = cputime;
    for i = 0:(L-1)
        for j = 0:(L-1)
            P((i*ss+1):(i+1)*ss,(j*ss+1):(j+1)*ss) = PreP(:,:,t,(i+1),(j+1));
        end
    end
    A = inv(P)*e*inv(e'*inv(P)*e);
    for i = 1:L
        x(:,t) = x(:,t) + A((i-1)*ss+1:i*ss,:)'*xfilt(:,t,i);
    end
    time = time + cputime - tt;
    P0(:,:,t) = inv(e'*inv(P)*e);
    Ptra(t) = trace(P0(:,:,t));
end