更新：1、航迹关联时增加高度门限判断，同时加大统计距离门限，提高机动目标关联概率；
2、修复TAS波束控制时，数据率判断有误的问题； 3、更新文档； Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
@@ -417,157 +417,108 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
|
||||
//遍历所有点迹航迹 计算点迹和航迹的统计距离
|
||||
for (size_t loop_of_track = 0; loop_of_track<trust_track->size();loop_of_track++)
|
||||
{
|
||||
// if((*trust_track)[loop_of_track].manual_tracking_flag==0)
|
||||
// {
|
||||
(*trust_track)[loop_of_track].point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1
|
||||
(*trust_track)[loop_of_track].point_flag = 0; //航迹的point_flag置为0 关联上点后再置为1
|
||||
|
||||
for (size_t loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
|
||||
{
|
||||
for (size_t loop_of_point = 0; loop_of_point<point_process.size();loop_of_point++)
|
||||
{
|
||||
|
||||
//时间差
|
||||
double delta_T = (point_process[loop_of_point].CPI_Time - (*trust_track)[loop_of_track].T_track)/1000.0;
|
||||
//航迹X P
|
||||
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
|
||||
memcpy(X1,(*trust_track)[loop_of_track].X1,6*sizeof(double));
|
||||
memcpy(X2,(*trust_track)[loop_of_track].X2,6*sizeof(double));
|
||||
memcpy(X3,(*trust_track)[loop_of_track].X3,6*sizeof(double));
|
||||
memcpy(P1,(*trust_track)[loop_of_track].P1,6*6*sizeof(double));
|
||||
memcpy(P2,(*trust_track)[loop_of_track].P2,6*6*sizeof(double));
|
||||
memcpy(P3,(*trust_track)[loop_of_track].P3,6*6*sizeof(double));
|
||||
double v_track = sqrt(pow((*trust_track)[loop_of_track].X[1],2)+pow((*trust_track)[loop_of_track].X[4],2));
|
||||
double h_track = (*trust_track)[loop_of_track].Height;
|
||||
double r_track = sqrt(pow((*trust_track)[loop_of_track].X[0],2)+pow((*trust_track)[loop_of_track].X[3],2));
|
||||
//时间差
|
||||
double delta_T = (point_process[loop_of_point].CPI_Time - (*trust_track)[loop_of_track].T_track)/1000.0;
|
||||
//航迹X P
|
||||
double X1[6],X2[6],X3[6],P1[6][6],P2[6][6],P3[6][6];
|
||||
memcpy(X1,(*trust_track)[loop_of_track].X1,6*sizeof(double));
|
||||
memcpy(X2,(*trust_track)[loop_of_track].X2,6*sizeof(double));
|
||||
memcpy(X3,(*trust_track)[loop_of_track].X3,6*sizeof(double));
|
||||
memcpy(P1,(*trust_track)[loop_of_track].P1,6*6*sizeof(double));
|
||||
memcpy(P2,(*trust_track)[loop_of_track].P2,6*6*sizeof(double));
|
||||
memcpy(P3,(*trust_track)[loop_of_track].P3,6*6*sizeof(double));
|
||||
double v_track = sqrt(pow((*trust_track)[loop_of_track].X[1],2)+pow((*trust_track)[loop_of_track].X[4],2));
|
||||
double h_track = (*trust_track)[loop_of_track].Height;
|
||||
double r_track = sqrt(pow((*trust_track)[loop_of_track].X[0],2)+pow((*trust_track)[loop_of_track].X[3],2));
|
||||
|
||||
//量测信息
|
||||
double Z[3] = {point_process[loop_of_point].Range,
|
||||
point_process[loop_of_point].Azimuth,
|
||||
point_process[loop_of_point].Velocity};
|
||||
double prt = point_process[loop_of_point].PRF_index;
|
||||
double freq_ind = point_process[loop_of_point].Freq_index;
|
||||
double h_point = point_process[loop_of_point].Height;
|
||||
//量测信息
|
||||
double Z[3] = {point_process[loop_of_point].Range,
|
||||
point_process[loop_of_point].Azimuth,
|
||||
point_process[loop_of_point].Velocity};
|
||||
double prt = point_process[loop_of_point].PRF_index;
|
||||
double freq_ind = point_process[loop_of_point].Freq_index;
|
||||
double h_point = point_process[loop_of_point].Height;
|
||||
|
||||
//计算统计距离d
|
||||
double d1,d2,d3;
|
||||
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
|
||||
//计算统计距离d
|
||||
double d1,d2,d3;
|
||||
IMM_d_cal(v_track,X1,P1,X2,P2, X3,P3,Z,prt,freq_ind,delta_T,&d1,&d2,&d3,Work_Parameter);
|
||||
|
||||
//计算高度门限
|
||||
// double d_h;
|
||||
// if (r_track<1000)
|
||||
// {
|
||||
// if(abs(h_track-h_point)<=150)
|
||||
// {
|
||||
// d_h = 1;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// d_h = 0;
|
||||
// }
|
||||
//判定统计距离门限，thd_d
|
||||
bool thd_d = false;
|
||||
|
||||
// }
|
||||
// else if (r_track<2000 && r_track>=1000)
|
||||
// {
|
||||
// if(abs(h_track-h_point)<=200)
|
||||
// {
|
||||
// d_h = 1;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// d_h = 0;
|
||||
// }
|
||||
|
||||
// }
|
||||
// else if(r_track>=2000 && r_track<3000 )
|
||||
// {
|
||||
// if(abs(h_track-h_point)<=200)
|
||||
// {
|
||||
// d_h = 1;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// d_h = 0;
|
||||
// }
|
||||
// }
|
||||
// else if(r_track>=3000 && r_track<5000)
|
||||
// {
|
||||
// if(abs(h_track-h_point)<=300)
|
||||
// {
|
||||
// d_h = 1;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// d_h = 0;
|
||||
// }
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// d_h = 1;
|
||||
// }
|
||||
|
||||
// std::cout << delta_T<<" "<<d1<<" "<<d2<<" "<<d3;
|
||||
// std::cout << h_track<<" "<<h_point<<" "<<d_h;
|
||||
// d_h = 1;
|
||||
|
||||
//计算俯仰门限
|
||||
bool d_p = true;
|
||||
|
||||
// double p_track = atan2(h_track, r_track); //航迹俯仰
|
||||
// double p_point = asin(h_point/point_process[loop_of_point].Range); //点迹俯仰
|
||||
|
||||
// if( (r_track > 3000 && abs(p_track - p_point)*180/PI <= 7) || \
|
||||
// (r_track > 1000 && r_track <= 3000 && abs(p_track - p_point)*180/PI <= 5) || \
|
||||
// (r_track <= 1000 && abs(p_track - p_point)*180/PI <= 10)){
|
||||
// d_p = true;
|
||||
// }
|
||||
|
||||
//计算距离门限，R=航迹点与量测点的距离，VT=航向速度乘以扫描时间差
|
||||
double& t_x0 = (*trust_track)[loop_of_track].X[0];
|
||||
double& t_y0 = (*trust_track)[loop_of_track].X[3];
|
||||
double a_track = atan2(t_y0, t_x0); //航迹方位角
|
||||
double& r_point = point_process[loop_of_point].Range; //点迹距离
|
||||
double a_delta = abs(a_track - point_process[loop_of_point].Azimuth); //方位差
|
||||
if(a_delta > PI){
|
||||
a_delta = 2*PI - a_delta;
|
||||
}
|
||||
|
||||
double R = sqrt(r_track * r_track + r_point*r_point - 2*r_track*r_point*cos(a_delta));
|
||||
double VT = v_track*delta_T;
|
||||
|
||||
bool d_r = true;
|
||||
|
||||
if(VT < 100 && R > 300){
|
||||
//VT<100m，使用固定门限300m
|
||||
d_r = false;
|
||||
}else if((VT>=100 && VT<=300 && R/VT>3)){
|
||||
d_r = false;
|
||||
}else if(VT>300 && R > 1200){
|
||||
d_r = false;
|
||||
}
|
||||
|
||||
//小于关联门限 保存关联信息
|
||||
if(
|
||||
( r_track<=500 && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/1000.0) && d_p && d_r)//
|
||||
||( (r_track>500&&r_track<1000) && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD/100.0) && d_p && d_r)//
|
||||
||( r_track>=1000 && (d1*d1<ASSO_THORD*ASSO_THORD || d2*d2<ASSO_THORD*ASSO_THORD || d3*d3<ASSO_THORD*ASSO_THORD) && d_p && d_r)//
|
||||
)
|
||||
{
|
||||
//std::cout << "asooooooooooo";
|
||||
|
||||
struct asso_info associated_info_tmp;
|
||||
associated_info_tmp.d1=d1;
|
||||
associated_info_tmp.d2=d2;
|
||||
associated_info_tmp.d3=d3;
|
||||
associated_info_tmp.point_index=static_cast<int>(loop_of_point)+1;
|
||||
associated_info_tmp.track_index=static_cast<int>(loop_of_track)+1;
|
||||
if(d1<=d2&&d1<=d3)
|
||||
associated_info_tmp.d_min=d1;
|
||||
else if(d2<=d1&& d2<=d3)
|
||||
associated_info_tmp.d_min=d2;
|
||||
else
|
||||
associated_info_tmp.d_min=d3;
|
||||
|
||||
associated_info.push_back(associated_info_tmp);
|
||||
}
|
||||
if( (r_track <= 500 && \
|
||||
(d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD / 1000.0)) || \
|
||||
((r_track > 500 && r_track < 1000) && \
|
||||
(d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD / 100.0)) || \
|
||||
(r_track >= 1000 && \
|
||||
(d1*d1 < ASSO_THORD*ASSO_THORD || d2*d2 < ASSO_THORD*ASSO_THORD || d3*d3 < ASSO_THORD*ASSO_THORD))\
|
||||
){
|
||||
thd_d = true;
|
||||
}
|
||||
|
||||
//判定高度门限，thd_h
|
||||
bool thd_h = false;
|
||||
double delta_h = abs(h_track-h_point);
|
||||
|
||||
if( (r_track <= 1000 && delta_h <= 150) || \
|
||||
((r_track > 1000 && r_track <= 2000) && delta_h <= 200) || \
|
||||
((r_track > 2000 && r_track <= 3000) && delta_h <= 250) || \
|
||||
((r_track > 3000 && r_track <= 5000) && delta_h <= 300) || \
|
||||
r_track > 5000
|
||||
){
|
||||
thd_h = true;
|
||||
}
|
||||
|
||||
//判定距离门限，R=航迹点与量测点的距离，VT=航向速度乘以扫描时间差
|
||||
double& t_x0 = (*trust_track)[loop_of_track].X[0];
|
||||
double& t_y0 = (*trust_track)[loop_of_track].X[3];
|
||||
double a_track = atan2(t_y0, t_x0); //航迹方位角
|
||||
double& r_point = point_process[loop_of_point].Range; //点迹距离
|
||||
double a_delta = abs(a_track - point_process[loop_of_point].Azimuth); //方位差
|
||||
if(a_delta > PI){
|
||||
a_delta = 2*PI - a_delta;
|
||||
}
|
||||
|
||||
double R = sqrt(r_track * r_track + r_point*r_point - 2*r_track*r_point*cos(a_delta));
|
||||
double VT = v_track*delta_T;
|
||||
|
||||
bool thd_r = true;
|
||||
|
||||
if(VT < 100 && R > 300){
|
||||
//VT<100m，使用固定门限300m
|
||||
thd_r = false;
|
||||
}else if((VT>=100 && VT<=300 && R/VT>3)){
|
||||
thd_r = false;
|
||||
}else if(VT>300 && R > 1200){
|
||||
thd_r = false;
|
||||
}
|
||||
|
||||
//小于关联门限 保存关联信息
|
||||
if(thd_d && thd_h && thd_r)
|
||||
{
|
||||
//std::cout << "asooooooooooo";
|
||||
|
||||
struct asso_info associated_info_tmp;
|
||||
associated_info_tmp.d1=d1;
|
||||
associated_info_tmp.d2=d2;
|
||||
associated_info_tmp.d3=d3;
|
||||
associated_info_tmp.point_index=static_cast<int>(loop_of_point)+1;
|
||||
associated_info_tmp.track_index=static_cast<int>(loop_of_track)+1;
|
||||
if(d1<=d2&&d1<=d3)
|
||||
associated_info_tmp.d_min=d1;
|
||||
else if(d2<=d1&& d2<=d3)
|
||||
associated_info_tmp.d_min=d2;
|
||||
else
|
||||
associated_info_tmp.d_min=d3;
|
||||
|
||||
associated_info.push_back(associated_info_tmp);
|
||||
}
|
||||
}
|
||||
// }
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user