Implementing probabilistic velocity motion model

Question

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

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 — Wolfie
– Wolfie, Commented Apr 9 at 9:56
Please edit the question to limit it to a specific problem with enough detail to identify an adequate answer. — Community
– Community Bot, Commented Apr 9 at 15:32

Adriaan · Accepted Answer · 2025-09-03 09:34:07Z

0

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

edited Sep 3 at 9:34

Adriaan

18.2k7 gold badges48 silver badges88 bronze badges

answered Sep 3 at 9:03

Samuele Bani

105 bronze badges

Sign up to request clarification or add additional context in comments.

1 Comment

Community Sep 3 at 10:54

Your answer could be improved with additional supporting information. Please edit to add further details, such as citations or documentation, so that others can confirm that your answer is correct. You can find more information on how to write good answers in the help center.

Collectives™ on Stack Overflow

Implementing probabilistic velocity motion model

1 Answer 1

1 Comment

Your Answer

Hot Network Questions

Collectives™ on Stack Overflow

1 Answer 1

1 Comment

Your Answer

Sign up or log in

Post as a guest

Related