简单航迹分布式融合C源码

两条航迹分布式融合，包含源代码，原理简明易懂，加权法，方便可进行算法优化。matlab直接运行

function [ X_XS_XH_P ] = kal（ A0A1A2 ）
%KAL 此处显示有关此函数的摘要
%   此处显示详细说明

%% 初始化
ORDER =3;
PHIS=0.;
TS=.1;
% A0=400000;
% A1=-6000.;
% A2=-16.1;
XH=0;
XDH=0;
XDDH=0;
SIGNOISE=100.;
% P
P=99999999999999.*eye（ORDERORDER）;
% PHI
PHI=eye（ORDERORDER）;
PHI（12）=TS;
PHI（13）=.5*TS*TS;
PHI（23）=TS;
% H
H=eye（1ORDER）;
% R
R=SIGNOISE^2;
% Q
Q=zeros（ORDERORDER）;
Q（11）=PHIS*TS^5/20;
Q（12）=PHIS*TS^4/8;
Q（13）=PHIS*TS^3/6;
Q（21）=Q（12）;
Q（22）=PHIS*TS^3/3;
Q（23）=PHIS*TS^2/2;
Q（31）=Q（13）;
Q（32）=Q（23）;
Q（33）=PHIS*TS;
% IDN
IDN=eye（ORDER）;


TT=[];
X_=[];XD_=[];XDD_=[];
XS_=[];
XH_=[];XDH_=[];XDDH_=[];
SP11_=[];SP22_=[];SP33_=[];
XHERR_=[];XDHERR_=[];XDDHERR_=[];

%% KF滤波循环
for T=0:TS:30
    % Riccati方程计算:滤波器增益K
    M=PHI*P*PHI‘+Q;
    K=M*H‘*inv（H*M*H‘+R）;
    P=（IDN-K*H）*M;
    % 信号真实值
    X=A0+A1*T+A2*T*T;
    XD=A1+2*A2*T;
    XDD=2*A2;
    %信号测量值
    XS=X+normrnd（0SIGNOISE）;
    % KF估计
        % 残差RES_k
    RES=XS-XH-TS*XDH-.5*TS*TS*XDDH;
        % KF方程
    XH=XH+XDH*TS+.5*TS*TS*XDDH+K（11）*RES;
    XDH=XDH+XDDH*TS+K（21）*RES;
    XDDH=XDDH+K（31）*RES;
    % 估计结果分析
    SP11=sqrt（P（11））;
    SP22=sqrt（P（22））;
    SP33=sqrt（P（33））;
    XHERR=X-XH;
    XDHERR=XD-XDH;
    XDDHERR=XDD-XDDH;
    % 存储滤波计算结果
    TT=[TT;T];
    X_=[X_;X];XD_=[XD_;XD];XDD_=[XDD_;XDD];
    XS_=[XS_;XS];
    XH_=[XH_;XH];XDH_=[XDH_;XDH];XDDH_=[XDDH_;XDDH];
    SP11_=[SP11_;SP11];SP22_=[SP22_;SP22];SP33_=[SP33_;SP33];
    XHERR_=[XHERR_;XHERR];XDHERR_=[XDHERR_;XDHERR];XDDHERR_=[XDDHERR_;XDDHERR];
end

end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2017-10-19 15:58  tracking_fusion\
     文件        1658  2017-05-20 15:20  tracking_fusion\kalman.m
     文件         369  2017-05-20 15:17  tracking_fusion\measuretrack.m
     文件         330  2017-05-20 15:17  tracking_fusion\realtrack.m
     文件         357  2017-05-18 14:14  tracking_fusion\single_track.m
     文件       46361  2017-05-18 13:39  tracking_fusion\track.fig
     文件        6168  2017-05-18 14:31  tracking_fusion\track.m
     文件         390  2017-05-20 15:17  tracking_fusion\trackfusion.m
     文件         672  2017-05-12 18:03  tracking_fusion\trackfusion2.m
     文件         799  2017-05-20 23:59  tracking_fusion\trackfusion3.m