trackfusion.asv

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

warning off MATLAB:singularMatrix

sennum      = 5;                 %传感器个数
ss                = 3;                 %状态的维数
T                  = 300;            %采样的次数
SampleT    = 0.01;            %时隙
init_X          = [ 0; 0; 0 ];     %初值
detaw          = 1;                 %系统噪声的方差
mcnum        = 1;                 %蒙特卡罗实验的次数
init_V          = 0.1*eye(3);  %P0值设定
[m,n]            = size(init_X);
StartTime   = 50;%这个变量留待后面说明

norela = 1;%噪声是否相关的标记位，0为不相关，1为相关

noisew = zeros( 1, T );%为噪声矩阵、x阵、y阵、R、S、Spre开辟空间
x = zeros( ss, T );
xu = zeros(ss, T);
y = zeros( 1, T, ss);
R = zeros( 1, 1, ss);
S = zeros( 1, 1, ss);
Spre = zeros( ss, ss );

F = [1, SampleT, (SampleT^2)/2; 0, 1, SampleT; 0, 0, 1];%系统矩阵
W = [0; 0; 1];%噪声影响矩阵
Q = detaw;
L = sennum;



%detas数组是各个传感器观测噪声的方差，a数组是噪声不独立时各传感器噪声产生的权重，H是观测矩阵
detas(1) = 5;
detas(2) = 5;
detas(3) = 5;
detas(4) = 5;
detas(5) = 5;
a(1) = 0.5;
a(2) = 0.5;
a(3) = 0.5;
a(4) = 0.5;
a(5) = 0.5;
H(:,:,1) = [ 1, 0, 0 ];
H(:,:,2) = [ 0, 1, 0 ];
H(:,:,3) = [ 0, 0, 1 ];
H(:,:,4) = [ 1, 1, 0 ];
H(:,:,5) = [ 0, 1, 1 ];

if mcnum == 1%蒙特卡洛实验次数为1，不进行蒙特卡洛实验。
    if norela == 1%如果噪声相关
        for t = 1:T%这个循环用于产生真实轨迹和观测值
            noisew(t) = detaw^0.5*randn(1);%系统噪声（高斯噪声）
            if t == 1
                x(:,1) = init_X;%初值
                xu(:,1) = init_X;
            else
                x(:,t) = F*x(:,t-1) + W*noisew(t);
                xu(:,1) = F*xu(:,t-1);
            end
            for i = 1:L%这个循环用于产生观测噪声
                ipus = detas(i)^0.5*randn(1);
                noisev = a(i)*noisew(t) + ipus;
                y(:,t,i) = H(:,:,i)*x(:,t) + noisev;
            end
        end
        for i = 1:L%这两个循环用于产生S、R、Spre矩阵，这三个矩阵将用于融合过程
            S(:,:,i) = a(i)*detaw;
            R(:,:,i) = a(i)^2*detaw + detas(i);
        end
        for i = 1:L
            for j = 1:L
                if i == j
                    Spre(i,i) = a(i)^2*detaw + detas(i);
                else
                    Spre(i,j) = a(i)*a(j)*detaw;
                end
            end
        end
    else%如果噪声不相关
        for t = 1:T%这个循环用于产生真实轨迹和观测值
            noisew(t) = detaw^0.5*randn(1);
            if t == 1
                x(:,1) = init_X;
            else
                x(:,t) = F*x(:,t-1) + W*noisew(t);
            end
            for i = 1:L%这个循环用于产生观测噪声
                noisev = detas(i)^0.5*randn(1);
                y(:,t,i) = H(:,:,i)*x(:,t) + noisev;
            end
        end
        for i = 1:L
            S(:,:,i) = 0;
            R(:,:,i) = detas(i);
        end
        for i = 1:L
            for j = 1:L
                Spre(i,j) = 0;
            end
        end
    end

    t = cputime;
    [xfilt, Vfilt, VVfilt, K, J, An] = kalman_filter(y, F, H, detaw, W, S, R, init_X, init_V);%Kalman滤波
    filttime = cputime - t;

    t = cputime;
    PreP = Pretreat(F, H, Q, K, Vfilt, J, An, Spre, S, R, W);%计算P
    PrePtime = cputime - t;
    [xcter1, Ptra1, wmtime] = matcen(xfilt,PreP); %矩阵
    [xcter2, Ptra2, watime] = center(xfilt,Vfilt); %加权平均
    [xcter3, Ptra3, wstime] = lincen(xfilt,PreP);%标量

    Twa = filttime + watime;
    Twm = filttime + PrePtime + wmtime;
    Tws = filttime + PrePtime + wstime;
    watime = 1;
    wmtime = Twm/Twa;
    wstime = Tws/Twa;

    dfilt1 = x - xcter1;
    A1 = sum(dfilt1.^2);
    mse_filt1 = sqrt(sum(A1(StartTime:T)))/(T-StartTime);


    dfilt2 = x - xcter2;
    A2 = sum(dfilt2.^2);
    mse_filt2 = sqrt(sum(A2(StartTime:T)))/(T-StartTime);


    dfilt3 = x - xcter3;
    A3 = sum(dfilt3.^2);
    mse_filt3 = sqrt(sum(A3(StartTime:T)))/(T-StartTime);
