pretreat.m

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

function [PreP] = Pretreat(F, H, Q, K, Vfilt, J, An, Spre, S, R, W)

[ss,os,T,L] = size(K);

I = eye(ss);

%for i = 1:L
%    for j = 1:L
%        init_P(:,:,i,j) = Vfilt(:,:,1,i);
%    end
%end
%init_P = 0.1*eye(ss);

for t = 1:T
    for i = 1:L
        for j = 1:L
            if i == j
                PreP(:,:,t,i,i) = Vfilt(:,:,t,i);
            else
                if t == 1
                    %PreP(:,:,1,i,j) = [I - K(:,:,t,i)*H(:,:,i)]*[An(:,:,t,i)*init_P*An(:,:,t,j)' + W*Q*W' - J(:,:,t,j)*R(:,:,j)*J(:,:,t,j)' - J(:,:,t,i)*R(:,:,i)*J(:,:,t,i)' + J(:,:,t,i)*Spre(i,j)*J(:,:,t,j)' + An(:,:,t,i)*K(:,:,t,i)*[Spre(i,j)*J(:,:,t,j)' - S(:,:,i)'*W'] + [J(:,:,t,i)*Spre(i,j) - W*S(:,:,j)]*K(:,:,t,j)'*An(:,:,t,j)']*[I - K(:,:,t,j)*H(:,:,j)]' + K(:,:,t,i)*Spre(i,j)*K(:,:,t,j)';
                    %PreP(:,:,1,i,j) = [I - K(:,:,t,i)*H(:,:,i)]*[An(:,:,t,i)*init_P(:,:,i,j)*An(:,:,t,j)' + W*Q*W' - J(:,:,t,j)*R(:,:,j)*J(:,:,t,j)' - J(:,:,t,i)*R(:,:,i)*J(:,:,t,i)' + J(:,:,t,i)*Spre(i,j)*J(:,:,t,j)' + An(:,:,t,i)*K(:,:,t,i)*[Spre(i,j)*J(:,:,t,j)' - S(:,:,i)'*W'] + [J(:,:,t,i)*Spre(i,j) - W*S(:,:,j)]*K(:,:,t,j)'*An(:,:,t,j)']*[I - K(:,:,t,j)*H(:,:,j)]' + K(:,:,t,i)*Spre(i,j)*K(:,:,t,j)';
                    PreP(:,:,1,i,j) = Vfilt(:,:,1,i);
                else
                    PreP(:,:,t,i,j) = [I - K(:,:,t,i)*H(:,:,i)]*[An(:,:,t,i)*PreP(:,:,t-1,i,j)*An(:,:,t,j)' + W*Q*W' - J(:,:,t,j)*R(:,:,j)*J(:,:,t,j)' - J(:,:,t,i)*R(:,:,i)*J(:,:,t,i)' + J(:,:,t,i)*Spre(i,j)*J(:,:,t,j)' + An(:,:,t,i)*K(:,:,t,i)*[Spre(i,j)*J(:,:,t,j)' - S(:,:,i)'*W'] + [J(:,:,t,i)*Spre(i,j) - W*S(:,:,j)]*K(:,:,t,j)'*An(:,:,t,j)']*[I - K(:,:,t,j)*H(:,:,j)]' + K(:,:,t,i)*Spre(i,j)*K(:,:,t,j)';
                end
            end
        end
    end
end