p_observed.m

来自「SLAM Gridsim use with sparer sensor」· M 代码 · 共 27 行

function [p_obs] = p_observed(sensor, env)

F=sensor.F;
rmax=sensor.rmax;
rho=sensor.rho;
dB=sensor.db;
sb=sensor.sb;
sr=sensor.sr;
ee=sensor.ee;
ef=sensor.ef;

delta=env.delta;
M=env.M;
d=env.d;

Ee = ((1-d)*(1-ee)+d*ef);
Ef = (d*(1-ef)+(1-d)*ee);

p_obs=0;
for r=0:floor(sr/delta)
    p_obs=p_obs + r;
end
for r=ceil(sr/delta):ceil((rmax+sr)/delta)
    p_obs=p_obs+r*Ee^(dB*max(0,r*delta-sr)^2/delta^2);
    % version with p_n
    %p_obs=p_obs + r * min(1, (Ee^(dB*r^2) + 0.5*Ee^(dB*(r*delta-sr)^2/delta^2)*min(1, Ef*dB*(2*r*delta*sr-sr)^2/delta^2)));
end