diff --git a/data_process_class_dll/data_process.cpp b/data_process_class_dll/data_process.cpp index 39f427a..8f4f515 100644 --- a/data_process_class_dll/data_process.cpp +++ b/data_process_class_dll/data_process.cpp @@ -18,7 +18,7 @@ int Data_Process::data_preprocess(struct DataRev Data_Input[150]) //数据存入缓存区域 Data_buffer if(Data_Input[0].point_type == 0) //tws数据 { - data_num=Data_Input[0].Point_Sum; + data_num=min(Data_Input[0].Point_Sum, 150); TAS_track_idx = Data_Input[0].TAS_track_index; for (int i=0; i - + EnvironmentId diff --git a/data_process_class_dll/dot_coh_tas.cpp b/data_process_class_dll/dot_coh_tas.cpp index aeeffb0..fa8a820 100644 --- a/data_process_class_dll/dot_coh_tas.cpp +++ b/data_process_class_dll/dot_coh_tas.cpp @@ -10,7 +10,9 @@ int Dot_Coh_TAS::dot_coh_tas_process(QVector *data_in QVector *point_recv_tas //输出点迹 ) { - + if (data_input->size() == 0) { + return 1; + } for (int loop_of_point=0; loop_of_pointsize()-1;loop_of_point++ ) { diff --git a/data_process_class_dll/kalman.cpp b/data_process_class_dll/kalman.cpp index 1064912..97f3bf4 100644 --- a/data_process_class_dll/kalman.cpp +++ b/data_process_class_dll/kalman.cpp @@ -29,7 +29,7 @@ void kalman:: kalman_filter_init_2dots(double Z0[2], double Z1[2], double T,doub R[0][0]=(pow(lambda_theta,-2)-2)*rho*rho*cos(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1+lambda_theta1*cos(2*theta)); R[1][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*sin(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*(1-lambda_theta1*cos(2*theta)); R[0][1]=(pow(lambda_theta,-2)-2)*rho*rho*sin(theta)*cos(theta)+0.5*(rho*rho+SIGMA_R*SIGMA_R)*lambda_theta1*sin(2*theta); - R[1][0]=R[1][1]; + R[1][0]=R[0][1]; P[0][0]=R[0][0]; @@ -358,10 +358,10 @@ double kalman:: d_cal_with_doppler(double F[6][6], double Q[6][6] , double Z[2], //Z(k+1|k) - double x=X[0]; - double vx=X[1]; - double y=X[3]; - double vy=X[4]; + double x=X_pred[0]; + double vx=X_pred[1]; + double y=X_pred[3]; + double vy=X_pred[4]; double h31=-y*(vx*y-vy*x)/((x*x+y*y)*sqrt(x*x+y*y)); double h32=-x/sqrt(x*x+y*y); double h34=-x*(vy*x-vx*y)/((x*x+y*y)*sqrt(x*x+y*y)); diff --git a/data_process_class_dll/parameters.h b/data_process_class_dll/parameters.h index 94dda83..1986b30 100644 --- a/data_process_class_dll/parameters.h +++ b/data_process_class_dll/parameters.h @@ -70,7 +70,7 @@ //#define DATA_RATE_MIDDLE 1 //#define DATA_RATE_FAR 1 -#define DATA_RATE_TAS 0.3 +#define DATA_RATE_TAS 0.33 #define SIGMA_R 10.0 //测量误差 #define SIGMA_A 0.02 @@ -109,9 +109,9 @@ #define H_F_WIN_LEN 3 -#define TAS_QUEUE_LENGTH 4 //Tas调用的cpi数 -#define MAX_TAS_NUM 4 -#define T_TAS_PRED 0.7 +#define TAS_QUEUE_LENGTH 1 //Tas调用的cpi数 +#define MAX_TAS_NUM 1 +#define T_TAS_PRED 0.33 #define UNCONF_TARGET 0 diff --git a/data_process_class_dll/tas_ctrl.cpp b/data_process_class_dll/tas_ctrl.cpp index 429bcde..1cbddc0 100644 --- a/data_process_class_dll/tas_ctrl.cpp +++ b/data_process_class_dll/tas_ctrl.cpp @@ -257,14 +257,14 @@ void TAS_Ctrl::tas_beam_output(QVector *trust_track, double H_track; double X_now[6]; for (int i=0;isize();i++ ) + { + if((*trust_track)[i].Track_Index == tas_target_queue[0].Index) { - if((*trust_track)[i].Track_Index == tas_target_queue[0].Index) - { - H_track = (*trust_track)[i].Height; - for(int ii=0;ii<6;ii++) - X_now[ii]=(*trust_track)[i].X[ii]; - } + H_track = (*trust_track)[i].Height; + for(int ii=0;ii<6;ii++) + X_now[ii]=(*trust_track)[i].X[ii]; } + } //预测目标位置 计算跟踪波束波位号 俯仰角 diff --git a/data_process_class_dll/track_init.cpp b/data_process_class_dll/track_init.cpp index d9b0a84..7e0bbaa 100644 --- a/data_process_class_dll/track_init.cpp +++ b/data_process_class_dll/track_init.cpp @@ -694,7 +694,7 @@ void Track_Init::tmp_track_to_trust_track(QVector *trust_track, Direction_Angle=atan2(Track_to_start[i][j].X[3],Track_to_start[i][j].X[1]); if(Direction_Angle<0) Direction_Angle=Direction_Angle+2*PI; - Trust_Track_Output[*Trust_track_num_Output-1][j-1].Direction_Angle=Direction_Angle/PI*180; + Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=Direction_Angle/PI*180; diff --git a/data_process_class_dll/track_init_direct_tracking.cpp b/data_process_class_dll/track_init_direct_tracking.cpp index 477545e..481fddc 100644 --- a/data_process_class_dll/track_init_direct_tracking.cpp +++ b/data_process_class_dll/track_init_direct_tracking.cpp @@ -370,13 +370,14 @@ void Track_Init_Direct_Tracking::tmp_track_to_trust_track( QVector memcpy(trust_track_tmp.P2, P, 6*6*sizeof(double)); memcpy(trust_track_tmp.X3, X, 6*sizeof(double)); memcpy(trust_track_tmp.P3, P, 6*6*sizeof(double)); - (*trust_track).push_back(trust_track_tmp); trust_track_tmp.pitch_num = (*Iter)[L-1].pitch_num; std::memcpy(trust_track_tmp.speed_dim, (*Iter)[L-1].speed_dim, sizeof(trust_track_tmp.speed_dim)); std::memcpy(trust_track_tmp.range_dim, (*Iter)[L-1].range_dim, sizeof(trust_track_tmp.range_dim)); + (*trust_track).push_back(trust_track_tmp); + //输出航迹更新信息 *Trust_track_num_Output=*Trust_track_num_Output+1; for (int j=0;j