UKF for radar tracking
Hi folks, I have a static radar which can measure rho, phi, theta and radial velocity of the object. I would like to implement Unscented Kalman Filter for object tracking. As reference I used 2D ukf tracking and UKF from the robot_localization. I'm using following state vector:
state_vector = [px, py, pz, vx, vy, vz]
prediction model:
double p_x = Xsig_aug_(0,i);
double p_y = Xsig_aug_(1,i);
double p_z = Xsig_aug_(2,i);
double v_x = Xsig_aug_(3,i);
double v_y = Xsig_aug_(4,i);
double v_z = Xsig_aug_(5,i);
double a_x = Xsig_aug_(6,i);
double a_y = Xsig_aug_(7,i);
double a_z = Xsig_aug_(8,i);
double px_p, py_p, pz_p, vx_p, vy_p, vz_p;
px_p = p_x + v_x*delta_t + 0.5*delta_t*delta_t*a_x;
py_p = p_y + v_y*delta_t + 0.5*delta_t*delta_t*a_y;
pz_p = p_z + v_z*delta_t + 0.5*delta_t*delta_t*a_z;
vx_p = v_x + a_x*delta_t;
vy_p = v_y + a_y*delta_t;
vz_p = v_z + a_z*delta_t;
and measurement model like this:
Zsig(0,i) = sqrt(p_x*p_x + p_y*p_y + p_z*p_z); //rho
Zsig(1,i) = atan(p_y/p_x); //phi
Zsig(2,i) = atan(sqrt(p_x*p_x + p_y*p_y)/p_z); //theta
Zsig(3,i) = sqrt(v_x*v_x + v_y*v_y + v_z*v_z); //vel_rad
Q and R matrices aren't dynamically recalculated and are set during the initialization step. At this point seems pretty simple and straight forward. However the data I'm visualizing in rviz after prediction step and radar update is far from ground truth. I don't know if I have to blame my process noise parameters, measurement noise parameters, initial covariance (which is identical matrix 6x6) and during the augmentation it turns to (9x9). Does anyone have some tips how I can debug this algorithm and understand which part exactly has to be changed?
