waiwaylee/radar_data_process
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新：1、kalman_filter_init_2dots函数中量测噪声协方差矩阵R中R[1][0]赋值错误；

Browse Source 
2、数据预处理中波位号判断规则修改；
3、数据预处理中增加点迹数量限制，避免超过数组上限；
4、Dot_Coh_TAS::dot_coh_tas_process中增加容器非空判断；
5、d_cal_with_doppler中计算雅可比矩阵改为用预测值计算；
6、tmp_track_to_trust_track中数组索引为负的问题修复；
7、tmp_track_to_trust_track中对容器push_back后在修改元素可能导致无法修改容器中的元素；
8、修改部分参数，适配机扫模式；

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-07-06 14:21:49 +08:00
parent 28fcf06060
commit e9b8a3245a
9 changed files with 25 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/kalman.cpp
+5 -5
View File
@@ -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));