%%
    TI = StartTime:T;
    for finum = 1:ss;
        figure
        hold on
        plot(TI,x(finum,StartTime:T),'-k');
        plot(TI, xcter1(finum,StartTime:T), '-r');
        plot(TI, xcter2(finum,StartTime:T), '-g');
        plot(TI, xcter3(finum,StartTime:T), '-m');
        xlabel('采样次数')
        ylabel('位移')
        legend('真值','矩阵加权','加权平均', '标量加权',4);
        title('融合结果比较');
        hold off
    end

    TI = 1:T;
    figure
    hold on
    title('融合中心P0的迹的比较');
    plot(TI,Ptra1,'-r');
    plot(TI,Ptra2,'-g');
    plot(TI,Ptra3,'-b');
    xlabel('采样次数')
    ylabel('P0的迹')
    legend('矩阵加权P0的迹','加权平均P0的迹', '标量加权P0的迹',3);
    hold off

    for senth = 1:sennum
        figure
        hold on
        plot(TI, H(:,:,senth)*x(:,:),'-k');
        plot(TI, y(:,:,senth), 'g*');
        plot(TI, H(:,:,senth)*xfilt(:,:,senth), 'rx:');
        hold off
        legend('真值','观测值','滤波器值',3);
        xlabel('采样次数')
        ylabel('位移')

        A = H(:,:,senth) * ( xfilt(:,:,senth) - x(:,:) );
        AA = y(:,:,senth) - H(:,:,senth) * x(:,:);
        figure
        hold on
        title('误差');
        xlabel('采样次数')
        ylabel('error')
        plot(TI,A,'-r');
        plot(TI,AA,'g*');
        legend('单传感器kalman滤波后误差','观测值与真值的误差',2);
        hold off
    end

    watime %加权平均
    wmtime %矩阵加权
    wstime %标量加权
    wm_RMS = mse_filt1  %矩阵加权
    wa_RMS = mse_filt2 %加权平均
    ws_RMS = mse_filt3 %标量加权
%%

else%蒙特卡洛实验
    %蒙特卡洛实验的过程只是将上述运行一遍的过程多次运行，并将求得的结果取平均
    wmT=0;
    wsT=0;
    wmm=0;
    waa=0;
    wss=0;
    for ttt = 1:mcnum
        if norela == 1%如果噪声相关
            for t = 1:T
                noisew(t) = detaw^0.5*randn(1);%系统噪声（高斯噪声）
                if t == 1
                    x(:,1) = init_X;
                else
                    x(:,t) = F*x(:,t-1) + W*noisew(t);
                end
                for i = 1:L
                    ipus = detas(i)^0.5*randn(1);
                    noisev = a(i)*noisew(t) + ipus;
                    y(:,t,i) = H(:,:,i)*x(:,t) + noisev;
                end
            end
            for i = 1:L
                S(:,:,i) = a(i)*detaw;
                R(:,:,i) = a(i)^2*detaw + detas(i);
            end
            for i = 1:L
                for j = 1:L
                    if i == j
                        Spre(i,i) = a(i)^2*detaw + detas(i);
                    else
                        Spre(i,j) = a(i)*a(j)*detaw;
                    end
                end
            end
        else%如果噪声不相关
            for t = 1:T
                noisew(t) = detaw^0.5*randn(1);
                if t == 1
                    x(:,1) = init_X;
                else
                    x(:,t) = F*x(:,t-1) + W*noisew(t);
                end
                for i = 1:L
                    noisev = detas(i)^0.5*randn(1);
                    y(:,t,i) = H(:,:,i)*x(:,t) + noisev;
                end
            end
            for i = 1:L
                S(:,:,i) = 0;
                R(:,:,i) = detas(i);
            end
            for i = 1:L
                for j = 1:L
                    Spre(i,j) = 0;
                end
            end
        end

        t = cputime;
        [xfilt, Vfilt, VVfilt, K, J, An] = kalman_filter(y, F, H, detaw, W, S, R, init_X, init_V);
        filttime = cputime - t;

        t = cputime;
        PreP = Pretreat(F, H, Q, K, Vfilt, J, An, Spre, S, R, W);%计算P
        PrePtime = cputime - t;
        [xcter1, Ptra1, wmtime] = matcen(xfilt,PreP); %矩阵
        [xcter2, Ptra2, watime] = center(xfilt,Vfilt); %加权平均
        [xcter3, Ptra3, wstime] = lincen(xfilt,PreP);%标量

        Twa = filttime + watime;
        Twm = filttime + PrePtime + wmtime;
        Tws = filttime + PrePtime + wstime;
        watime = 1;
        wmtime = Twm/Twa;
        wstime = Tws/Twa;
        wmT = wmT + wmtime/mcnum;
        wsT = wsT + wstime/mcnum;

        dfilt1 = x - xcter1;
        A1 = sum(dfilt1.^2);
        wm_RMS = sqrt(sum(A1(StartTime:T)))/(T-StartTime);
        wmm = wmm + wm_RMS/mcnum;
        %mse_filt1 = sqrt(sum(A1));


        dfilt2 = x - xcter2;
        A2 = sum(dfilt2.^2);
        wa_RMS = sqrt(sum(A2(StartTime:T)))/(T-StartTime);
        waa = waa + wa_RMS/mcnum;
        %mse_filt2 = sqrt(sum(A2));


        dfilt3 = x - xcter3;
        A3 = sum(dfilt3.^2);
        ws_RMS = sqrt(sum(A3(StartTime:T)))/(T-StartTime);
        wss = wss + ws_RMS/mcnum;
        %mse_filt3 = sqrt(sum(A3));

    end
    wmm
    waa
    wss
    wmT
    wsT
end