滤波跟踪基于matlab最近邻算法多目标航迹关联含Matlab源码 2093期

Posted 2022-09-09 海神之光

tags:

篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了滤波跟踪基于matlab最近邻算法多目标航迹关联含Matlab源码 2093期相关的知识，希望对你有一定的参考价值。

一、最近邻算法多目标航迹关联简介

1 引言
多传感器的航迹关联是多传感器信息融合系统中的关键技术之一,现有的航迹关联技术大多是在没有系统偏差情况下进行的。存在系统偏差会造成这些关联算法性能急剧下降。而有系统偏差的图像配准方法和基于目标参照拓扑的航迹关联方法,是基于航迹历史信息的航迹关联算法,实时性不高。提出的全局最近邻算法在无系统偏差情况下具有很好的关联效果,但在有系统偏差情况下,该算法会随着系统偏差的增加其关联性能急剧的下降。基于此,全局最近邻算法的基础上,采用了一种修正全局最近邻的航迹关联算法。

文中利用航迹位置偏差和航迹距离偏差进行航迹预关联,继而为全局最近邻的航迹关联算法提供可靠的先验信息,

2 系统模型
在笛卡尔坐标系中, 分布于(XA,YB)处的雷达A和(XB,YB)处的雷达B。雷达的量测值为(Rs,θs),其中s表示雷达A和雷达B,Rs表示量测距离,θs表示量测方位角。雷达s的量测模型是:

雷达s的本地极坐标系转换到公共笛卡尔坐标系中的坐标值是:

其中,(Xs,Ys)表示雷达s的量测值转换到公共笛卡尔坐标系中的值。

3 基于全局最近邻法的航迹关联
若雷达A中的航迹和雷达B的航迹预关联成功,则定义两航迹距离为:

对预关联失败的两航迹间的距离定义为:

D(i,j)=+∞ (22)

对同一时刻雷达A和雷达B的所有航迹都进行预关联可得到关联距离矩阵DnA×nB。得到关联距离矩阵后,可以将航迹关联视为一个最优分配问题,分配原则应满足:1)每一行只能分配一个元素;2)每一列只能分配一个元素;3)所有分配元素值的总和要最小。航迹关联问题可描述为:

其中,a(i,j)=1两航迹关联成功,a(i,j)=0表示两航迹关联失败。

二、部分源代码

clc;
clear all;
close all;
T=4; %采样间隔
spoint=40;%控制采样数据点
strack=30;%控制航迹对数
num=1; %仿真次数

deltrou1=170; %%% 信息源1的参数
deltsita1=0.3*pi/180;
deltv1=12;

deltrou2=180; %%% 信息源2的参数
deltsita2=0.3*pi/180;
deltv2=20;
coordinate1=[125e3,125e3]; %%% 信息源1的坐标
coordinate2=[235e3,130e3]; %%% 信息源2的坐标

Ect=zeros(num,spoint); %各次仿真的正确关联率
Eet=zeros(num,spoint); %各次仿真的错误关联率
Ec=zeros(1,spoint); %平均正确关联率
Ee=zeros(1,spoint); %平均错误关联率

for z=1:num %蒙特卡洛仿真次数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% 变量的设置 %%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
X1G=zeros(2,spoint,strack);%存储站1估计值
X2G=zeros(2,spoint,strack);%存储站2估计值
XY1=zeros(4,1,strack); %存储站1预测值
XY2=zeros(4,1,strack); %存储站2预测值

Z1L=zeros(4,spoint,strack);%存储理论值
Z2L=zeros(4,spoint,strack);%存储理论值
Z1G=zeros(4,spoint,strack);%存储站1观测值
Z2G=zeros(4,spoint,strack);%存储站2观测值

Z1LL=zeros(4,spoint,strack);%存储理论值
Z2LL=zeros(4,spoint,strack);%存储理论值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% 参数的设置 %%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A=[1,T,0,0;...
    0,1,0,0;...
    0,0,1,T;...
    0,0,0,1];
H=[1 0 0 0;...
    0 0 1 0];    %观测矩阵
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% 设置航迹，初始点在(0，0),(380000,0),(380000,270000),(0,270000)区域内正态分布，
%%% 初始速度和初始航向分别在4—1200m/s和0—2π之间均匀分布
centerpointx=380e3/2;
centerpointy=270e3/2;
for j=1:strack
    pointx=centerpointx+centerpointx*randn(1)/5;
    pointy=centerpointy+centerpointy*randn(1)/5;
    sita=2*pi*rand(1);
    vx=(4+(1200-4)*rand(1))*cos(sita);
    vy=(4+(1200-4)*rand(1))*sin(sita);
    Z1L(:,1,j)=[pointx,vx,pointy,vy]';         %%% 先设定每对航迹第一个点的 x,y 值
    Z2L(:,1,j)=[pointx,vx,pointy,vy]';
end
for j=1:strack
    for k=1:spoint-1
        Z1L(:,k+1,j)=A*Z1L(:,k,j);                    %存储理论值     站1,2
        Z2L(:,k+1,j)=A*Z2L(:,k,j);
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% 转换到各自的 rou-sita坐标下加上噪声---这才是模拟的雷达直接测得的数据：距离，角度，速度
Drousita1=zeros(3,spoint,strack);
Drousita2=zeros(3,spoint,strack);
for j=1:strack
    for i=1:spoint
        Drousita1(1,i,j)=sqrt((Z1L(1,i,j)-coordinate1(1))^2+(Z1L(3,i,j)-coordinate1(2))^2)+deltrou1*randn(1);
        if (Z1L(3,i,j)-coordinate1(2))>0 &&(Z1L(1,i,j)-coordinate1(1))>0
            Drousita1(2,i,j)=atan((Z1L(3,i,j)-coordinate1(2))/(Z1L(1,i,j)-coordinate1(1)))+deltsita1*randn(1);
        elseif (Z1L(3,i,j)-coordinate1(2))>0 &&(Z1L(1,i,j)-coordinate1(1))<0
            Drousita1(2,i,j)=pi+atan((Z1L(3,i,j)-coordinate1(2))/(Z1L(1,i,j)-coordinate1(1)))+deltsita1*randn(1);
        elseif (Z1L(3,i,j)-coordinate1(2))<0 &&(Z1L(1,i,j)-coordinate1(1))<0
            Drousita1(2,i,j)=pi+atan((Z1L(3,i,j)-coordinate1(2))/(Z1L(1,i,j)-coordinate1(1)))+deltsita1*randn(1);
        else
            Drousita1(2,i,j)=atan((Z1L(3,i,j)-coordinate1(2))/(Z1L(1,i,j)-coordinate1(1)))+deltsita1*randn(1);
        end
        Drousita1(3,i,j)=sqrt(Z1L(2,i,j)^2+Z1L(4,i,j)^2)+deltv1*randn(1);
    end