function w=weight(sensed,x,parameter) switch parameter case 1 % enkel US data_US; dUS_i=sensed(1); dUS=x(1); w_US=1/(sqrt(2*pi)*sigma_US)*exp(-(dUS-dUS_i)^2/(2*sigma_US^2)); w=w_US; case 2 % enkel RF data_RF; rho_i=sensed(1); %rho meausured theta_i=sensed(2); %theta meausured rho=x(1); %rho of the current particle theta=x(2); %theta of the current particle %sigma_theta=abs(asin(sigma_rho/rho)); %w=1/(sqrt(2*pi)*sigma_rho)*exp(-(rho-rho_i)^2/(2*sigma_rho^2)) *1/(sqrt(2*pi)*sigma_theta)*exp(-(theta-theta_i)^2/(2*sigma_theta^2)) *1/(sqrt(2*pi)*sigma_phi)*exp(-(phi-phi_i)^2/(2*sigma_phi^2)); q=unwrap([theta,theta_i]); theta=q(1); theta_i=q(2); w_RF=1/(sqrt(2*pi)*sigma_rho)*exp(-(rho-rho_i)^2/(2*sigma_rho^2)) *1/(sqrt(2*pi)*sigma_theta)*exp(-(theta-theta_i)^2/(2*sigma_theta^2)) ; w=w_RF; case 3 % RF en US data_RF; rho_i=sensed(1); %rho meausured theta_i=sensed(2); %theta meausured rho=x(1); %rho of the current particle theta=x(2); %theta of the current particle q=unwrap([theta,theta_i]); theta=q(1); theta_i=q(2); w_RF=1/(sqrt(2*pi)*sigma_rho)*exp(-(rho-rho_i)^2/(2*sigma_rho^2)) *1/(sqrt(2*pi)*sigma_theta)*exp(-(theta-theta_i)^2/(2*sigma_theta^2)) ; %US data_US; dUS_i=sensed(3); dUS=x(3); w_US=1/(sqrt(2*pi)*sigma_US)*exp(-(dUS-dUS_i)^2/(2*sigma_US^2)); w=w_US*w_RF; end