center.m

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

function [xcter,Ptra, time] = center(xfilt,Vfilt)
%这个函数用于实现融合中心的加权平均算法
%输入：
%xfilt 各个传感器的滤波估计
%Vfilt 各个传感器的滤波方差
%输出:
%xcter 融合中心的估计值

[ss T L] = size(xfilt);%ss:状态数 T:时间长度 L:传感器个数

time = 0;

for i = 1:T
    tt = cputime;
    InvP = zeros(ss);
    PreX = zeros(ss,1);
    for j = 1:L
        InvP = InvP + inv(Vfilt(:,:,i,j));
        PreX = PreX + inv(Vfilt(:,:,i,j))*xfilt(:,i,j);
    end
    P0 = inv(InvP);
    xcter(:,i) = P0*PreX;
    time = time + cputime - tt;
    Ptra(i) = trace(P0);
end