I'm struggling to implement Unscented Kalman Filter for tracking objects using radar. My state vector contains [x y z vx vy vz] and I can measure [rho phi theta velocity]. So everything looks trivial at the begging, because state estimation is simply
x = rho * sin(theta) * cos(phi); y = rho * sin(theta) * sin(phi); z = rho * cos(theta); vx = v * sin(theta) * cos(phi); vy = v * sin(theta) * sin(phi); vz = v * cos(theta);
Measurement model is also well-known:
rho = sqrt(p_x*p_x + p_y*p_y + p_z*p_z); phi = atan(p_y/p_x); theta = atan(sqrt(p_x*p_x + p_y*p_y)/p_z); velocity = sqrt(v_x*v_x + v_y*v_y + v_z*v_z);
My predictions are based on the constant velocity model and looks like this:
//predicted state values px_p = p_x + v_x*delta_t; py_p = p_y + v_y*delta_t; pz_p = p_z + v_z*delta_t; vx_p = v_x + err_x*delta_t; vy_p = v_y + err_y*delta_t; vz_p = v_z + err_z*delta_t;
And... it doesn't work. The only one case when it is working is constant velocity along x-axis with. Could anyone explain me what am I doing wrong? What should be Q-matrix in this case? Appreciate any tips and hints.