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.

• What is err_x? It looks like an acceleration term. – holmeski Aug 3 '20 at 10:08
• @holmeski, I used this as an reference. And it's indeed acceleration noise. – Victoria Kepler Aug 3 '20 at 12:02
• Noise terms should be used to drive Q, they should not be in your propagation equations. How is Q being calculated currently? – holmeski Aug 3 '20 at 12:50
• @holmeski, Q is 3x3 matrix where on the diagonal are: err_x is the squared longitudinal acceleration noise, err_y is the yaw acceleration noise and err_z is pitch acceleration noise. They should be in the propagation equations, but while writing this comment I realized that they should be used differently. I'm going to get back to the thread after testing new implementation :) – Victoria Kepler Aug 3 '20 at 13:21
• @holmeski, in the robot_localization package I found ukf implementation with a transferFunction_ and processNoiseCovariance_ which look quite similar to what I need for prediction, but I'm not sure how to change it for my case, because I don't have roll-measurement and I can't use it in my process, also I'm not sure that my phi and theta can be used as pitch and yaw. Could you clarify this for me? – Victoria Kepler Aug 4 '20 at 11:16