mn逻辑航迹起始算法

使用matlab开发的M/N逻辑方法，实现航迹起始

%该程序是航迹起始中的MN逻辑算法
%仿真环境：假设5个目标做匀速直线运动，使用一个2D雷达对这个目标进行跟踪，5个目标的初始
%位置为（55000m，55000m）、（45000m，45000m）、（35000m，35000m）、（25000m，25000m）
%（15000m，15000m），五个目标的速度均为Vx=500m/sVy=0m/s。同时假设雷达的采样周期T=5s，
%雷达的测向误差和测距误差分别为0.3度和40m。
%算法的参数假设：取门限为4，采用3/4逻辑发起始航迹.
function MN_Logic

clear;
close all;
clc;
format long
%目标的初始位置
Point_1=[5500055000];
Point_2=[4500045000];
Point_3=[3500035000];
Point_4=[2500025000];
Point_5=[1500015000];
Point=[Point_1;Point_2;Point_3;Point_4;Point_5];
%5个目标的速度为
Speed=[5000];
%雷达的采样周期为
Ts=5;
%雷达进行扫描次数
N=4;
%确定最大最小速度的限制条件
Vmax=[1000100];
Vmin=[0-100];
%雷达的测向误差和测距误差分别为0.3度和40m
%极坐标转化为直角坐标系。x=r*sin（a）y=r*cos（a）
err=diag（trans（Point_3））;
err_cov=err^2;
%门限为
threshold=4;
%系统的模型为：
F=[1 0 Ts 0
   0 1 0 Ts
   0 0 1 0
   0 0 0 1];
H=[1 0 0 0
   0 1 0 0];
Gamma=[Ts*Ts/2;Ts*Ts/2;Ts;Ts];
Q=norm（err）;%过程噪声协方差阵
R=err_cov;%量测噪声的协方差矩阵
randn（‘state‘sum（100*clock））; % 设置随机数发生器 
for i=1:N
    %杂波个数是按照泊松分布的，求杂波的个数J。初始化参数
    theta=100;
    r=rand;
    total=0;
    J=0;
    for j=0:10000
        total=total+theta^j/gamma（j+1）;
        if  （total            total=total+theta^（j+1）/gamma（j+2）;
            if （total>=exp（theta）*r）
                J=j+1;
                break;
            else
                total=total-theta^（j+1）/gamma（j+2）;
            end
        end
    end
    %每个周期的J个杂波按均匀分布分布随机的分布在雷达视域范围
    noise=rand（J2）*10*10^4;
    %雷达扫描一次后的信号，包括目标信号和杂波信号
    signal（::i）={[noise;Point]};
    %扫描一次后，目标位置的更新
    Point=Point+repmat（Ts*Speed51）+Q*rands（52）;
end
%对第一次雷达扫描的数据进行关联
k=1;
for m=1:（length（cell2mat（signal（::1））））
    for n=1:（length（cell2mat（signal（::2））））
        %计算距离矢量dij
        mes=cell2mat（signal（::1））;%第一次扫描的数据
        mes_1=cell2mat（signal（::1+1））;%第二次扫描的数据
        d（1）=max（0mes_1（n1）-mes（m1）-Vmax（1）*Ts）+max（0-mes_1（n1）+mes（m1）+Vmin（1）*Ts）;
        d（2）=max（0mes_1（n2）-mes（m2）-Vmax（2）*Ts）+max（0-mes_1（n2）+mes（m2）+Vmin（2）*Ts）;
        %计算归一化距离平方
        err1=diag（trans（mes_1（n:）））;
        err2=diag（trans（mes（m:）））;
        err_cov1=err1^2;
        err_cov2=err2^2;
        D（mn）=d*（err_cov1+err_cov2）^-1*d‘;
        if （D（mn）<=threshold）
            pair（k:）={mes（m:）;mes_1（n:）};  %对落入相关门波的第二次扫描量测建立可能的航迹
            %计算由前两次量测组成的直线进行外推
            x_init（k:）=[mes_1（n:）（mes_1（n:）-mes（m:））/Ts];%利用前两个观测值来对初始条件进行估计
            
            %计算协方差的更新
            %%初始协方差的选取：参考刘刚博士的选择方式
            err=diag（trans（mes_1（n:）））;
            err_cov=err^2;
%             Px0（::k）=diag（[diag（2*err_cov）;diag（err）/Ts]）;
            Px0（::k）=[err_cov（11） 0 err_cov（11）/Ts 0;
                0  err_cov（22） 0 err_cov（22）/Ts;
                err_cov（11）/Ts 0 err_cov（11）/Ts^2 0;
                0 err_cov（22）/Ts 0 err_cov（22）/Ts^2];
            x_forest（k:）=F*x_init（k:）‘;%状态的一步预测
            out_forest（k:）=H*x_forest（k:）‘;%观测的一步预测
            P（::k）=F*Px0（::k）*F‘+Gamma*Q*Gamma‘;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件       10720  2009-03-30 19:09  MNLogic\MN_Logic（theta=100）.fig
     文件        7422  2009-03-30 19:07  MNLogic\MN_Logic（theta=50）.fig
     文件        6040  2009-05-21 20:09  MNLogic\MN_Logic.m
     目录           0  2014-07-07 15:24  MNLogic\