lincen.m

来自「基于kalman滤波器的信息融合 用MATLAB应用举例实现」· M 代码 · 共 36 行

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

time = 0;

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

e = ones(L,1);
A = zeros(L,L);
x = zeros(ss,T);



for t = 1:T
    tt = cputime;
    for i = 1:L
        for j = 1:L
            A(i,j) = trace(PreP(:,:,t,i,j));
        end
    end
    a = inv(A)*e*inv(e'*inv(A)*e);
    for i = 1:L
        x(:,t) = x(:,t) + a(i)*xfilt(:,t,i);
    end
    time = time + cputime - tt;
    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
    Im = [a(1)*eye(ss)];
    for i = 2:L
        Im = [Im,a(i)*eye(ss)];
    end
    P0 = Im*P*Im';
    Ptra(t) = trace(P0);
end