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.

  • $\begingroup$ What is err_x? It looks like an acceleration term. $\endgroup$ – holmeski Aug 3 at 10:08
  • $\begingroup$ @holmeski, I used this as an reference. And it's indeed acceleration noise. $\endgroup$ – Victoria Kepler Aug 3 at 12:02
  • 1
    $\begingroup$ Noise terms should be used to drive Q, they should not be in your propagation equations. How is Q being calculated currently? $\endgroup$ – holmeski Aug 3 at 12:50
  • $\begingroup$ @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 :) $\endgroup$ – Victoria Kepler Aug 3 at 13:21
  • $\begingroup$ @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? $\endgroup$ – Victoria Kepler Aug 4 at 11:16

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.