航迹融合算法MATLAB仿真程序

clear;
clc;
close all;

T=2;
N=900/T;
N1=400/T;
N2=600/T;
N3=610/T;
N4=660/T;

x=zeros（N1）;
y=zeros（N1）;
zx=zeros（N1）;
zy=zeros（N1）;

M = 100; % 仿真次数

Delta0=10;
Delta1=150;
Delta2=150;
Delta12=2;


 
%产生真实轨迹
t=2:T:400;  %沿y轴匀速直线
x0=1000+0*t‘;
y0=12000-15*t‘;
t=402:T:600;  %慢转弯
x1=x0（N1）+0.075*（（t‘-400）.^2）/2;
y1=y0（N1）-15*（t‘-400）+0.075*（（t‘-400）.^2）/2;
t=602:T:610  %匀速
vx=0.075*（600-400）;
x2=x1（N2-N1）+vx*（t‘-600）;
y2=y1（N2-N1）+0*t‘;
t=612:T:660  %快转弯
x3=x2（N3-N2）+（vx*（t‘-610）-0.3*（（t‘-610）.^2）/2）;
y3=y2（N3-N2）-0.3*（（t‘-610）.^2）/2;
t=662:T:900  %匀速直线
vy=-0.3*（660-610）;
x4=x3（N4-N3）+0*t‘;
y4=y3（N4-N3）+vy*（t‘-660）;

x=[x0;x1;x2;x3;x4]; %真实轨迹数据
y=[y0;y1;y2;y3;y4]; %真实轨迹数据

re_x1=zeros（MN）;  %记录每次仿真滤波估计数据
re_y1=zeros（MN）;
re_x2=zeros（MN）;  
re_y2=zeros（MN）;
re_x3=zeros（MN）;  %记录每次仿真滤波估计数据
re_y3=zeros（MN）;
re_x4=zeros（MN）;  
re_y4=zeros（MN）;

for m=1:M
    
%产生观测数据
nx1=randn（N1）*Delta1; %传感器1观测噪声
ny1=randn（N1）*Delta1