matlabrobotics

Implementing probabilistic velocity motion model


I'm implementing in MATLAB the probabilistic version of velocity motion model (VMM), as described in the book "Probabilistic Robotics", page 97. The pseudocode is the following.

enter image description here

How can I handle the situation when the denominator in μ is 0 (i.e. μ = Inf)? What (finite) value should μ be when this happen?

At the moment, my implementation is the following, and it's working, but putting μ = 0 is arbitrary and I'd prefer a better value.

function [p] = PBL_VMM_probability(x_t, x_tm1, u_t, alpha, T)
% Velocity Motion Model (VMM) for computation of probability.
% IN - x_t: pose at current timestep
% IN - x_tm1: pose at previous timestep
% IN - u_t: control action at current timestep (proprioceptive data)
% IN - alpha: vector of noise parameters
% IN - T: sampling period
% OUT - p: probability

% Extraction of components from pose at current timestep
x_c = x_t(1);
y_c = x_t(2);
theta_c = x_t(3);

% Extraction of components from pose at previous timestep
x = x_tm1(1);
y = x_tm1(2);
theta = x_tm1(3);

% Extraction of proprioceptive data
v = u_t(1);
omega = u_t(2);

% Noise parameters
sigma_v = sqrt(alpha(1)*(v^2) + alpha(2)*(omega^2));
sigma_omega = sqrt(alpha(3)*(v^2) + alpha(4)*(omega^2));
sigma_gamma = sqrt(alpha(5)*(v^2) + alpha(6)*(omega^2));

% Computation of probability
mu_numerator = (x - x_c)*cos(theta) + (y - y_c)*sin(theta);
mu_denominator = (y - y_c)*cos(theta) - (x - x_c)*cos(theta);
if mu_denominator == 0
    mu = 0;
else
    mu = 0.5*(mu_numerator/mu_denominator);
end
x_star = (x + x_c)/2 + mu*(y - y_c);
y_star = (y + y_c)/2 + mu*(x_c - x);
r_star = sqrt((x - x_star)^2 + (y - y_star)^2);
dtheta = atan2((y_c - y_star), (x_c - x_star)) - atan2((y - y_star), (x - x_star));
v_tilde = v - (dtheta/T)*r_star;
omega_tilde = omega - dtheta/T;
gamma_tilde = (theta_c - theta)/T - dtheta/T;
p_v = normpdf(v_tilde, 0, sigma_v);
p_omega = normpdf(omega_tilde, 0, sigma_omega);
p_gamma = normpdf(gamma_tilde, 0, sigma_gamma);
p = p_v*p_omega*p_gamma;
end

Solution

  • The answer to my question is the comment by Wolfie:

    Note that your definition of mu_denominator is wrong, you've used cos in both terms, but one should be sin. Fixing that would at least rule out the case where cos(theta)=0 causes the denominator to be zero. Could you not then just use something like if abs(mu_denominator) < 1e-9; mu_denominator = sign(mu_denominator)*1e-9; end since a small denominator means your system is expecting mu to be large, not 0 as you've got it at the minute